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