diff --git a/.cproject b/.cproject index 7d8fa61b6..4228b4ac0 100644 --- a/.cproject +++ b/.cproject @@ -317,6 +317,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -343,7 +351,6 @@ make - tests/testBayesTree.run true false @@ -351,7 +358,6 @@ make - testBinaryBayesNet.run true false @@ -399,7 +405,6 @@ make - testSymbolicBayesNet.run true false @@ -407,7 +412,6 @@ make - tests/testSymbolicFactor.run true false @@ -415,7 +419,6 @@ make - testSymbolicFactorGraph.run true false @@ -431,20 +434,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -533,22 +527,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -565,6 +543,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -589,26 +583,42 @@ true true - + make - -j2 - all + -j5 + testValues.run true true true - + make - -j2 - check + -j5 + testOrdering.run true true true - + make - -j2 - clean + -j5 + testKey.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j6 -j8 + testWhiteNoiseFactor.run true true true @@ -653,42 +663,26 @@ true true - + make - -j5 - testValues.run + -j2 + all true true true - + make - -j5 - testOrdering.run + -j2 + check true true true - + make - -j5 - testKey.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j6 -j8 - testWhiteNoiseFactor.run + -j2 + clean true true true @@ -1079,7 +1073,6 @@ make - testGraph.run true false @@ -1087,7 +1080,6 @@ make - testJunctionTree.run true false @@ -1095,7 +1087,6 @@ make - testSymbolicBayesNetB.run true false @@ -1263,7 +1254,6 @@ make - testErrors.run true false @@ -1309,6 +1299,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1389,14 +1387,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1703,6 +1693,7 @@ make + testSimulated2DOriented.run true false @@ -1742,6 +1733,7 @@ make + testSimulated2D.run true false @@ -1749,6 +1741,7 @@ make + testSimulated3D.run true false @@ -1986,6 +1979,22 @@ true true + + make + -j5 + Pose2SLAMExample_graphviz.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graph.run + true + true + true + make -j4 @@ -2004,6 +2013,7 @@ make + tests/testGaussianISAM2 true false @@ -2025,6 +2035,102 @@ true true + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + make -j3 @@ -2226,7 +2332,6 @@ cpack - -G DEB true false @@ -2234,7 +2339,6 @@ cpack - -G RPM true false @@ -2242,7 +2346,6 @@ cpack - -G TGZ true false @@ -2250,7 +2353,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2416,98 +2518,34 @@ true true - + make - -j2 - testRot3.run + -j5 + testSpirit.run true true true - + make - -j2 - testRot2.run + -j5 + testWrap.run true true true - + make - -j2 - testPose3.run + -j5 + check.wrap true true true - + make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run + -j5 + wrap true true true @@ -2551,38 +2589,6 @@ false true - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - wrap - true - true - true - diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index d7bfb21f9..ad8f9dde6 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -10,7 +10,7 @@ * -------------------------------------------------------------------------- */ /** - * @file Pose2SLAMExample_graph->cpp + * @file Pose2SLAMExample_graph.cpp * @brief Read graph from file and perform GraphSLAM * @date June 3, 2012 * @author Frank Dellaert @@ -20,23 +20,19 @@ #include #include #include -#include -#include #include -#include -#include using namespace std; using namespace gtsam; -int main(int argc, char** argv) { +int main (int argc, char** argv) { - // Read File and create graph and initial estimate + // Read File, create graph and initial estimate // we are in build/examples, data is in examples/Data - NonlinearFactorGraph::shared_ptr graph ; + NonlinearFactorGraph::shared_ptr graph; Values::shared_ptr initial; - SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 5.0*M_PI/180.0)); - boost::tie(graph,initial) = load2D("../../examples/Data/w100.graph",model); + SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.05, 0.05, 5.0 * M_PI / 180.0)); + boost::tie(graph, initial) = load2D("../../examples/Data/w100.graph", model); initial->print("Initial estimate:\n"); // Add a Gaussian prior on first poses @@ -53,5 +49,5 @@ int main(int argc, char** argv) { cout.precision(2); cout << "\nP3:\n" << marginals.marginalCovariance(99) << endl; -return 0; + return 0; } diff --git a/examples/Pose2SLAMExample_graphviz.cpp b/examples/Pose2SLAMExample_graphviz.cpp new file mode 100644 index 000000000..179d86cb7 --- /dev/null +++ b/examples/Pose2SLAMExample_graphviz.cpp @@ -0,0 +1,71 @@ +/* ---------------------------------------------------------------------------- + + * 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_graphviz.cpp + * @brief Save factor graph as graphviz dot file + * @date Sept 6, 2013 + * @author Frank Dellaert + */ + +#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; + + // 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(PriorFactor(1, Pose2(0, 0, 0), priorNoise)); + + // 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(BetweenFactor(1, 2, Pose2(2, 0, 0 ), model)); + graph.push_back(BetweenFactor(2, 3, Pose2(2, 0, M_PI_2), model)); + graph.push_back(BetweenFactor(3, 4, Pose2(2, 0, M_PI_2), model)); + graph.push_back(BetweenFactor(4, 5, Pose2(2, 0, M_PI_2), model)); + + // 2c. Add the loop closure constraint + graph.push_back(BetweenFactor(5, 2, Pose2(2, 0, M_PI_2), model)); + + // 3. Create the data structure to hold the initial estimate to the solution + // For illustrative purposes, these have been deliberately set to incorrect values + Values initial; + initial.insert(1, Pose2(0.5, 0.0, 0.2 )); + initial.insert(2, Pose2(2.3, 0.1, -0.2 )); + initial.insert(3, Pose2(4.1, 0.1, M_PI_2)); + initial.insert(4, Pose2(4.0, 2.0, M_PI )); + initial.insert(5, Pose2(2.1, 2.1, -M_PI_2)); + + // Single Step Optimization using Levenberg-Marquardt + Values result = LevenbergMarquardtOptimizer(graph, initial).optimize(); + + // save factor graph as graphviz dot file + // Render to PDF using "dot Pose2SLAMExample.dot -Tpdf > graph.pdf" + ofstream os("Pose2SLAMExample.dot"); + graph.saveGraph(os, result); + + // Also print out to console + graph.saveGraph(cout, result); + + return 0; +}