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 <unordered_set>
28 
29 #include <iostream>
30 #include <stdint.h>
31 
32 #include "g2o/config.h"
33 #include "g2o/core/sparse_optimizer.h"
34 #include "g2o/core/block_solver.h"
35 #include "g2o/core/solver.h"
36 #include "g2o/core/robust_kernel_impl.h"
37 #include "g2o/core/optimization_algorithm_levenberg.h"
38 #include "g2o/solvers/dense/linear_solver_dense.h"
39 #include "g2o/types/icp/types_icp.h"
40 #include "g2o/solvers/structure_only/structure_only_solver.h"
41 #include "g2o/stuff/sampler.h"
42 
43 #if defined G2O_HAVE_CHOLMOD
44 #include "g2o/solvers/cholmod/linear_solver_cholmod.h"
45 #else
46 #include "g2o/solvers/eigen/linear_solver_eigen.h"
47 #endif
48 
49 using namespace Eigen;
50 using namespace std;
51 
52 class Sample {
53  public:
uniform(int from,int to)54   static int uniform(int from, int to) { return static_cast<int>(g2o::Sampler::uniformRand(from, to)); }
55 };
56 
main(int argc,const char * argv[])57 int main(int argc, const char* argv[])
58 {
59   if (argc<2)
60   {
61     cout << endl;
62     cout << "Please type: " << endl;
63     cout << "ba_demo [PIXEL_NOISE] [OUTLIER RATIO] [ROBUST_KERNEL] [STRUCTURE_ONLY] [DENSE]" << endl;
64     cout << endl;
65     cout << "PIXEL_NOISE: noise in image space (E.g.: 1)" << endl;
66     cout << "OUTLIER_RATIO: probability of spuroius observation  (default: 0.0)" << endl;
67     cout << "ROBUST_KERNEL: use robust kernel (0 or 1; default: 0==false)" << endl;
68     cout << "STRUCTURE_ONLY: performe structure-only BA to get better point initializations (0 or 1; default: 0==false)" << endl;
69     cout << "DENSE: Use dense solver (0 or 1; default: 0==false)" << endl;
70     cout << endl;
71     cout << "Note, if OUTLIER_RATIO is above 0, ROBUST_KERNEL should be set to 1==true." << endl;
72     cout << endl;
73     exit(0);
74   }
75 
76   double PIXEL_NOISE = atof(argv[1]);
77 
78   double OUTLIER_RATIO = 0.0;
79 
80   if (argc>2)
81   {
82     OUTLIER_RATIO = atof(argv[2]);
83   }
84 
85   bool ROBUST_KERNEL = false;
86   if (argc>3)
87   {
88     ROBUST_KERNEL = atoi(argv[3]) != 0;
89   }
90   bool STRUCTURE_ONLY = false;
91   if (argc>4)
92   {
93     STRUCTURE_ONLY = atoi(argv[4]) != 0;
94   }
95 
96   bool DENSE = false;
97   if (argc>5)
98   {
99     DENSE = atoi(argv[5]) != 0;
100   }
101 
102   cout << "PIXEL_NOISE: " <<  PIXEL_NOISE << endl;
103   cout << "OUTLIER_RATIO: " << OUTLIER_RATIO<<  endl;
104   cout << "ROBUST_KERNEL: " << ROBUST_KERNEL << endl;
105   cout << "STRUCTURE_ONLY: " << STRUCTURE_ONLY<< endl;
106   cout << "DENSE: "<<  DENSE << endl;
107 
108 
109 
110   g2o::SparseOptimizer optimizer;
111   optimizer.setVerbose(false);
112   std::unique_ptr<g2o::BlockSolver_6_3::LinearSolverType> linearSolver;
113   if (DENSE)
114   {
115     linearSolver = g2o::make_unique<g2o::LinearSolverDense<g2o::BlockSolver_6_3::PoseMatrixType>>();
116     cerr << "Using DENSE" << endl;
117   }
118   else
119   {
120 #ifdef G2O_HAVE_CHOLMOD
121 	cerr << "Using CHOLMOD" << endl;
122     linearSolver = g2o::make_unique<g2o::LinearSolverCholmod<g2o::BlockSolver_6_3::PoseMatrixType>>();
123 #else
124     linearSolver = g2o::make_unique<g2o::LinearSolverEigen<g2o::BlockSolver_6_3::PoseMatrixType>>();
125 	cerr << "Using CSPARSE" << endl;
126 #endif
127   }
128 
129   g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(
130     g2o::make_unique<g2o::BlockSolver_6_3>(std::move(linearSolver)));
131 
132   optimizer.setAlgorithm(solver);
133 
134   // set up 500 points
135   vector<Vector3d> true_points;
136   for (size_t i=0;i<500; ++i)
137   {
138     true_points.push_back(Vector3d((g2o::Sampler::uniformRand(0., 1.)-0.5)*3,
139                                    g2o::Sampler::uniformRand(0., 1.)-0.5,
140                                    g2o::Sampler::uniformRand(0., 1.)+10));
141   }
142 
143 
144   Vector2d focal_length(500,500); // pixels
145   Vector2d principal_point(320,240); // 640x480 image
146   double baseline = 0.075;      // 7.5 cm baseline
147 
148 
149   vector<Eigen::Isometry3d,
150       aligned_allocator<Eigen::Isometry3d> > true_poses;
151 
152   // set up camera params
153   g2o::VertexSCam::setKcam(focal_length[0],focal_length[1],
154                            principal_point[0],principal_point[1],
155                            baseline);
156 
157   // set up 5 vertices, first 2 fixed
158   int vertex_id = 0;
159   for (size_t i=0; i<5; ++i)
160   {
161 
162 
163     Vector3d trans(i*0.04-1.,0,0);
164 
165     Eigen:: Quaterniond q;
166     q.setIdentity();
167     Eigen::Isometry3d pose;
168     pose = q;
169     pose.translation() = trans;
170 
171 
172     g2o::VertexSCam * v_se3
173         = new g2o::VertexSCam();
174 
175     v_se3->setId(vertex_id);
176     v_se3->setEstimate(pose);
177     v_se3->setAll();            // set aux transforms
178 
179     if (i<2)
180       v_se3->setFixed(true);
181 
182     optimizer.addVertex(v_se3);
183     true_poses.push_back(pose);
184     vertex_id++;
185   }
186 
187   int point_id=vertex_id;
188   int point_num = 0;
189   double sum_diff2 = 0;
190 
191   cout << endl;
192   unordered_map<int,int> pointid_2_trueid;
193   unordered_set<int> inliers;
194 
195   // add point projections to this vertex
196   for (size_t i=0; i<true_points.size(); ++i)
197   {
198     g2o::VertexSBAPointXYZ * v_p
199         = new g2o::VertexSBAPointXYZ();
200 
201 
202     v_p->setId(point_id);
203     v_p->setMarginalized(true);
204     v_p->setEstimate(true_points.at(i)
205         + Vector3d(g2o::Sampler::gaussRand(0., 1),
206                    g2o::Sampler::gaussRand(0., 1),
207                    g2o::Sampler::gaussRand(0., 1)));
208 
209     int num_obs = 0;
210 
211     for (size_t j=0; j<true_poses.size(); ++j)
212     {
213       Vector3d z;
214       dynamic_cast<g2o::VertexSCam*>
215         (optimizer.vertices().find(j)->second)
216         ->mapPoint(z,true_points.at(i));
217 
218       if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480)
219       {
220         ++num_obs;
221       }
222     }
223 
224     if (num_obs>=2)
225     {
226       optimizer.addVertex(v_p);
227 
228       bool inlier = true;
229       for (size_t j=0; j<true_poses.size(); ++j)
230       {
231         Vector3d z;
232         dynamic_cast<g2o::VertexSCam*>
233           (optimizer.vertices().find(j)->second)
234           ->mapPoint(z,true_points.at(i));
235 
236         if (z[0]>=0 && z[1]>=0 && z[0]<640 && z[1]<480)
237         {
238           double sam = g2o::Sampler::uniformRand(0., 1.);
239           if (sam<OUTLIER_RATIO)
240           {
241             z = Vector3d(Sample::uniform(64,640),
242                          Sample::uniform(0,480),
243                          Sample::uniform(0,64)); // disparity
244             z(2) = z(0) - z(2); // px' now
245 
246             inlier= false;
247           }
248 
249           z += Vector3d(g2o::Sampler::gaussRand(0., PIXEL_NOISE),
250                         g2o::Sampler::gaussRand(0., PIXEL_NOISE),
251                         g2o::Sampler::gaussRand(0., PIXEL_NOISE/16.0));
252 
253           g2o::Edge_XYZ_VSC * e
254               = new g2o::Edge_XYZ_VSC();
255 
256 
257           e->vertices()[0]
258               = dynamic_cast<g2o::OptimizableGraph::Vertex*>(v_p);
259 
260           e->vertices()[1]
261               = dynamic_cast<g2o::OptimizableGraph::Vertex*>
262               (optimizer.vertices().find(j)->second);
263 
264           e->setMeasurement(z);
265           //e->inverseMeasurement() = -z;
266           e->information() = Matrix3d::Identity();
267 
268           if (ROBUST_KERNEL) {
269             g2o::RobustKernelHuber* rk = new g2o::RobustKernelHuber;
270             e->setRobustKernel(rk);
271           }
272 
273           optimizer.addEdge(e);
274 
275 
276         }
277 
278       }
279 
280       if (inlier)
281       {
282         inliers.insert(point_id);
283         Vector3d diff = v_p->estimate() - true_points[i];
284 
285         sum_diff2 += diff.dot(diff);
286       }
287      // else
288      //   cout << "Point: " << point_id <<  "has at least one spurious observation" <<endl;
289 
290       pointid_2_trueid.insert(make_pair(point_id,i));
291 
292       ++point_id;
293       ++point_num;
294     }
295 
296   }
297 
298   cout << endl;
299   optimizer.initializeOptimization();
300 
301   optimizer.setVerbose(true);
302 
303   if (STRUCTURE_ONLY)
304   {
305     cout << "Performing structure-only BA:"   << endl;
306     g2o::StructureOnlySolver<3> structure_only_ba;
307     g2o::OptimizableGraph::VertexContainer points;
308     for (g2o::OptimizableGraph::VertexIDMap::const_iterator it = optimizer.vertices().begin(); it != optimizer.vertices().end(); ++it) {
309       g2o::OptimizableGraph::Vertex* v = static_cast<g2o::OptimizableGraph::Vertex*>(it->second);
310       if (v->dimension() == 3)
311         points.push_back(v);
312     }
313 
314     structure_only_ba.calc(points, 10);
315   }
316 
317     cout << endl;
318   cout << "Performing full BA:" << endl;
319   optimizer.optimize(10);
320 
321   cout << endl;
322   cout << "Point error before optimisation (inliers only): " << sqrt(sum_diff2/inliers.size()) << endl;
323 
324 
325   point_num = 0;
326   sum_diff2 = 0;
327 
328 
329   for (unordered_map<int,int>::iterator it=pointid_2_trueid.begin();
330        it!=pointid_2_trueid.end(); ++it)
331   {
332 
333     g2o::HyperGraph::VertexIDMap::iterator v_it
334         = optimizer.vertices().find(it->first);
335 
336     if (v_it==optimizer.vertices().end())
337     {
338       cerr << "Vertex " << it->first << " not in graph!" << endl;
339       exit(-1);
340     }
341 
342     g2o::VertexSBAPointXYZ * v_p
343         = dynamic_cast< g2o::VertexSBAPointXYZ * > (v_it->second);
344 
345     if (v_p==0)
346     {
347       cerr << "Vertex " << it->first << "is not a PointXYZ!" << endl;
348       exit(-1);
349     }
350 
351     Vector3d diff = v_p->estimate()-true_points[it->second];
352 
353     if (inliers.find(it->first)==inliers.end())
354       continue;
355 
356     sum_diff2 += diff.dot(diff);
357 
358     ++point_num;
359   }
360 
361   cout << "Point error after optimisation (inliers only): " << sqrt(sum_diff2/inliers.size()) << endl;
362   cout << endl;
363 
364 }
365