From 5b717c801ff94f71f0ab025ca440e51c8c85928b Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 13 May 2014 21:13:16 -0400 Subject: [PATCH 01/10] Moved eclipse targets into tests folder --- .cproject | 2010 ++++++++++++++++++++++++++--------------------------- 1 file changed, 1000 insertions(+), 1010 deletions(-) diff --git a/.cproject b/.cproject index 49afd8e90..7a99a565d 100644 --- a/.cproject +++ b/.cproject @@ -542,6 +542,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -568,7 +576,6 @@ make - tests/testBayesTree.run true false @@ -576,7 +583,6 @@ make - testBinaryBayesNet.run true false @@ -624,7 +630,6 @@ make - testSymbolicBayesNet.run true false @@ -632,7 +637,6 @@ make - tests/testSymbolicFactor.run true false @@ -640,7 +644,6 @@ make - testSymbolicFactorGraph.run true false @@ -656,7 +659,6 @@ make - tests/testBayesTree true false @@ -678,6 +680,134 @@ true true + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + make -j5 @@ -718,6 +848,62 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -734,6 +920,118 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + clean all + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j6 -j8 + testAntiFactor.run + true + true + true + + + make + -j6 -j8 + testBetweenFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + make -j5 @@ -774,30 +1072,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - make -j5 @@ -846,6 +1120,38 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + timeCalibratedCamera.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + clean + true + true + true + make -j5 @@ -1014,6 +1320,291 @@ true true + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run + true + true + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + + + make + -j2 + testVSLAMGraph + true + true + true + + + make + -j5 + check.tests + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j5 + testMarginals.run + true + true + true + + + make + -j5 + testGaussianISAM2.run + true + true + true + + + make + -j5 + testSymbolicFactorGraphB.run + true + true + true + + + make + -j2 + timeSequentialOnDataset.run + true + true + true + + + make + -j5 + testGradientDescentOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + -j2 + testNonlinearISAM.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testExtendedKalmanFilter.run + true + true + true + + + make + -j5 + timing.tests + true + true + true + + + make + -j5 + testNonlinearFactor.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + testGaussianJunctionTreeB.run + true + true + true + + + make + testGraph.run + true + false + true + + + make + testJunctionTree.run + true + false + true + + + make + testSymbolicBayesNetB.run + true + false + true + + + make + -j5 + testGaussianISAM.run + true + true + true + + + make + -j5 + testDoglegOptimizer.run + true + true + true + + + make + -j5 + testNonlinearFactorGraph.run + true + true + true + + + make + -j5 + testIterative.run + true + true + true + + + make + -j5 + testSubgraphSolver.run + true + true + true + + + make + -j5 + testGaussianFactorGraphB.run + true + true + true + + + make + -j5 + testSummarization.run + true + true + true + make -j5 @@ -1120,7 +1711,6 @@ make - testErrors.run true false @@ -1166,6 +1756,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -1350,6 +1948,54 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -1456,6 +2102,7 @@ make + testSimulated2DOriented.run true false @@ -1495,6 +2142,7 @@ make + testSimulated2D.run true false @@ -1502,6 +2150,7 @@ make + testSimulated3D.run true false @@ -1515,6 +2164,22 @@ true true + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + make -j5 @@ -1555,6 +2220,182 @@ false true + + make + -j2 + SimpleRotation.run + true + true + true + + + make + -j5 + CameraResectioning.run + true + true + true + + + make + -j5 + PlanarSLAMExample.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + easyPoint2KalmanFilter.run + true + true + true + + + make + -j2 + elaboratePoint2KalmanFilter.run + true + true + true + + + make + -j5 + Pose2SLAMExample.run + true + true + true + + + make + -j2 + Pose2SLAMwSPCG_easy.run + true + true + true + + + make + -j5 + UGM_small.run + true + true + true + + + make + -j5 + LocalizationExample.run + true + true + true + + + make + -j5 + OdometryExample.run + true + true + true + + + make + -j5 + RangeISAMExample_plaza2.run + true + true + true + + + make + -j5 + SelfCalibrationExample.run + true + true + true + + + make + -j5 + SFMExample.run + true + true + true + + + make + -j5 + VisualISAMExample.run + true + true + true + + + make + -j5 + VisualISAM2Example.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graphviz.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graph.run + true + true + true + + + make + -j5 + SFMExample_bal.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + + tests/testGaussianISAM2 + true + false + true + make -j2 @@ -1571,6 +2412,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 @@ -1772,7 +2709,6 @@ cpack - -G DEB true false @@ -1780,7 +2716,6 @@ cpack - -G RPM true false @@ -1788,7 +2723,6 @@ cpack - -G TGZ true false @@ -1796,7 +2730,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -1970,6 +2903,38 @@ true 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 + make -j2 @@ -2009,981 +2974,6 @@ false true - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j6 -j8 - testAntiFactor.run - true - true - true - - - make - -j6 -j8 - testBetweenFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testDiscreteFactor.run - true - true - true - - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteFactorGraph.run - true - true - true - - - make - -j5 - testDiscreteConditional.run - true - true - true - - - make - -j5 - testDiscreteMarginals.run - true - true - true - - - make - -j2 - vSFMexample.run - true - true - true - - - make - -j2 - testVSLAMGraph - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - -j5 - testMarginals.run - true - true - true - - - make - -j5 - testGaussianISAM2.run - true - true - true - - - make - -j5 - testSymbolicFactorGraphB.run - true - true - true - - - make - -j2 - timeSequentialOnDataset.run - true - true - true - - - make - -j5 - testGradientDescentOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testGaussianBayesNet.run - true - true - true - - - make - -j2 - testNonlinearISAM.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testExtendedKalmanFilter.run - true - true - true - - - make - -j5 - timing.tests - true - true - true - - - make - -j5 - testNonlinearFactor.run - true - true - true - - - make - -j5 - clean - true - true - true - - - make - -j5 - testGaussianJunctionTreeB.run - true - true - true - - - make - - testGraph.run - true - false - true - - - make - - testJunctionTree.run - true - false - true - - - make - - testSymbolicBayesNetB.run - true - false - true - - - make - -j5 - testGaussianISAM.run - true - true - true - - - make - -j5 - testDoglegOptimizer.run - true - true - true - - - make - -j5 - testNonlinearFactorGraph.run - true - true - true - - - make - -j5 - testIterative.run - true - true - true - - - make - -j5 - testSubgraphSolver.run - true - true - true - - - make - -j5 - testGaussianFactorGraphB.run - true - true - true - - - make - -j5 - testSummarization.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - testCal3DS2.run - true - true - true - - - make - -j5 - testCal3Bundler.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j5 - testVector.run - true - true - true - - - make - -j5 - testMatrix.run - true - true - true - - - make - -j2 - SimpleRotation.run - true - true - true - - - make - -j5 - CameraResectioning.run - true - true - true - - - make - -j5 - PlanarSLAMExample.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - easyPoint2KalmanFilter.run - true - true - true - - - make - -j2 - elaboratePoint2KalmanFilter.run - true - true - true - - - make - -j5 - Pose2SLAMExample.run - true - true - true - - - make - -j2 - Pose2SLAMwSPCG_easy.run - true - true - true - - - make - -j5 - UGM_small.run - true - true - true - - - make - -j5 - LocalizationExample.run - true - true - true - - - make - -j5 - OdometryExample.run - true - true - true - - - make - -j5 - RangeISAMExample_plaza2.run - true - true - true - - - make - -j5 - SelfCalibrationExample.run - true - true - true - - - make - -j5 - SFMExample.run - true - true - true - - - make - -j5 - VisualISAMExample.run - true - true - true - - - make - -j5 - VisualISAM2Example.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graphviz.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graph.run - true - true - true - - - make - -j5 - SFMExample_bal.run - true - true - true - - - make - -j4 - testImuFactor.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - tests/testGaussianISAM2 - true - false - 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 - -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 - From e1c0ad83359e38fcdaf6670ee47da9314dab6ccc Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 14 May 2014 14:08:29 -0400 Subject: [PATCH 02/10] Changed 1e-20 to 1e-16. 1e-20 was cutting it too close on 32-bit system, resulting in divide by zero later --- gtsam/geometry/Unit3.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index 88359e0f8..db4c8da83 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -151,9 +151,9 @@ Vector Unit3::localCoordinates(const Unit3& y) const { double dot = p.dot(q); // Check for special cases - if (std::abs(dot - 1.0) < 1e-20) + if (std::abs(dot - 1.0) < 1e-16) return (Vector(2) << 0, 0); - else if (std::abs(dot + 1.0) < 1e-20) + else if (std::abs(dot + 1.0) < 1e-16) return (Vector(2) << M_PI, 0); else { // no special case From d0a2db6646b31d8e06bcf95a4f2a84f7dc96de0f Mon Sep 17 00:00:00 2001 From: Chris Beall Date: Wed, 14 May 2014 19:14:38 -0400 Subject: [PATCH 03/10] Fix for infinite loop on 32-bit Linux. --- gtsam/nonlinear/DoglegOptimizerImpl.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/gtsam/nonlinear/DoglegOptimizerImpl.h b/gtsam/nonlinear/DoglegOptimizerImpl.h index a6c04681b..dea4113f7 100644 --- a/gtsam/nonlinear/DoglegOptimizerImpl.h +++ b/gtsam/nonlinear/DoglegOptimizerImpl.h @@ -191,7 +191,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate( if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY) stay = false; // If not searching, just return with the new Delta else if(mode == SEARCH_EACH_ITERATION) { - if(newDelta == Delta || lastAction == DECREASED_DELTA) + if(fabs(newDelta - Delta) < 1e-15 || lastAction == DECREASED_DELTA) stay = false; // Searching, but Newton's solution is within trust region so keep the same trust region else { stay = true; // Searching and increased Delta, so try again to increase Delta From 0fad2513558d475df0137a0b7dc408222e8c8120 Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 16 May 2014 10:03:18 -0400 Subject: [PATCH 04/10] added example reading g2o file --- examples/Pose2SLAMExampleRobust_g2o.cpp | 115 ++++++++++++++++++++++++ gtsam/slam/dataset.cpp | 4 +- 2 files changed, 117 insertions(+), 2 deletions(-) create mode 100644 examples/Pose2SLAMExampleRobust_g2o.cpp diff --git a/examples/Pose2SLAMExampleRobust_g2o.cpp b/examples/Pose2SLAMExampleRobust_g2o.cpp new file mode 100644 index 000000000..c5655a077 --- /dev/null +++ b/examples/Pose2SLAMExampleRobust_g2o.cpp @@ -0,0 +1,115 @@ +/* ---------------------------------------------------------------------------- + + * 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 A 2D Pose SLAM example that reads input from g2o and uses robust kernels in optimization + * @date May 15, 2014 + * @author Luca Carlone + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; +using namespace gtsam; + +#define LINESIZE 81920 + +int main(const int argc, const char *argv[]){ + + if (argc < 2) + std::cout << "Please specify input file (in g2o format) and output file" << std::endl; + const string g2oFile = argv[1]; + + ifstream is(g2oFile.c_str()); + if (!is) + throw std::invalid_argument("File not found!"); + + std::cout << "Reading g2o file: " << g2oFile << std::endl; + // READ INITIAL GUESS FROM G2O FILE + Values initial; + string tag; + while (is) { + if(! (is >> tag)) + break; + + if (tag == "VERTEX_SE2") { + int id; + double x, y, yaw; + is >> id >> x >> y >> yaw; + initial.insert(id, Pose2(x, y, yaw)); + } + is.ignore(LINESIZE, '\n'); + } + is.clear(); /* clears the end-of-file and error flags */ + is.seekg(0, ios::beg); + // initial.print("initial guess"); + + // READ MEASUREMENTS FROM G2O FILE + NonlinearFactorGraph graph; + while (is) { + if(! (is >> tag)) + break; + + if (tag == "EDGE_SE2") { + int id1, id2; + double x, y, yaw; + double I11, I12, I13, I22, I23, I33; + + is >> id1 >> id2 >> x >> y >> yaw; + is >> I11 >> I12 >> I13 >> I22 >> I23 >> I33; + + // Try to guess covariance matrix layout + Matrix m(3,3); + m << I11, I12, I13, I12, I22, I23, I13, I23, I33; + + Pose2 l1Xl2(x, y, yaw); + + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances((Vector(3) << 1/I11, 1/I22, 1/I33)); + + NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, l1Xl2, model)); + graph.add(factor); + } + is.ignore(LINESIZE, '\n'); + } + // graph.print("graph"); + + // GaussNewtonParams parameters; + // Stop iterating once the change in error between steps is less than this value + // parameters.relativeErrorTol = 1e-5; + // Do not perform more than N iteration steps + // parameters.maxIterations = 100; + // Create the optimizer ... + std::cout << "Optimizing the factor graph" << std::endl; + GaussNewtonOptimizer optimizer(graph, initial); // , parameters); + // ... and optimize + Values result = optimizer.optimize(); + // result.print("results"); + std::cout << "Optimization complete" << std::endl; + + const string outputFile = argv[2]; + std::cout << "Writing results to file: " << outputFile << std::endl; + + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.0, 0.0, 0.0)); + save2D(graph, result, model, outputFile); + + return 0; +} diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 4a7166f38..17d996a52 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -258,8 +258,8 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, if (!factor) continue; - Pose2 pose = factor->measured().inverse(); - stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " " + Pose2 pose = factor->measured(); //.inverse(); + stream << "EDGE2 " << factor->key1() << " " << factor->key2() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl; From 5a6d71969044e7274a8fc00a73cb811f7a29cf4c Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 16 May 2014 20:50:06 -0400 Subject: [PATCH 05/10] included priors and robust model in example --- examples/Pose2SLAMExampleRobust_g2o.cpp | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/examples/Pose2SLAMExampleRobust_g2o.cpp b/examples/Pose2SLAMExampleRobust_g2o.cpp index c5655a077..94a19f7b2 100644 --- a/examples/Pose2SLAMExampleRobust_g2o.cpp +++ b/examples/Pose2SLAMExampleRobust_g2o.cpp @@ -85,12 +85,17 @@ int main(const int argc, const char *argv[]){ noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances((Vector(3) << 1/I11, 1/I22, 1/I33)); - NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, l1Xl2, model)); + NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, l1Xl2, + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), model))); graph.add(factor); } is.ignore(LINESIZE, '\n'); } - // graph.print("graph"); + + // otherwise GTSAM cannot solve the problem + NonlinearFactorGraph graphWithPrior = graph; + noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 0.01, 0.01, 0.001)); + graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); // GaussNewtonParams parameters; // Stop iterating once the change in error between steps is less than this value @@ -99,7 +104,7 @@ int main(const int argc, const char *argv[]){ // parameters.maxIterations = 100; // Create the optimizer ... std::cout << "Optimizing the factor graph" << std::endl; - GaussNewtonOptimizer optimizer(graph, initial); // , parameters); + GaussNewtonOptimizer optimizer(graphWithPrior, initial); // , parameters); // ... and optimize Values result = optimizer.optimize(); // result.print("results"); From c167430389ed0b9ac6fa3a9e872d23d83b2f2fe6 Mon Sep 17 00:00:00 2001 From: Luca Date: Mon, 19 May 2014 10:53:59 -0400 Subject: [PATCH 06/10] included Tukey --- examples/Pose2SLAMExampleRobust_g2o.cpp | 9 ++++++++- 1 file changed, 8 insertions(+), 1 deletion(-) diff --git a/examples/Pose2SLAMExampleRobust_g2o.cpp b/examples/Pose2SLAMExampleRobust_g2o.cpp index 94a19f7b2..e326838f2 100644 --- a/examples/Pose2SLAMExampleRobust_g2o.cpp +++ b/examples/Pose2SLAMExampleRobust_g2o.cpp @@ -85,13 +85,20 @@ int main(const int argc, const char *argv[]){ noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances((Vector(3) << 1/I11, 1/I22, 1/I33)); + // NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, l1Xl2, + // noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model))); + NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, l1Xl2, - noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.0), model))); + noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), model))); + graph.add(factor); } is.ignore(LINESIZE, '\n'); } + //std::cout << "Robust kernel: Huber" << std::endl; + std::cout << "Robust kernel: Tukey" << std::endl; + // otherwise GTSAM cannot solve the problem NonlinearFactorGraph graphWithPrior = graph; noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 0.01, 0.01, 0.001)); From 04533107263d72e39f99bb58b218eb37210d5163 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 20 May 2014 16:35:45 -0400 Subject: [PATCH 07/10] adding functions to read/write g2o files --- examples/Pose2SLAMExampleRobust_g2o.cpp | 69 +-------------- gtsam/slam/dataset.cpp | 108 ++++++++++++++++++++++++ gtsam/slam/dataset.h | 10 +++ 3 files changed, 122 insertions(+), 65 deletions(-) diff --git a/examples/Pose2SLAMExampleRobust_g2o.cpp b/examples/Pose2SLAMExampleRobust_g2o.cpp index e326838f2..ad560ab59 100644 --- a/examples/Pose2SLAMExampleRobust_g2o.cpp +++ b/examples/Pose2SLAMExampleRobust_g2o.cpp @@ -39,89 +39,28 @@ int main(const int argc, const char *argv[]){ std::cout << "Please specify input file (in g2o format) and output file" << std::endl; const string g2oFile = argv[1]; - ifstream is(g2oFile.c_str()); - if (!is) - throw std::invalid_argument("File not found!"); - - std::cout << "Reading g2o file: " << g2oFile << std::endl; - // READ INITIAL GUESS FROM G2O FILE - Values initial; - string tag; - while (is) { - if(! (is >> tag)) - break; - - if (tag == "VERTEX_SE2") { - int id; - double x, y, yaw; - is >> id >> x >> y >> yaw; - initial.insert(id, Pose2(x, y, yaw)); - } - is.ignore(LINESIZE, '\n'); - } - is.clear(); /* clears the end-of-file and error flags */ - is.seekg(0, ios::beg); - // initial.print("initial guess"); - - // READ MEASUREMENTS FROM G2O FILE NonlinearFactorGraph graph; - while (is) { - if(! (is >> tag)) - break; - - if (tag == "EDGE_SE2") { - int id1, id2; - double x, y, yaw; - double I11, I12, I13, I22, I23, I33; - - is >> id1 >> id2 >> x >> y >> yaw; - is >> I11 >> I12 >> I13 >> I22 >> I23 >> I33; - - // Try to guess covariance matrix layout - Matrix m(3,3); - m << I11, I12, I13, I12, I22, I23, I13, I23, I33; - - Pose2 l1Xl2(x, y, yaw); - - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Variances((Vector(3) << 1/I11, 1/I22, 1/I33)); - - // NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, l1Xl2, - // noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model))); - - NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, l1Xl2, - noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), model))); - - graph.add(factor); - } - is.ignore(LINESIZE, '\n'); - } - - //std::cout << "Robust kernel: Huber" << std::endl; - std::cout << "Robust kernel: Tukey" << std::endl; + Values initial; + readG2o(g2oFile, graph, initial); // otherwise GTSAM cannot solve the problem NonlinearFactorGraph graphWithPrior = graph; noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 0.01, 0.01, 0.001)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); - // GaussNewtonParams parameters; - // Stop iterating once the change in error between steps is less than this value - // parameters.relativeErrorTol = 1e-5; - // Do not perform more than N iteration steps - // parameters.maxIterations = 100; // Create the optimizer ... std::cout << "Optimizing the factor graph" << std::endl; GaussNewtonOptimizer optimizer(graphWithPrior, initial); // , parameters); // ... and optimize Values result = optimizer.optimize(); - // result.print("results"); std::cout << "Optimization complete" << std::endl; const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.0, 0.0, 0.0)); - save2D(graph, result, model, outputFile); + writeG2o(graph, result, model, outputFile); + std::cout << "done! " << std::endl; return 0; } diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 17d996a52..6e0cf11ba 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -537,6 +537,114 @@ bool readBundler(const string& filename, SfM_data &data) return true; } +/* ************************************************************************* */ +bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, + const kernelFunctionType kernelFunction = QUADRATIC){ + + ifstream is(g2oFile.c_str()); + if (!is){ + throw std::invalid_argument("File not found!"); + return false; + } + + std::cout << "Reading g2o file: " << g2oFile << std::endl; + // READ INITIAL GUESS FROM G2O FILE + string tag; + while (is) { + if(! (is >> tag)) + break; + + if (tag == "VERTEX_SE2") { + int id; + double x, y, yaw; + is >> id >> x >> y >> yaw; + initial.insert(id, Pose2(x, y, yaw)); + } + is.ignore(LINESIZE, '\n'); + } + is.clear(); /* clears the end-of-file and error flags */ + is.seekg(0, ios::beg); + // initial.print("initial guess"); + + // READ MEASUREMENTS FROM G2O FILE + while (is) { + if(! (is >> tag)) + break; + + if (tag == "EDGE_SE2") { + int id1, id2; + double x, y, yaw; + double I11, I12, I13, I22, I23, I33; + + is >> id1 >> id2 >> x >> y >> yaw; + is >> I11 >> I12 >> I13 >> I22 >> I23 >> I33; + Pose2 l1Xl2(x, y, yaw); + noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << I11, I22, I33)); + + switch (kernelFunction) { + {case QUADRATIC: + NonlinearFactor::shared_ptr factor(new BetweenFactor(id1, id2, l1Xl2, model)); + graph.add(factor); + break;} + {case HUBER: + NonlinearFactor::shared_ptr huberFactor(new BetweenFactor(id1, id2, l1Xl2, + noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model))); + graph.add(huberFactor); + break;} + {case TUKEY: + NonlinearFactor::shared_ptr tukeyFactor(new BetweenFactor(id1, id2, l1Xl2, + noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), model))); + graph.add(tukeyFactor); + break;} + } + } + is.ignore(LINESIZE, '\n'); + } + // Output which kernel is used + switch (kernelFunction) { + case QUADRATIC: + std::cout << "Robust kernel: None" << std::endl; break; + case HUBER: + std::cout << "Robust kernel: Huber" << std::endl; break; + case TUKEY: + std::cout << "Robust kernel: Tukey" << std::endl; break; + } + return true; +} + +/* ************************************************************************* */ +bool writeG2o(const string& filename, const NonlinearFactorGraph& graph, const Values& estimate){ + + fstream stream(filename.c_str(), fstream::out); + + // save poses + BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) + { + const Pose2& pose = dynamic_cast(key_value.value); + stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " + << pose.y() << " " << pose.theta() << endl; + } + + // save edges + BOOST_FOREACH(boost::shared_ptr factor_, graph) + { + boost::shared_ptr > factor = + boost::dynamic_pointer_cast >(factor_); + if (!factor) + continue; + +// Matrix sqrtInfo = factor->get_Noise Model (). +// Matrix info = sqrtInfo.tra + + Pose2 pose = factor->measured(); //.inverse(); + stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " + << pose.x() << " " << pose.y() << " " << pose.theta() << " " + << info(0, 0) << " " << info(0, 1) << " " << info(1, 1) << " " + << info(2, 2) << " " << info(0, 2) << " " << info(1, 2) << endl; + } + stream.close(); +} + /* ************************************************************************* */ bool readBAL(const string& filename, SfM_data &data) { diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 64ccd37b5..42c8e4f9a 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -117,6 +117,16 @@ struct SfM_data */ GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data); +/** + * @brief This function parses a g2o file and stores the measurements into a + * NonlinearFactorGraph and the initial guess in a Values structure + * @param filename The name of the g2o file + * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2) + * @return initial Values containing the initial guess (VERTEX_SE2) + */ +enum kernelFunctionType { QUADRATIC, HUBER, TUKEY }; +bool readG2o2D(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction); + /** * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a * SfM_data structure From 2430b0bbf03949acc14eeb4d2666da9d8b5251fe Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 20 May 2014 16:53:37 -0400 Subject: [PATCH 08/10] added functions to read/write g2o files --- gtsam/slam/dataset.cpp | 16 ++++++++++------ gtsam/slam/dataset.h | 11 ++++++++++- 2 files changed, 20 insertions(+), 7 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 6e0cf11ba..9a1fb3303 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -539,7 +539,7 @@ bool readBundler(const string& filename, SfM_data &data) /* ************************************************************************* */ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, - const kernelFunctionType kernelFunction = QUADRATIC){ + const kernelFunctionType kernelFunction){ ifstream is(g2oFile.c_str()); if (!is){ @@ -613,7 +613,7 @@ bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& in } /* ************************************************************************* */ -bool writeG2o(const string& filename, const NonlinearFactorGraph& graph, const Values& estimate){ +bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate){ fstream stream(filename.c_str(), fstream::out); @@ -633,16 +633,20 @@ bool writeG2o(const string& filename, const NonlinearFactorGraph& graph, const V if (!factor) continue; -// Matrix sqrtInfo = factor->get_Noise Model (). -// Matrix info = sqrtInfo.tra + SharedNoiseModel model = factor->get_noiseModel(); + boost::shared_ptr diagonalModel = + boost::dynamic_pointer_cast(model); + if (!diagonalModel) + throw std::invalid_argument("writeG2o: invalid noise model (current version assumes diagonal noise model)!"); Pose2 pose = factor->measured(); //.inverse(); stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << " " - << info(0, 0) << " " << info(0, 1) << " " << info(1, 1) << " " - << info(2, 2) << " " << info(0, 2) << " " << info(1, 2) << endl; + << diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " " + << diagonalModel->precision(1) << " " << 0.0 << " " << diagonalModel->precision(2) << endl; } stream.close(); + return true; } /* ************************************************************************* */ diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 42c8e4f9a..7bbc88f70 100644 --- a/gtsam/slam/dataset.h +++ b/gtsam/slam/dataset.h @@ -125,7 +125,16 @@ GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data); * @return initial Values containing the initial guess (VERTEX_SE2) */ enum kernelFunctionType { QUADRATIC, HUBER, TUKEY }; -bool readG2o2D(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction); +bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction = QUADRATIC); + +/** + * @brief This function writes a g2o file from + * NonlinearFactorGraph and a Values structure + * @param filename The name of the g2o file to write + * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2) + * @return estimate Values containing the values (VERTEX_SE2) + */ +bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate); /** * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a From 9a2f282e4b6da7eea756f2d9a309e2a885585bf7 Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 20 May 2014 16:53:58 -0400 Subject: [PATCH 09/10] made simpler example using g2o input file --- ...ExampleRobust_g2o.cpp => Pose2SLAMExample_g2o.cpp} | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) rename examples/{Pose2SLAMExampleRobust_g2o.cpp => Pose2SLAMExample_g2o.cpp} (83%) diff --git a/examples/Pose2SLAMExampleRobust_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp similarity index 83% rename from examples/Pose2SLAMExampleRobust_g2o.cpp rename to examples/Pose2SLAMExample_g2o.cpp index ad560ab59..26fee923c 100644 --- a/examples/Pose2SLAMExampleRobust_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -31,35 +31,30 @@ using namespace std; using namespace gtsam; -#define LINESIZE 81920 int main(const int argc, const char *argv[]){ if (argc < 2) - std::cout << "Please specify input file (in g2o format) and output file" << std::endl; + std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl; const string g2oFile = argv[1]; NonlinearFactorGraph graph; Values initial; readG2o(g2oFile, graph, initial); - // otherwise GTSAM cannot solve the problem + // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = graph; noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 0.01, 0.01, 0.001)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); - // Create the optimizer ... std::cout << "Optimizing the factor graph" << std::endl; GaussNewtonOptimizer optimizer(graphWithPrior, initial); // , parameters); - // ... and optimize Values result = optimizer.optimize(); std::cout << "Optimization complete" << std::endl; const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - - noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.0, 0.0, 0.0)); - writeG2o(graph, result, model, outputFile); + writeG2o(outputFile, graph, result); std::cout << "done! " << std::endl; return 0; From 78fe603933f949e6bc9ba7ce7948be03973162df Mon Sep 17 00:00:00 2001 From: Luca Date: Tue, 20 May 2014 16:56:01 -0400 Subject: [PATCH 10/10] re-established previous function --- gtsam/slam/dataset.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 9a1fb3303..1029d7615 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -258,8 +258,8 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config, if (!factor) continue; - Pose2 pose = factor->measured(); //.inverse(); - stream << "EDGE2 " << factor->key1() << " " << factor->key2() << " " + Pose2 pose = factor->measured().inverse(); + stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " " << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl;