Now use Vector unknowns -> exactly same result as ceres...
							parent
							
								
									a305218bd9
								
							
						
					
					
						commit
						0fd1ff7b26
					
				|  | @ -45,6 +45,7 @@ typedef GeneralSFMFactor<Camera, Point3> SfmFactor; | |||
| #include <gtsam/3rdparty/ceres/example.h> | ||||
| #include <gtsam/nonlinear/ExpressionFactor.h> | ||||
| #include <gtsam/nonlinear/AdaptAutoDiff.h> | ||||
| // See http://www.cs.cornell.edu/~snavely/bundler/bundler-v0.3-manual.html for conventions
 | ||||
| // Special version of Cal3Bundler so that default constructor = 0,0,0
 | ||||
| struct CeresCalibration: public Cal3Bundler { | ||||
|   CeresCalibration(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0, | ||||
|  | @ -76,14 +77,15 @@ int main(int argc, char* argv[]) { | |||
|   using symbol_shorthand::P; | ||||
| 
 | ||||
|   // Load BAL file (default is tiny)
 | ||||
|   string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre"); | ||||
|   //string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre");
 | ||||
|   string defaultFilename = "/home/frank/problem-16-22106-pre.txt"; | ||||
|   SfM_data db; | ||||
|   bool success = readBAL(argc > 1 ? argv[1] : defaultFilename, db); | ||||
|   if (!success) | ||||
|     throw runtime_error("Could not access file!"); | ||||
| 
 | ||||
| #ifndef USE_GTSAM_FACTOR | ||||
|   AdaptAutoDiff<SnavelyProjection, Point2, Camera, Point3> snavely; | ||||
|   AdaptAutoDiff<SnavelyProjection, 2, 9, 3> snavely; | ||||
| #endif | ||||
| 
 | ||||
|   // Build graph
 | ||||
|  | @ -96,11 +98,11 @@ int main(int argc, char* argv[]) { | |||
| #ifdef USE_GTSAM_FACTOR | ||||
|       graph.push_back(SfmFactor(z, unit2, i, P(j))); | ||||
| #else | ||||
|       Expression<Camera> camera_(i); | ||||
|       Expression<Point3> point_(P(j)); | ||||
|       Expression<Vector9> camera_(i); | ||||
|       Expression<Vector3> point_(P(j)); | ||||
|       // Snavely expects measurements in OpenGL format, with y increasing upwards
 | ||||
|       graph.addExpressionFactor(unit2, Point2(z.x(), -z.y()), | ||||
|                                 Expression<Point2>(snavely, camera_, point_)); | ||||
|       graph.addExpressionFactor(unit2, Vector2(z.x(), -z.y()), | ||||
|                                 Expression<Vector2>(snavely, camera_, point_)); | ||||
| #endif | ||||
|     } | ||||
|   } | ||||
|  | @ -113,21 +115,31 @@ int main(int argc, char* argv[]) { | |||
| #else | ||||
|     // readBAL converts to GTSAM format, so we need to convert back !
 | ||||
|     Camera ceresCamera(gtsam2openGL(camera.pose()), camera.calibration()); | ||||
|     initial.insert((i++), ceresCamera); | ||||
|     Vector9 v9 = Camera().localCoordinates(ceresCamera); | ||||
|     initial.insert((i++), v9); | ||||
| #endif | ||||
|   } | ||||
|   BOOST_FOREACH(const SfM_Track& track, db.tracks) | ||||
|   BOOST_FOREACH(const SfM_Track& track, db.tracks) { | ||||
| #ifdef USE_GTSAM_FACTOR | ||||
|     initial.insert(P(j++), track.p); | ||||
| #else | ||||
|     Vector3 v3 = track.p.vector(); | ||||
|     initial.insert(P(j++), v3); | ||||
| #endif | ||||
|   } | ||||
| 
 | ||||
|   // Check projection of first point in first camera
 | ||||
|   Point2 expected = db.tracks.front().measurements.front().second; | ||||
| #ifdef USE_GTSAM_FACTOR | ||||
|   Camera camera = initial.at<Camera>(0); | ||||
|   Point3 point = initial.at<Point3>(P(0)); | ||||
| #ifdef USE_GTSAM_FACTOR | ||||
|   Point2 actual = camera.project(point); | ||||
| #else | ||||
|   Point2 opengl = snavely(camera, point); | ||||
|   Point2 actual(opengl.x(), -opengl.y()); | ||||
|   Vector9 camera = initial.at<Vector9>(0); | ||||
|   Vector3 point = initial.at<Vector3>(P(0)); | ||||
|   Point2 z = snavely(camera, point); | ||||
|   // Again: fix y to increase upwards
 | ||||
|   Point2 actual(z.x(), -z.y()); | ||||
| #endif | ||||
|   assert_equal(expected,actual,10); | ||||
| 
 | ||||
|  | @ -149,8 +161,7 @@ int main(int argc, char* argv[]) { | |||
|   LevenbergMarquardtParams params; | ||||
|   LevenbergMarquardtParams::SetCeresDefaults(¶ms); | ||||
|   params.setOrdering(ordering); | ||||
|   params.setVerbosity("ERROR"); | ||||
|   params.setVerbosityLM("TRYLAMBDA"); | ||||
|   params.setVerbosityLM("SUMMARY"); | ||||
|   LevenbergMarquardtOptimizer lm(graph, initial, params); | ||||
|   Values result = lm.optimize(); | ||||
| 
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue