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