g2o
ba_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 #include <iostream>
29 #include <stdint.h>
30 
31 #include <unordered_set>
32 
34 #include "g2o/core/block_solver.h"
35 #include "g2o/core/solver.h"
41 //#include "g2o/math_groups/se3quat.h"
43 
44 using namespace Eigen;
45 using namespace std;
46 
47 
48 class Sample {
49 public:
50  static int uniform(int from, int to);
51  static double uniform();
52  static double gaussian(double sigma);
53 };
54 
55 static double uniform_rand(double lowerBndr, double upperBndr){
56  return lowerBndr + ((double) std::rand() / (RAND_MAX + 1.0)) * (upperBndr - lowerBndr);
57 }
58 
59 static double gauss_rand(double mean, double sigma){
60  double x, y, r2;
61  do {
62  x = -1.0 + 2.0 * uniform_rand(0.0, 1.0);
63  y = -1.0 + 2.0 * uniform_rand(0.0, 1.0);
64  r2 = x * x + y * y;
65  } while (r2 > 1.0 || r2 == 0.0);
66  return mean + sigma * y * std::sqrt(-2.0 * log(r2) / r2);
67 }
68 
69 int Sample::uniform(int from, int to){
70  return static_cast<int>(uniform_rand(from, to));
71 }
72 
73 double Sample::uniform(){
74  return uniform_rand(0., 1.);
75 }
76 
77 double Sample::gaussian(double sigma){
78  return gauss_rand(0., sigma);
79 }
80 
81 int main(int argc, const char* argv[]){
82  if (argc<2)
83  {
84  cout << endl;
85  cout << "Please type: " << endl;
86  cout << "ba_demo [PIXEL_NOISE] [OUTLIER RATIO] [ROBUST_KERNEL] [STRUCTURE_ONLY] [DENSE]" << endl;
87  cout << endl;
88  cout << "PIXEL_NOISE: noise in image space (E.g.: 1)" << endl;
89  cout << "OUTLIER_RATIO: probability of spuroius observation (default: 0.0)" << endl;
90  cout << "ROBUST_KERNEL: use robust kernel (0 or 1; default: 0==false)" << endl;
91  cout << "STRUCTURE_ONLY: performe structure-only BA to get better point initializations (0 or 1; default: 0==false)" << endl;
92  cout << "DENSE: Use dense solver (0 or 1; default: 0==false)" << endl;
93  cout << endl;
94  cout << "Note, if OUTLIER_RATIO is above 0, ROBUST_KERNEL should be set to 1==true." << endl;
95  cout << endl;
96  exit(0);
97  }
98 
99  double PIXEL_NOISE = atof(argv[1]);
100  double OUTLIER_RATIO = 0.0;
101 
102  if (argc>2) {
103  OUTLIER_RATIO = atof(argv[2]);
104  }
105 
106  bool ROBUST_KERNEL = false;
107  if (argc>3){
108  ROBUST_KERNEL = atoi(argv[3]) != 0;
109  }
110  bool STRUCTURE_ONLY = false;
111  if (argc>4){
112  STRUCTURE_ONLY = atoi(argv[4]) != 0;
113  }
114 
115  bool DENSE = false;
116  if (argc>5){
117  DENSE = atoi(argv[5]) != 0;
118  }
119 
120  cout << "PIXEL_NOISE: " << PIXEL_NOISE << endl;
121  cout << "OUTLIER_RATIO: " << OUTLIER_RATIO<< endl;
122  cout << "ROBUST_KERNEL: " << ROBUST_KERNEL << endl;
123  cout << "STRUCTURE_ONLY: " << STRUCTURE_ONLY<< endl;
124  cout << "DENSE: "<< DENSE << endl;
125 
126 
127 
128  g2o::SparseOptimizer optimizer;
129  optimizer.setVerbose(false);
131  if (DENSE) {
132  linearSolver= new g2o::LinearSolverDense<g2o
134  } else {
135  linearSolver
138  }
139 
140 
141  g2o::BlockSolver_6_3 * solver_ptr
142  = new g2o::BlockSolver_6_3(linearSolver);
144  optimizer.setAlgorithm(solver);
145 
146 
147  vector<Vector3d> true_points;
148  for (size_t i=0;i<500; ++i)
149  {
150  true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
151  Sample::uniform()-0.5,
152  Sample::uniform()+3));
153  }
154 
155  double focal_length= 1000.;
156  Vector2d principal_point(320., 240.);
157 
158  vector<g2o::SE3Quat,
159  aligned_allocator<g2o::SE3Quat> > true_poses;
160  g2o::CameraParameters * cam_params
161  = new g2o::CameraParameters (focal_length, principal_point, 0.);
162  cam_params->setId(0);
163 
164  if (!optimizer.addParameter(cam_params)) {
165  assert(false);
166  }
167 
168  int vertex_id = 0;
169  for (size_t i=0; i<15; ++i) {
170  Vector3d trans(i*0.04-1.,0,0);
171 
172  Eigen:: Quaterniond q;
173  q.setIdentity();
174  g2o::SE3Quat pose(q,trans);
175  g2o::VertexSE3Expmap * v_se3
176  = new g2o::VertexSE3Expmap();
177  v_se3->setId(vertex_id);
178  if (i<2){
179  v_se3->setFixed(true);
180  }
181  v_se3->setEstimate(pose);
182  optimizer.addVertex(v_se3);
183  true_poses.push_back(pose);
184  vertex_id++;
185  }
186  int point_id=vertex_id;
187  int point_num = 0;
188  double sum_diff2 = 0;
189 
190  cout << endl;
191  unordered_map<int,int> pointid_2_trueid;
192  unordered_set<int> inliers;
193 
194  for (size_t i=0; i<true_points.size(); ++i){
196  = new g2o::VertexSBAPointXYZ();
197  v_p->setId(point_id);
198  v_p->setMarginalized(true);
199  v_p->setEstimate(true_points.at(i)
200  + Vector3d(Sample::gaussian(1),
201  Sample::gaussian(1),
202  Sample::gaussian(1)));
203  int num_obs = 0;
204  for (size_t j=0; j<true_poses.size(); ++j){
205  Vector2d z = cam_params->cam_map(true_poses.at(j).map(true_points.at(i)));
206  if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480){
207  ++num_obs;
208  }
209  }
210  if (num_obs>=2){
211  optimizer.addVertex(v_p);
212  bool inlier = true;
213  for (size_t j=0; j<true_poses.size(); ++j){
214  Vector2d z
215  = cam_params->cam_map(true_poses.at(j).map(true_points.at(i)));
216 
217  if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480){
218  double sam = Sample::uniform();
219  if (sam<OUTLIER_RATIO){
220  z = Vector2d(Sample::uniform(0,640),
221  Sample::uniform(0,480));
222  inlier= false;
223  }
224  z += Vector2d(Sample::gaussian(PIXEL_NOISE),
225  Sample::gaussian(PIXEL_NOISE));
227  = new g2o::EdgeProjectXYZ2UV();
228  e->setVertex(0, dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p));
229  e->setVertex(1, dynamic_cast<g2o::OptimizableGraph::Vertex*>
230  (optimizer.vertices().find(j)->second));
231  e->setMeasurement(z);
232  e->information() = Matrix2d::Identity();
233  if (ROBUST_KERNEL) {
235  e->setRobustKernel(rk);
236  }
237  e->setParameterId(0, 0);
238  optimizer.addEdge(e);
239  }
240  }
241 
242  if (inlier){
243  inliers.insert(point_id);
244  Vector3d diff = v_p->estimate() - true_points[i];
245 
246  sum_diff2 += diff.dot(diff);
247  }
248  pointid_2_trueid.insert(make_pair(point_id,i));
249  ++point_id;
250  ++point_num;
251  }
252  }
253  cout << endl;
254  optimizer.initializeOptimization();
255  optimizer.setVerbose(true);
256  if (STRUCTURE_ONLY){
257  g2o::StructureOnlySolver<3> structure_only_ba;
258  cout << "Performing structure-only BA:" << endl;
260  for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
261  g2o::OptimizableGraph::Vertex* v = static_cast<g2o::OptimizableGraph::Vertex*>(it->second);
262  if (v->dimension() == 3)
263  points.push_back(v);
264  }
265  structure_only_ba.calc(points, 10);
266  }
267  //optimizer.save("test.g2o");
268  cout << endl;
269  cout << "Performing full BA:" << endl;
270  optimizer.optimize(10);
271  cout << endl;
272  cout << "Point error before optimisation (inliers only): " << sqrt(sum_diff2/point_num) << endl;
273  point_num = 0;
274  sum_diff2 = 0;
275  for (unordered_map<int,int>::iterator it=pointid_2_trueid.begin();
276  it!=pointid_2_trueid.end(); ++it){
277  g2o::HyperGraph::VertexIDMap::iterator v_it
278  = optimizer.vertices().find(it->first);
279  if (v_it==optimizer.vertices().end()){
280  cerr << "Vertex " << it->first << " not in graph!" << endl;
281  exit(-1);
282  }
284  = dynamic_cast< g2o::VertexSBAPointXYZ * > (v_it->second);
285  if (v_p==0){
286  cerr << "Vertex " << it->first << "is not a PointXYZ!" << endl;
287  exit(-1);
288  }
289  Vector3d diff = v_p->estimate()-true_points[it->second];
290  if (inliers.find(it->first)==inliers.end())
291  continue;
292  sum_diff2 += diff.dot(diff);
293  ++point_num;
294  }
295  cout << "Point error after optimisation (inliers only): " << sqrt(sum_diff2/point_num) << endl;
296  cout << endl;
297 }
std::vector< OptimizableGraph::Vertex * > VertexContainer
vector container for vertices
Vector2D cam_map(const Vector3D &trans_xyz) const
BlockSolver< BlockSolverTraits< 6, 3 > > BlockSolver_6_3
Definition: block_solver.h:181
Huber Cost Function.
Implementation of the Levenberg Algorithm.
int optimize(int iterations, bool online=false)
int main(int argc, const char *argv[])
Definition: ba_demo.cpp:81
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
Traits::PoseMatrixType PoseMatrixType
Definition: block_solver.h:101
void setVertex(size_t i, Vertex *v)
Definition: hyper_graph.h:194
static double gauss_rand(double mean, double sigma)
Definition: ba_demo.cpp:59
const VertexIDMap & vertices() const
Definition: hyper_graph.h:225
Point vertex, XYZ.
Definition: types_sba.h:137
void setId(int id_)
Definition: parameter.cpp:35
SE3 Vertex parameterized internally with a transformation matrix and externally with its exponential ...
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_rand(double lowerBndr, double upperBndr)
Definition: ba_demo.cpp:55
static double uniform()
Definition: ba_demo.cpp:73
int dimension() const
dimension of the estimated state belonging to this node
bool setParameterId(int argNum, int paramId)
void setVerbose(bool verbose)
void setAlgorithm(OptimizationAlgorithm *algorithm)
basic solver for Ax = b which has to reimplemented for different linear algebra libraries ...
bool addParameter(Parameter *p)
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
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)
virtual bool addEdge(HyperGraph::Edge *e)
virtual bool addVertex(HyperGraph::Vertex *v, Data *userData)
static double gaussian(double sigma)
Definition: ba_demo.cpp:77
OptimizationAlgorithm::SolverResult calc(OptimizableGraph::VertexContainer &vertices, int num_iters, int num_max_trials=10)