g2o
gicp_sba_demo.cpp
Go to the documentation of this file.
1 // g2o - General Graph Optimization
2 // Copyright (C) 2011 Kurt Konolige
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 <random>
29 #include <iostream>
30 #include <stdint.h>
31 
33 #include "g2o/core/block_solver.h"
34 #include "g2o/core/solver.h"
38 
39 using namespace Eigen;
40 using namespace std;
41 using namespace g2o;
42 
43 // sampling distributions
44  class Sample
45  {
46 
47  static default_random_engine gen_real;
48  static default_random_engine gen_int;
49  public:
50  static int uniform(int from, int to);
51 
52  static double uniform();
53 
54  static double gaussian(double sigma);
55  };
56 
57 
58  default_random_engine Sample::gen_real;
59  default_random_engine Sample::gen_int;
60 
61  int Sample::uniform(int from, int to)
62  {
63  uniform_int_distribution<int> unif(from, to);
64  int sam = unif(gen_int);
65  return sam;
66  }
67 
68  double Sample::uniform()
69  {
70  std::uniform_real_distribution<double> unif(0.0, 1.0);
71  double sam = unif(gen_real);
72  return sam;
73  }
74 
75  double Sample::gaussian(double sigma)
76  {
77  std::normal_distribution<double> gauss(0.0, sigma);
78  double sam = gauss(gen_real);
79  return sam;
80  }
81 
82 
83 //
84 // set up simulated system with noise, optimize it
85 //
86 
87 int main(int argc, char **argv)
88 {
89  int num_points = 0;
90 
91  // check for arg, # of points to use in projection SBA
92  if (argc > 1)
93  num_points = atoi(argv[1]);
94 
95  double euc_noise = 0.1; // noise in position, m
96  double pix_noise = 1.0; // pixel noise
97  // double outlier_ratio = 0.1;
98 
99 
100  SparseOptimizer optimizer;
101  optimizer.setVerbose(false);
102 
103  // variable-size block solver
104  BlockSolverX::LinearSolverType * linearSolver
107 
108 
109  BlockSolverX * solver_ptr
110  = new BlockSolverX(linearSolver);
111 
113 
114  optimizer.setAlgorithm(solver);
115 
116  vector<Vector3d> true_points;
117  for (size_t i=0;i<1000; ++i)
118  {
119  true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
120  Sample::uniform()-0.5,
121  Sample::uniform()+10));
122  }
123 
124 
125  // set up camera params
126  Vector2d focal_length(500,500); // pixels
127  Vector2d principal_point(320,240); // 640x480 image
128  double baseline = 0.075; // 7.5 cm baseline
129 
130  // set up camera params and projection matrices on vertices
131  g2o::VertexSCam::setKcam(focal_length[0],focal_length[1],
132  principal_point[0],principal_point[1],
133  baseline);
134 
135 
136  // set up two poses
137  int vertex_id = 0;
138  for (size_t i=0; i<2; ++i)
139  {
140  // set up rotation and translation for this node
141  Vector3d t(0,0,i);
142  Quaterniond q;
143  q.setIdentity();
144 
145  Eigen::Isometry3d cam; // camera pose
146  cam = q;
147  cam.translation() = t;
148 
149  // set up node
150  VertexSCam *vc = new VertexSCam();
151  vc->setEstimate(cam);
152  vc->setId(vertex_id); // vertex id
153 
154  cerr << t.transpose() << " | " << q.coeffs().transpose() << endl;
155 
156  // set first cam pose fixed
157  if (i==0)
158  vc->setFixed(true);
159 
160  // make sure projection matrices are set
161  vc->setAll();
162 
163  // add to optimizer
164  optimizer.addVertex(vc);
165 
166  vertex_id++;
167  }
168 
169  // set up point matches for GICP
170  for (size_t i=0; i<true_points.size(); ++i)
171  {
172  // get two poses
173  VertexSE3* vp0 =
174  dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second);
175  VertexSE3* vp1 =
176  dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
177 
178  // calculate the relative 3D position of the point
179  Vector3d pt0,pt1;
180  pt0 = vp0->estimate().inverse() * true_points[i];
181  pt1 = vp1->estimate().inverse() * true_points[i];
182 
183  // add in noise
184  pt0 += Vector3d(Sample::gaussian(euc_noise ),
185  Sample::gaussian(euc_noise ),
186  Sample::gaussian(euc_noise ));
187 
188  pt1 += Vector3d(Sample::gaussian(euc_noise ),
189  Sample::gaussian(euc_noise ),
190  Sample::gaussian(euc_noise ));
191 
192  // form edge, with normals in varioius positions
193  Vector3d nm0, nm1;
194  nm0 << 0, i, 1;
195  nm1 << 0, i, 1;
196  nm0.normalize();
197  nm1.normalize();
198 
199  Edge_V_V_GICP * e // new edge with correct cohort for caching
200  = new Edge_V_V_GICP();
201 
202  e->vertices()[0] // first viewpoint
203  = dynamic_cast<OptimizableGraph::Vertex*>(vp0);
204 
205  e->vertices()[1] // second viewpoint
206  = dynamic_cast<OptimizableGraph::Vertex*>(vp1);
207 
208  EdgeGICP meas;
209 
210  meas.pos0 = pt0;
211  meas.pos1 = pt1;
212  meas.normal0 = nm0;
213  meas.normal1 = nm1;
214  e->setMeasurement(meas);
215  meas = e->measurement();
216  // e->inverseMeasurement().pos() = -kp;
217 
218  // use this for point-plane
219  e->information() = meas.prec0(0.01);
220 
221  // use this for point-point
222  // e->information().setIdentity();
223 
224  // e->setRobustKernel(true);
225  //e->setHuberWidth(0.01);
226 
227  optimizer.addEdge(e);
228  }
229 
230  // set up SBA projections with some number of points
231 
232  true_points.clear();
233  for (int i=0;i<num_points; ++i)
234  {
235  true_points.push_back(Vector3d((Sample::uniform()-0.5)*3,
236  Sample::uniform()-0.5,
237  Sample::uniform()+10));
238  }
239 
240 
241  // add point projections to this vertex
242  for (size_t i=0; i<true_points.size(); ++i)
243  {
245  = new g2o::VertexSBAPointXYZ();
246 
247 
248  v_p->setId(vertex_id++);
249  v_p->setMarginalized(true);
250  v_p->setEstimate(true_points.at(i)
251  + Vector3d(Sample::gaussian(1),
252  Sample::gaussian(1),
253  Sample::gaussian(1)));
254 
255  optimizer.addVertex(v_p);
256 
257  for (size_t j=0; j<2; ++j)
258  {
259  Vector3d z;
260  dynamic_cast<g2o::VertexSCam*>
261  (optimizer.vertices().find(j)->second)
262  ->mapPoint(z,true_points.at(i));
263 
264  if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480)
265  {
266  z += Vector3d(Sample::gaussian(pix_noise),
267  Sample::gaussian(pix_noise),
268  Sample::gaussian(pix_noise/16.0));
269 
271  = new g2o::Edge_XYZ_VSC();
272 
273  e->vertices()[0]
274  = dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p);
275 
276  e->vertices()[1]
277  = dynamic_cast<g2o::OptimizableGraph::Vertex*>
278  (optimizer.vertices().find(j)->second);
279 
280  e->setMeasurement(z);
281  //e->inverseMeasurement() = -z;
282  e->information() = Matrix3d::Identity();
283 
284  //e->setRobustKernel(false);
285  //e->setHuberWidth(1);
286 
287  optimizer.addEdge(e);
288  }
289 
290  }
291  } // done with adding projection points
292 
293 
294 
295  // move second cam off of its true position
296  VertexSE3* vc =
297  dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second);
298  Eigen::Isometry3d cam = vc->estimate();
299  cam.translation() = Vector3d(-0.1,0.1,0.2);
300  vc->setEstimate(cam);
301  optimizer.initializeOptimization();
302  optimizer.computeActiveErrors();
303  cout << "Initial chi2 = " << FIXED(optimizer.chi2()) << endl;
304 
305  optimizer.setVerbose(true);
306 
307  optimizer.optimize(20);
308 
309  cout << endl << "Second vertex should be near 0,0,1" << endl;
310  cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(0)->second)
311  ->estimate().translation().transpose() << endl;
312  cout << dynamic_cast<VertexSE3*>(optimizer.vertices().find(1)->second)
313  ->estimate().translation().transpose() << endl;
314 }
int main(int argc, char **argv)
Implementation of the Levenberg Algorithm.
static default_random_engine gen_int
Definition: gicp_demo.cpp:48
int optimize(int iterations, bool online=false)
Vector3D normal0
Definition: types_icp.h:69
virtual void setMeasurement(const Measurement &m)
Definition: base_edge.h:76
Traits::PoseMatrixType PoseMatrixType
Definition: block_solver.h:101
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
double chi2() const
returns the chi2 of the current configuration
const VertexContainer & vertices() const
Definition: hyper_graph.h:178
void setVerbose(bool verbose)
Vector3D pos1
Definition: types_icp.h:66
linear solver which uses CSparse
void setAlgorithm(OptimizationAlgorithm *algorithm)
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.
Vector3D normal1
Definition: types_icp.h:69
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
Matrix3D prec0(double e)
Definition: types_icp.h:114
3D pose Vertex, represented as an Isometry3D
Definition: vertex_se3.h:50
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)
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
EIGEN_STRONG_INLINE const Measurement & measurement() const
accessor functions for the measurement represented by the edge
Definition: base_edge.h:75
static default_random_engine gen_real
Definition: gicp_demo.cpp:47
BlockSolver< BlockSolverTraits< Eigen::Dynamic, Eigen::Dynamic > > BlockSolverX
Definition: block_solver.h:179
Vector3D pos0
Definition: types_icp.h:66