diff --git a/.cproject b/.cproject index 8a2eaaf1c..e6af94066 100644 --- a/.cproject +++ b/.cproject @@ -584,6 +584,7 @@ make + tests/testBayesTree.run true false @@ -591,6 +592,7 @@ make + testBinaryBayesNet.run true false @@ -638,6 +640,7 @@ make + testSymbolicBayesNet.run true false @@ -645,6 +648,7 @@ make + tests/testSymbolicFactor.run true false @@ -652,6 +656,7 @@ make + testSymbolicFactorGraph.run true false @@ -667,6 +672,7 @@ make + tests/testBayesTree true false @@ -1098,6 +1104,7 @@ make + testErrors.run true false @@ -1327,6 +1334,46 @@ true true + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + make -j2 @@ -1409,7 +1456,6 @@ make - testSimulated2DOriented.run true false @@ -1449,7 +1495,6 @@ make - testSimulated2D.run true false @@ -1457,7 +1502,6 @@ make - testSimulated3D.run true false @@ -1471,46 +1515,6 @@ true true - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFMap.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - make -j5 @@ -1776,6 +1780,7 @@ cpack + -G DEB true false @@ -1783,6 +1788,7 @@ cpack + -G RPM true false @@ -1790,6 +1796,7 @@ cpack + -G TGZ true false @@ -1797,6 +1804,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2675,6 +2683,7 @@ make + testGraph.run true false @@ -2682,6 +2691,7 @@ make + testJunctionTree.run true false @@ -2689,6 +2699,7 @@ make + testSymbolicBayesNetB.run true false @@ -3118,6 +3129,14 @@ true true + + make + -j4 + SFMExampleExpressions_bal.run + true + true + true + make -j5 @@ -3288,7 +3307,6 @@ make - tests/testGaussianISAM2 true false diff --git a/examples/SFMExampleExpressions_bal.cpp b/examples/SFMExampleExpressions_bal.cpp new file mode 100644 index 000000000..7c79deb2a --- /dev/null +++ b/examples/SFMExampleExpressions_bal.cpp @@ -0,0 +1,117 @@ +/* ---------------------------------------------------------------------------- + + * 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 +#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; + assert(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::pose,camera0_); + // Finally, we say it should be equal to first guess + graph.addExpressionFactor(pose0_, mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 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/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 1d9ddd6d4..694782c16 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -41,6 +41,17 @@ Point2_ project(const Point3_& p_cam) { return Point2_(PinholeCamera::project_to_camera, p_cam); } +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_); +} + 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);