diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp new file mode 100644 index 000000000..66beefb35 --- /dev/null +++ b/examples/SFMExampleExpressions_bal.cpp @@ -0,0 +1,121 @@ +/* ---------------------------------------------------------------------------- + + * 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 SFMExampleExpressions_bal.cpp + * @brief A structure-from-motion example done with Expressions + * @author Frank Dellaert + * @date January 2015 + */ + +/** + * This is the Expression version of SFMExample + * See detailed description of headers there, this focuses on explaining the AD part + */ + +// The two new headers that allow using our Automatic Differentiation Expression framework +#include +#include + +// Header order is close to far +#include +#include +#include // for loading BAL datasets ! +#include + +using namespace std; +using namespace gtsam; +using namespace noiseModel; +using symbol_shorthand::C; +using symbol_shorthand::P; + +// An SfM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration +// and has a total of 9 free parameters + +/* ************************************************************************* */ +int main(int argc, char* argv[]) { + + // Find default file, but if an argument is given, try loading a file + string filename = findExampleDataFile("dubrovnik-3-7-pre"); + if (argc > 1) + filename = string(argv[1]); + + // Load the SfM data from file + SfM_data mydata; + readBAL(filename, mydata); + cout + << boost::format("read %1% tracks on %2% cameras\n") + % mydata.number_tracks() % mydata.number_cameras(); + + // Create a factor graph + ExpressionFactorGraph graph; + + // Here we don't use a PriorFactor but directly the ExpressionFactor class + // First, we create an expression to the pose from the first camera + Expression camera0_(C(0)); + // Then, to get its pose: + Pose3_ pose0_(&SfM_Camera::getPose, camera0_); + // Finally, we say it should be equal to first guess + graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(), + noiseModel::Isotropic::Sigma(6, 0.1)); + + // similarly, we create a prior on the first point + Point3_ point0_(P(0)); + graph.addExpressionFactor(point0_, mydata.tracks[0].p, + noiseModel::Isotropic::Sigma(3, 0.1)); + + // We share *one* noiseModel between all projection factors + noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2, + 1.0); // one pixel in u and v + + // Simulated measurements from each camera pose, adding them to the factor graph + size_t j = 0; + BOOST_FOREACH(const SfM_Track& track, mydata.tracks) { + // Leaf expression for j^th point + Point3_ point_('p', j); + BOOST_FOREACH(const SfM_Measurement& m, track.measurements) { + size_t i = m.first; + Point2 uv = m.second; + // Leaf expression for i^th camera + Expression camera_(C(i)); + // Below an expression for the prediction of the measurement: + Point2_ predict_ = project2(camera_, point_); + // Again, here we use an ExpressionFactor + graph.addExpressionFactor(predict_, uv, noise); + } + j += 1; + } + + // Create initial estimate + Values initial; + size_t i = 0; + j = 0; + BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) + initial.insert(C(i++), camera); + BOOST_FOREACH(const SfM_Track& track, mydata.tracks) + initial.insert(P(j++), track.p); + + /* Optimize the graph and print results */ + Values result; + try { + LevenbergMarquardtParams params; + params.setVerbosity("ERROR"); + LevenbergMarquardtOptimizer lm(graph, initial, params); + result = lm.optimize(); + } catch (exception& e) { + cout << e.what(); + } + cout << "final error: " << graph.error(result) << endl; + + return 0; +} +/* ************************************************************************* */ + diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 43177dc42..0e11adaed 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -43,7 +43,7 @@ int main (int argc, char* argv[]) { // Load the SfM data from file SfM_data mydata; - assert(readBAL(filename, mydata)); + readBAL(filename, mydata); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); // Create a factor graph diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 12eb0235d..46df47ecb 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -151,11 +151,20 @@ public: return pose_; } - /// return pose + /// return pose, constant version inline const Pose3& pose() const { return pose_; } + /// return pose, with derivative + inline const Pose3& getPose(gtsam::OptionalJacobian<6, dimension> H) const { + if (H) { + H->setZero(); + H->block(0, 0, 6, 6) = I_6x6; + } + return pose_; + } + /// return calibration inline Calibration& calibration() { return K_; diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 6cb84ec85..20f7a3231 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -51,8 +51,21 @@ static const Point3 point4_inf( 0.16,-0.16, -1.0); /* ************************************************************************* */ TEST( PinholeCamera, constructor) { - EXPECT(assert_equal( camera.calibration(), K)); - EXPECT(assert_equal( camera.pose(), pose)); + EXPECT(assert_equal( K, camera.calibration())); + EXPECT(assert_equal( pose, camera.pose())); +} + +//****************************************************************************** +TEST(PinholeCamera, Pose) { + + Matrix actualH; + EXPECT(assert_equal(pose, camera.getPose(actualH))); + + // Check derivative + boost::function f = // + boost::bind(&Camera::getPose,_1,boost::none); + Matrix numericalH = numericalDerivative11(f,camera); + EXPECT(assert_equal(numericalH, actualH, 1e-9)); } /* ************************************************************************* */ diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 1d9ddd6d4..f7314c73f 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -9,6 +9,7 @@ #include #include +#include #include namespace gtsam { @@ -19,7 +20,7 @@ typedef Expression Point2_; typedef Expression Rot2_; typedef Expression Pose2_; -Point2_ transform_to(const Pose2_& x, const Point2_& p) { +inline Point2_ transform_to(const Pose2_& x, const Point2_& p) { return Point2_(x, &Pose2::transform_to, p); } @@ -29,24 +30,36 @@ typedef Expression Point3_; typedef Expression Rot3_; typedef Expression Pose3_; -Point3_ transform_to(const Pose3_& x, const Point3_& p) { +inline Point3_ transform_to(const Pose3_& x, const Point3_& p) { return Point3_(x, &Pose3::transform_to, p); } // Projection typedef Expression Cal3_S2_; +typedef Expression Cal3Bundler_; -Point2_ project(const Point3_& p_cam) { +inline Point2_ project(const Point3_& p_cam) { return Point2_(PinholeCamera::project_to_camera, p_cam); } -Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, +template +Point2 project4(const CAMERA& camera, const Point3& p, + OptionalJacobian<2, CAMERA::dimension> Dcam, OptionalJacobian<2, 3> Dpoint) { + return camera.project2(p, Dcam, Dpoint); +} + +template +Point2_ project2(const Expression& camera_, const Point3_& p_) { + return Point2_(project4, camera_, p_); +} + +inline Point2 project6(const Pose3& x, const Point3& p, const Cal3_S2& K, OptionalJacobian<2, 6> Dpose, OptionalJacobian<2, 3> Dpoint, OptionalJacobian<2, 5> Dcal) { return PinholeCamera(x, K).project(p, Dpose, Dpoint, Dcal); } -Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) { +inline Point2_ project3(const Pose3_& x, const Point3_& p, const Cal3_S2_& K) { return Point2_(project6, x, p, K); }