diff --git a/gtsam/slam/sba.h b/gtsam/slam/sba.h new file mode 100644 index 000000000..8e6f02ff4 --- /dev/null +++ b/gtsam/slam/sba.h @@ -0,0 +1,70 @@ +/* ---------------------------------------------------------------------------- + + * 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 sba.h + * @brief a suite for sparse bundle adjustment + * @date Jul 5, 2012 + * @author ydjian + */ + +#pragma once + +#include +#include + +namespace sba { + + using namespace gtsam; + + /** + * Graph class, inherited from NonlinearFactorGraph, used as a convenience for MATLAB wrapper + * @addtogroup SLAM + */ + struct Graph: public visualSLAM::Graph { + + /// Default constructor + Graph(){} + + /// Copy constructor given any other NonlinearFactorGraph + Graph(const NonlinearFactorGraph& graph): visualSLAM::Graph(graph) {} + + /** + * Add a 2d projection measurement + * @param z the measurement + * @param model the noise model for the measurement + * @param cameraKey variable key for the pose+calibration + * @param pointKey variable key for the point + */ + template + void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index cameraKey, const Index pointKey) { + typedef GeneralSFMFactor SFMFactor; + boost::shared_ptr factor(new SFMFactor(z, model, cameraKey, pointKey)); + push_back(factor); + } + + /** + * Add a 2d projection measurement + * @param z the measurement + * @param model the noise model for the measurement + * @param poseKey variable key for the pose + * @param pointKey variable key for the point + * @param calibKey variable key for the calibration + */ + template + void addMeasurement(const Point2 &z, const SharedNoiseModel& model, const Index posekey, const Index pointkey, const Index calibkey) { + typedef GeneralSFMFactor2 SFMFactor; + boost::shared_ptr factor(new SFMFactor(z, model, posekey, pointkey, calibkey)); + push_back(factor); + } + }; + +} diff --git a/gtsam/slam/tests/testSba.cpp b/gtsam/slam/tests/testSba.cpp new file mode 100644 index 000000000..8d441aa34 --- /dev/null +++ b/gtsam/slam/tests/testSba.cpp @@ -0,0 +1,193 @@ +/* ---------------------------------------------------------------------------- + + * 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 testSba.cpp + * @brief + * @date Jul 5, 2012 + * @author Yong-Dian Jian + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace boost; +using namespace gtsam; + +/* ************************************************************************* */ + +static SharedNoiseModel sigma(noiseModel::Unit::Create(2)); + +// Convenience for named keys +using symbol_shorthand::X; /* pose3 */ +using symbol_shorthand::K; /* calibration */ +using symbol_shorthand::C; /* camera = [pose calibration] */ +using symbol_shorthand::L; /* point3 */ + +static Point3 landmark1(-1.0,-1.0, 0.0); +static Point3 landmark2(-1.0, 1.0, 0.0); +static Point3 landmark3( 1.0, 1.0, 0.0); +static Point3 landmark4( 1.0,-1.0, 0.0); + +static Pose3 pose1(Matrix_(3,3, + 1., 0., 0., + 0.,-1., 0., + 0., 0.,-1.), + Point3(0,0,6.25)); + +static Pose3 pose2(Matrix_(3,3, + 1., 0., 0., + 0.,-1., 0., + 0., 0.,-1.), + Point3(0,0,5.00)); + +static Cal3_S2 calib1 (625, 625, 0, 0, 0); +static Cal3_S2 calib2 (625, 625, 0, 0, 0); + + +typedef PinholeCamera Camera; + +static Camera camera1(pose1, calib1); +static Camera camera2(pose2, calib2); + +/* ************************************************************************* */ +sba::Graph testGraph1() { + Point2 z11(-100, 100); + Point2 z12(-100,-100); + Point2 z13( 100,-100); + Point2 z14( 100, 100); + Point2 z21(-125, 125); + Point2 z22(-125,-125); + Point2 z23( 125,-125); + Point2 z24( 125, 125); + + + sba::Graph g; + g.addMeasurement(z11, sigma, C(1), L(1)); + g.addMeasurement(z12, sigma, C(1), L(2)); + g.addMeasurement(z13, sigma, C(1), L(3)); + g.addMeasurement(z14, sigma, C(1), L(4)); + g.addMeasurement(z21, sigma, C(2), L(1)); + g.addMeasurement(z22, sigma, C(2), L(2)); + g.addMeasurement(z23, sigma, C(2), L(3)); + g.addMeasurement(z24, sigma, C(2), L(4)); + return g; +} + +sba::Graph testGraph2() { + Point2 z11(-100, 100); + Point2 z12(-100,-100); + Point2 z13( 100,-100); + Point2 z14( 100, 100); + Point2 z21(-125, 125); + Point2 z22(-125,-125); + Point2 z23( 125,-125); + Point2 z24( 125, 125); + + sba::Graph g; + g.addMeasurement(z11, sigma, X(1), L(1), K(1)); + g.addMeasurement(z12, sigma, X(1), L(2), K(1)); + g.addMeasurement(z13, sigma, X(1), L(3), K(1)); + g.addMeasurement(z14, sigma, X(1), L(4), K(1)); + g.addMeasurement(z21, sigma, X(2), L(1), K(1)); + g.addMeasurement(z22, sigma, X(2), L(2), K(1)); + g.addMeasurement(z23, sigma, X(2), L(3), K(1)); + g.addMeasurement(z24, sigma, X(2), L(4), K(1)); + return g; +} + +/* ************************************************************************* */ +TEST( optimizeLM1, sba ) +{ + // build a graph + sba::Graph graph(testGraph1()); + + // add 3 landmark constraints + graph.addPointConstraint(L(1), landmark1); + graph.addPointConstraint(L(2), landmark2); + graph.addPointConstraint(L(3), landmark3); + + // Create an initial values structure corresponding to the ground truth + Values initialEstimate; + initialEstimate.insert(C(1), camera1); + initialEstimate.insert(C(2), camera2); + initialEstimate.insert(L(1), landmark1); + initialEstimate.insert(L(2), landmark2); + initialEstimate.insert(L(3), landmark3); + initialEstimate.insert(L(4), landmark4); + + // Create an ordering of the variables + Ordering ordering; + ordering += L(1),L(2),L(3),L(4),C(1),C(2); + + // Create an optimizer and check its error + // We expect the initial to be zero because config is the ground truth + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering); + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); + + // Iterate once, and the config should not have changed because we started + // with the ground truth + optimizer.iterate(); + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); + + // check if correct + CHECK(assert_equal(initialEstimate, optimizer.values())); +} + +/* ************************************************************************* */ +TEST( optimizeLM2, sba ) +{ + // build a graph + sba::Graph graph(testGraph2()); + + // add 3 landmark constraints + graph.addPointConstraint(L(1), landmark1); + graph.addPointConstraint(L(2), landmark2); + graph.addPointConstraint(L(3), landmark3); + + // Create an initial values structure corresponding to the ground truth + Values initialEstimate; + initialEstimate.insert(X(1), pose1); + initialEstimate.insert(X(2), pose2); + initialEstimate.insert(L(1), landmark1); + initialEstimate.insert(L(2), landmark2); + initialEstimate.insert(L(3), landmark3); + initialEstimate.insert(L(4), landmark4); + initialEstimate.insert(K(1), calib2); + + // Create an ordering of the variables + Ordering ordering; + ordering += L(1),L(2),L(3),L(4),X(1),X(2),K(1); + + // Create an optimizer and check its error + // We expect the initial to be zero because config is the ground truth + LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, ordering); + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); + + // Iterate once, and the config should not have changed because we started + // with the ground truth + optimizer.iterate(); + DOUBLES_EQUAL(0.0, optimizer.error(), 1e-9); + + // check if correct + CHECK(assert_equal(initialEstimate, optimizer.values())); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */