g2o
sba_demo.cpp
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 H. Strasdat
3 // All rights reserved.
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are
7 // met:
8 //
9 // * Redistributions of source code must retain the above copyright notice,
10 // this list of conditions and the following disclaimer.
11 // * Redistributions in binary form must reproduce the above copyright
12 // notice, this list of conditions and the following disclaimer in the
13 // documentation and/or other materials provided with the distribution.
14 //
15 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
16 // IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
17 // TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
18 // PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
19 // HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
20 // SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED
21 // TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22 // PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23 // LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24 // NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 
27 #include <Eigen/StdVector>
28 
29 #include <unordered_set>
30 
31 #include <iostream>
32 #include <stdint.h>
33 
34 #include "g2o/config.h"
36 #include "g2o/core/block_solver.h"
37 #include "g2o/core/solver.h"
43 
44 #if defined G2O_HAVE_CHOLMOD
46 #elif defined G2O_HAVE_CSPARSE
48 #endif
49 
50 using namespace Eigen;
51 using namespace std;
52 
53 
54 class Sample
55 {
56 public:
57  static int uniform(int from, int to);
58  static double uniform();
59  static double gaussian(double sigma);
60 };
61 
62 static double uniform_rand(double lowerBndr, double upperBndr)
63 {
64  return lowerBndr + ((double) std::rand() / (RAND_MAX + 1.0)) * (upperBndr - lowerBndr);
65 }
66 
67 static double gauss_rand(double mean, double sigma)
68 {
69  double x, y, r2;
70  do {
71  x = -1.0 + 2.0 * uniform_rand(0.0, 1.0);
72  y = -1.0 + 2.0 * uniform_rand(0.0, 1.0);
73  r2 = x * x + y * y;
74  } while (r2 > 1.0 || r2 == 0.0);
75  return mean + sigma * y * std::sqrt(-2.0 * log(r2) / r2);
76 }
77 
78 int Sample::uniform(int from, int to)
79 {
80  return static_cast<int>(uniform_rand(from, to));
81 }
82 
83 double Sample::uniform()
84 {
85  return uniform_rand(0., 1.);
86 }
87 
88 double Sample::gaussian(double sigma)
89 {
90  return gauss_rand(0., sigma);
91 }
92 
93 int main(int argc, const char* argv[])
94 {
95  if (argc<2)
96  {
97  cout << endl;
98  cout << "Please type: " << endl;
99  cout << "ba_demo [PIXEL_NOISE] [OUTLIER RATIO] [ROBUST_KERNEL] [STRUCTURE_ONLY] [DENSE]" << endl;
100  cout << endl;
101  cout << "PIXEL_NOISE: noise in image space (E.g.: 1)" << endl;
102  cout << "OUTLIER_RATIO: probability of spuroius observation (default: 0.0)" << endl;
103  cout << "ROBUST_KERNEL: use robust kernel (0 or 1; default: 0==false)" << endl;
104  cout << "STRUCTURE_ONLY: performe structure-only BA to get better point initializations (0 or 1; default: 0==false)" << endl;
105  cout << "DENSE: Use dense solver (0 or 1; default: 0==false)" << endl;
106  cout << endl;
107  cout << "Note, if OUTLIER_RATIO is above 0, ROBUST_KERNEL should be set to 1==true." << endl;
108  cout << endl;
109  exit(0);
110  }
111 
112  double PIXEL_NOISE = atof(argv[1]);
113 
114  double OUTLIER_RATIO = 0.0;
115 
116  if (argc>2)
117  {
118  OUTLIER_RATIO = atof(argv[2]);
119  }
120 
121  bool ROBUST_KERNEL = false;
122  if (argc>3)
123  {
124  ROBUST_KERNEL = atoi(argv[3]) != 0;
125  }
126  bool STRUCTURE_ONLY = false;
127  if (argc>4)
128  {
129  STRUCTURE_ONLY = atoi(argv[4]) != 0;
130  }
131 
132  bool DENSE = false;
133  if (argc>5)
134  {
135  DENSE = atoi(argv[5]) != 0;
136  }
137 
138  cout << "PIXEL_NOISE: " << PIXEL_NOISE << endl;
139  cout << "OUTLIER_RATIO: " << OUTLIER_RATIO<< endl;
140  cout << "ROBUST_KERNEL: " << ROBUST_KERNEL << endl;
141  cout << "STRUCTURE_ONLY: " << STRUCTURE_ONLY<< endl;
142  cout << "DENSE: "<< DENSE << endl;
143 
144 
145 
146  g2o::SparseOptimizer optimizer;
147  optimizer.setVerbose(false);
149  if (DENSE)
150  {
152  cerr << "Using DENSE" << endl;
153  }
154  else
155  {
156 #ifdef G2O_HAVE_CHOLMOD
157  cerr << "Using CHOLMOD" << endl;
159 #elif defined G2O_HAVE_CSPARSE
161  cerr << "Using CSPARSE" << endl;
162 #else
163 #error neither CSparse nor Cholmod are available
164 #endif
165  }
166 
167 
168  g2o::BlockSolver_6_3 * solver_ptr
169  = new g2o::BlockSolver_6_3(linearSolver);
170 
172 
173  optimizer.setAlgorithm(solver);
174 
175  // set up 500 points
176  vector<Vector3d> true_points;
177  for (size_t i=0;i<500; ++i)
178  {
179  true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
180  Sample::uniform()-0.5,
181  Sample::uniform()+10));
182  }
183 
184 
185  Vector2d focal_length(500,500); // pixels
186  Vector2d principal_point(320,240); // 640x480 image
187  double baseline = 0.075; // 7.5 cm baseline
188 
189 
190  vector<Eigen::Isometry3d,
191  aligned_allocator<Eigen::Isometry3d> > true_poses;
192 
193  // set up camera params
194  g2o::VertexSCam::setKcam(focal_length[0],focal_length[1],
195  principal_point[0],principal_point[1],
196  baseline);
197 
198  // set up 5 vertices, first 2 fixed
199  int vertex_id = 0;
200  for (size_t i=0; i<5; ++i)
201  {
202 
203 
204  Vector3d trans(i*0.04-1.,0,0);
205 
206  Eigen:: Quaterniond q;
207  q.setIdentity();
208  Eigen::Isometry3d pose;
209  pose = q;
210  pose.translation() = trans;
211 
212 
213  g2o::VertexSCam * v_se3
214  = new g2o::VertexSCam();
215 
216  v_se3->setId(vertex_id);
217  v_se3->setEstimate(pose);
218  v_se3->setAll(); // set aux transforms
219 
220  if (i<2)
221  v_se3->setFixed(true);
222 
223  optimizer.addVertex(v_se3);
224  true_poses.push_back(pose);
225  vertex_id++;
226  }
227 
228  int point_id=vertex_id;
229  int point_num = 0;
230  double sum_diff2 = 0;
231 
232  cout << endl;
233  unordered_map<int,int> pointid_2_trueid;
234  unordered_set<int> inliers;
235 
236  // add point projections to this vertex
237  for (size_t i=0; i<true_points.size(); ++i)
238  {
240  = new g2o::VertexSBAPointXYZ();
241 
242 
243  v_p->setId(point_id);
244  v_p->setMarginalized(true);
245  v_p->setEstimate(true_points.at(i)
246  + Vector3d(Sample::gaussian(1),
247  Sample::gaussian(1),
248  Sample::gaussian(1)));
249 
250  int num_obs = 0;
251 
252  for (size_t j=0; j<true_poses.size(); ++j)
253  {
254  Vector3d z;
255  dynamic_cast<g2o::VertexSCam*>
256  (optimizer.vertices().find(j)->second)
257  ->mapPoint(z,true_points.at(i));
258 
259  if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480)
260  {
261  ++num_obs;
262  }
263  }
264 
265  if (num_obs>=2)
266  {
267  optimizer.addVertex(v_p);
268 
269  bool inlier = true;
270  for (size_t j=0; j<true_poses.size(); ++j)
271  {
272  Vector3d z;
273  dynamic_cast<g2o::VertexSCam*>
274  (optimizer.vertices().find(j)->second)
275  ->mapPoint(z,true_points.at(i));
276 
277  if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480)
278  {
279  double sam = Sample::uniform();
280  if (sam<OUTLIER_RATIO)
281  {
282  z = Vector3d(Sample::uniform(64,640),
283  Sample::uniform(0,480),
284  Sample::uniform(0,64)); // disparity
285  z(2) = z(0) - z(2); // px' now
286 
287  inlier= false;
288  }
289 
290  z += Vector3d(Sample::gaussian(PIXEL_NOISE),
291  Sample::gaussian(PIXEL_NOISE),
292  Sample::gaussian(PIXEL_NOISE/16.0));
293 
295  = new g2o::Edge_XYZ_VSC();
296 
297 
298  e->vertices()[0]
299  = dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p);
300 
301  e->vertices()[1]
302  = dynamic_cast<g2o::OptimizableGraph::Vertex*>
303  (optimizer.vertices().find(j)->second);
304 
305  e->setMeasurement(z);
306  //e->inverseMeasurement() = -z;
307  e->information() = Matrix3d::Identity();
308 
309  if (ROBUST_KERNEL) {
311  e->setRobustKernel(rk);
312  }
313 
314  optimizer.addEdge(e);
315 
316 
317  }
318 
319  }
320 
321  if (inlier)
322  {
323  inliers.insert(point_id);
324  Vector3d diff = v_p->estimate() - true_points[i];
325 
326  sum_diff2 += diff.dot(diff);
327  }
328  // else
329  // cout << "Point: " << point_id << "has at least one spurious observation" <<endl;
330 
331  pointid_2_trueid.insert(make_pair(point_id,i));
332 
333  ++point_id;
334  ++point_num;
335  }
336 
337  }
338 
339  cout << endl;
340  optimizer.initializeOptimization();
341 
342  optimizer.setVerbose(true);
343 
344  if (STRUCTURE_ONLY)
345  {
346  cout << "Performing structure-only BA:" << endl;
347  g2o::StructureOnlySolver<3> structure_only_ba;
349  for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
350  g2o::OptimizableGraph::Vertex* v = static_cast<g2o::OptimizableGraph::Vertex*>(it->second);
351  if (v->dimension() == 3)
352  points.push_back(v);
353  }
354 
355  structure_only_ba.calc(points, 10);
356  }
357 
358  cout << endl;
359  cout << "Performing full BA:" << endl;
360  optimizer.optimize(10);
361 
362  cout << endl;
363  cout << "Point error before optimisation (inliers only): " << sqrt(sum_diff2/point_num) << endl;
364 
365 
366  point_num = 0;
367  sum_diff2 = 0;
368 
369 
370  for (unordered_map<int,int>::iterator it=pointid_2_trueid.begin();
371  it!=pointid_2_trueid.end(); ++it)
372  {
373 
374  g2o::HyperGraph::VertexIDMap::iterator v_it
375  = optimizer.vertices().find(it->first);
376 
377  if (v_it==optimizer.vertices().end())
378  {
379  cerr << "Vertex " << it->first << " not in graph!" << endl;
380  exit(-1);
381  }
382 
384  = dynamic_cast< g2o::VertexSBAPointXYZ * > (v_it->second);
385 
386  if (v_p==0)
387  {
388  cerr << "Vertex " << it->first << "is not a PointXYZ!" << endl;
389  exit(-1);
390  }
391 
392  Vector3d diff = v_p->estimate()-true_points[it->second];
393 
394  if (inliers.find(it->first)==inliers.end())
395  continue;
396 
397  sum_diff2 += diff.dot(diff);
398 
399  ++point_num;
400  }
401 
402  cout << "Point error after optimisation (inliers only): " << sqrt(sum_diff2/point_num) << endl;
403  cout << endl;
404 
405 }
std::vector< OptimizableGraph::Vertex * > VertexContainer
vector container for vertices
BlockSolver< BlockSolverTraits< 6, 3 > > BlockSolver_6_3
Definition: block_solver.h:181
Huber Cost Function.
Implementation of the Levenberg Algorithm.
int main(int argc, const char *argv[])
Definition: sba_demo.cpp:93
int optimize(int iterations, bool online=false)
This is a solver for "structure-only" optimization".
virtual void setMeasurement(const Measurement &m)
Definition: base_edge.h:76
void setRobustKernel(RobustKernel *ptr)
linear solver using dense cholesky decomposition
const VertexIDMap & vertices() const
Definition: hyper_graph.h:225
Point vertex, XYZ.
Definition: types_sba.h:137
Traits::LinearSolverType LinearSolverType
Definition: block_solver.h:110
virtual void setId(int id)
sets the id of the node in the graph be sure that the graph keeps consistent after changing the id ...
static double uniform()
Definition: ba_demo.cpp:73
int dimension() const
dimension of the estimated state belonging to this node
const VertexContainer & vertices() const
Definition: hyper_graph.h:178
void setVerbose(bool verbose)
linear solver which uses CSparse
void setAlgorithm(OptimizationAlgorithm *algorithm)
basic solver for Ax = b which has to reimplemented for different linear algebra libraries ...
void setEstimate(const EstimateType &et)
set the estimate for the vertex also calls updateCache()
Definition: base_vertex.h:101
A general case Vertex for optimization.
EIGEN_STRONG_INLINE const InformationType & information() const
information matrix of the constraint
Definition: base_edge.h:67
static double gauss_rand(double mean, double sigma)
Definition: sba_demo.cpp:67
void setMarginalized(bool marginalized)
true => this node should be marginalized out during the optimization
const EstimateType & estimate() const
return the current estimate of the vertex
Definition: base_vertex.h:99
Implementation of a solver operating on the blocks of the Hessian.
Definition: block_solver.h:96
void setFixed(bool fixed)
true => this node should be considered fixed during the optimization
virtual bool initializeOptimization(HyperGraph::EdgeSet &eset)
static void setKcam(double fx, double fy, double cx, double cy, double tx)
Definition: types_icp.h:302
virtual bool addEdge(HyperGraph::Edge *e)
virtual bool addVertex(HyperGraph::Vertex *v, Data *userData)
static double uniform_rand(double lowerBndr, double upperBndr)
Definition: sba_demo.cpp:62
Point vertex, XYZ, is in types_sba.
Definition: types_icp.h:373
Stereo camera vertex, derived from SE3 class. Note that we use the actual pose of the vertex as its p...
Definition: types_icp.h:250
static double gaussian(double sigma)
Definition: ba_demo.cpp:77
OptimizationAlgorithm::SolverResult calc(OptimizableGraph::VertexContainer &vertices, int num_iters, int num_max_trials=10)