Split script into two scripts, isolated common code
Frank Dellaert
parent
ed9938b70c
commit
7ec056f5cc
|
|
@ -11,162 +11,46 @@
|
|||
|
||||
/**
|
||||
* @file timeSFMBAL.cpp
|
||||
* @brief time structure from motion with BAL file
|
||||
* @brief time structure from motion with BAL file, conventional GeneralSFMFactor
|
||||
* @author Frank Dellaert
|
||||
* @date June 6, 2015
|
||||
*/
|
||||
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include "timeSFMBAL.h"
|
||||
|
||||
#include <gtsam/slam/GeneralSFMFactor.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/inference/FactorGraph.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <stddef.h>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
//#define USE_GTSAM_FACTOR
|
||||
#ifdef USE_GTSAM_FACTOR
|
||||
#include <gtsam/slam/GeneralSFMFactor.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
typedef PinholeCamera<Cal3Bundler> Camera;
|
||||
typedef GeneralSFMFactor<Camera, Point3> SfmFactor;
|
||||
#else
|
||||
#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,
|
||||
double v0 = 0) :
|
||||
Cal3Bundler(f, k1, k2, u0, v0) {
|
||||
}
|
||||
CeresCalibration(const Cal3Bundler& cal) :
|
||||
Cal3Bundler(cal) {
|
||||
}
|
||||
CeresCalibration retract(const Vector& d) const {
|
||||
return CeresCalibration(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0());
|
||||
}
|
||||
Vector3 localCoordinates(const CeresCalibration& T2) const {
|
||||
return T2.vector() - vector();
|
||||
}
|
||||
};
|
||||
|
||||
namespace gtsam {
|
||||
template<>
|
||||
struct traits<CeresCalibration> : public internal::Manifold<CeresCalibration> {
|
||||
};
|
||||
}
|
||||
|
||||
// With that, camera below behaves like Snavely's 9-dim vector
|
||||
typedef PinholeCamera<CeresCalibration> Camera;
|
||||
#endif
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
using symbol_shorthand::P;
|
||||
// parse options and read BAL file
|
||||
SfM_data db = preamble(argc, argv);
|
||||
|
||||
// Load BAL file (default is tiny)
|
||||
//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, 2, 9, 3> snavely;
|
||||
#endif
|
||||
|
||||
// Build graph
|
||||
SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
|
||||
// Build graph using conventional GeneralSFMFactor
|
||||
NonlinearFactorGraph graph;
|
||||
for (size_t j = 0; j < db.number_tracks(); j++) {
|
||||
BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) {
|
||||
size_t i = m.first;
|
||||
Point2 z = m.second;
|
||||
#ifdef USE_GTSAM_FACTOR
|
||||
graph.push_back(SfmFactor(z, unit2, i, P(j)));
|
||||
#else
|
||||
Expression<Vector9> camera_(i);
|
||||
Expression<Vector3> point_(P(j));
|
||||
// Snavely expects measurements in OpenGL format, with y increasing upwards
|
||||
graph.addExpressionFactor(unit2, Vector2(z.x(), -z.y()),
|
||||
Expression<Vector2>(snavely, camera_, point_));
|
||||
#endif
|
||||
graph.push_back(SfmFactor(z, gNoiseModel, C(i), P(j)));
|
||||
}
|
||||
}
|
||||
|
||||
Values initial;
|
||||
size_t i = 0, j = 0;
|
||||
BOOST_FOREACH(const SfM_Camera& camera, db.cameras) {
|
||||
#ifdef USE_GTSAM_FACTOR
|
||||
initial.insert((i++), camera);
|
||||
#else
|
||||
// readBAL converts to GTSAM format, so we need to convert back !
|
||||
Camera ceresCamera(gtsam2openGL(camera.pose()), camera.calibration());
|
||||
Vector9 v9 = Camera().localCoordinates(ceresCamera);
|
||||
initial.insert((i++), v9);
|
||||
#endif
|
||||
}
|
||||
BOOST_FOREACH(const SfM_Track& track, db.tracks) {
|
||||
#ifdef USE_GTSAM_FACTOR
|
||||
BOOST_FOREACH (const SfM_Camera& camera, db.cameras)
|
||||
initial.insert(C(i++), camera);
|
||||
BOOST_FOREACH (const SfM_Track& track, db.tracks)
|
||||
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));
|
||||
Point2 actual = camera.project(point);
|
||||
#else
|
||||
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);
|
||||
|
||||
// Create Schur-complement ordering
|
||||
#ifdef CCOLAMD
|
||||
vector<Key> pointKeys;
|
||||
for (size_t j = 0; j < db.number_tracks(); j++) pointKeys.push_back(P(j));
|
||||
Ordering ordering = Ordering::colamdConstrainedFirst(graph, pointKeys, true);
|
||||
#else
|
||||
Ordering ordering;
|
||||
for (size_t j = 0; j < db.number_tracks(); j++)
|
||||
ordering.push_back(P(j));
|
||||
for (size_t i = 0; i < db.number_cameras(); i++)
|
||||
ordering.push_back(i);
|
||||
#endif
|
||||
|
||||
// Optimize
|
||||
// Set parameters to be similar to ceres
|
||||
LevenbergMarquardtParams params;
|
||||
LevenbergMarquardtParams::SetCeresDefaults(¶ms);
|
||||
params.setOrdering(ordering);
|
||||
params.setVerbosityLM("SUMMARY");
|
||||
LevenbergMarquardtOptimizer lm(graph, initial, params);
|
||||
Values result = lm.optimize();
|
||||
|
||||
tictoc_finishedIteration_();
|
||||
tictoc_print_();
|
||||
|
||||
return 0;
|
||||
return optimize(db, graph, initial);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,84 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file timeSFMBAL.h
|
||||
* @brief Common code for timeSFMBAL scripts
|
||||
* @author Frank Dellaert
|
||||
* @date July 5, 2015
|
||||
*/
|
||||
|
||||
#include <gtsam/slam/dataset.h>
|
||||
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/linear/NoiseModel.h>
|
||||
#include <gtsam/inference/Ordering.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/base/timing.h>
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
using symbol_shorthand::C;
|
||||
using symbol_shorthand::K;
|
||||
using symbol_shorthand::P;
|
||||
|
||||
static bool gUseSchur = true;
|
||||
static SharedNoiseModel gNoiseModel = noiseModel::Unit::Create(2);
|
||||
|
||||
// parse options and read BAL file
|
||||
SfM_data preamble(int argc, char* argv[]) {
|
||||
// primitive argument parsing:
|
||||
if (argc > 2) {
|
||||
if (string(argv[1]) == "--colamd")
|
||||
gUseSchur = false;
|
||||
else
|
||||
throw runtime_error("Usage: timeSFMBALxxx [--colamd] [BALfile]");
|
||||
}
|
||||
|
||||
// Load BAL file
|
||||
SfM_data db;
|
||||
string defaultFilename = findExampleDataFile("dubrovnik-16-22106-pre");
|
||||
bool success = readBAL(argc > 1 ? argv[argc] : defaultFilename, db);
|
||||
if (!success) throw runtime_error("Could not access file!");
|
||||
return db;
|
||||
}
|
||||
|
||||
// Create ordering and optimize
|
||||
int optimize(const SfM_data& db, const NonlinearFactorGraph& graph,
|
||||
const Values& initial) {
|
||||
using symbol_shorthand::P;
|
||||
|
||||
// Set parameters to be similar to ceres
|
||||
LevenbergMarquardtParams params;
|
||||
LevenbergMarquardtParams::SetCeresDefaults(¶ms);
|
||||
params.setVerbosityLM("SUMMARY");
|
||||
|
||||
if (gUseSchur) {
|
||||
// Create Schur-complement ordering
|
||||
Ordering ordering;
|
||||
for (size_t j = 0; j < db.number_tracks(); j++) ordering.push_back(P(j));
|
||||
for (size_t i = 0; i < db.number_cameras(); i++) ordering.push_back(C(i));
|
||||
params.setOrdering(ordering);
|
||||
}
|
||||
|
||||
// Optimize
|
||||
LevenbergMarquardtOptimizer lm(graph, initial, params);
|
||||
Values result = lm.optimize();
|
||||
|
||||
tictoc_finishedIteration_();
|
||||
tictoc_print_();
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,92 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
* Atlanta, Georgia 30332-0415
|
||||
* All Rights Reserved
|
||||
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
|
||||
* See LICENSE for the license information
|
||||
|
||||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file timeSFMBALautodiff.cpp
|
||||
* @brief time structure from motion with BAL file, Ceres autodiff version
|
||||
* @author Frank Dellaert
|
||||
* @date July 5, 2015
|
||||
*/
|
||||
|
||||
#include "timeSFMBAL.h"
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/nonlinear/ExpressionFactor.h>
|
||||
#include <gtsam/nonlinear/AdaptAutoDiff.h>
|
||||
#include <gtsam/3rdparty/ceres/example.h>
|
||||
|
||||
#include <boost/foreach.hpp>
|
||||
#include <stddef.h>
|
||||
#include <stdexcept>
|
||||
#include <string>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// See http://www.cs.cornell.edu/~snavely/bundler/bundler-v0.3-manual.html
|
||||
// Special version of Cal3Bundler so that default constructor = 0,0,0
|
||||
// This is only used in localCoordinates below
|
||||
struct CeresCalibration : public Cal3Bundler {
|
||||
CeresCalibration(double f = 0, double k1 = 0, double k2 = 0, double u0 = 0,
|
||||
double v0 = 0)
|
||||
: Cal3Bundler(f, k1, k2, u0, v0) {}
|
||||
CeresCalibration(const Cal3Bundler& cal) : Cal3Bundler(cal) {}
|
||||
CeresCalibration retract(const Vector& d) const {
|
||||
return CeresCalibration(fx() + d(0), k1() + d(1), k2() + d(2), u0(), v0());
|
||||
}
|
||||
Vector3 localCoordinates(const CeresCalibration& T2) const {
|
||||
return T2.vector() - vector();
|
||||
}
|
||||
};
|
||||
|
||||
namespace gtsam {
|
||||
template <>
|
||||
struct traits<CeresCalibration> : public internal::Manifold<CeresCalibration> {
|
||||
};
|
||||
}
|
||||
|
||||
// With that, camera below behaves like Snavely's 9-dim vector
|
||||
typedef PinholeCamera<CeresCalibration> Camera;
|
||||
|
||||
int main(int argc, char* argv[]) {
|
||||
// parse options and read BAL file
|
||||
SfM_data db = preamble(argc, argv);
|
||||
|
||||
AdaptAutoDiff<SnavelyProjection, 2, 9, 3> snavely;
|
||||
|
||||
// Build graph
|
||||
NonlinearFactorGraph graph;
|
||||
for (size_t j = 0; j < db.number_tracks(); j++) {
|
||||
BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) {
|
||||
size_t i = m.first;
|
||||
Point2 z = m.second;
|
||||
Expression<Vector9> camera_(C(i));
|
||||
Expression<Vector3> point_(P(j));
|
||||
// Expects measurements in OpenGL format, with y increasing upwards
|
||||
graph.addExpressionFactor(gNoiseModel, Vector2(z.x(), -z.y()),
|
||||
Expression<Vector2>(snavely, camera_, point_));
|
||||
}
|
||||
}
|
||||
|
||||
Values initial;
|
||||
size_t i = 0, j = 0;
|
||||
BOOST_FOREACH (const SfM_Camera& camera, db.cameras) {
|
||||
// readBAL converts to GTSAM format, so we need to convert back !
|
||||
Camera ceresCamera(gtsam2openGL(camera.pose()), camera.calibration());
|
||||
Vector9 v9 = Camera().localCoordinates(ceresCamera);
|
||||
initial.insert(C(i++), v9);
|
||||
}
|
||||
BOOST_FOREACH (const SfM_Track& track, db.tracks) {
|
||||
Vector3 v3 = track.p.vector();
|
||||
initial.insert(P(j++), v3);
|
||||
}
|
||||
|
||||
return optimize(db, graph, initial);
|
||||
}
|
||||
Loading…
Reference in New Issue