diff --git a/.cproject b/.cproject index 4385a9461..8e4541162 100644 --- a/.cproject +++ b/.cproject @@ -518,10 +518,18 @@ true true - + make -j5 - ExpressionExample.run + SFMExampleExpressions.run + true + true + true + + + make + -j5 + Pose2SLAMExampleExpressions.run true true true @@ -592,6 +600,7 @@ make + tests/testBayesTree.run true false @@ -599,6 +608,7 @@ make + testBinaryBayesNet.run true false @@ -646,6 +656,7 @@ make + testSymbolicBayesNet.run true false @@ -653,6 +664,7 @@ make + tests/testSymbolicFactor.run true false @@ -660,6 +672,7 @@ make + testSymbolicFactorGraph.run true false @@ -675,6 +688,7 @@ make + tests/testBayesTree true false @@ -1010,6 +1024,7 @@ make + testErrors.run true false @@ -1239,6 +1254,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 @@ -1321,7 +1376,6 @@ make - testSimulated2DOriented.run true false @@ -1361,7 +1415,6 @@ make - testSimulated2D.run true false @@ -1369,7 +1422,6 @@ make - testSimulated3D.run true false @@ -1383,46 +1435,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 @@ -1680,6 +1692,7 @@ cpack + -G DEB true false @@ -1687,6 +1700,7 @@ cpack + -G RPM true false @@ -1694,6 +1708,7 @@ cpack + -G TGZ true false @@ -1701,6 +1716,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2427,6 +2443,7 @@ make + testGraph.run true false @@ -2434,6 +2451,7 @@ make + testJunctionTree.run true false @@ -2441,6 +2459,7 @@ make + testSymbolicBayesNetB.run true false @@ -2928,7 +2947,6 @@ make - tests/testGaussianISAM2 true false diff --git a/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp b/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp new file mode 100644 index 000000000..590e83b70 --- /dev/null +++ b/gtsam_unstable/examples/Pose2SLAMExampleExpressions.cpp @@ -0,0 +1,88 @@ +/* ---------------------------------------------------------------------------- + + * 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 Pose2SLAMExample.cpp + * @brief Expressions version of Pose2SLAMExample.cpp + * @date Oct 2, 2014 + * @author Frank Dellaert + * @author Yong Dian Jian + */ + +// 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 + +using namespace std; +using namespace gtsam; + +int main(int argc, char** argv) { + + // 1. Create a factor graph container and add factors to it + NonlinearFactorGraph graph; + + // Create Expressions for unknowns + Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5); + + // 2a. Add a prior on the first pose, setting it to the origin + noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); + graph.push_back(BADFactor(priorNoise, Pose2(0, 0, 0), x1)); + + // For simplicity, we will use the same noise model for odometry and loop closures + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); + + // 2b. Add odometry factors + graph.push_back(BADFactor(model, Pose2(2, 0, 0 ), between(x1,x2))); + graph.push_back(BADFactor(model, Pose2(2, 0, M_PI_2), between(x2,x3))); + graph.push_back(BADFactor(model, Pose2(2, 0, M_PI_2), between(x3,x4))); + graph.push_back(BADFactor(model, Pose2(2, 0, M_PI_2), between(x4,x5))); + + // 2c. Add the loop closure constraint + graph.push_back(BADFactor(model, Pose2(2, 0, M_PI_2), between(x5,x2))); + graph.print("\nFactor Graph:\n"); // print + + // 3. Create the data structure to hold the initialEstimate estimate to the solution + // For illustrative purposes, these have been deliberately set to incorrect values + Values initialEstimate; + initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 )); + initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 )); + initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2)); + initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI )); + initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2)); + initialEstimate.print("\nInitial Estimate:\n"); // print + + // 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer + GaussNewtonParams parameters; + parameters.relativeErrorTol = 1e-5; + parameters.maxIterations = 100; + GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters); + Values result = optimizer.optimize(); + result.print("Final Result:\n"); + + // 5. Calculate and print marginal covariances for all variables + cout.precision(3); + Marginals marginals(graph, result); + cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl; + cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl; + cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl; + cout << "x4 covariance:\n" << marginals.marginalCovariance(4) << endl; + cout << "x5 covariance:\n" << marginals.marginalCovariance(5) << endl; + + return 0; +} diff --git a/gtsam_unstable/slam/expressions.h b/gtsam_unstable/slam/expressions.h index 406456f50..1acde67d3 100644 --- a/gtsam_unstable/slam/expressions.h +++ b/gtsam_unstable/slam/expressions.h @@ -13,16 +13,37 @@ namespace gtsam { +// Generics + +template +Expression between(const Expression& t1, const Expression& t2) { + return Expression(t1, &T::between, t2); +} + +// 2D Geometry + typedef Expression Point2_; +typedef Expression Rot2_; +typedef Expression Pose2_; + +Point2_ transform_to(const Pose2_& x, const Point2_& p) { + return Point2_(x, &Pose2::transform_to, p); +} + +// 3D Geometry + typedef Expression Point3_; typedef Expression Rot3_; typedef Expression Pose3_; -typedef Expression Cal3_S2_; Point3_ transform_to(const Pose3_& x, const Point3_& p) { return Point3_(x, &Pose3::transform_to, p); } +// Projection + +typedef Expression Cal3_S2_; + Point2_ project(const Point3_& p_cam) { return Point2_(PinholeCamera::project_to_camera, p_cam); }