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

For exact comparison with Ceres, use exact same AutoDiff model

release/4.3a0
dellaert 2015-06-17 09:06:21 -07:00
parent 178c1aec88
commit 6efc708c5e
1 changed files with 61 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

67
timing/timeSFMBAL.cpp
View File

@ -16,11 +16,14 @@
* @date June 6, 2015
*/
#include <gtsam/3rdparty/ceres/example.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/nonlinear/ExpressionFactor.h>
#include <gtsam/nonlinear/AdaptAutoDiff.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Values.h>
@ -39,6 +42,32 @@ using namespace gtsam;
//#define TERNARY
// 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> CeresCamera;
int main(int argc, char* argv[]) {
typedef GeneralSFMFactor<PinholeCamera<Cal3Bundler>, Point3> sfmFactor;
using symbol_shorthand::P;
@ -47,17 +76,41 @@ int main(int argc, char* argv[]) {
string defaultFilename = findExampleDataFile("dubrovnik-3-7-pre");
SfM_data db;
bool success = readBAL(argc > 1 ? argv[1] : defaultFilename, db);
if (!success) throw runtime_error("Could not access file!");
if (!success)
throw runtime_error("Could not access file!");
typedef AdaptAutoDiff<SnavelyProjection, Point2, CeresCamera, Point3> Adaptor;
// Build graph
SharedNoiseModel unit2 = noiseModel::Unit::Create(2);
NonlinearFactorGraph graph;
for (size_t j = 0; j < db.number_tracks(); j++) {
BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements)
graph.push_back(sfmFactor(m.second, unit2, m.first, P(j)));
BOOST_FOREACH (const SfM_Measurement& m, db.tracks[j].measurements) {
size_t i = m.first;
Point2 measurement = m.second;
#ifdef USE_GTSAM_FACTOR
graph.push_back(sfmFactor(measurement, unit2, i, P(j)));
#else
Expression<CeresCamera> camera_(i);
Expression<Point3> point_(P(j));
graph.addExpressionFactor(unit2, measurement,
Expression<Point2>(Adaptor(), camera_, point_));
#endif
}
}
Values initial = initialCamerasAndPointsEstimate(db);
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
CeresCamera ceresCamera(camera.pose(), camera.calibration());
initial.insert((i++), ceresCamera);
#endif
}
BOOST_FOREACH(const SfM_Track& track, db.tracks)
initial.insert(P(j++), track.p);
// Create Schur-complement ordering
#ifdef CCOLAMD
@ -66,8 +119,10 @@ int main(int argc, char* argv[]) {
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);
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