12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged in feature/timeSFMBALversions (pull request #169) 

Feature/timesfmbalversions
release/4.3a0
Frank Dellaert 2015-07-05 12:03:29 -07:00
parent ed9938b70c b6adeec6ac
commit cf052850dd
6 changed files with 306 additions and 130 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/slam/expressions.h
View File

@ -36,6 +36,10 @@ inline Point3_ transform_to(const Pose3_& x, const Point3_& p) {
return Point3_(x, &Pose3::transform_to, p);
}
inline Point3_ transform_from(const Pose3_& x, const Point3_& p) {
return Point3_(x, &Pose3::transform_from, p);
}
// Projection
typedef Expression<Cal3_S2> Cal3_S2_;

144
timing/timeSFMBAL.cpp
View File

@ -11,162 +11,46 @@
/**
* @file timeSFMBAL.cpp
* @brief time structure from motion with BAL file
* @brief time SFM 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(&params);
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);
}

87
timing/timeSFMBAL.h Normal file
View File

@ -0,0 +1,87 @@
/* ----------------------------------------------------------------------------
* 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 (strcmp(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 - 1] : 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, bool separateCalibration = false) {
using symbol_shorthand::P;
// Set parameters to be similar to ceres
LevenbergMarquardtParams params;
LevenbergMarquardtParams::SetCeresDefaults(&params);
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));
if (separateCalibration) ordering.push_back(K(i));
}
params.setOrdering(ordering);
}
// Optimize
LevenbergMarquardtOptimizer lm(graph, initial, params);
Values result = lm.optimize();
tictoc_finishedIteration_();
tictoc_print_();
return 0;
}

75
timing/timeSFMBALautodiff.cpp Normal file
View File

@ -0,0 +1,75 @@
/* ----------------------------------------------------------------------------
* 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 SFM 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
// as to why so much gymnastics is needed to massage the initial estimates and
// measurements: basically, Snavely does not use computer vision conventions
// but OpenGL conventions :-(
typedef PinholeCamera<Cal3Bundler> 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 !
Pose3 openGLpose = gtsam2openGL(camera.pose());
Vector9 v9;
v9 << Pose3::Logmap(openGLpose), camera.calibration().vector();
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);
}

63
timing/timeSFMBALcamTnav.cpp Normal file
View File

@ -0,0 +1,63 @@
/* ----------------------------------------------------------------------------
* 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 timeSFMBALcamTnav.cpp
* @brief time SFM with BAL file, expressions with camTnav pose
* @author Frank Dellaert
* @date July 5, 2015
*/
#include "timeSFMBAL.h"
#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Point3.h>
#include <boost/foreach.hpp>
using namespace std;
using namespace gtsam;
int main(int argc, char* argv[]) {
// parse options and read BAL file
SfM_data db = preamble(argc, argv);
// 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;
Pose3_ camTnav_(C(i));
Cal3Bundler_ calibration_(K(i));
Point3_ nav_point_(P(j));
graph.addExpressionFactor(
gNoiseModel, z,
uncalibrate(calibration_, // now using transform_from !!!:
project(transform_from(camTnav_, nav_point_))));
}
}
Values initial;
size_t i = 0, j = 0;
BOOST_FOREACH (const SfM_Camera& camera, db.cameras) {
initial.insert(C(i), camera.pose().inverse()); // inverse !!!
initial.insert(K(i), camera.calibration());
i += 1;
}
BOOST_FOREACH (const SfM_Track& track, db.tracks)
initial.insert(P(j++), track.p);
bool separateCalibration = true;
return optimize(db, graph, initial, separateCalibration);
}

63
timing/timeSFMBALnavTcam.cpp Normal file
View File

@ -0,0 +1,63 @@
/* ----------------------------------------------------------------------------
* 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 timeSFMBALnavTcam.cpp
* @brief time SFM with BAL file, expressions with navTcam pose
* @author Frank Dellaert
* @date July 5, 2015
*/
#include "timeSFMBAL.h"
#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Point3.h>
#include <boost/foreach.hpp>
using namespace std;
using namespace gtsam;
int main(int argc, char* argv[]) {
// parse options and read BAL file
SfM_data db = preamble(argc, argv);
// 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;
Pose3_ navTcam_(C(i));
Cal3Bundler_ calibration_(K(i));
Point3_ nav_point_(P(j));
graph.addExpressionFactor(
gNoiseModel, z,
uncalibrate(calibration_,
project(transform_to(navTcam_, nav_point_))));
}
}
Values initial;
size_t i = 0, j = 0;
BOOST_FOREACH (const SfM_Camera& camera, db.cameras) {
initial.insert(C(i), camera.pose());
initial.insert(K(i), camera.calibration());
i += 1;
}
BOOST_FOREACH (const SfM_Track& track, db.tracks)
initial.insert(P(j++), track.p);
bool separateCalibration = true;
return optimize(db, graph, initial, separateCalibration);
}