diff --git a/.cproject b/.cproject index d5a6ca4d4..1783efa98 100644 --- a/.cproject +++ b/.cproject @@ -568,7 +568,6 @@ make - tests/testBayesTree.run true false @@ -576,7 +575,6 @@ make - testBinaryBayesNet.run true false @@ -624,7 +622,6 @@ make - testSymbolicBayesNet.run true false @@ -632,7 +629,6 @@ make - tests/testSymbolicFactor.run true false @@ -640,7 +636,6 @@ make - testSymbolicFactorGraph.run true false @@ -656,12 +651,19 @@ make - tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -678,6 +680,142 @@ 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 + testCal3Unified.run + true + true + true + make -j5 @@ -718,6 +856,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 +928,30 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + clean all + true + true + true + make -j2 @@ -806,6 +1024,46 @@ true true + + make + -j5 + testWrap.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 @@ -974,6 +1232,307 @@ 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 + testMatrix.run + true + true + true + + + make + -j5 + testVector.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 @@ -1080,7 +1639,6 @@ make - testErrors.run true false @@ -1126,6 +1684,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -1310,6 +1876,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 @@ -1318,6 +1932,70 @@ true true + + make + -j5 + testAntiFactor.run + true + true + true + + + make + -j5 + testBetweenFactor.run + true + true + true + + + make + -j5 + testDataset.run + true + true + true + + + make + -j5 + testEssentialMatrixFactor.run + true + true + true + + + make + -j5 + testGeneralSFMFactor_Cal3Bundler.run + true + true + true + + + make + -j5 + testGeneralSFMFactor.run + true + true + true + + + make + -j5 + testProjectionFactor.run + true + true + true + + + make + -j5 + testRotateFactor.run + true + true + true + make -j5 @@ -1416,6 +2094,7 @@ make + testSimulated2DOriented.run true false @@ -1455,6 +2134,7 @@ make + testSimulated2D.run true false @@ -1462,6 +2142,7 @@ make + testSimulated3D.run true false @@ -1515,1140 +2196,6 @@ false true - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testLieConfig.run - true - true - true - - - make - -j3 - install - true - false - true - - - make - -j2 - clean - true - true - true - - - make - -j1 - check - true - false - true - - - make - -j5 - all - true - true - true - - - cmake - .. - true - false - true - - - make - -j5 - gtsam-shared - true - true - true - - - make - -j5 - gtsam-static - true - true - true - - - make - -j5 - timing - true - true - true - - - make - -j5 - examples - true - true - true - - - make - -j5 - VERBOSE=1 all - true - true - true - - - make - -j5 - VERBOSE=1 check - true - true - true - - - make - -j5 - check.base - true - true - true - - - make - -j5 - timing.base - true - true - true - - - make - -j2 VERBOSE=1 - check.geometry - true - false - true - - - make - -j5 - timing.geometry - true - true - true - - - make - -j2 VERBOSE=1 - check.inference - true - false - true - - - make - -j5 - timing.inference - true - true - true - - - make - -j2 VERBOSE=1 - check.linear - true - false - true - - - make - -j5 - timing.linear - true - true - true - - - make - -j2 VERBOSE=1 - check.nonlinear - true - false - true - - - make - -j5 - timing.nonlinear - true - true - true - - - make - -j2 VERBOSE=1 - check.slam - true - false - true - - - make - -j5 - timing.slam - true - true - true - - - make - -j5 - wrap_gtsam - true - true - true - - - make - VERBOSE=1 - wrap_gtsam - true - false - true - - - cpack - - -G DEB - true - false - true - - - cpack - - -G RPM - true - false - true - - - cpack - - -G TGZ - true - false - true - - - cpack - - --config CPackSourceConfig.cmake - true - false - true - - - make - -j5 - check.discrete - true - true - true - - - make - -j5 - check.discrete_unstable - true - true - true - - - make - -j5 - check.base_unstable - true - true - true - - - make - -j5 - check.dynamics_unstable - true - true - true - - - make - -j5 - check.slam_unstable - true - true - true - - - make - -j5 - check.unstable - true - true - true - - - make - -j5 - wrap_gtsam_build - true - true - true - - - make - -j5 - wrap_gtsam_unstable_build - true - true - true - - - make - -j5 - wrap_gtsam_unstable - true - true - true - - - make - -j5 - wrap - true - true - true - - - make - -j5 - wrap_gtsam_distclean - true - true - true - - - make - -j5 - wrap_gtsam_unstable_distclean - true - true - true - - - make - -j5 - doc - true - true - true - - - make - -j5 - doc_clean - true - true - true - - - make - -j5 - check - true - true - true - - - make - -j5 - check.geometry_unstable - true - true - true - - - make - -j5 - check.linear_unstable - true - true - true - - - make - -j6 -j8 - gtsam_unstable-shared - true - true - true - - - make - -j6 -j8 - gtsam_unstable-static - true - true - true - - - make - -j6 -j8 - check.nonlinear_unstable - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. - true - false - true - - - make - -j2 - testGaussianFactor.run - true - 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 - -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 - -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 - testWrap.run - 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 - testMatrix.run - true - true - true - - - make - -j5 - testVector.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 - testAntiFactor.run - true - true - true - - - make - -j5 - testBetweenFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - make -j2 @@ -2883,11 +2430,28 @@ make + tests/testGaussianISAM2 true false true + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testLieConfig.run + true + true + true + make -j2 @@ -2984,6 +2548,401 @@ true true + + make + -j3 + install + true + false + true + + + make + -j2 + clean + true + true + true + + + make + -j1 + check + true + false + true + + + make + -j5 + all + true + true + true + + + cmake + .. + true + false + true + + + make + -j5 + gtsam-shared + true + true + true + + + make + -j5 + gtsam-static + true + true + true + + + make + -j5 + timing + true + true + true + + + make + -j5 + examples + true + true + true + + + make + -j5 + VERBOSE=1 all + true + true + true + + + make + -j5 + VERBOSE=1 check + true + true + true + + + make + -j5 + check.base + true + true + true + + + make + -j5 + timing.base + true + true + true + + + make + -j2 VERBOSE=1 + check.geometry + true + false + true + + + make + -j5 + timing.geometry + true + true + true + + + make + -j2 VERBOSE=1 + check.inference + true + false + true + + + make + -j5 + timing.inference + true + true + true + + + make + -j2 VERBOSE=1 + check.linear + true + false + true + + + make + -j5 + timing.linear + true + true + true + + + make + -j2 VERBOSE=1 + check.nonlinear + true + false + true + + + make + -j5 + timing.nonlinear + true + true + true + + + make + -j2 VERBOSE=1 + check.slam + true + false + true + + + make + -j5 + timing.slam + true + true + true + + + make + -j5 + wrap_gtsam + true + true + true + + + make + VERBOSE=1 + wrap_gtsam + true + false + true + + + cpack + -G DEB + true + false + true + + + cpack + -G RPM + true + false + true + + + cpack + -G TGZ + true + false + true + + + cpack + --config CPackSourceConfig.cmake + true + false + true + + + make + -j5 + check.discrete + true + true + true + + + make + -j5 + check.discrete_unstable + true + true + true + + + make + -j5 + check.base_unstable + true + true + true + + + make + -j5 + check.dynamics_unstable + true + true + true + + + make + -j5 + check.slam_unstable + true + true + true + + + make + -j5 + check.unstable + true + true + true + + + make + -j5 + wrap_gtsam_build + true + true + true + + + make + -j5 + wrap_gtsam_unstable_build + true + true + true + + + make + -j5 + wrap_gtsam_unstable + true + true + true + + + make + -j5 + wrap + true + true + true + + + make + -j5 + wrap_gtsam_distclean + true + true + true + + + make + -j5 + wrap_gtsam_unstable_distclean + true + true + true + + + make + -j5 + doc + true + true + true + + + make + -j5 + doc_clean + true + true + true + + + make + -j5 + check + true + true + true + + + make + -j5 + check.geometry_unstable + true + true + true + + + make + -j5 + check.linear_unstable + true + true + true + + + make + -j6 -j8 + gtsam_unstable-shared + true + true + true + + + make + -j6 -j8 + gtsam_unstable-static + true + true + true + + + make + -j6 -j8 + check.nonlinear_unstable + true + true + true + + + make + -j5 + check.tests + true + true + true + make -j5 @@ -3008,6 +2967,45 @@ true true + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + true + diff --git a/CMakeLists.txt b/CMakeLists.txt index d64814829..8ca25e132 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,7 +34,7 @@ if(${CMAKE_SOURCE_DIR} STREQUAL ${CMAKE_BINARY_DIR}) message(FATAL_ERROR "In-source builds not allowed. Please make a new directory (called a build directory) and run CMake from there. You may need to remove CMakeCache.txt. ") endif() -# See whether gtsam_unstable is available (it will be present only if we're using an SVN checkout) +# See whether gtsam_unstable is available (it will be present only if we're using a git checkout) if(EXISTS "${PROJECT_SOURCE_DIR}/gtsam_unstable" AND IS_DIRECTORY "${PROJECT_SOURCE_DIR}/gtsam_unstable") set(GTSAM_UNSTABLE_AVAILABLE 1) else() @@ -93,7 +93,7 @@ if(MSVC) # Disable autolinking if(NOT Boost_USE_STATIC_LIBS) add_definitions(-DBOOST_ALL_NO_LIB) - add_definitions(-DBOOST_ALL_DYN_LINK) + add_definitions(-DBOOST_ALL_DYN_LINK) endif() endif() diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 3a4ee9cff..b46a53198 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -31,7 +31,7 @@ int main(const int argc, const char *argv[]) { // Read graph from file string g2oFile; if (argc < 2) - g2oFile = "../../examples/Data/noisyToyGraph.txt"; + g2oFile = findExampleDataFile("noisyToyGraph.txt"); else g2oFile = argv[1]; diff --git a/examples/Pose2SLAMExample_graph.cpp b/examples/Pose2SLAMExample_graph.cpp index 449144fef..86be74c75 100644 --- a/examples/Pose2SLAMExample_graph.cpp +++ b/examples/Pose2SLAMExample_graph.cpp @@ -32,7 +32,8 @@ int main (int argc, char** argv) { 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); + string graph_file = findExampleDataFile("w100.graph"); + boost::tie(graph, initial) = load2D(graph_file, model); initial->print("Initial estimate:\n"); // Add a Gaussian prior on first poses diff --git a/examples/Pose2SLAMExample_lago.cpp b/examples/Pose2SLAMExample_lago.cpp index 1f5d80d7b..bb434f3ce 100644 --- a/examples/Pose2SLAMExample_lago.cpp +++ b/examples/Pose2SLAMExample_lago.cpp @@ -32,7 +32,7 @@ int main(const int argc, const char *argv[]) { // Read graph from file string g2oFile; if (argc < 2) - g2oFile = "../../examples/Data/noisyToyGraph.txt"; + g2oFile = findExampleDataFile("noisyToyGraph.txt"); else g2oFile = argv[1]; diff --git a/examples/Pose2SLAMwSPCG.cpp b/examples/Pose2SLAMwSPCG.cpp index 2b2f63064..8e8e28570 100644 --- a/examples/Pose2SLAMwSPCG.cpp +++ b/examples/Pose2SLAMwSPCG.cpp @@ -104,7 +104,7 @@ int main(int argc, char** argv) { LevenbergMarquardtParams parameters; parameters.verbosity = NonlinearOptimizerParams::ERROR; parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA; - parameters.linearSolverType = NonlinearOptimizerParams::CONJUGATE_GRADIENT; + parameters.linearSolverType = NonlinearOptimizerParams::Iterative; { parameters.iterativeParams = boost::make_shared(); diff --git a/examples/RangeISAMExample_plaza2.cpp b/examples/RangeISAMExample_plaza2.cpp index 29b02a271..252372f39 100644 --- a/examples/RangeISAMExample_plaza2.cpp +++ b/examples/RangeISAMExample_plaza2.cpp @@ -40,6 +40,7 @@ #include #include #include +#include // Standard headers, added last, so we know headers above work on their own #include @@ -59,9 +60,8 @@ namespace NM = gtsam::noiseModel; typedef pair TimedOdometry; list readOdometry() { list odometryList; - ifstream is("../../examples/Data/Plaza2_DR.txt"); - if (!is) - throw runtime_error("../../examples/Data/Plaza2_DR.txt file not found"); + string data_file = findExampleDataFile("Plaza2_DR.txt"); + ifstream is(data_file.c_str()); while (is) { double t, distance_traveled, delta_heading; @@ -78,9 +78,8 @@ list readOdometry() { typedef boost::tuple RangeTriple; vector readTriples() { vector triples; - ifstream is("../../examples/Data/Plaza2_TD.txt"); - if (!is) - throw runtime_error("../../examples/Data/Plaza2_TD.txt file not found"); + string data_file = findExampleDataFile("Plaza2_TD.txt"); + ifstream is(data_file.c_str()); while (is) { double t, sender, receiver, range; diff --git a/examples/StereoVOExample.cpp b/examples/StereoVOExample.cpp index 8a04d13a3..19725798c 100644 --- a/examples/StereoVOExample.cpp +++ b/examples/StereoVOExample.cpp @@ -24,47 +24,48 @@ */ #include -#include +#include +#include +#include #include #include -#include -#include -#include #include -#include -#include - - using namespace std; using namespace gtsam; int main(int argc, char** argv){ + //create graph object, add first pose at origin with key '1' NonlinearFactorGraph graph; - Pose3 first_pose = Pose3(); - graph.push_back(NonlinearEquality(1, first_pose)); + Pose3 first_pose; + graph.push_back(NonlinearEquality(1, Pose3())); //create factor noise model with 3 sigmas of value 1 const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1); //create stereo camera calibration object with .2m between cameras const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)); + //create and add stereo factors between first pose (key value 1) and the three landmarks graph.push_back(GenericStereoFactor(StereoPoint2(520, 480, 440), model, 1, 3, K)); graph.push_back(GenericStereoFactor(StereoPoint2(120, 80, 440), model, 1, 4, K)); graph.push_back(GenericStereoFactor(StereoPoint2(320, 280, 140), model, 1, 5, K)); + //create and add stereo factors between second pose and the three landmarks graph.push_back(GenericStereoFactor(StereoPoint2(570, 520, 490), model, 2, 3, K)); graph.push_back(GenericStereoFactor(StereoPoint2(70, 20, 490), model, 2, 4, K)); graph.push_back(GenericStereoFactor(StereoPoint2(320, 270, 115), model, 2, 5, K)); + //create Values object to contain initial estimates of camera poses and landmark locations - Values initial_estimate = Values(); + Values initial_estimate; + //create and add iniital estimates initial_estimate.insert(1, first_pose); initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1))); initial_estimate.insert(3, Point3(1, 1, 5)); initial_estimate.insert(4, Point3(-1, 1, 5)); initial_estimate.insert(5, Point3(0, -0.5, 5)); + //create Levenberg-Marquardt optimizer for resulting factor graph, optimize LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate); Values result = optimizer.optimize(); @@ -72,4 +73,4 @@ int main(int argc, char** argv){ result.print("Final result:\n"); return 0; -} \ No newline at end of file +} diff --git a/gtsam.h b/gtsam.h index b7178d534..678e2a3d6 100644 --- a/gtsam.h +++ b/gtsam.h @@ -1481,9 +1481,7 @@ class GaussianISAM { #include virtual class IterativeOptimizationParameters { - string getKernel() const ; string getVerbosity() const; - void setKernel(string s) ; void setVerbosity(string s) ; void print() const; }; @@ -1516,7 +1514,7 @@ virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters { void print() const; }; -class SubgraphSolver { +virtual class SubgraphSolver { SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); SubgraphSolver(const gtsam::GaussianFactorGraph &Ab1, const gtsam::GaussianFactorGraph &Ab2, const gtsam::SubgraphSolverParameters ¶meters, const gtsam::Ordering& ordering); gtsam::VectorValues optimize() const; @@ -1855,12 +1853,12 @@ virtual class NonlinearOptimizerParams { void setLinearSolverType(string solver); void setOrdering(const gtsam::Ordering& ordering); - void setIterativeParams(const gtsam::SubgraphSolverParameters ¶ms); + void setIterativeParams(gtsam::IterativeOptimizationParameters* params); bool isMultifrontal() const; bool isSequential() const; bool isCholmod() const; - bool isCG() const; + bool isIterative() const; }; bool checkConvergence(double relativeErrorTreshold, @@ -2199,6 +2197,25 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor { void serialize() const; }; +#include +template +virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor { + + SmartProjectionPoseFactor(double rankTol, double linThreshold, + bool manageDegeneracy, bool enableEPI, const POSE& body_P_sensor); + + SmartProjectionPoseFactor(double rankTol); + SmartProjectionPoseFactor(); + + void add(const gtsam::Point2& measured_i, size_t poseKey_i, const gtsam::noiseModel::Base* noise_i, + const CALIBRATION* K_i); + + // enabling serialization functionality + //void serialize() const; +}; + +typedef gtsam::SmartProjectionPoseFactor SmartProjectionPose3Factor; + #include template @@ -2301,7 +2318,8 @@ virtual class ConstantBias : gtsam::Value { #include class ImuFactorPreintegratedMeasurements { // Standard Constructor - ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance, Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); + ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration); + ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance); ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs); // Testable @@ -2339,6 +2357,15 @@ class CombinedImuFactorPreintegratedMeasurements { Matrix biasAccCovariance, Matrix biasOmegaCovariance, Matrix biasAccOmegaInit); + CombinedImuFactorPreintegratedMeasurements( + const gtsam::imuBias::ConstantBias& bias, + Matrix measuredAccCovariance, + Matrix measuredOmegaCovariance, + Matrix integrationErrorCovariance, + Matrix biasAccCovariance, + Matrix biasOmegaCovariance, + Matrix biasAccOmegaInit, + bool use2ndOrderIntegration); CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs); // Testable @@ -2352,8 +2379,7 @@ class CombinedImuFactorPreintegratedMeasurements { virtual class CombinedImuFactor : gtsam::NonlinearFactor { CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j, - const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis, - const gtsam::noiseModel::Base* model); + const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis); // Standard Interface gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index e6c97cb1a..c75eff022 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -25,7 +25,7 @@ namespace gtsam { /* ************************************************************************* */ Cal3DS2::Cal3DS2(const Vector &v): - fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){} + fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){} /* ************************************************************************* */ Matrix Cal3DS2::K() const { @@ -34,32 +34,64 @@ Matrix Cal3DS2::K() const { /* ************************************************************************* */ Vector Cal3DS2::vector() const { - return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); + return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_); } /* ************************************************************************* */ -void Cal3DS2::print(const std::string& s) const { - gtsam::print(K(), s + ".K"); - gtsam::print(Vector(k()), s + ".k"); +void Cal3DS2::print(const std::string& s_) const { + gtsam::print(K(), s_ + ".K"); + gtsam::print(Vector(k()), s_ + ".k"); } /* ************************************************************************* */ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || - fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol) + fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol) return false; return true; } /* ************************************************************************* */ -Point2 Cal3DS2::uncalibrate(const Point2& p, - boost::optional H1, - boost::optional H2) const { +static Eigen::Matrix D2dcalibration(double x, double y, double xx, + double yy, double xy, double rr, double r4, double pnx, double pny, + const Eigen::Matrix& DK) { + Eigen::Matrix DR1; + DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; + Eigen::Matrix DR2; + DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // + y * rr, y * r4, rr + 2 * yy, 2 * xy; + Eigen::Matrix D; + D << DR1, DK * DR2; + return D; +} - // parameters - const double fx = fx_, fy = fy_, s = s_; - const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_; +/* ************************************************************************* */ +static Eigen::Matrix D2dintrinsic(double x, double y, double rr, + double g, double k1, double k2, double p1, double p2, + const Eigen::Matrix& DK) { + const double drdx = 2. * x; + const double drdy = 2. * y; + const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; + const double dgdy = k1 * drdy + k2 * 2. * rr * drdy; + + // Dx = 2*p1*xy + p2*(rr+2*xx); + // Dy = 2*p2*xy + p1*(rr+2*yy); + const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x); + const double dDxdy = 2. * p1 * x + p2 * drdy; + const double dDydx = 2. * p2 * y + p1 * drdx; + const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); + + Eigen::Matrix DR; + DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // + y * dgdx + dDydx, g + y * dgdy + dDydy; + + return DK * DR; +} + +/* ************************************************************************* */ +Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional H1, + boost::optional H2) const { // rr = x^2 + y^2; // g = (1 + k(1)*rr + k(2)*rr^2); @@ -68,40 +100,29 @@ Point2 Cal3DS2::uncalibrate(const Point2& p, const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y; const double rr = xx + yy; const double r4 = rr * rr; - const double g = 1. + k1 * rr + k2 * r4; - const double dx = 2. * k3 * xy + k4 * (rr + 2. * xx); - const double dy = 2. * k4 * xy + k3 * (rr + 2. * yy); + const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor - const double pnx = g*x + dx; - const double pny = g*y + dy; + // tangential component + const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx); + const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy); - // Inlined derivative for calibration - if (H1) { - *H1 = (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr, - fx * x * r4 + s * y * r4, fx * 2. * xy + s * (rr + 2. * yy), - fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0, - fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy)); - } - // Inlined derivative for points - if (H2) { - const double dr_dx = 2. * x; - const double dr_dy = 2. * y; - const double dg_dx = k1 * dr_dx + k2 * 2. * rr * dr_dx; - const double dg_dy = k1 * dr_dy + k2 * 2. * rr * dr_dy; + // Radial and tangential distortion applied + const double pnx = g * x + dx; + const double pny = g * y + dy; - const double dDx_dx = 2. * k3 * y + k4 * (dr_dx + 4. * x); - const double dDx_dy = 2. * k3 * x + k4 * dr_dy; - const double dDy_dx = 2. * k4 * y + k3 * dr_dx; - const double dDy_dy = 2. * k4 * x + k3 * (dr_dy + 4. * y); + Eigen::Matrix DK; + if (H1 || H2) DK << fx_, s_, 0.0, fy_; - Matrix DK = (Matrix(2, 2) << fx, s_, 0.0, fy); - Matrix DR = (Matrix(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy, - y * dg_dx + dDy_dx, g + y * dg_dy + dDy_dy); + // Derivative for calibration + if (H1) + *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); - *H2 = DK * DR; - } + // Derivative for points + if (H2) + *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); - return Point2(fx * pnx + s * pny + u0_, fy * pny + v0_); + // Regular uncalibrate after distortion + return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); } /* ************************************************************************* */ @@ -118,14 +139,14 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const { // iterate until the uncalibrate is close to the actual pixel coordinate const int maxIterations = 10; int iteration; - for ( iteration = 0; iteration < maxIterations; ++iteration ) { - if ( uncalibrate(pn).distance(pi) <= tol ) break; - const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y; + for (iteration = 0; iteration < maxIterations; ++iteration) { + if (uncalibrate(pn).distance(pi) <= tol) break; + const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y; const double rr = xx + yy; - const double g = (1+k1_*rr+k2_*rr*rr); - const double dx = 2*k3_*xy + k4_*(rr+2*xx); - const double dy = 2*k4_*xy + k3_*(rr+2*yy); - pn = (invKPi - Point2(dx,dy))/g; + const double g = (1 + k1_ * rr + k2_ * rr * rr); + const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx); + const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); + pn = (invKPi - Point2(dx, dy)) / g; } if ( iteration >= maxIterations ) @@ -136,47 +157,28 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const { /* ************************************************************************* */ Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const { - //const double fx = fx_, fy = fy_, s = s_; - const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_; - //const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y; - const double x = p.x(), y = p.y(), xx = x*x, yy = y*y; + const double x = p.x(), y = p.y(), xx = x * x, yy = y * y; const double rr = xx + yy; - const double dr_dx = 2*x; - const double dr_dy = 2*y; - const double r4 = rr*rr; - const double g = 1 + k1*rr + k2*r4; - const double dg_dx = k1*dr_dx + k2*2*rr*dr_dx; - const double dg_dy = k1*dr_dy + k2*2*rr*dr_dy; - - // Dx = 2*k3*xy + k4*(rr+2*xx); - // Dy = 2*k4*xy + k3*(rr+2*yy); - const double dDx_dx = 2*k3*y + k4*(dr_dx + 4*x); - const double dDx_dy = 2*k3*x + k4*dr_dy; - const double dDy_dx = 2*k4*y + k3*dr_dx; - const double dDy_dy = 2*k4*x + k3*(dr_dy + 4*y); - - Matrix DK = (Matrix(2, 2) << fx_, s_, 0.0, fy_); - Matrix DR = (Matrix(2, 2) << g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy, - y*dg_dx + dDy_dx, g + y*dg_dy + dDy_dy); - - return DK * DR; + const double r4 = rr * rr; + const double g = (1 + k1_ * rr + k2_ * r4); + Eigen::Matrix DK; + DK << fx_, s_, 0.0, fy_; + return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); } /* ************************************************************************* */ Matrix Cal3DS2::D2d_calibration(const Point2& p) const { - const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y; + const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y; const double rr = xx + yy; - const double r4 = rr*rr; - const double fx = fx_, fy = fy_, s = s_; - const double g = (1+k1_*rr+k2_*r4); - const double dx = 2*k3_*xy + k4_*(rr+2*xx); - const double dy = 2*k4_*xy + k3_*(rr+2*yy); - const double pnx = g*x + dx; - const double pny = g*y + dy; - - return (Matrix(2, 9) << - pnx, 0.0, pny, 1.0, 0.0, fx*x*rr + s*y*rr, fx*x*r4 + s*y*r4, fx*2*xy + s*(rr+2*yy), fx*(rr+2*xx) + s*(2*xy), - 0.0, pny, 0.0, 0.0, 1.0, fy*y*rr , fy*y*r4 , fy*(rr+2*yy) , fy*(2*xy) ); + const double r4 = rr * rr; + const double g = (1 + k1_ * rr + k2_ * r4); + const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx); + const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); + const double pnx = g * x + dx; + const double pny = g * y + dy; + Eigen::Matrix DK; + DK << fx_, s_, 0.0, fy_; + return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index ced05086b..51fe958d6 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -27,6 +27,15 @@ namespace gtsam { * @brief Calibration of a camera with radial distortion * @addtogroup geometry * \nosubgrouping + * + * Uses same distortionmodel as OpenCV, with + * http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html + * but using only k1,k2,p1, and p2 coefficients. + * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] + * rr = Pn.x^2 + Pn.y^2 + * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; + * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] + * pi = K*pn */ class GTSAM_EXPORT Cal3DS2 : public DerivedValue { @@ -34,28 +43,22 @@ protected: double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point double k1_, k2_ ; // radial 2nd-order and 4th-order - double k3_, k4_ ; // tangential distortion - - // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] - // rr = Pn.x^2 + Pn.y^2 - // \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; - // k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] - // pi = K*pn + double p1_, p2_ ; // tangential distortion public: Matrix K() const ; - Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, k3_, k4_); } + Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); } Vector vector() const ; /// @name Standard Constructors /// @{ /// Default Constructor with only unit focal length - Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {} + Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {} Cal3DS2(double fx, double fy, double s, double u0, double v0, - double k1, double k2, double k3 = 0.0, double k4 = 0.0) : - fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {} + double k1, double k2, double p1 = 0.0, double p2 = 0.0) : + fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {} /// @} /// @name Advanced Constructors @@ -92,18 +95,30 @@ public: /// image center in y inline double py() const { return v0_;} + /// First distortion coefficient + inline double k1() const { return k1_;} + + /// Second distortion coefficient + inline double k2() const { return k2_;} + + /// First tangential distortion coefficient + inline double p1() const { return p1_;} + + /// Second tangential distortion coefficient + inline double p2() const { return p2_;} + /** - * convert intrinsic coordinates xy to image coordinates uv + * convert intrinsic coordinates xy to (distorted) image coordinates uv * @param p point in intrinsic coordinates * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates - * @return point in image coordinates + * @return point in (distorted) image coordinates */ Point2 uncalibrate(const Point2& p, boost::optional Dcal = boost::none, boost::optional Dp = boost::none) const ; - /// Conver a pixel coordinate to ideal coordinate + /// Convert (distorted) image coordinates uv to intrinsic coordinates xy Point2 calibrate(const Point2& p, const double tol=1e-5) const; /// Derivative of uncalibrate wrpt intrinsic coordinates @@ -148,8 +163,8 @@ private: ar & BOOST_SERIALIZATION_NVP(v0_); ar & BOOST_SERIALIZATION_NVP(k1_); ar & BOOST_SERIALIZATION_NVP(k2_); - ar & BOOST_SERIALIZATION_NVP(k3_); - ar & BOOST_SERIALIZATION_NVP(k4_); + ar & BOOST_SERIALIZATION_NVP(p1_); + ar & BOOST_SERIALIZATION_NVP(p2_); } diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index 808cb84a4..e7b408982 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -43,7 +43,7 @@ void Cal3Unified::print(const std::string& s) const { bool Cal3Unified::equals(const Cal3Unified& K, double tol) const { if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol || fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol || - fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol || + fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol || fabs(xi_ - K.xi_) > tol) return false; return true; diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index a1d47332a..58e024c27 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -31,6 +31,14 @@ namespace gtsam { * @brief Calibration of a omni-directional camera with mirror + lens radial distortion * @addtogroup geometry * \nosubgrouping + * + * Similar to Cal3DS2, does distortion but has additional mirror parameter xi + * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] + * Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})] + * rr = Pn.x^2 + Pn.y^2 + * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; + * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] + * pi = K*pn */ class GTSAM_EXPORT Cal3Unified : public Cal3DS2 { @@ -41,13 +49,6 @@ private: double xi_; // mirror parameter - // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ] - // Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})] - // rr = Pn.x^2 + Pn.y^2 - // \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ; - // k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] - // pi = K*pn - public: //Matrix K() const ; //Eigen::Vector4d k() const { return Base::k(); } @@ -60,8 +61,8 @@ public: Cal3Unified() : Base(), xi_(0) {} Cal3Unified(double fx, double fy, double s, double u0, double v0, - double k1, double k2, double k3 = 0.0, double k4 = 0.0, double xi = 0.0) : - Base(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {} + double k1, double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) : + Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {} /// @} /// @name Advanced Constructors diff --git a/gtsam/geometry/tests/testCal3DS2.cpp b/gtsam/geometry/tests/testCal3DS2.cpp index fc457767e..eddd3a674 100644 --- a/gtsam/geometry/tests/testCal3DS2.cpp +++ b/gtsam/geometry/tests/testCal3DS2.cpp @@ -60,6 +60,8 @@ TEST( Cal3DS2, Duncalibrate1) K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); CHECK(assert_equal(numerical,computed,1e-5)); + Matrix separate = K.D2d_calibration(p); + CHECK(assert_equal(numerical,separate,1e-5)); } /* ************************************************************************* */ @@ -68,6 +70,8 @@ TEST( Cal3DS2, Duncalibrate2) Matrix computed; K.uncalibrate(p, boost::none, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); CHECK(assert_equal(numerical,computed,1e-5)); + Matrix separate = K.D2d_intrinsic(p); + CHECK(assert_equal(numerical,separate,1e-5)); } /* ************************************************************************* */ diff --git a/gtsam/inference/Ordering.h b/gtsam/inference/Ordering.h index ea2ad7eda..7b1a2bb2e 100644 --- a/gtsam/inference/Ordering.h +++ b/gtsam/inference/Ordering.h @@ -17,9 +17,11 @@ #pragma once +#include #include #include +#include #include #include #include @@ -135,6 +137,15 @@ namespace gtsam { static GTSAM_EXPORT Ordering COLAMDConstrained(const VariableIndex& variableIndex, const FastMap& groups); + /// Return a natural Ordering. Typically used by iterative solvers + template + static Ordering Natural(const FactorGraph &fg) { + FastSet src = fg.keys(); + std::vector keys(src.begin(), src.end()); + std::stable_sort(keys.begin(), keys.end()); + return Ordering(keys); + } + /// @} /// @name Testable @{ diff --git a/gtsam/linear/ConjugateGradientSolver.cpp b/gtsam/linear/ConjugateGradientSolver.cpp new file mode 100644 index 000000000..0505f6c01 --- /dev/null +++ b/gtsam/linear/ConjugateGradientSolver.cpp @@ -0,0 +1,49 @@ +/* + * ConjugateGradientSolver.cpp + * + * Created on: Jun 4, 2014 + * Author: ydjian + */ + +#include +#include +#include + +using namespace std; + +namespace gtsam { + +/*****************************************************************************/ +void ConjugateGradientParameters::print(ostream &os) const { + Base::print(os); + cout << "ConjugateGradientParameters" << endl + << "minIter: " << minIterations_ << endl + << "maxIter: " << maxIterations_ << endl + << "resetIter: " << reset_ << endl + << "eps_rel: " << epsilon_rel_ << endl + << "eps_abs: " << epsilon_abs_ << endl; +} + +/*****************************************************************************/ +std::string ConjugateGradientParameters::blasTranslator(const BLASKernel value) { + std::string s; + switch (value) { + case ConjugateGradientParameters::GTSAM: s = "GTSAM" ; break; + default: s = "UNDEFINED" ; break; + } + return s; +} + +/*****************************************************************************/ +ConjugateGradientParameters::BLASKernel ConjugateGradientParameters::blasTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "GTSAM") return ConjugateGradientParameters::GTSAM; + + /* default is SBM */ + return ConjugateGradientParameters::GTSAM; +} + + +} + + diff --git a/gtsam/linear/ConjugateGradientSolver.h b/gtsam/linear/ConjugateGradientSolver.h index d1b3b2c7e..6e8509309 100644 --- a/gtsam/linear/ConjugateGradientSolver.h +++ b/gtsam/linear/ConjugateGradientSolver.h @@ -12,14 +12,15 @@ #pragma once #include +#include namespace gtsam { /** * parameters for the conjugate gradient method */ +class GTSAM_EXPORT ConjugateGradientParameters : public IterativeOptimizationParameters { -class ConjugateGradientParameters : public IterativeOptimizationParameters { public: typedef IterativeOptimizationParameters Base; typedef boost::shared_ptr shared_ptr; @@ -30,14 +31,23 @@ public: double epsilon_rel_; ///< threshold for relative error decrease double epsilon_abs_; ///< threshold for absolute error decrease - ConjugateGradientParameters() - : minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3), epsilon_abs_(1e-3){} + /* Matrix Operation Kernel */ + enum BLASKernel { + GTSAM = 0, ///< Jacobian Factor Graph of GTSAM + } blas_kernel_ ; - ConjugateGradientParameters(size_t minIterations, size_t maxIterations, size_t reset, double epsilon_rel, double epsilon_abs) - : minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset), epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs){} + ConjugateGradientParameters() + : minIterations_(1), maxIterations_(500), reset_(501), epsilon_rel_(1e-3), + epsilon_abs_(1e-3), blas_kernel_(GTSAM) {} + + ConjugateGradientParameters(size_t minIterations, size_t maxIterations, size_t reset, + double epsilon_rel, double epsilon_abs, BLASKernel blas) + : minIterations_(minIterations), maxIterations_(maxIterations), reset_(reset), + epsilon_rel_(epsilon_rel), epsilon_abs_(epsilon_abs), blas_kernel_(blas) {} ConjugateGradientParameters(const ConjugateGradientParameters &p) - : Base(p), minIterations_(p.minIterations_), maxIterations_(p.maxIterations_), reset_(p.reset_), epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_) {} + : Base(p), minIterations_(p.minIterations_), maxIterations_(p.maxIterations_), reset_(p.reset_), + epsilon_rel_(p.epsilon_rel_), epsilon_abs_(p.epsilon_abs_), blas_kernel_(GTSAM) {} /* general interface */ inline size_t minIterations() const { return minIterations_; } @@ -61,15 +71,81 @@ public: inline void setEpsilon_rel(double value) { epsilon_rel_ = value; } inline void setEpsilon_abs(double value) { epsilon_abs_ = value; } - virtual void print() const { - Base::print(); - std::cout << "ConjugateGradientParameters" << std::endl - << "minIter: " << minIterations_ << std::endl - << "maxIter: " << maxIterations_ << std::endl - << "resetIter: " << reset_ << std::endl - << "eps_rel: " << epsilon_rel_ << std::endl - << "eps_abs: " << epsilon_abs_ << std::endl; - } + + void print() const { Base::print(); } + virtual void print(std::ostream &os) const; + + static std::string blasTranslator(const BLASKernel k) ; + static BLASKernel blasTranslator(const std::string &s) ; }; +/*********************************************************************************************/ +/* + * A template of linear preconditioned conjugate gradient method. + * System class should support residual(v, g), multiply(v,Av), leftPrecondition(v, S^{-t}v, + * rightPrecondition(v, S^{-1}v), scal(alpha,v), dot(v,v), axpy(alpha,x,y) + * Note that the residual is in the preconditioned domain. Refer to Section 9.2 of Saad's book. + */ +template +V preconditionedConjugateGradient(const S &system, const V &initial, const ConjugateGradientParameters ¶meters) { + + V estimate, residual, direction, q1, q2; + estimate = residual = direction = q1 = q2 = initial; + + system.residual(estimate, q1); /* q1 = b-Ax */ + system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */ + system.rightPrecondition(residual, direction);/* d = S^{-1} r */ + + double currentGamma = system.dot(residual, residual), prevGamma, alpha, beta; + + const size_t iMaxIterations = parameters.maxIterations(), + iMinIterations = parameters.minIterations(), + iReset = parameters.reset() ; + const double threshold = std::max(parameters.epsilon_abs(), + parameters.epsilon() * parameters.epsilon() * currentGamma); + + if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY ) + std::cout << "[PCG] epsilon = " << parameters.epsilon() + << ", max = " << parameters.maxIterations() + << ", reset = " << parameters.reset() + << ", ||r0||^2 = " << currentGamma + << ", threshold = " << threshold << std::endl; + + size_t k; + for ( k = 1 ; k <= iMaxIterations && (currentGamma > threshold || k <= iMinIterations) ; k++ ) { + + if ( k % iReset == 0 ) { + system.residual(estimate, q1); /* q1 = b-Ax */ + system.leftPrecondition(q1, residual); /* r = S^{-T} (b-Ax) */ + system.rightPrecondition(residual, direction); /* d = S^{-1} r */ + currentGamma = system.dot(residual, residual); + } + system.multiply(direction, q1); /* q1 = A d */ + alpha = currentGamma / system.dot(direction, q1); /* alpha = gamma / (d' A d) */ + system.axpy(alpha, direction, estimate); /* estimate += alpha * direction */ + system.leftPrecondition(q1, q2); /* q2 = S^{-T} * q1 */ + system.axpy(-alpha, q2, residual); /* residual -= alpha * q2 */ + prevGamma = currentGamma; + currentGamma = system.dot(residual, residual); /* gamma = |residual|^2 */ + beta = currentGamma / prevGamma; + system.rightPrecondition(residual, q1); /* q1 = S^{-1} residual */ + system.scal(beta, direction); + system.axpy(1.0, q1, direction); /* direction = q1 + beta * direction */ + + if (parameters.verbosity() >= ConjugateGradientParameters::ERROR ) + std::cout << "[PCG] k = " << k + << ", alpha = " << alpha + << ", beta = " << beta + << ", ||r||^2 = " << currentGamma + << std::endl; + } + if (parameters.verbosity() >= ConjugateGradientParameters::COMPLEXITY ) + std::cout << "[PCG] iterations = " << k + << ", ||r||^2 = " << currentGamma + << std::endl; + + return estimate; +} + + } diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index d6836783b..438a06783 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -54,6 +54,20 @@ namespace gtsam { return keys; } + /* ************************************************************************* */ + std::map GaussianFactorGraph::getKeyDimMap() const { + map spec; + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, *this ) { + for ( GaussianFactor::const_iterator it = gf->begin() ; it != gf->end() ; it++ ) { + map::iterator it2 = spec.find(*it); + if ( it2 == spec.end() ) { + spec.insert(make_pair(*it, gf->getDim(it))); + } + } + } + return spec; + } + /* ************************************************************************* */ vector GaussianFactorGraph::getkeydim() const { // First find dimensions of each variable diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 692310f26..910b25d1e 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -138,6 +138,9 @@ namespace gtsam { typedef FastSet Keys; Keys keys() const; + /* return a map of (Key, dimension) */ + std::map getKeyDimMap() const; + std::vector getkeydim() const; /** unnormalized error */ diff --git a/gtsam/linear/IterativeSolver.cpp b/gtsam/linear/IterativeSolver.cpp index 2bba3e12b..ab3ccde13 100644 --- a/gtsam/linear/IterativeSolver.cpp +++ b/gtsam/linear/IterativeSolver.cpp @@ -6,25 +6,42 @@ */ #include +#include +#include #include +#include #include +using namespace std; + namespace gtsam { -std::string IterativeOptimizationParameters::getKernel() const { return kernelTranslator(kernel_); } -std::string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); } -void IterativeOptimizationParameters::setKernel(const std::string &src) { kernel_ = kernelTranslator(src); } -void IterativeOptimizationParameters::setVerbosity(const std::string &src) { verbosity_ = verbosityTranslator(src); } +/*****************************************************************************/ +string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); } -IterativeOptimizationParameters::Kernel IterativeOptimizationParameters::kernelTranslator(const std::string &src) { - std::string s = src; boost::algorithm::to_upper(s); - if (s == "CG") return IterativeOptimizationParameters::CG; - /* default is cg */ - else return IterativeOptimizationParameters::CG; +/*****************************************************************************/ +void IterativeOptimizationParameters::setVerbosity(const string &src) { verbosity_ = verbosityTranslator(src); } + +/*****************************************************************************/ +void IterativeOptimizationParameters::print() const { + print(cout); } -IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const std::string &src) { - std::string s = src; boost::algorithm::to_upper(s); +/*****************************************************************************/ +void IterativeOptimizationParameters::print(ostream &os) const { + os << "IterativeOptimizationParameters:" << endl + << "verbosity: " << verbosityTranslator(verbosity_) << endl; +} + +/*****************************************************************************/ + ostream& operator<<(ostream &os, const IterativeOptimizationParameters &p) { + p.print(os); + return os; +} + +/*****************************************************************************/ +IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const string &src) { + string s = src; boost::algorithm::to_upper(s); if (s == "SILENT") return IterativeOptimizationParameters::SILENT; else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY; else if (s == "ERROR") return IterativeOptimizationParameters::ERROR; @@ -32,18 +49,85 @@ IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verb else return IterativeOptimizationParameters::SILENT; } -std::string IterativeOptimizationParameters::kernelTranslator(IterativeOptimizationParameters::Kernel k) { - if ( k == CG ) return "CG"; - else return "UNKNOWN"; -} - -std::string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) { +/*****************************************************************************/ + string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity) { if (verbosity == SILENT) return "SILENT"; else if (verbosity == COMPLEXITY) return "COMPLEXITY"; else if (verbosity == ERROR) return "ERROR"; else return "UNKNOWN"; } +/*****************************************************************************/ +VectorValues IterativeSolver::optimize( + const GaussianFactorGraph &gfg, + boost::optional keyInfo, + boost::optional&> lambda) +{ + return optimize( + gfg, + keyInfo ? *keyInfo : KeyInfo(gfg), + lambda ? *lambda : std::map()); +} + +/*****************************************************************************/ +VectorValues IterativeSolver::optimize ( + const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda) +{ + return optimize(gfg, keyInfo, lambda, keyInfo.x0()); +} + +/****************************************************************************/ +KeyInfo::KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering) + : ordering_(ordering) { + initialize(fg); +} + +/****************************************************************************/ +KeyInfo::KeyInfo(const GaussianFactorGraph &fg) + : ordering_(Ordering::Natural(fg)) { + initialize(fg); +} + +/****************************************************************************/ +void KeyInfo::initialize(const GaussianFactorGraph &fg){ + const map colspec = fg.getKeyDimMap(); + const size_t n = ordering_.size(); + size_t start = 0; + + for ( size_t i = 0 ; i < n ; ++i ) { + const size_t key = ordering_[i]; + const size_t dim = colspec.find(key)->second; + insert(make_pair(key, KeyInfoEntry(i, dim, start))); + start += dim ; + } + numCols_ = start; +} + +/****************************************************************************/ +vector KeyInfo::colSpec() const { + std::vector result(size(), 0); + BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) { + result[item.second.index()] = item.second.dim(); + } + return result; +} + +/****************************************************************************/ +VectorValues KeyInfo::x0() const { + VectorValues result; + BOOST_FOREACH ( const gtsam::KeyInfo::value_type &item, *this ) { + result.insert(item.first, Vector::Zero(item.second.dim())); + } + return result; +} + +/****************************************************************************/ +Vector KeyInfo::x0vector() const { + return Vector::Zero(numCols_); +} + } diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index 988293a4f..3c397c9e9 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -11,17 +11,27 @@ #pragma once +#include #include - +#include +#include +#include +#include +#include +#include #include -#include +#include namespace gtsam { // Forward declarations - class VectorValues; + class KeyInfo; + class KeyInfoEntry; class GaussianFactorGraph; + class Values; + class VectorValues; + /************************************************************************************/ /** * parameters for iterative linear solvers */ @@ -30,55 +40,100 @@ namespace gtsam { public: typedef boost::shared_ptr shared_ptr; - enum Kernel { CG = 0 } kernel_ ; ///< Iterative Method Kernel enum Verbosity { SILENT = 0, COMPLEXITY, ERROR } verbosity_; public: - IterativeOptimizationParameters(const IterativeOptimizationParameters &p) - : kernel_(p.kernel_), verbosity_(p.verbosity_) {} - - IterativeOptimizationParameters(const Kernel kernel = CG, const Verbosity verbosity = SILENT) - : kernel_(kernel), verbosity_(verbosity) {} + IterativeOptimizationParameters(Verbosity v = SILENT) + : verbosity_(v) {} virtual ~IterativeOptimizationParameters() {} - /* general interface */ - inline Kernel kernel() const { return kernel_; } + /* utility */ inline Verbosity verbosity() const { return verbosity_; } - - /* matlab interface */ - std::string getKernel() const ; std::string getVerbosity() const; - void setKernel(const std::string &s) ; void setVerbosity(const std::string &s) ; - virtual void print() const { - std::cout << "IterativeOptimizationParameters" << std::endl - << "kernel: " << kernelTranslator(kernel_) << std::endl - << "verbosity: " << verbosityTranslator(verbosity_) << std::endl; - } + /* matlab interface */ + void print() const ; + + /* virtual print function */ + virtual void print(std::ostream &os) const ; + + /* for serialization */ + friend std::ostream& operator<<(std::ostream &os, const IterativeOptimizationParameters &p); - static Kernel kernelTranslator(const std::string &s); static Verbosity verbosityTranslator(const std::string &s); - static std::string kernelTranslator(Kernel k); static std::string verbosityTranslator(Verbosity v); }; + /************************************************************************************/ class GTSAM_EXPORT IterativeSolver { public: typedef boost::shared_ptr shared_ptr; IterativeSolver() {} virtual ~IterativeSolver() {} - /* interface to the nonlinear optimizer */ - virtual VectorValues optimize () = 0; + /* interface to the nonlinear optimizer, without metadata, damping and initial estimate */ + VectorValues optimize ( + const GaussianFactorGraph &gfg, + boost::optional = boost::none, + boost::optional&> lambda = boost::none + ); - /* interface to the nonlinear optimizer */ - virtual VectorValues optimize (const VectorValues &initial) = 0; + /* interface to the nonlinear optimizer, without initial estimate */ + VectorValues optimize ( + const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda + ); + + /* interface to the nonlinear optimizer that the subclasses have to implement */ + virtual VectorValues optimize ( + const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda, + const VectorValues &initial + ) = 0; - /* update interface to the nonlinear optimizer */ - virtual void replaceFactors(const boost::shared_ptr &factorGraph, const double lambda) {} }; + /************************************************************************************/ + /* Handy data structure for iterative solvers + * key to (index, dimension, colstart) */ + class GTSAM_EXPORT KeyInfoEntry : public boost::tuple { + public: + typedef boost::tuple Base; + KeyInfoEntry(){} + KeyInfoEntry(size_t idx, size_t d, Key start) : Base(idx, d, start) {} + const size_t index() const { return this->get<0>(); } + const size_t dim() const { return this->get<1>(); } + const size_t colstart() const { return this->get<2>(); } + }; + + /************************************************************************************/ + class GTSAM_EXPORT KeyInfo : public std::map { + public: + typedef std::map Base; + KeyInfo() : numCols_(0) {} + KeyInfo(const GaussianFactorGraph &fg); + KeyInfo(const GaussianFactorGraph &fg, const Ordering &ordering); + + std::vector colSpec() const ; + VectorValues x0() const; + Vector x0vector() const; + + inline size_t numCols() const { return numCols_; } + inline const Ordering & ordering() const { return ordering_; } + + protected: + + void initialize(const GaussianFactorGraph &fg); + + Ordering ordering_; + size_t numCols_; + + }; + + } diff --git a/gtsam/linear/PCGSolver.cpp b/gtsam/linear/PCGSolver.cpp new file mode 100644 index 000000000..27eb57b44 --- /dev/null +++ b/gtsam/linear/PCGSolver.cpp @@ -0,0 +1,201 @@ +/* + * PCGSolver.cpp + * + * Created on: Feb 14, 2012 + * Author: ydjian + */ + +#include +//#include +//#include +//#include +//#include +#include +#include +//#include +//#include +//#include +//#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +/*****************************************************************************/ +void PCGSolverParameters::print(ostream &os) const { + Base::print(os); + os << "PCGSolverParameters:" << endl; + preconditioner_->print(os); +} + +/*****************************************************************************/ +PCGSolver::PCGSolver(const PCGSolverParameters &p) { + preconditioner_ = createPreconditioner(p.preconditioner_); +} + +/*****************************************************************************/ +VectorValues PCGSolver::optimize ( + const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda, + const VectorValues &initial) +{ + /* build preconditioner */ + preconditioner_->build(gfg, keyInfo, lambda); + + /* apply pcg */ + const Vector sol = preconditionedConjugateGradient( + GaussianFactorGraphSystem(gfg, *preconditioner_, keyInfo, lambda), + initial.vector(keyInfo.ordering()), parameters_); + + return buildVectorValues(sol, keyInfo); +} + +/*****************************************************************************/ +GaussianFactorGraphSystem::GaussianFactorGraphSystem( + const GaussianFactorGraph &gfg, + const Preconditioner &preconditioner, + const KeyInfo &keyInfo, + const std::map &lambda) + : gfg_(gfg), preconditioner_(preconditioner), keyInfo_(keyInfo), lambda_(lambda) {} + +/*****************************************************************************/ +void GaussianFactorGraphSystem::residual(const Vector &x, Vector &r) const { + /* implement b-Ax, assume x and r are pre-allocated */ + + /* reset r to b */ + getb(r); + + /* substract A*x */ + Vector Ax = Vector::Zero(r.rows(), 1); + multiply(x, Ax); + r -= Ax ; +} + +/*****************************************************************************/ +void GaussianFactorGraphSystem::multiply(const Vector &x, Vector& Ax) const { + /* implement Ax, assume x and Ax are pre-allocated */ + + /* reset y */ + Ax.setZero(); + + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { + if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { + /* accumulate At A x*/ + for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { + const Matrix Ai = jf->getA(it); + /* this map lookup should be replaced */ + const KeyInfoEntry &entry = keyInfo_.find(*it)->second; + Ax.segment(entry.colstart(), entry.dim()) + += Ai.transpose() * (Ai * x.segment(entry.colstart(), entry.dim())); + } + } + else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { + /* accumulate H x */ + + /* use buffer to avoid excessive table lookups */ + const size_t sz = hf->size(); + vector y; + y.reserve(sz); + for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) { + /* initialize y to zeros */ + y.push_back(zero(hf->getDim(it))); + } + + for (HessianFactor::const_iterator j = hf->begin(); j != hf->end(); j++ ) { + /* retrieve the key mapping */ + const KeyInfoEntry &entry = keyInfo_.find(*j)->second; + // xj is the input vector + const Vector xj = x.segment(entry.colstart(), entry.dim()); + size_t idx = 0; + for (HessianFactor::const_iterator i = hf->begin(); i != hf->end(); i++, idx++ ) { + if ( i == j ) y[idx] += hf->info(j, j).selfadjointView() * xj; + else y[idx] += hf->info(i, j).knownOffDiagonal() * xj; + } + } + + /* accumulate to r */ + for(DenseIndex i = 0; i < (DenseIndex) sz; ++i) { + /* retrieve the key mapping */ + const KeyInfoEntry &entry = keyInfo_.find(hf->keys()[i])->second; + Ax.segment(entry.colstart(), entry.dim()) += y[i]; + } + } + else { + throw invalid_argument("GaussianFactorGraphSystem::multiply gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); + } + } +} + +/*****************************************************************************/ +void GaussianFactorGraphSystem::getb(Vector &b) const { + /* compute rhs, assume b pre-allocated */ + + /* reset */ + b.setZero(); + + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg_ ) { + if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { + const Vector rhs = jf->getb(); + /* accumulate At rhs */ + for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { + /* this map lookup should be replaced */ + const KeyInfoEntry &entry = keyInfo_.find(*it)->second; + b.segment(entry.colstart(), entry.dim()) += jf->getA(it).transpose() * rhs ; + } + } + else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { + /* accumulate g */ + for (HessianFactor::const_iterator it = hf->begin(); it != hf->end(); it++) { + const KeyInfoEntry &entry = keyInfo_.find(*it)->second; + b.segment(entry.colstart(), entry.dim()) += hf->linearTerm(it); + } + } + else { + throw invalid_argument("GaussianFactorGraphSystem::getb gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); + } + } +} + +/**********************************************************************************/ +void GaussianFactorGraphSystem::leftPrecondition(const Vector &x, Vector &y) const +{ preconditioner_.transposeSolve(x, y); } + +/**********************************************************************************/ +void GaussianFactorGraphSystem::rightPrecondition(const Vector &x, Vector &y) const +{ preconditioner_.solve(x, y); } + +/**********************************************************************************/ +VectorValues buildVectorValues(const Vector &v, + const Ordering &ordering, + const map & dimensions) { + VectorValues result; + + DenseIndex offset = 0; + for ( size_t i = 0 ; i < ordering.size() ; ++i ) { + const Key &key = ordering[i]; + map::const_iterator it = dimensions.find(key); + if ( it == dimensions.end() ) { + throw invalid_argument("buildVectorValues: inconsistent ordering and dimensions"); + } + const size_t dim = it->second; + result.insert(key, v.segment(offset, dim)); + offset += dim; + } + + return result; +} + +/**********************************************************************************/ +VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo) { + VectorValues result; + BOOST_FOREACH ( const KeyInfo::value_type &item, keyInfo ) { + result.insert(item.first, v.segment(item.second.colstart(), item.second.dim())); + } + return result; +} + +} diff --git a/gtsam/linear/PCGSolver.h b/gtsam/linear/PCGSolver.h new file mode 100644 index 000000000..0201000ad --- /dev/null +++ b/gtsam/linear/PCGSolver.h @@ -0,0 +1,109 @@ +/* + * PCGSolver.h + * + * Created on: Jan 31, 2012 + * Author: Yong-Dian Jian + */ + +#pragma once + +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace gtsam { + +class GaussianFactorGraph; +class KeyInfo; +class Preconditioner; +struct PreconditionerParameters; + +/*****************************************************************************/ +struct GTSAM_EXPORT PCGSolverParameters: public ConjugateGradientParameters { +public: + typedef ConjugateGradientParameters Base; + typedef boost::shared_ptr shared_ptr; + + PCGSolverParameters() {} + + virtual void print(std::ostream &os) const; + + /* interface to preconditioner parameters */ + inline const PreconditionerParameters& preconditioner() const { + return *preconditioner_; + } + + boost::shared_ptr preconditioner_; +}; + +/*****************************************************************************/ +/* A virtual base class for the preconditioned conjugate gradient solver */ +class GTSAM_EXPORT PCGSolver: public IterativeSolver { +public: + typedef IterativeSolver Base; + typedef boost::shared_ptr shared_ptr; + +protected: + + PCGSolverParameters parameters_; + boost::shared_ptr preconditioner_; + +public: + /* Interface to initialize a solver without a problem */ + PCGSolver(const PCGSolverParameters &p); + virtual ~PCGSolver() {} + + using IterativeSolver::optimize; + + virtual VectorValues optimize(const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, const std::map &lambda, + const VectorValues &initial); + +}; + +/*****************************************************************************/ +class GTSAM_EXPORT GaussianFactorGraphSystem { +public: + + GaussianFactorGraphSystem(const GaussianFactorGraph &gfg, + const Preconditioner &preconditioner, const KeyInfo &info, + const std::map &lambda); + + const GaussianFactorGraph &gfg_; + const Preconditioner &preconditioner_; + const KeyInfo &keyInfo_; + const std::map &lambda_; + + void residual(const Vector &x, Vector &r) const; + void multiply(const Vector &x, Vector& y) const; + void leftPrecondition(const Vector &x, Vector &y) const; + void rightPrecondition(const Vector &x, Vector &y) const; + inline void scal(const double alpha, Vector &x) const { + x *= alpha; + } + inline double dot(const Vector &x, const Vector &y) const { + return x.dot(y); + } + inline void axpy(const double alpha, const Vector &x, Vector &y) const { + y += alpha * x; + } + + void getb(Vector &b) const; +}; + +/* utility functions */ +/**********************************************************************************/ +VectorValues buildVectorValues(const Vector &v, const Ordering &ordering, + const std::map & dimensions); + +/**********************************************************************************/ +VectorValues buildVectorValues(const Vector &v, const KeyInfo &keyInfo); + +} + diff --git a/gtsam/linear/Preconditioner.cpp b/gtsam/linear/Preconditioner.cpp new file mode 100644 index 000000000..c180f1160 --- /dev/null +++ b/gtsam/linear/Preconditioner.cpp @@ -0,0 +1,214 @@ +/* + * Preconditioner.cpp + * + * Created on: Jun 2, 2014 + * Author: ydjian + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace std; + +namespace gtsam { + +/*****************************************************************************/ +void PreconditionerParameters::print() const { + print(cout); +} + +/***************************************************************************************/ +void PreconditionerParameters::print(ostream &os) const { + os << "PreconditionerParameters" << endl + << "kernel: " << kernelTranslator(kernel_) << endl + << "verbosity: " << verbosityTranslator(verbosity_) << endl; +} + +/*****************************************************************************/ + ostream& operator<<(ostream &os, const PreconditionerParameters &p) { + p.print(os); + return os; +} + +/***************************************************************************************/ +PreconditionerParameters::Kernel PreconditionerParameters::kernelTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "GTSAM") return PreconditionerParameters::GTSAM; + else if (s == "CHOLMOD") return PreconditionerParameters::CHOLMOD; + /* default is cholmod */ + else return PreconditionerParameters::CHOLMOD; +} + +/***************************************************************************************/ +PreconditionerParameters::Verbosity PreconditionerParameters::verbosityTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "SILENT") return PreconditionerParameters::SILENT; + else if (s == "COMPLEXITY") return PreconditionerParameters::COMPLEXITY; + else if (s == "ERROR") return PreconditionerParameters::ERROR; + /* default is default */ + else return PreconditionerParameters::SILENT; +} + +/***************************************************************************************/ +std::string PreconditionerParameters::kernelTranslator(PreconditionerParameters::Kernel k) { + if ( k == GTSAM ) return "GTSAM"; + if ( k == CHOLMOD ) return "CHOLMOD"; + else return "UNKNOWN"; +} + +/***************************************************************************************/ +std::string PreconditionerParameters::verbosityTranslator(PreconditionerParameters::Verbosity verbosity) { + if (verbosity == SILENT) return "SILENT"; + else if (verbosity == COMPLEXITY) return "COMPLEXITY"; + else if (verbosity == ERROR) return "ERROR"; + else return "UNKNOWN"; +} + +/***************************************************************************************/ +BlockJacobiPreconditioner::BlockJacobiPreconditioner() + : Base(), buffer_(0), bufferSize_(0), nnz_(0) {} + +/***************************************************************************************/ +BlockJacobiPreconditioner::~BlockJacobiPreconditioner() { clean(); } + +/***************************************************************************************/ +void BlockJacobiPreconditioner::solve(const Vector& y, Vector &x) const { + + const size_t n = dims_.size(); + double *ptr = buffer_, *dst = x.data(); + + std::copy(y.data(), y.data() + y.rows(), x.data()); + + for ( size_t i = 0 ; i < n ; ++i ) { + const size_t d = dims_[i]; + const size_t sz = d*d; + + const Eigen::Map R(ptr, d, d); + Eigen::Map b(dst, d, 1); + R.triangularView().solveInPlace(b); + + dst += d; + ptr += sz; + } +} + +/***************************************************************************************/ +void BlockJacobiPreconditioner::transposeSolve(const Vector& y, Vector& x) const { + const size_t n = dims_.size(); + double *ptr = buffer_, *dst = x.data(); + + std::copy(y.data(), y.data() + y.rows(), x.data()); + + for ( size_t i = 0 ; i < n ; ++i ) { + const size_t d = dims_[i]; + const size_t sz = d*d; + + const Eigen::Map R(ptr, d, d); + Eigen::Map b(dst, d, 1); + R.transpose().triangularView().solveInPlace(b); + + dst += d; + ptr += sz; + } +} + +/***************************************************************************************/ +void BlockJacobiPreconditioner::build( + const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda) +{ + const size_t n = keyInfo.size(); + dims_ = keyInfo.colSpec(); + + /* prepare the buffer of block diagonals */ + std::vector blocks; blocks.reserve(n); + + /* allocate memory for the factorization of block diagonals */ + size_t nnz = 0; + for ( size_t i = 0 ; i < n ; ++i ) { + const size_t dim = dims_[i]; + blocks.push_back(Matrix::Zero(dim, dim)); + // nnz += (((dim)*(dim+1)) >> 1); // d*(d+1) / 2 ; + nnz += dim*dim; + } + + /* compute the block diagonal by scanning over the factors */ + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { + for ( JacobianFactor::const_iterator it = jf->begin() ; it != jf->end() ; ++it ) { + const KeyInfoEntry &entry = keyInfo.find(*it)->second; + const Matrix &Ai = jf->getA(it); + blocks[entry.index()] += (Ai.transpose() * Ai); + } + } + else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { + for ( HessianFactor::const_iterator it = hf->begin() ; it != hf->end() ; ++it ) { + const KeyInfoEntry &entry = keyInfo.find(*it)->second; + const Matrix &Hii = hf->info(it, it).selfadjointView(); + blocks[entry.index()] += Hii; + } + } + else { + throw invalid_argument("BlockJacobiPreconditioner::build gfg contains a factor that is neither a JacobianFactor nor a HessianFactor."); + } + } + + /* if necessary, allocating the memory for cacheing the factorization results */ + if ( nnz > bufferSize_ ) { + clean(); + buffer_ = new double [nnz]; + bufferSize_ = nnz; + } + nnz_ = nnz; + + /* factorizing the blocks respectively */ + double *ptr = buffer_; + for ( size_t i = 0 ; i < n ; ++i ) { + /* use eigen to decompose Di */ + const Matrix R = blocks[i].llt().matrixL().transpose(); + + /* store the data in the buffer */ + size_t sz = dims_[i]*dims_[i] ; + std::copy(R.data(), R.data() + sz, ptr); + + /* advance the pointer */ + ptr += sz; + } +} + +/*****************************************************************************/ +void BlockJacobiPreconditioner::clean() { + if ( buffer_ ) { + delete [] buffer_; + buffer_ = 0; + bufferSize_ = 0; + nnz_ = 0; + } +} + +/***************************************************************************************/ +boost::shared_ptr createPreconditioner(const boost::shared_ptr parameters) { + + if ( DummyPreconditionerParameters::shared_ptr dummy = boost::dynamic_pointer_cast(parameters) ) { + return boost::make_shared(); + } + else if ( BlockJacobiPreconditionerParameters::shared_ptr blockJacobi = boost::dynamic_pointer_cast(parameters) ) { + return boost::make_shared(); + } + else if ( SubgraphPreconditionerParameters::shared_ptr subgraph = boost::dynamic_pointer_cast(parameters) ) { + return boost::make_shared(*subgraph); + } + + throw invalid_argument("createPreconditioner: unexpected preconditioner parameter type"); +} + +} + + diff --git a/gtsam/linear/Preconditioner.h b/gtsam/linear/Preconditioner.h new file mode 100644 index 000000000..10ceb78a9 --- /dev/null +++ b/gtsam/linear/Preconditioner.h @@ -0,0 +1,170 @@ +/* + * Preconditioner.h + * + * Created on: Jun 2, 2014 + * Author: ydjian + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace gtsam { + +class GaussianFactorGraph; +class KeyInfo; +class VectorValues; + +/* parameters for the preconditioner */ +struct GTSAM_EXPORT PreconditionerParameters { + + typedef boost::shared_ptr shared_ptr; + + enum Kernel { /* Preconditioner Kernel */ + GTSAM = 0, + CHOLMOD /* experimental */ + } kernel_ ; + + enum Verbosity { + SILENT = 0, + COMPLEXITY = 1, + ERROR = 2 + } verbosity_ ; + + PreconditionerParameters(): kernel_(GTSAM), verbosity_(SILENT) {} + PreconditionerParameters(const PreconditionerParameters &p) : kernel_(p.kernel_), verbosity_(p.verbosity_) {} + virtual ~PreconditionerParameters() {} + + /* general interface */ + inline Kernel kernel() const { return kernel_; } + inline Verbosity verbosity() const { return verbosity_; } + + void print() const ; + + virtual void print(std::ostream &os) const ; + + static Kernel kernelTranslator(const std::string &s); + static Verbosity verbosityTranslator(const std::string &s); + static std::string kernelTranslator(Kernel k); + static std::string verbosityTranslator(Verbosity v); + + /* for serialization */ + friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p); + }; + +/* PCG aims to solve the problem: A x = b by reparametrizing it as + * S^t A S y = S^t b or M A x = M b, where A \approx S S, or A \approx M + * The goal of this class is to provide a general interface to all preconditioners */ +class GTSAM_EXPORT Preconditioner { +public: + typedef boost::shared_ptr shared_ptr; + typedef std::vector Dimensions; + + /* Generic Constructor and Destructor */ + Preconditioner() {} + virtual ~Preconditioner() {} + + /* Computation Interfaces */ + + /* implement x = S^{-1} y */ + virtual void solve(const Vector& y, Vector &x) const = 0; +// virtual void solve(const VectorValues& y, VectorValues &x) const = 0; + + /* implement x = S^{-T} y */ + virtual void transposeSolve(const Vector& y, Vector& x) const = 0; +// virtual void transposeSolve(const VectorValues& y, VectorValues &x) const = 0; + +// /* implement x = S^{-1} S^{-T} y */ +// virtual void fullSolve(const Vector& y, Vector &x) const = 0; +// virtual void fullSolve(const VectorValues& y, VectorValues &x) const = 0; + + /* build/factorize the preconditioner */ + virtual void build( + const GaussianFactorGraph &gfg, + const KeyInfo &info, + const std::map &lambda + ) = 0; +}; + +/*******************************************************************************************/ +struct GTSAM_EXPORT DummyPreconditionerParameters : public PreconditionerParameters { + typedef PreconditionerParameters Base; + typedef boost::shared_ptr shared_ptr; + DummyPreconditionerParameters() : Base() {} + virtual ~DummyPreconditionerParameters() {} +}; + +/*******************************************************************************************/ +class GTSAM_EXPORT DummyPreconditioner : public Preconditioner { +public: + typedef Preconditioner Base; + typedef boost::shared_ptr shared_ptr; + +public: + + DummyPreconditioner() : Base() {} + virtual ~DummyPreconditioner() {} + + /* Computation Interfaces for raw vector */ + virtual void solve(const Vector& y, Vector &x) const { x = y; } +// virtual void solve(const VectorValues& y, VectorValues& x) const { x = y; } + + virtual void transposeSolve(const Vector& y, Vector& x) const { x = y; } +// virtual void transposeSolve(const VectorValues& y, VectorValues& x) const { x = y; } + +// virtual void fullSolve(const Vector& y, Vector &x) const { x = y; } +// virtual void fullSolve(const VectorValues& y, VectorValues& x) const { x = y; } + + virtual void build( + const GaussianFactorGraph &gfg, + const KeyInfo &info, + const std::map &lambda + ) {} +}; + +/*******************************************************************************************/ +struct GTSAM_EXPORT BlockJacobiPreconditionerParameters : public PreconditionerParameters { + typedef PreconditionerParameters Base; + BlockJacobiPreconditionerParameters() : Base() {} + virtual ~BlockJacobiPreconditionerParameters() {} +}; + +/*******************************************************************************************/ +class GTSAM_EXPORT BlockJacobiPreconditioner : public Preconditioner { +public: + typedef Preconditioner Base; + BlockJacobiPreconditioner() ; + virtual ~BlockJacobiPreconditioner() ; + + /* Computation Interfaces for raw vector */ + virtual void solve(const Vector& y, Vector &x) const; + virtual void transposeSolve(const Vector& y, Vector& x) const ; +// virtual void fullSolve(const Vector& y, Vector &x) const ; + + virtual void build( + const GaussianFactorGraph &gfg, + const KeyInfo &info, + const std::map &lambda + ) ; + +protected: + + void clean() ; + + std::vector dims_; + double *buffer_; + size_t bufferSize_; + size_t nnz_; +}; + +/*********************************************************************************************/ +/* factory method to create preconditioners */ +boost::shared_ptr createPreconditioner(const boost::shared_ptr parameters); + +} + + diff --git a/gtsam/linear/SubgraphPreconditioner.cpp b/gtsam/linear/SubgraphPreconditioner.cpp index 44cb8c146..f5ee4ddfc 100644 --- a/gtsam/linear/SubgraphPreconditioner.cpp +++ b/gtsam/linear/SubgraphPreconditioner.cpp @@ -15,126 +15,646 @@ * @author: Frank Dellaert */ +#include #include #include #include #include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include using namespace std; namespace gtsam { - /* ************************************************************************* */ - static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { - GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); - BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { - JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf); - if( !jf ) { - jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) +/* ************************************************************************* */ +static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) { + GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); + BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) { + JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf); + if( !jf ) { + jf = boost::make_shared(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky) + } + result->push_back(jf); + } + return result; +} + +/*****************************************************************************/ +static std::vector iidSampler(const vector &weight, const size_t n) { + + /* compute the sum of the weights */ + const double sum = std::accumulate(weight.begin(), weight.end(), 0.0); + + /* make a normalized and accumulated version of the weight vector */ + const size_t m = weight.size(); + vector w; w.reserve(m); + for ( size_t i = 0 ; i < m ; ++i ) { + w.push_back(weight[i]/sum); + } + + vector acc(m); + std::partial_sum(w.begin(),w.end(),acc.begin()); + + /* iid sample n times */ + vector result; result.reserve(n); + const double denominator = (double)RAND_MAX; + for ( size_t i = 0 ; i < n ; ++i ) { + const double value = rand() / denominator; + /* binary search the interval containing "value" */ + vector::iterator it = std::lower_bound(acc.begin(), acc.end(), value); + size_t idx = it - acc.begin(); + result.push_back(idx); + } + return result; +} + +/*****************************************************************************/ +vector uniqueSampler(const vector &weight, const size_t n) { + + const size_t m = weight.size(); + if ( n > m ) throw std::invalid_argument("uniqueSampler: invalid input size"); + + vector result; + + size_t count = 0; + std::vector touched(m, false); + while ( count < n ) { + std::vector localIndices; localIndices.reserve(n-count); + std::vector localWeights; localWeights.reserve(n-count); + + /* collect data */ + for ( size_t i = 0 ; i < m ; ++i ) { + if ( !touched[i] ) { + localIndices.push_back(i); + localWeights.push_back(weight[i]); } - result->push_back(jf); - } - return result; - } - - /* ************************************************************************* */ - SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2, - const sharedBayesNet& Rc1, const sharedValues& xbar) : - Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))) { - } - - /* ************************************************************************* */ - // x = xbar + inv(R1)*y - VectorValues SubgraphPreconditioner::x(const VectorValues& y) const { - return *xbar_ + Rc1_->backSubstitute(y); - } - - /* ************************************************************************* */ - double SubgraphPreconditioner::error(const VectorValues& y) const { - Errors e(y); - VectorValues x = this->x(y); - Errors e2 = Ab2()->gaussianErrors(x); - return 0.5 * (dot(e, e) + dot(e2,e2)); - } - - /* ************************************************************************* */ - // gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), - VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const { - VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */ - Errors e = (*Ab2()*x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */ - VectorValues v = VectorValues::Zero(x); - Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ - return y + Rc1()->backSubstituteTranspose(v); - } - - /* ************************************************************************* */ - // Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] - Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { - - Errors e(y); - VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */ - Errors e2 = *Ab2() * x; /* A2*x */ - e.splice(e.end(), e2); - return e; - } - - /* ************************************************************************* */ - // In-place version that overwrites e - void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const { - - Errors::iterator ei = e.begin(); - for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) { - *ei = y[i]; } - // Add A2 contribution - VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y - Ab2()->multiplyInPlace(x, ei); // use iterator version - } - - /* ************************************************************************* */ - // Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2 - VectorValues SubgraphPreconditioner::operator^(const Errors& e) const { - - Errors::const_iterator it = e.begin(); - VectorValues y = zero(); - for ( Key i = 0 ; i < y.size() ; ++i, ++it ) - y[i] = *it ; - transposeMultiplyAdd2(1.0,it,e.end(),y); - return y; - } - - /* ************************************************************************* */ - // y += alpha*A'*e - void SubgraphPreconditioner::transposeMultiplyAdd - (double alpha, const Errors& e, VectorValues& y) const { - - Errors::const_iterator it = e.begin(); - for ( Key i = 0 ; i < y.size() ; ++i, ++it ) { - const Vector& ei = *it; - axpy(alpha, ei, y[i]); + /* sampling and cache results */ + vector samples = iidSampler(localWeights, n-count); + BOOST_FOREACH ( const size_t &id, samples ) { + if ( touched[id] == false ) { + touched[id] = true ; + result.push_back(id); + if ( ++count >= n ) break; + } } - transposeMultiplyAdd2(alpha, it, e.end(), y); + } + return result; +} + +/****************************************************************************/ +Subgraph::Subgraph(const std::vector &indices) { + edges_.reserve(indices.size()); + BOOST_FOREACH ( const size_t &idx, indices ) { + edges_.push_back(SubgraphEdge(idx, 1.0)); + } +} + +/****************************************************************************/ +std::vector Subgraph::edgeIndices() const { + std::vector eid; eid.reserve(size()); + BOOST_FOREACH ( const SubgraphEdge &edge, edges_ ) { + eid.push_back(edge.index_); + } + return eid; +} + +/****************************************************************************/ +void Subgraph::save(const std::string &fn) const { + std::ofstream os(fn.c_str()); + boost::archive::text_oarchive oa(os); + oa << *this; + os.close(); +} + +/****************************************************************************/ +Subgraph::shared_ptr Subgraph::load(const std::string &fn) { + std::ifstream is(fn.c_str()); + boost::archive::text_iarchive ia(is); + Subgraph::shared_ptr subgraph(new Subgraph()); + ia >> *subgraph; + is.close(); + return subgraph; +} + +/****************************************************************************/ +std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge) { + if ( edge.weight() != 1.0 ) + os << edge.index() << "(" << std::setprecision(2) << edge.weight() << ")"; + else + os << edge.index() ; + return os; +} + +/****************************************************************************/ +std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph) { + os << "Subgraph" << endl; + BOOST_FOREACH ( const SubgraphEdge &e, subgraph.edges() ) { + os << e << ", " ; + } + return os; +} + +/*****************************************************************************/ +void SubgraphBuilderParameters::print() const { + print(cout); +} + +/***************************************************************************************/ +void SubgraphBuilderParameters::print(ostream &os) const { + os << "SubgraphBuilderParameters" << endl + << "skeleton: " << skeletonTranslator(skeleton_) << endl + << "skeleton weight: " << skeletonWeightTranslator(skeletonWeight_) << endl + << "augmentation weight: " << augmentationWeightTranslator(augmentationWeight_) << endl + ; +} + +/*****************************************************************************/ +ostream& operator<<(ostream &os, const SubgraphBuilderParameters &p) { + p.print(os); + return os; +} + +/*****************************************************************************/ +SubgraphBuilderParameters::Skeleton SubgraphBuilderParameters::skeletonTranslator(const std::string &src){ + std::string s = src; boost::algorithm::to_upper(s); + if (s == "NATURALCHAIN") return NATURALCHAIN; + else if (s == "BFS") return BFS; + else if (s == "KRUSKAL") return KRUSKAL; + throw invalid_argument("SubgraphBuilderParameters::skeletonTranslator undefined string " + s); + return KRUSKAL; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::skeletonTranslator(Skeleton w) { + if ( w == NATURALCHAIN )return "NATURALCHAIN"; + else if ( w == BFS ) return "BFS"; + else if ( w == KRUSKAL )return "KRUSKAL"; + else return "UNKNOWN"; +} + +/****************************************************************/ +SubgraphBuilderParameters::SkeletonWeight SubgraphBuilderParameters::skeletonWeightTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "EQUAL") return EQUAL; + else if (s == "RHS") return RHS_2NORM; + else if (s == "LHS") return LHS_FNORM; + else if (s == "RANDOM") return RANDOM; + throw invalid_argument("SubgraphBuilderParameters::skeletonWeightTranslator undefined string " + s); + return EQUAL; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::skeletonWeightTranslator(SkeletonWeight w) { + if ( w == EQUAL ) return "EQUAL"; + else if ( w == RHS_2NORM ) return "RHS"; + else if ( w == LHS_FNORM ) return "LHS"; + else if ( w == RANDOM ) return "RANDOM"; + else return "UNKNOWN"; +} + +/****************************************************************/ +SubgraphBuilderParameters::AugmentationWeight SubgraphBuilderParameters::augmentationWeightTranslator(const std::string &src) { + std::string s = src; boost::algorithm::to_upper(s); + if (s == "SKELETON") return SKELETON; +// else if (s == "STRETCH") return STRETCH; +// else if (s == "GENERALIZED_STRETCH") return GENERALIZED_STRETCH; + throw invalid_argument("SubgraphBuilder::Parameters::augmentationWeightTranslator undefined string " + s); + return SKELETON; +} + +/****************************************************************/ +std::string SubgraphBuilderParameters::augmentationWeightTranslator(AugmentationWeight w) { + if ( w == SKELETON ) return "SKELETON"; +// else if ( w == STRETCH ) return "STRETCH"; +// else if ( w == GENERALIZED_STRETCH ) return "GENERALIZED_STRETCH"; + else return "UNKNOWN"; +} + +/****************************************************************/ +std::vector SubgraphBuilder::buildTree(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const { + const SubgraphBuilderParameters &p = parameters_; + switch (p.skeleton_) { + case SubgraphBuilderParameters::NATURALCHAIN: + return natural_chain(gfg); + break; + case SubgraphBuilderParameters::BFS: + return bfs(gfg); + break; + case SubgraphBuilderParameters::KRUSKAL: + return kruskal(gfg, ordering, w); + break; + default: + cerr << "SubgraphBuilder::buildTree undefined skeleton type" << endl; + break; + } + return vector(); +} + +/****************************************************************/ +std::vector SubgraphBuilder::unary(const GaussianFactorGraph &gfg) const { + std::vector result ; + size_t idx = 0; + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + if ( gf->size() == 1 ) { + result.push_back(idx); + } + idx++; + } + return result; +} + +/****************************************************************/ +std::vector SubgraphBuilder::natural_chain(const GaussianFactorGraph &gfg) const { + std::vector result ; + size_t idx = 0; + BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) { + if ( gf->size() == 2 ) { + const Key k0 = gf->keys()[0], k1 = gf->keys()[1]; + if ( (k1-k0) == 1 || (k0-k1) == 1 ) + result.push_back(idx); + } + idx++; + } + return result; +} + +/****************************************************************/ +std::vector SubgraphBuilder::bfs(const GaussianFactorGraph &gfg) const { + const VariableIndex variableIndex(gfg); + /* start from the first key of the first factor */ + Key seed = gfg[0]->keys()[0]; + + const size_t n = variableIndex.size(); + + /* each vertex has self as the predecessor */ + std::vector result; + result.reserve(n-1); + + /* Initialize */ + std::queue q; + q.push(seed); + + std::set flags; + flags.insert(seed); + + /* traversal */ + while ( !q.empty() ) { + const size_t head = q.front(); q.pop(); + BOOST_FOREACH ( const size_t id, variableIndex[head] ) { + const GaussianFactor &gf = *gfg[id]; + BOOST_FOREACH ( const size_t key, gf.keys() ) { + if ( flags.count(key) == 0 ) { + q.push(key); + flags.insert(key); + result.push_back(id); + } + } + } + } + return result; +} + +/****************************************************************/ +std::vector SubgraphBuilder::kruskal(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const { + const VariableIndex variableIndex(gfg); + const size_t n = variableIndex.size(); + const vector idx = sort_idx(w) ; + + /* initialize buffer */ + vector result; + result.reserve(n-1); + + // container for acsendingly sorted edges + DSFVector D(n) ; + + size_t count = 0 ; double sum = 0.0 ; + BOOST_FOREACH (const size_t id, idx) { + const GaussianFactor &gf = *gfg[id]; + if ( gf.keys().size() != 2 ) continue; + const size_t u = ordering.find(gf.keys()[0])->second, + u_root = D.find(u), + v = ordering.find(gf.keys()[1])->second, + v_root = D.find(v) ; + if ( u_root != v_root ) { + D.merge(u_root, v_root) ; + result.push_back(id) ; + sum += w[id] ; + if ( ++count == n-1 ) break ; + } + } + return result; +} + +/****************************************************************/ +std::vector SubgraphBuilder::sample(const std::vector &weights, const size_t t) const { + return uniqueSampler(weights, t); +} + +/****************************************************************/ +Subgraph::shared_ptr SubgraphBuilder::operator() (const GaussianFactorGraph &gfg) const { + + const SubgraphBuilderParameters &p = parameters_; + const Ordering inverse_ordering = Ordering::Natural(gfg); + const FastMap forward_ordering = inverse_ordering.invert(); + const size_t n = inverse_ordering.size(), t = n * p.complexity_ ; + + vector w = weights(gfg); + const vector tree = buildTree(gfg, forward_ordering, w); + + /* sanity check */ + if ( tree.size() != n-1 ) { + throw runtime_error("SubgraphBuilder::operator() tree.size() != n-1 failed "); } - /* ************************************************************************* */ - // y += alpha*inv(R1')*A2'*e2 - void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha, + /* down weight the tree edges to zero */ + BOOST_FOREACH ( const size_t id, tree ) { + w[id] = 0.0; + } + + /* decide how many edges to augment */ + std::vector offTree = sample(w, t); + + vector subgraph = unary(gfg); + subgraph.insert(subgraph.end(), tree.begin(), tree.end()); + subgraph.insert(subgraph.end(), offTree.begin(), offTree.end()); + + return boost::make_shared(subgraph); +} + +/****************************************************************/ +SubgraphBuilder::Weights SubgraphBuilder::weights(const GaussianFactorGraph &gfg) const +{ + const size_t m = gfg.size() ; + Weights weight; weight.reserve(m); + + BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg ) { + switch ( parameters_.skeletonWeight_ ) { + case SubgraphBuilderParameters::EQUAL: + weight.push_back(1.0); + break; + case SubgraphBuilderParameters::RHS_2NORM: + { + if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { + weight.push_back(jf->getb().norm()); + } + else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { + weight.push_back(hf->linearTerm().norm()); + } + } + break; + case SubgraphBuilderParameters::LHS_FNORM: + { + if ( JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast(gf) ) { + weight.push_back(std::sqrt(jf->getA().squaredNorm())); + } + else if ( HessianFactor::shared_ptr hf = boost::dynamic_pointer_cast(gf) ) { + weight.push_back(std::sqrt(hf->information().squaredNorm())); + } + } + break; + + case SubgraphBuilderParameters::RANDOM: + weight.push_back(std::rand()%100 + 1.0); + break; + + default: + throw invalid_argument("SubgraphBuilder::weights: undefined weight scheme "); + break; + } + } + return weight; +} + +/* ************************************************************************* */ +SubgraphPreconditioner::SubgraphPreconditioner(const SubgraphPreconditionerParameters &p) : + parameters_(p) {} + +/* ************************************************************************* */ +SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2, + const sharedBayesNet& Rc1, const sharedValues& xbar, const SubgraphPreconditionerParameters &p) : + Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), + b2bar_(new Errors(-Ab2_->gaussianErrors(*xbar))), parameters_(p) { +} + +/* ************************************************************************* */ +// x = xbar + inv(R1)*y +VectorValues SubgraphPreconditioner::x(const VectorValues& y) const { + return *xbar_ + Rc1_->backSubstitute(y); +} + +/* ************************************************************************* */ +double SubgraphPreconditioner::error(const VectorValues& y) const { + Errors e(y); + VectorValues x = this->x(y); + Errors e2 = Ab2()->gaussianErrors(x); + return 0.5 * (dot(e, e) + dot(e2,e2)); +} + +/* ************************************************************************* */ +// gradient is y + inv(R1')*A2'*(A2*inv(R1)*y-b2bar), +VectorValues SubgraphPreconditioner::gradient(const VectorValues& y) const { + VectorValues x = Rc1()->backSubstitute(y); /* inv(R1)*y */ + Errors e = (*Ab2()*x - *b2bar()); /* (A2*inv(R1)*y-b2bar) */ + VectorValues v = VectorValues::Zero(x); + Ab2()->transposeMultiplyAdd(1.0, e, v); /* A2'*(A2*inv(R1)*y-b2bar) */ + return y + Rc1()->backSubstituteTranspose(v); +} + +/* ************************************************************************* */ +// Apply operator A, A*y = [I;A2*inv(R1)]*y = [y; A2*inv(R1)*y] +Errors SubgraphPreconditioner::operator*(const VectorValues& y) const { + + Errors e(y); + VectorValues x = Rc1()->backSubstitute(y); /* x=inv(R1)*y */ + Errors e2 = *Ab2() * x; /* A2*x */ + e.splice(e.end(), e2); + return e; +} + +/* ************************************************************************* */ +// In-place version that overwrites e +void SubgraphPreconditioner::multiplyInPlace(const VectorValues& y, Errors& e) const { + + Errors::iterator ei = e.begin(); + for ( Key i = 0 ; i < y.size() ; ++i, ++ei ) { + *ei = y[i]; + } + + // Add A2 contribution + VectorValues x = Rc1()->backSubstitute(y); // x=inv(R1)*y + Ab2()->multiplyInPlace(x, ei); // use iterator version +} + +/* ************************************************************************* */ +// Apply operator A', A'*e = [I inv(R1')*A2']*e = e1 + inv(R1')*A2'*e2 +VectorValues SubgraphPreconditioner::operator^(const Errors& e) const { + + Errors::const_iterator it = e.begin(); + VectorValues y = zero(); + for ( Key i = 0 ; i < y.size() ; ++i, ++it ) + y[i] = *it ; + transposeMultiplyAdd2(1.0,it,e.end(),y); + return y; +} + +/* ************************************************************************* */ +// y += alpha*A'*e +void SubgraphPreconditioner::transposeMultiplyAdd +(double alpha, const Errors& e, VectorValues& y) const { + + Errors::const_iterator it = e.begin(); + for ( Key i = 0 ; i < y.size() ; ++i, ++it ) { + const Vector& ei = *it; + axpy(alpha, ei, y[i]); + } + transposeMultiplyAdd2(alpha, it, e.end(), y); +} + +/* ************************************************************************* */ +// y += alpha*inv(R1')*A2'*e2 +void SubgraphPreconditioner::transposeMultiplyAdd2 (double alpha, Errors::const_iterator it, Errors::const_iterator end, VectorValues& y) const { - // create e2 with what's left of e - // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ? - Errors e2; - while (it != end) e2.push_back(*(it++)); + // create e2 with what's left of e + // TODO can we avoid creating e2 by passing iterator to transposeMultiplyAdd ? + Errors e2; + while (it != end) e2.push_back(*(it++)); - VectorValues x = VectorValues::Zero(y); // x = 0 - Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2 - axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x + VectorValues x = VectorValues::Zero(y); // x = 0 + Ab2_->transposeMultiplyAdd(1.0,e2,x); // x += A2'*e2 + axpy(alpha, Rc1_->backSubstituteTranspose(x), y); // y += alpha*inv(R1')*x +} + +/* ************************************************************************* */ +void SubgraphPreconditioner::print(const std::string& s) const { + cout << s << endl; + Ab2_->print(); +} + +/*****************************************************************************/ +void SubgraphPreconditioner::solve(const Vector& y, Vector &x) const +{ + /* copy first */ + std::copy(y.data(), y.data() + y.rows(), x.data()); + + /* in place back substitute */ + BOOST_REVERSE_FOREACH(const boost::shared_ptr & cg, *Rc1_) { + /* collect a subvector of x that consists of the parents of cg (S) */ + const Vector xParent = getSubvector(x, keyInfo_, FastVector(cg->beginParents(), cg->endParents())); + const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector(cg->beginFrontals(), cg->endFrontals())); + + /* compute the solution for the current pivot */ + const Vector solFrontal = cg->get_R().triangularView().solve(rhsFrontal - cg->get_S() * xParent); + + /* assign subvector of sol to the frontal variables */ + setSubvector(solFrontal, keyInfo_, FastVector(cg->beginFrontals(), cg->endFrontals()), x); + } +} + +/*****************************************************************************/ +void SubgraphPreconditioner::transposeSolve(const Vector& y, Vector& x) const +{ + /* copy first */ + std::copy(y.data(), y.data() + y.rows(), x.data()); + + /* in place back substitute */ + BOOST_FOREACH(const boost::shared_ptr & cg, *Rc1_) { + const Vector rhsFrontal = getSubvector(x, keyInfo_, FastVector(cg->beginFrontals(), cg->endFrontals())); +// const Vector solFrontal = cg->get_R().triangularView().transpose().solve(rhsFrontal); + const Vector solFrontal = cg->get_R().transpose().triangularView().solve(rhsFrontal); + + // Check for indeterminant solution + if ( solFrontal.hasNaN()) throw IndeterminantLinearSystemException(cg->keys().front()); + + /* assign subvector of sol to the frontal variables */ + setSubvector(solFrontal, keyInfo_, FastVector(cg->beginFrontals(), cg->endFrontals()), x); + + /* substract from parent variables */ + for (GaussianConditional::const_iterator it = cg->beginParents(); it != cg->endParents(); it++) { + KeyInfo::const_iterator it2 = keyInfo_.find(*it); + Eigen::Map rhsParent(x.data()+it2->second.colstart(), it2->second.dim(), 1); + rhsParent -= Matrix(cg->getA(it)).transpose() * solFrontal; + } + } +} + +/*****************************************************************************/ +void SubgraphPreconditioner::build(const GaussianFactorGraph &gfg, const KeyInfo &keyInfo, const std::map &lambda) +{ + /* identify the subgraph structure */ + const SubgraphBuilder builder(parameters_.builderParams_); + Subgraph::shared_ptr subgraph = builder(gfg); + + keyInfo_ = keyInfo; + + /* build factor subgraph */ + GaussianFactorGraph::shared_ptr gfg_subgraph = buildFactorSubgraph(gfg, *subgraph, true); + + /* factorize and cache BayesNet */ + Rc1_ = gfg_subgraph->eliminateSequential(); +} + +/*****************************************************************************/ +Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector &keys) { + + /* a cache of starting index and dim */ + typedef vector > Cache; + Cache cache; + + /* figure out dimension by traversing the keys */ + size_t d = 0; + BOOST_FOREACH ( const Key &key, keys ) { + const KeyInfoEntry &entry = keyInfo.find(key)->second; + cache.push_back(make_pair(entry.colstart(), entry.dim())); + d += entry.dim(); } - /* ************************************************************************* */ - void SubgraphPreconditioner::print(const std::string& s) const { - cout << s << endl; - Ab2_->print(); + /* use the cache to fill the result */ + Vector result = Vector::Zero(d, 1); + size_t idx = 0; + BOOST_FOREACH ( const Cache::value_type &p, cache ) { + result.segment(idx, p.second) = src.segment(p.first, p.second) ; + idx += p.second; } + + return result; +} + +/*****************************************************************************/ +void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector &keys, Vector &dst) { + /* use the cache */ + size_t idx = 0; + BOOST_FOREACH ( const Key &key, keys ) { + const KeyInfoEntry &entry = keyInfo.find(key)->second; + dst.segment(entry.colstart(), entry.dim()) = src.segment(idx, entry.dim()) ; + idx += entry.dim(); + } +} + +/*****************************************************************************/ +boost::shared_ptr +buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone) { + + GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph()); + result->reserve(subgraph.size()); + BOOST_FOREACH ( const SubgraphEdge &e, subgraph ) { + const size_t idx = e.index(); + if ( clone ) result->push_back(gfg[idx]->clone()); + else result->push_back(gfg[idx]); + } + return result; +} + } // nsamespace gtsam diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index d9d7524b6..86bee2188 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -12,15 +12,16 @@ /** * @file SubgraphPreconditioner.h * @date Dec 31, 2009 - * @author Frank Dellaert + * @author Frank Dellaert, Yong-Dian Jian */ #pragma once #include -#include #include - +#include +#include +#include #include namespace gtsam { @@ -30,6 +31,144 @@ namespace gtsam { class GaussianFactorGraph; class VectorValues; + struct GTSAM_EXPORT SubgraphEdge { + size_t index_; /* edge id */ + double weight_; /* edge weight */ + SubgraphEdge() : index_(0), weight_(1.0) {} + SubgraphEdge(const SubgraphEdge &e) : index_(e.index()), weight_(e.weight()) {} + SubgraphEdge(const size_t index, const double weight = 1.0): index_(index), weight_(weight) {} + inline size_t index() const { return index_; } + inline double weight() const { return weight_; } + inline bool isUnitWeight() const { return (weight_ == 1.0); } + friend std::ostream &operator<<(std::ostream &os, const SubgraphEdge &edge); + private: + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(index_); + ar & BOOST_SERIALIZATION_NVP(weight_); + } + }; + + /**************************************************************************/ + class GTSAM_EXPORT Subgraph { + public: + typedef boost::shared_ptr shared_ptr; + typedef std::vector vector_shared_ptr; + typedef std::vector Edges; + typedef std::vector EdgeIndices; + typedef Edges::iterator iterator; + typedef Edges::const_iterator const_iterator; + + protected: + Edges edges_; /* index to the factors */ + + public: + Subgraph() {} + Subgraph(const Subgraph &subgraph) : edges_(subgraph.edges()) {} + Subgraph(const Edges &edges) : edges_(edges) {} + Subgraph(const std::vector &indices) ; + + inline const Edges& edges() const { return edges_; } + inline const size_t size() const { return edges_.size(); } + EdgeIndices edgeIndices() const; + + iterator begin() { return edges_.begin(); } + const_iterator begin() const { return edges_.begin(); } + iterator end() { return edges_.end(); } + const_iterator end() const { return edges_.end(); } + + void save(const std::string &fn) const; + static shared_ptr load(const std::string &fn); + friend std::ostream &operator<<(std::ostream &os, const Subgraph &subgraph); + + private: + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar & BOOST_SERIALIZATION_NVP(edges_); + } + }; + + /****************************************************************************/ + struct GTSAM_EXPORT SubgraphBuilderParameters { + public: + typedef boost::shared_ptr shared_ptr; + + enum Skeleton { + /* augmented tree */ + NATURALCHAIN = 0, /* natural ordering of the graph */ + BFS, /* breadth-first search tree */ + KRUSKAL, /* maximum weighted spanning tree */ + } skeleton_ ; + + enum SkeletonWeight { /* how to weigh the graph edges */ + EQUAL = 0, /* every block edge has equal weight */ + RHS_2NORM, /* use the 2-norm of the rhs */ + LHS_FNORM, /* use the frobenius norm of the lhs */ + RANDOM, /* bounded random edge weight */ + } skeletonWeight_ ; + + enum AugmentationWeight { /* how to weigh the graph edges */ + SKELETON = 0, /* use the same weights in building the skeleton */ +// STRETCH, /* stretch in the laplacian sense */ +// GENERALIZED_STRETCH /* the generalized stretch defined in jian2013iros */ + } augmentationWeight_ ; + + double complexity_; + + SubgraphBuilderParameters() + : skeleton_(KRUSKAL), skeletonWeight_(RANDOM), augmentationWeight_(SKELETON), complexity_(1.0) {} + virtual ~SubgraphBuilderParameters() {} + + /* for serialization */ + void print() const ; + virtual void print(std::ostream &os) const ; + friend std::ostream& operator<<(std::ostream &os, const PreconditionerParameters &p); + + static Skeleton skeletonTranslator(const std::string &s); + static std::string skeletonTranslator(Skeleton w); + static SkeletonWeight skeletonWeightTranslator(const std::string &s); + static std::string skeletonWeightTranslator(SkeletonWeight w); + static AugmentationWeight augmentationWeightTranslator(const std::string &s); + static std::string augmentationWeightTranslator(AugmentationWeight w); + }; + + /*****************************************************************************/ + class GTSAM_EXPORT SubgraphBuilder { + + public: + typedef SubgraphBuilder Base; + typedef boost::shared_ptr shared_ptr; + typedef std::vector Weights; + + SubgraphBuilder(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) + : parameters_(p) {} + virtual ~SubgraphBuilder() {} + virtual boost::shared_ptr operator() (const GaussianFactorGraph &jfg) const ; + + private: + std::vector buildTree(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &weights) const ; + std::vector unary(const GaussianFactorGraph &gfg) const ; + std::vector natural_chain(const GaussianFactorGraph &gfg) const ; + std::vector bfs(const GaussianFactorGraph &gfg) const ; + std::vector kruskal(const GaussianFactorGraph &gfg, const FastMap &ordering, const std::vector &w) const ; + std::vector sample(const std::vector &weights, const size_t t) const ; + Weights weights(const GaussianFactorGraph &gfg) const; + SubgraphBuilderParameters parameters_; + + }; + + /*******************************************************************************************/ + struct GTSAM_EXPORT SubgraphPreconditionerParameters : public PreconditionerParameters { + typedef PreconditionerParameters Base; + typedef boost::shared_ptr shared_ptr; + SubgraphPreconditionerParameters(const SubgraphBuilderParameters &p = SubgraphBuilderParameters()) + : Base(), builderParams_(p) {} + virtual ~SubgraphPreconditionerParameters() {} + SubgraphBuilderParameters builderParams_; + }; + /** * Subgraph conditioner class, as explained in the RSS 2010 submission. * Starting with a graph A*x=b, we split it in two systems A1*x=b1 and A2*x=b2 @@ -37,7 +176,7 @@ namespace gtsam { * To use the class, give the Bayes Net R1*x=c1 and Graph A2*x=b2. * Then solve for yhat using CG, and solve for xhat = system.x(yhat). */ - class GTSAM_EXPORT SubgraphPreconditioner { + class GTSAM_EXPORT SubgraphPreconditioner : public Preconditioner { public: typedef boost::shared_ptr shared_ptr; @@ -52,9 +191,12 @@ namespace gtsam { sharedValues xbar_; ///< A1 \ b1 sharedErrors b2bar_; ///< A2*xbar - b2 + KeyInfo keyInfo_; + SubgraphPreconditionerParameters parameters_; + public: - SubgraphPreconditioner(); + SubgraphPreconditioner(const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters()); /** * Constructor @@ -62,7 +204,10 @@ namespace gtsam { * @param Rc1: the Bayes Net R1*x=c1 * @param xbar: the solution to R1*x=c1 */ - SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar); + SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar, + const SubgraphPreconditionerParameters &p = SubgraphPreconditionerParameters()); + + virtual ~SubgraphPreconditioner() {} /** print the object */ void print(const std::string& s = "SubgraphPreconditioner") const; @@ -80,7 +225,6 @@ namespace gtsam { * Add zero-mean i.i.d. Gaussian prior terms to each variable * @param sigma Standard deviation of Gaussian */ -// SubgraphPreconditioner add_priors(double sigma) const; /* x = xbar + inv(R1)*y */ VectorValues x(const VectorValues& y) const; @@ -119,6 +263,54 @@ namespace gtsam { * y += alpha*A'*[e1;e2] = [alpha*e1; alpha*inv(R1')*A2'*e2] */ void transposeMultiplyAdd(double alpha, const Errors& e, VectorValues& y) const; + + /*****************************************************************************/ + /* implement virtual functions of Preconditioner */ + + /* Computation Interfaces for Vector */ + virtual void solve(const Vector& y, Vector &x) const; + virtual void transposeSolve(const Vector& y, Vector& x) const ; + + virtual void build( + const GaussianFactorGraph &gfg, + const KeyInfo &info, + const std::map &lambda + ) ; + /*****************************************************************************/ }; + /* get subvectors */ + Vector getSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector &keys); + + /* set subvectors */ + void setSubvector(const Vector &src, const KeyInfo &keyInfo, const FastVector &keys, Vector &dst); + + + /* build a factor subgraph, which is defined as a set of weighted edges (factors) */ + boost::shared_ptr + buildFactorSubgraph(const GaussianFactorGraph &gfg, const Subgraph &subgraph, const bool clone); + + + /* sort the container and return permutation index with default comparator */ + template + std::vector sort_idx(const Container &src) + { + typedef typename Container::value_type T; + const size_t n = src.size() ; + std::vector > tmp; + tmp.reserve(n); + for ( size_t i = 0 ; i < n ; i++ ) + tmp.push_back(std::make_pair(i, src[i])); + + /* sort */ + std::stable_sort(tmp.begin(), tmp.end()) ; + + /* copy back */ + std::vector idx; idx.reserve(n); + for ( size_t i = 0 ; i < n ; i++ ) { + idx.push_back(tmp[i].first) ; + } + return idx; + } + } // namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.cpp b/gtsam/linear/SubgraphSolver.cpp index 034fdeeaf..6f10d04ad 100644 --- a/gtsam/linear/SubgraphSolver.cpp +++ b/gtsam/linear/SubgraphSolver.cpp @@ -14,6 +14,7 @@ #include #include #include +#include #include #include #include @@ -143,4 +144,11 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) { return boost::tie(At, Ac); } +/****************************************************************************/ +VectorValues SubgraphSolver::optimize ( + const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda, + const VectorValues &initial +) { return VectorValues(); } } // \namespace gtsam diff --git a/gtsam/linear/SubgraphSolver.h b/gtsam/linear/SubgraphSolver.h index f6557a2c2..ac8a9da87 100644 --- a/gtsam/linear/SubgraphSolver.h +++ b/gtsam/linear/SubgraphSolver.h @@ -13,22 +13,23 @@ #include -#include #include - #include +#include namespace gtsam { // Forward declarations class GaussianFactorGraph; class GaussianBayesNet; + class SubgraphPreconditioner; class GTSAM_EXPORT SubgraphSolverParameters : public ConjugateGradientParameters { public: typedef ConjugateGradientParameters Base; SubgraphSolverParameters() : Base() {} - virtual void print() const { Base::print(); } + void print() const { Base::print(); } + virtual void print(std::ostream &os) const { Base::print(os); } }; /** @@ -61,7 +62,7 @@ public: protected: Parameters parameters_; Ordering ordering_; - SubgraphPreconditioner::shared_ptr pc_; ///< preconditioner object + boost::shared_ptr pc_; ///< preconditioner object public: /* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */ @@ -77,8 +78,17 @@ public: SubgraphSolver(const boost::shared_ptr &Rc1, const boost::shared_ptr &Ab2, const Parameters ¶meters, const Ordering& ordering); virtual ~SubgraphSolver() {} - virtual VectorValues optimize () ; - virtual VectorValues optimize (const VectorValues &initial) ; + + VectorValues optimize () ; + VectorValues optimize (const VectorValues &initial) ; + + /* interface to the nonlinear optimizer that the subclasses have to implement */ + virtual VectorValues optimize ( + const GaussianFactorGraph &gfg, + const KeyInfo &keyInfo, + const std::map &lambda, + const VectorValues &initial + ) ; protected: diff --git a/gtsam/navigation/CombinedImuFactor.h b/gtsam/navigation/CombinedImuFactor.h index 06e79324d..725274acc 100644 --- a/gtsam/navigation/CombinedImuFactor.h +++ b/gtsam/navigation/CombinedImuFactor.h @@ -11,7 +11,7 @@ /** * @file CombinedImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman + * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen **/ #pragma once @@ -71,8 +71,9 @@ namespace gtsam { Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + bool use2ndOrderIntegration_; ///< Controls the order of integration + ///< In the combined factor is also includes the biases and keeps the correlation between the preintegrated measurements and the biases ///< COVARIANCE OF: [PreintPOSITION PreintVELOCITY PreintROTATION BiasAcc BiasOmega] /** Default constructor, initialize with no IMU measurements */ @@ -83,12 +84,14 @@ namespace gtsam { const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc const Matrix3& biasAccCovariance, ///< Covariance matrix of biasAcc (random walk describing BIAS evolution) const Matrix3& biasOmegaCovariance, ///< Covariance matrix of biasOmega (random walk describing BIAS evolution) - const Matrix& biasAccOmegaInit ///< Covariance of biasAcc & biasOmega when preintegrating measurements + const Matrix& biasAccOmegaInit, ///< Covariance of biasAcc & biasOmega when preintegrating measurements + const bool use2ndOrderIntegration = false ///< Controls the order of integration ///< (this allows to consider the uncertainty of the BIAS choice when integrating the measurements) ) : biasHat(bias), measurementCovariance(21,21), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), - delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)) + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(Matrix::Zero(15,15)), + use2ndOrderIntegration_(use2ndOrderIntegration) { // COVARIANCE OF: [Integration AccMeasurement OmegaMeasurement BiasAccRandomWalk BiasOmegaRandomWalk (BiasAccInit BiasOmegaInit)] SIZE (21x21) measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), @@ -136,6 +139,19 @@ namespace gtsam { && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); } + void resetIntegration(){ + deltaPij = Vector3::Zero(); + deltaVij = Vector3::Zero(); + deltaRij = Rot3(); + deltaTij = 0.0; + delPdelBiasAcc = Matrix3::Zero(); + delPdelBiasOmega = Matrix3::Zero(); + delVdelBiasAcc = Matrix3::Zero(); + delVdelBiasOmega = Matrix3::Zero(); + delRdelBiasOmega = Matrix3::Zero(); + PreintMeasCov = Matrix::Zero(15,15); + } + /** Add a single IMU measurement to the preintegration. */ void integrateMeasurement( const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) @@ -163,11 +179,14 @@ namespace gtsam { // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ - // delPdelBiasAcc += delVdelBiasAcc * deltaT; - // delPdelBiasOmega += delVdelBiasOmega * deltaT; - delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT; - delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix() - * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; + if(!use2ndOrderIntegration_){ + delPdelBiasAcc += delVdelBiasAcc * deltaT; + delPdelBiasOmega += delVdelBiasOmega * deltaT; + }else{ + delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT; + delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix() + * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; + } delVdelBiasAcc += -deltaRij.matrix() * deltaT; delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; @@ -222,32 +241,31 @@ namespace gtsam { G_measCov_Gt.block(0,0,3,3) = deltaT * measurementCovariance.block(0,0,3,3); G_measCov_Gt.block(3,3,3,3) = (1/deltaT) * (H_vel_biasacc) * - (measurementCovariance.block(3,3,3,3) + measurementCovariance.block(9,9,3,3) + measurementCovariance.block(15,15,3,3) ) * - (H_vel_biasacc.transpose()); + (measurementCovariance.block(3,3,3,3) + measurementCovariance.block(15,15,3,3) ) * + (H_vel_biasacc.transpose()); G_measCov_Gt.block(6,6,3,3) = (1/deltaT) * (H_angles_biasomega) * - (measurementCovariance.block(6,6,3,3) + measurementCovariance.block(12,12,3,3) + measurementCovariance.block(18,18,3,3) ) * - (H_angles_biasomega.transpose()); + (measurementCovariance.block(6,6,3,3) + measurementCovariance.block(18,18,3,3) ) * + (H_angles_biasomega.transpose()); G_measCov_Gt.block(9,9,3,3) = deltaT * measurementCovariance.block(9,9,3,3); G_measCov_Gt.block(12,12,3,3) = deltaT * measurementCovariance.block(12,12,3,3); - // OFF BLOCK DIAGONAL TERMS - Matrix3 block24 = H_vel_biasacc * measurementCovariance.block(9,9,3,3); - G_measCov_Gt.block(3,9,3,3) = block24; - G_measCov_Gt.block(9,3,3,3) = block24.transpose(); - - Matrix3 block35 = H_angles_biasomega * measurementCovariance.block(12,12,3,3); - G_measCov_Gt.block(6,12,3,3) = block35; - G_measCov_Gt.block(12,6,3,3) = block35.transpose(); + // NEW OFF BLOCK DIAGONAL TERMS + Matrix3 block23 = H_vel_biasacc * measurementCovariance.block(18,15,3,3) * H_angles_biasomega.transpose(); + G_measCov_Gt.block(3,6,3,3) = block23; + G_measCov_Gt.block(6,3,3,3) = block23.transpose(); PreintMeasCov = F * PreintMeasCov * F.transpose() + G_measCov_Gt; // Update preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ - // deltaPij += deltaVij * deltaT; - deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + if(!use2ndOrderIntegration_){ + deltaPij += deltaVij * deltaT; + }else{ + deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + } deltaVij += deltaRij.matrix() * correctedAcc * deltaT; deltaRij = deltaRij * Rincr; deltaTij += deltaT; @@ -311,23 +329,40 @@ namespace gtsam { Vector3 omegaCoriolis_; boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + public: /** Shorthand for a smart pointer to a factor */ - typedef boost::shared_ptr shared_ptr; +#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 + typedef typename boost::shared_ptr shared_ptr; +#else + typedef boost::shared_ptr shared_ptr; +#endif /** Default constructor - only use for serialization */ CombinedImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix::Zero(6,6)) {} /** Constructor */ - CombinedImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias_i, Key bias_j, - const CombinedPreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - const SharedNoiseModel& model, boost::optional body_P_sensor = boost::none) : - Base(model, pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), + CombinedImuFactor( + Key pose_i, ///< previous pose key + Key vel_i, ///< previous velocity key + Key pose_j, ///< current pose key + Key vel_j, ///< current velocity key + Key bias_i, ///< previous bias key + Key bias_j, ///< current bias key + const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements + const Vector3& gravity, ///< gravity vector + const Vector3& omegaCoriolis, ///< rotation rate of inertial frame + boost::optional body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame + const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect + ) : + Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias_i, bias_j), preintegratedMeasurements_(preintegratedMeasurements), gravity_(gravity), omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor) { + body_P_sensor_(body_P_sensor), + use2ndOrderCoriolis_(use2ndOrderCoriolis){ } virtual ~CombinedImuFactor() {} @@ -360,7 +395,7 @@ namespace gtsam { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_) + && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) && equal_with_abs_tol(gravity_, e->gravity_, tol) && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); @@ -370,6 +405,10 @@ namespace gtsam { const CombinedPreintegratedMeasurements& preintegratedMeasurements() const { return preintegratedMeasurements_; } + const Vector3& gravity() const { return gravity_; } + + const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + /** implement functions needed to derive from Factor */ /** vector of errors */ @@ -414,19 +453,18 @@ namespace gtsam { const Matrix3 Jrinv_fRhat = Rot3::rightJacobianExpMapSO3inverse(Rot3::Logmap(fRhat)); - if(H1) { - H1->resize(15,6); + /* (*H1) << // dfP/dRi Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), // dfP/dPi - - Rot_i.matrix(), + - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij, // dfV/dRi Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), // dfV/dPi - Matrix3::Zero(), + skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij, // dfR/dRi Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), // dfR/dPi @@ -435,6 +473,40 @@ namespace gtsam { Matrix3::Zero(), Matrix3::Zero(), //dBiasOmega/dPi Matrix3::Zero(), Matrix3::Zero(); + */ + if(H1) { + H1->resize(15,6); + + Matrix3 dfPdPi; + Matrix3 dfVdPi; + if(use2ndOrderCoriolis_){ + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; + dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + } + else{ + dfPdPi = - Rot_i.matrix(); + dfVdPi = Matrix3::Zero(); + } + + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Matrix3::Zero(), + //dBiasAcc/dPi + Matrix3::Zero(), Matrix3::Zero(), + //dBiasOmega/dPi + Matrix3::Zero(), Matrix3::Zero(); } if(H2) { @@ -445,14 +517,13 @@ namespace gtsam { + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(), - //dBiasAcc/dVi - Matrix3::Zero(), - //dBiasOmega/dVi - Matrix3::Zero(); - + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Matrix3::Zero(), + //dBiasAcc/dVi + Matrix3::Zero(), + //dBiasOmega/dVi + Matrix3::Zero(); } if(H3) { @@ -524,7 +595,6 @@ namespace gtsam { Matrix3::Zero(), Matrix3::Identity(); } - // Evaluate residual error, according to [3] /* ---------------------------------------------------------------------------------------------------- */ const Vector3 fp = @@ -559,7 +629,8 @@ namespace gtsam { static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j, const CombinedPreintegratedMeasurements& preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none) + const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false) { const double& deltaTij = preintegratedMeasurements.deltaTij; @@ -571,18 +642,23 @@ namespace gtsam { // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij - + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; + Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij + + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; - vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij - + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); + vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + if(use2ndOrderCoriolis){ + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity + } const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) diff --git a/gtsam/navigation/ImuFactor.h b/gtsam/navigation/ImuFactor.h index f5b5293f8..d68af4f8b 100644 --- a/gtsam/navigation/ImuFactor.h +++ b/gtsam/navigation/ImuFactor.h @@ -11,7 +11,7 @@ /** * @file ImuFactor.h - * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman + * @author Luca Carlone, Stephen Williams, Richard Roberts, Vadim Indelman, David Jensen **/ #pragma once @@ -60,7 +60,7 @@ namespace gtsam { imuBias::ConstantBias biasHat; ///< Acceleration and angular rate bias values used during preintegration Matrix measurementCovariance; ///< (Raw measurements uncertainty) Covariance of the vector [integrationError measuredAcc measuredOmega] in R^(9X9) - Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, , in [2]) (in frame i) + Vector3 deltaPij; ///< Preintegrated relative position (does not take into account velocity at time i, see deltap+, in [2]) (in frame i) Vector3 deltaVij; ///< Preintegrated relative velocity (in global frame) Rot3 deltaRij; ///< Preintegrated relative orientation (in frame i) double deltaTij; ///< Time interval from i to j @@ -70,19 +70,20 @@ namespace gtsam { Matrix3 delVdelBiasAcc; ///< Jacobian of preintegrated velocity w.r.t. acceleration bias Matrix3 delVdelBiasOmega; ///< Jacobian of preintegrated velocity w.r.t. angular rate bias Matrix3 delRdelBiasOmega; ///< Jacobian of preintegrated rotation w.r.t. angular rate bias - Matrix PreintMeasCov; ///< Covariance matrix of the preintegrated measurements (first-order propagation from *measurementCovariance*) + bool use2ndOrderIntegration_; ///< Controls the order of integration /** Default constructor, initialize with no IMU measurements */ PreintegratedMeasurements( const imuBias::ConstantBias& bias, ///< Current estimate of acceleration and rotation rate biases const Matrix3& measuredAccCovariance, ///< Covariance matrix of measuredAcc const Matrix3& measuredOmegaCovariance, ///< Covariance matrix of measuredAcc - const Matrix3& integrationErrorCovariance ///< Covariance matrix of measuredAcc + const Matrix3& integrationErrorCovariance, ///< Covariance matrix of measuredAcc + const bool use2ndOrderIntegration = false ///< Controls the order of integration ) : biasHat(bias), measurementCovariance(9,9), deltaPij(Vector3::Zero()), deltaVij(Vector3::Zero()), deltaTij(0.0), delPdelBiasAcc(Matrix3::Zero()), delPdelBiasOmega(Matrix3::Zero()), delVdelBiasAcc(Matrix3::Zero()), delVdelBiasOmega(Matrix3::Zero()), - delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9) + delRdelBiasOmega(Matrix3::Zero()), PreintMeasCov(9,9), use2ndOrderIntegration_(use2ndOrderIntegration) { measurementCovariance << integrationErrorCovariance , Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), measuredAccCovariance, Matrix3::Zero(), @@ -127,6 +128,19 @@ namespace gtsam { && equal_with_abs_tol(delRdelBiasOmega, expected.delRdelBiasOmega, tol); } + void resetIntegration(){ + deltaPij = Vector3::Zero(); + deltaVij = Vector3::Zero(); + deltaRij = Rot3(); + deltaTij = 0.0; + delPdelBiasAcc = Matrix3::Zero(); + delPdelBiasOmega = Matrix3::Zero(); + delVdelBiasAcc = Matrix3::Zero(); + delVdelBiasOmega = Matrix3::Zero(); + delRdelBiasOmega = Matrix3::Zero(); + PreintMeasCov = Matrix::Zero(9,9); + } + /** Add a single IMU measurement to the preintegration. */ void integrateMeasurement( const Vector3& measuredAcc, ///< Measured linear acceleration (in body frame) @@ -143,11 +157,8 @@ namespace gtsam { // Then compensate for sensor-body displacement: we express the quantities (originally in the IMU frame) into the body frame if(body_P_sensor){ Matrix3 body_R_sensor = body_P_sensor->rotation().matrix(); - correctedOmega = body_R_sensor * correctedOmega; // rotation rate vector in the body frame - Matrix3 body_omega_body__cross = skewSymmetric(correctedOmega); - correctedAcc = body_R_sensor * correctedAcc - body_omega_body__cross * body_omega_body__cross * body_P_sensor->translation().vector(); // linear acceleration vector in the body frame } @@ -159,16 +170,19 @@ namespace gtsam { // Update Jacobians /* ----------------------------------------------------------------------------------------------------------------------- */ - // delPdelBiasAcc += delVdelBiasAcc * deltaT; - // delPdelBiasOmega += delVdelBiasOmega * deltaT; - delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT; - delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix() - * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; + if(!use2ndOrderIntegration_){ + delPdelBiasAcc += delVdelBiasAcc * deltaT; + delPdelBiasOmega += delVdelBiasOmega * deltaT; + }else{ + delPdelBiasAcc += delVdelBiasAcc * deltaT - 0.5 * deltaRij.matrix() * deltaT*deltaT; + delPdelBiasOmega += delVdelBiasOmega * deltaT - 0.5 * deltaRij.matrix() + * skewSymmetric(biasHat.correctAccelerometer(measuredAcc)) * deltaT*deltaT * delRdelBiasOmega; + } delVdelBiasAcc += -deltaRij.matrix() * deltaT; delVdelBiasOmega += -deltaRij.matrix() * skewSymmetric(correctedAcc) * deltaT * delRdelBiasOmega; delRdelBiasOmega = Rincr.inverse().matrix() * delRdelBiasOmega - Jr_theta_incr * deltaT; - // Update preintegrated mesurements covariance + // Update preintegrated measurements covariance /* ----------------------------------------------------------------------------------------------------------------------- */ Matrix3 Z_3x3 = Matrix3::Zero(); Matrix3 I_3x3 = Matrix3::Identity(); @@ -210,7 +224,11 @@ namespace gtsam { // Update preintegrated measurements /* ----------------------------------------------------------------------------------------------------------------------- */ - deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + if(!use2ndOrderIntegration_){ + deltaPij += deltaVij * deltaT; + }else{ + deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + } deltaVij += deltaRij.matrix() * correctedAcc * deltaT; deltaRij = deltaRij * Rincr; deltaTij += deltaT; @@ -274,24 +292,39 @@ namespace gtsam { Vector3 omegaCoriolis_; boost::optional body_P_sensor_; ///< The pose of the sensor in the body frame + bool use2ndOrderCoriolis_; ///< Controls whether higher order terms are included when calculating the Coriolis Effect + public: /** Shorthand for a smart pointer to a factor */ +#if !defined(_MSC_VER) && __GNUC__ == 4 && __GNUC_MINOR__ > 5 + typedef typename boost::shared_ptr shared_ptr; +#else typedef boost::shared_ptr shared_ptr; - +#endif /** Default constructor - only use for serialization */ ImuFactor() : preintegratedMeasurements_(imuBias::ConstantBias(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()) {} /** Constructor */ - ImuFactor(Key pose_i, Key vel_i, Key pose_j, Key vel_j, Key bias, - const PreintegratedMeasurements& preintegratedMeasurements, const Vector3& gravity, const Vector3& omegaCoriolis, - boost::optional body_P_sensor = boost::none) : + ImuFactor( + Key pose_i, ///< previous pose key + Key vel_i, ///< previous velocity key + Key pose_j, ///< current pose key + Key vel_j, ///< current velocity key + Key bias, ///< previous bias key + const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements + const Vector3& gravity, ///< gravity vector + const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame + boost::optional body_P_sensor = boost::none, ///< The Pose of the sensor frame in the body frame + const bool use2ndOrderCoriolis = false ///< When true, the second-order term is used in the calculation of the Coriolis Effect + ) : Base(noiseModel::Gaussian::Covariance(preintegratedMeasurements.PreintMeasCov), pose_i, vel_i, pose_j, vel_j, bias), preintegratedMeasurements_(preintegratedMeasurements), gravity_(gravity), omegaCoriolis_(omegaCoriolis), - body_P_sensor_(body_P_sensor) { + body_P_sensor_(body_P_sensor), + use2ndOrderCoriolis_(use2ndOrderCoriolis){ } virtual ~ImuFactor() {} @@ -323,7 +356,7 @@ namespace gtsam { virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const { const This *e = dynamic_cast (&expected); return e != NULL && Base::equals(*e, tol) - && preintegratedMeasurements_.equals(e->preintegratedMeasurements_) + && preintegratedMeasurements_.equals(e->preintegratedMeasurements_, tol) && equal_with_abs_tol(gravity_, e->gravity_, tol) && equal_with_abs_tol(omegaCoriolis_, e->omegaCoriolis_, tol) && ((!body_P_sensor_ && !e->body_P_sensor_) || (body_P_sensor_ && e->body_P_sensor_ && body_P_sensor_->equals(*e->body_P_sensor_))); @@ -333,6 +366,10 @@ namespace gtsam { const PreintegratedMeasurements& preintegratedMeasurements() const { return preintegratedMeasurements_; } + const Vector3& gravity() const { return gravity_; } + + const Vector3& omegaCoriolis() const { return omegaCoriolis_; } + /** implement functions needed to derive from Factor */ /** vector of errors */ @@ -378,22 +415,35 @@ namespace gtsam { if(H1) { H1->resize(9,6); - (*H1) << - // dfP/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij - + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), - // dfP/dPi - - Rot_i.matrix(), - // dfV/dRi - Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij - + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), - // dfV/dPi - Matrix3::Zero(), - // dfR/dRi - Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), - // dfR/dPi - Matrix3::Zero(); + + Matrix3 dfPdPi; + Matrix3 dfVdPi; + if(use2ndOrderCoriolis_){ + dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; + dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; + } + else{ + dfPdPi = - Rot_i.matrix(); + dfVdPi = Matrix3::Zero(); + } + + (*H1) << + // dfP/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij + + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + // dfP/dPi + dfPdPi, + // dfV/dRi + Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij + + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + // dfV/dPi + dfVdPi, + // dfR/dRi + Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), + // dfR/dPi + Matrix3::Zero(); } + if(H2) { H2->resize(9,3); (*H2) << @@ -402,11 +452,11 @@ namespace gtsam { + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper // dfV/dVi - Matrix3::Identity() - + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term - // dfR/dVi - Matrix3::Zero(); - + + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + // dfR/dVi + Matrix3::Zero(); } + if(H3) { H3->resize(9,6); @@ -418,6 +468,7 @@ namespace gtsam { // dfR/dPosej Jrinv_fRhat * ( Matrix3::Identity() ), Matrix3::Zero(); } + if(H4) { H4->resize(9,3); (*H4) << @@ -428,6 +479,7 @@ namespace gtsam { // dfR/dVj Matrix3::Zero(); } + if(H5) { const Matrix3 Jrinv_theta_bc = Rot3::rightJacobianExpMapSO3inverse(theta_biascorrected); @@ -475,7 +527,8 @@ namespace gtsam { /** predicted states from IMU */ static void Predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, - const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none) + const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional body_P_sensor = boost::none, + const bool use2ndOrderCoriolis = false) { const double& deltaTij = preintegratedMeasurements.deltaTij; @@ -487,18 +540,23 @@ namespace gtsam { // Predict state at time j /* ---------------------------------------------------------------------------------------------------- */ - const Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij - + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) - + vel_i * deltaTij - - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - + 0.5 * gravity * deltaTij*deltaTij; + Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij + + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + + vel_i * deltaTij + - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper + + 0.5 * gravity * deltaTij*deltaTij; - vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij - + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr - + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) - - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - + gravity * deltaTij); + vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij + + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term + + gravity * deltaTij); + + if(use2ndOrderCoriolis){ + pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position + vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity + } const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); // deltaRij_biascorrected is expmap(deltaRij) * expmap(delRdelBiasOmega * biasOmegaIncr) diff --git a/gtsam/navigation/tests/testCombinedImuFactor.cpp b/gtsam/navigation/tests/testCombinedImuFactor.cpp index 10d582287..1a8b6160d 100644 --- a/gtsam/navigation/tests/testCombinedImuFactor.cpp +++ b/gtsam/navigation/tests/testCombinedImuFactor.cpp @@ -183,7 +183,7 @@ TEST( CombinedImuFactor, ErrorWithBiases ) ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); noiseModel::Gaussian::shared_ptr Combinedmodel = noiseModel::Gaussian::Covariance(Combined_pre_int_data.PreintMeasCov); - CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis, Combinedmodel); + CombinedImuFactor Combinedfactor(X(1), V(1), X(2), V(2), B(1), B(2), Combined_pre_int_data, gravity, omegaCoriolis); Vector errorExpected = factor.evaluateError(x1, v1, x2, v2, bias); @@ -260,7 +260,7 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements ) EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc)); EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega)); EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); - EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega)); + EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } #include diff --git a/gtsam/navigation/tests/testImuFactor.cpp b/gtsam/navigation/tests/testImuFactor.cpp index 6cd286f68..a6894898b 100644 --- a/gtsam/navigation/tests/testImuFactor.cpp +++ b/gtsam/navigation/tests/testImuFactor.cpp @@ -136,8 +136,9 @@ TEST( ImuFactor, PreintegratedMeasurements ) Rot3 expectedDeltaR1 = Rot3::RzRyRx(0.5 * M_PI/100.0, 0.0, 0.0); double expectedDeltaT1(0.5); + bool use2ndOrderIntegration = true; // Actual preintegrated values - ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + ImuFactor::PreintegratedMeasurements actual1(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); actual1.integrateMeasurement(measuredAcc, measuredOmega, deltaT); EXPECT(assert_equal(Vector(expectedDeltaP1), Vector(actual1.deltaPij), 1e-6)); @@ -177,7 +178,8 @@ TEST( ImuFactor, Error ) Vector3 measuredOmega; measuredOmega << M_PI/100, 0, 0; Vector3 measuredAcc = x1.rotation().unrotate(-Point3(gravity)).vector(); double deltaT = 1.0; - ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero()); + bool use2ndOrderIntegration = true; + ImuFactor::PreintegratedMeasurements pre_int_data(bias, Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), use2ndOrderIntegration); pre_int_data.integrateMeasurement(measuredAcc, measuredOmega, deltaT); // Create factor @@ -217,7 +219,7 @@ TEST( ImuFactor, Error ) Matrix H1atop6 = H1a.topRows(6); EXPECT(assert_equal(H1etop6, H1atop6)); // rotations - EXPECT(assert_equal(RH1e, H1a.bottomRows(3))); // evaluate only the rotation residue + EXPECT(assert_equal(RH1e, H1a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations EXPECT(assert_equal(H2e, H2a)); @@ -226,7 +228,7 @@ TEST( ImuFactor, Error ) Matrix H3atop6 = H3a.topRows(6); EXPECT(assert_equal(H3etop6, H3atop6)); // rotations - EXPECT(assert_equal(RH3e, H3a.bottomRows(3))); // evaluate only the rotation residue + EXPECT(assert_equal(RH3e, H3a.bottomRows(3), 1e-5)); // 1e-5 needs to be added only when using quaternions for rotations EXPECT(assert_equal(H4e, H4a)); // EXPECT(assert_equal(H5e, H5a)); @@ -324,7 +326,7 @@ TEST( ImuFactor, PartialDerivativeExpmap ) Matrix3 actualdelRdelBiasOmega = - Jr * deltaT; // the delta bias appears with the minus sign // Compare Jacobians - EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega)); + EXPECT(assert_equal(expectedDelRdelBiasOmega, actualdelRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } @@ -436,7 +438,7 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements ) EXPECT(assert_equal(expectedDelVdelBiasAcc, preintegrated.delVdelBiasAcc)); EXPECT(assert_equal(expectedDelVdelBiasOmega, preintegrated.delVdelBiasOmega)); EXPECT(assert_equal(expectedDelRdelBiasAcc, Matrix::Zero(3,3))); - EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega)); + EXPECT(assert_equal(expectedDelRdelBiasOmega, preintegrated.delRdelBiasOmega, 1e-3)); // 1e-3 needs to be added only when using quaternions for rotations } //#include diff --git a/gtsam/nonlinear/DoglegOptimizer.cpp b/gtsam/nonlinear/DoglegOptimizer.cpp index 03da76eb6..082cc66c8 100644 --- a/gtsam/nonlinear/DoglegOptimizer.cpp +++ b/gtsam/nonlinear/DoglegOptimizer.cpp @@ -76,7 +76,7 @@ void DoglegOptimizer::iterate(void) { result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, dx_u, dx_n, bn, graph_, state_.values, state_.error, dlVerbose); } - else if ( params_.isCG() ) { + else if ( params_.isIterative() ) { throw runtime_error("Dogleg is not currently compatible with the linear conjugate gradient solver"); } else { diff --git a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h index f365fc524..5e8e3923c 100644 --- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.h +++ b/gtsam/nonlinear/LevenbergMarquardtOptimizer.h @@ -19,6 +19,7 @@ #pragma once #include +#include #include class NonlinearOptimizerMoreOptimizationTest; diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index a98cd614f..b4498bee4 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include @@ -106,23 +107,21 @@ VectorValues NonlinearOptimizer::solve(const GaussianFactorGraph &gfg, delta = gfg.optimize(*params.ordering, params.getEliminationFunction()); } else if (params.isSequential()) { // Sequential QR or Cholesky (decided by params.getEliminationFunction()) - delta = gfg.eliminateSequential(*params.ordering, - params.getEliminationFunction())->optimize(); - } else if (params.isCG()) { + delta = gfg.eliminateSequential(*params.ordering, params.getEliminationFunction())->optimize(); + } else if (params.isIterative()) { + // Conjugate Gradient -> needs params.iterativeParams if (!params.iterativeParams) - throw std::runtime_error( - "NonlinearOptimizer::solve: cg parameter has to be assigned ..."); - // the type of params.iterativeParams decides the type of CG solver - if (boost::dynamic_pointer_cast( - params.iterativeParams)) { - SubgraphSolver solver(gfg, - dynamic_cast(*params.iterativeParams), - *params.ordering); - delta = solver.optimize(); - } else { - throw std::runtime_error( - "NonlinearOptimizer::solve: special cg parameter type is not handled in LM solver ..."); + throw std::runtime_error("NonlinearOptimizer::solve: cg parameter has to be assigned ..."); + + if (boost::shared_ptr pcg = boost::dynamic_pointer_cast(params.iterativeParams) ) { + delta = PCGSolver(*pcg).optimize(gfg); + } + else if (boost::shared_ptr spcg = boost::dynamic_pointer_cast(params.iterativeParams) ) { + delta = SubgraphSolver(gfg, *spcg, *params.ordering).optimize(); + } + else { + throw std::runtime_error("NonlinearOptimizer::solve: special cg parameter type is not handled in LM solver ..."); } } else { throw std::runtime_error( diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.cpp b/gtsam/nonlinear/NonlinearOptimizerParams.cpp index 7b0e73985..b7ad342ca 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.cpp +++ b/gtsam/nonlinear/NonlinearOptimizerParams.cpp @@ -66,8 +66,8 @@ std::string NonlinearOptimizerParams::verbosityTranslator( /* ************************************************************************* */ void NonlinearOptimizerParams::setIterativeParams( - const SubgraphSolverParameters ¶ms) { - iterativeParams = boost::make_shared(params); + const boost::shared_ptr params) { + iterativeParams = params; } /* ************************************************************************* */ @@ -99,8 +99,8 @@ void NonlinearOptimizerParams::print(const std::string& str) const { case CHOLMOD: std::cout << " linear solver type: CHOLMOD\n"; break; - case CONJUGATE_GRADIENT: - std::cout << " linear solver type: CONJUGATE GRADIENT\n"; + case Iterative: + std::cout << " linear solver type: ITERATIVE\n"; break; default: std::cout << " linear solver type: (invalid)\n"; @@ -127,8 +127,8 @@ std::string NonlinearOptimizerParams::linearSolverTranslator( return "SEQUENTIAL_CHOLESKY"; case SEQUENTIAL_QR: return "SEQUENTIAL_QR"; - case CONJUGATE_GRADIENT: - return "CONJUGATE_GRADIENT"; + case Iterative: + return "ITERATIVE"; case CHOLMOD: return "CHOLMOD"; default: @@ -148,8 +148,8 @@ NonlinearOptimizerParams::LinearSolverType NonlinearOptimizerParams::linearSolve return SEQUENTIAL_CHOLESKY; if (linearSolverType == "SEQUENTIAL_QR") return SEQUENTIAL_QR; - if (linearSolverType == "CONJUGATE_GRADIENT") - return CONJUGATE_GRADIENT; + if (linearSolverType == "ITERATIVE") + return Iterative; if (linearSolverType == "CHOLMOD") return CHOLMOD; throw std::invalid_argument( diff --git a/gtsam/nonlinear/NonlinearOptimizerParams.h b/gtsam/nonlinear/NonlinearOptimizerParams.h index 10d468384..cd5496209 100644 --- a/gtsam/nonlinear/NonlinearOptimizerParams.h +++ b/gtsam/nonlinear/NonlinearOptimizerParams.h @@ -99,7 +99,7 @@ public: MULTIFRONTAL_QR, SEQUENTIAL_CHOLESKY, SEQUENTIAL_QR, - CONJUGATE_GRADIENT, /* Experimental Flag */ + Iterative, /* Experimental Flag */ CHOLMOD, /* Experimental Flag */ }; @@ -121,8 +121,8 @@ public: return (linearSolverType == CHOLMOD); } - inline bool isCG() const { - return (linearSolverType == CONJUGATE_GRADIENT); + inline bool isIterative() const { + return (linearSolverType == Iterative); } GaussianFactorGraph::Eliminate getEliminationFunction() const { @@ -148,7 +148,9 @@ public: void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); } - void setIterativeParams(const SubgraphSolverParameters& params); + + void setIterativeParams(const boost::shared_ptr params); + void setOrdering(const Ordering& ordering) { this->ordering = ordering; } diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 29b482861..6b849ba5a 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -288,7 +288,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ){ // result.print("results of 3 camera, 3 landmark optimization \n"); if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3,result.at(x3),1e-8)); if(isDebugTest) tictoc_print_(); } @@ -365,7 +365,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){ // result.print("results of 3 camera, 3 landmark optimization \n"); if(isDebugTest) result.at(x3).print("Smart: Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3,result.at(x3), 1e-7)); if(isDebugTest) tictoc_print_(); } @@ -428,7 +428,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ){ Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3,result.at(x3), 1e-8)); } /* *************************************************************************/ @@ -631,7 +631,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ){ Values result; LevenbergMarquardtOptimizer optimizer(graph, values, params); result = optimizer.optimize(); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3,result.at(x3), 1e-8)); } /* *************************************************************************/ @@ -697,7 +697,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){ Values result = optimizer.optimize(); if(isDebugTest) result.at(x3).print("Pose3 after optimization: "); - EXPECT(assert_equal(pose3,result.at(x3))); + EXPECT(assert_equal(pose3,result.at(x3), 1e-7)); } /* *************************************************************************/ diff --git a/gtsam_unstable/CMakeLists.txt b/gtsam_unstable/CMakeLists.txt index 6acb51ff6..cd42b8d4f 100644 --- a/gtsam_unstable/CMakeLists.txt +++ b/gtsam_unstable/CMakeLists.txt @@ -28,7 +28,7 @@ set (excluded_headers # "") "${CMAKE_CURRENT_SOURCE_DIR}/slam/serialization.h" ) -# assemble core libaries +# assemble core libraries foreach(subdir ${gtsam_unstable_subdirs}) # Build convenience libraries file(GLOB subdir_srcs "${subdir}/*.cpp") @@ -63,7 +63,7 @@ set(gtsam_unstable_soversion ${GTSAM_VERSION_MAJOR}) message(STATUS "GTSAM_UNSTABLE Version: ${gtsam_unstable_version}") message(STATUS "Install prefix: ${CMAKE_INSTALL_PREFIX}") -# build shared and static versions of the library +# build shared or static versions of the library if (GTSAM_BUILD_STATIC_LIBRARY) message(STATUS "Building GTSAM_UNSTABLE - static") add_library(gtsam_unstable STATIC ${gtsam_unstable_srcs}) @@ -116,7 +116,6 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX) wrap_and_install_library(gtsam_unstable.h "gtsam" "" "${mexFlags}") endif(GTSAM_INSTALL_MATLAB_TOOLBOX) - # Build examples add_subdirectory(examples) diff --git a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp index 40a0a8725..01c2ab3e1 100644 --- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp +++ b/gtsam_unstable/examples/SmartProjectionFactorExample.cpp @@ -108,9 +108,13 @@ int main(int argc, char** argv){ // QR is much slower than Cholesky, but numerically more stable graph.push_back(NonlinearEquality(Symbol('x',1),first_pose)); + LevenbergMarquardtParams params; + params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + params.verbosity = NonlinearOptimizerParams::ERROR; + cout << "Optimizing" << endl; //create Levenberg-Marquardt optimizer to optimize the factor graph - LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate); + LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate, params); Values result = optimizer.optimize(); cout << "Final result sample:" << endl; diff --git a/gtsam_unstable/gtsam_unstable.h b/gtsam_unstable/gtsam_unstable.h index 1aa840825..06a41f5ec 100644 --- a/gtsam_unstable/gtsam_unstable.h +++ b/gtsam_unstable/gtsam_unstable.h @@ -23,6 +23,7 @@ virtual class gtsam::NoiseModelFactor4; virtual class gtsam::GaussianFactor; virtual class gtsam::HessianFactor; virtual class gtsam::JacobianFactor; +virtual class gtsam::Cal3_S2; class gtsam::GaussianFactorGraph; class gtsam::NonlinearFactorGraph; class gtsam::Ordering; @@ -379,7 +380,6 @@ virtual class SmartRangeFactor : gtsam::NoiseModelFactor { }; - #include template virtual class RangeFactor : gtsam::NonlinearFactor { diff --git a/matlab/unstable_examples/+imuSimulator/+lib/antisim.m b/matlab/unstable_examples/+imuSimulator/+lib/antisim.m new file mode 100644 index 000000000..6e9194567 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/antisim.m @@ -0,0 +1,7 @@ +function S = antisim(v) + +% costruisce la matrice antisimmetrica S (3x3) a partire dal vettore v +% Uso: S = antisim(v) + +S=[0 -v(3) v(2); v(3) 0 -v(1); -v(2) v(1) 0]; + diff --git a/matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m b/matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m new file mode 100644 index 000000000..aef3a5f10 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/arrow3d.m @@ -0,0 +1,140 @@ +function arrowHandle = arrow3D(pos, deltaValues, colorCode, stemRatio, rhoRatio) + +% arrowHandle = arrow3D(pos, deltaValues, colorCode, stemRatio) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% Used to plot a single 3D arrow with a cylindrical stem and cone arrowhead +% pos = [X,Y,Z] - spatial location of the starting point of the arrow (end of stem) +% deltaValues = [QX,QY,QZ] - delta parameters denoting the magnitude of the arrow along the x,y,z-axes (relative to 'pos') +% colorCode - Color parameters as per the 'surf' command. For example, 'r', 'red', [1 0 0] are all examples of a red-colored arrow +% stemRatio - The ratio of the length of the stem in proportion to the arrowhead. For example, a call of: +% arrow3D([0,0,0], [100,0,0] , 'r', 0.82) will produce a red arrow of magnitude 100, with the arrowstem spanning a distance +% of 82 (note 0.82 ratio of length 100) while the arrowhead (cone) spans 18. +% rhoRatio - The ratio of the cylinder radius (0.05 is the default) +% value) +% +% Example: +% arrow3D([0,0,0], [4,3,7]); %---- arrow with default parameters +% axis equal; +% +% Author: Shawn Arseneau +% Created: September 14, 2006 +% Updated: September 18, 2006 +% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + if nargin<2 || nargin>5 + error('Incorrect number of inputs to arrow3D'); + end + if numel(pos)~=3 || numel(deltaValues)~=3 + error('pos and/or deltaValues is incorrect dimensions (should be three)'); + end + if nargin<3 + colorCode = 'interp'; + end + if nargin<4 + stemRatio = 0.75; + end + if nargin<5 + rhoRatio = 0.05; + end + + X = pos(1); %---- with this notation, there is no need to transpose if the user has chosen a row vs col vector + Y = pos(2); + Z = pos(3); + + [sphi, stheta, srho] = cart2sph(deltaValues(1), deltaValues(2), deltaValues(3)); + + %******************************************* CYLINDER == STEM ********************************************* + cylinderRadius = srho*rhoRatio; + cylinderLength = srho*stemRatio; + [CX,CY,CZ] = cylinder(cylinderRadius); + CZ = CZ.*cylinderLength; %---- lengthen + + %----- ROTATE CYLINDER + [row, col] = size(CX); %---- initial rotation to coincide with X-axis + + newEll = rotatePoints([0 0 -1], [CX(:), CY(:), CZ(:)]); + CX = reshape(newEll(:,1), row, col); + CY = reshape(newEll(:,2), row, col); + CZ = reshape(newEll(:,3), row, col); + + [row, col] = size(CX); + newEll = rotatePoints(deltaValues, [CX(:), CY(:), CZ(:)]); + stemX = reshape(newEll(:,1), row, col); + stemY = reshape(newEll(:,2), row, col); + stemZ = reshape(newEll(:,3), row, col); + + %----- TRANSLATE CYLINDER + stemX = stemX + X; + stemY = stemY + Y; + stemZ = stemZ + Z; + + %******************************************* CONE == ARROWHEAD ********************************************* + coneLength = srho*(1-stemRatio); + coneRadius = cylinderRadius*1.5; + incr = 4; %---- Steps of cone increments + coneincr = coneRadius/incr; + [coneX, coneY, coneZ] = cylinder(cylinderRadius*2:-coneincr:0); %---------- CONE + coneZ = coneZ.*coneLength; + + %----- ROTATE CONE + [row, col] = size(coneX); + newEll = rotatePoints([0 0 -1], [coneX(:), coneY(:), coneZ(:)]); + coneX = reshape(newEll(:,1), row, col); + coneY = reshape(newEll(:,2), row, col); + coneZ = reshape(newEll(:,3), row, col); + + newEll = rotatePoints(deltaValues, [coneX(:), coneY(:), coneZ(:)]); + headX = reshape(newEll(:,1), row, col); + headY = reshape(newEll(:,2), row, col); + headZ = reshape(newEll(:,3), row, col); + + %---- TRANSLATE CONE + V = [0, 0, srho*stemRatio]; %---- centerline for cylinder: the multiplier is to set the cone 'on the rim' of the cylinder + Vp = rotatePoints([0 0 -1], V); + Vp = rotatePoints(deltaValues, Vp); + headX = headX + Vp(1) + X; + headY = headY + Vp(2) + Y; + headZ = headZ + Vp(3) + Z; + %************************************************************************************************************ + hStem = surf(stemX, stemY, stemZ, 'FaceColor', colorCode, 'EdgeColor', 'none'); + hold on; + hHead = surf(headX, headY, headZ, 'FaceColor', colorCode, 'EdgeColor', 'none'); + + if nargout==1 + arrowHandle = [hStem, hHead]; + end + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/matlab/unstable_examples/+imuSimulator/+lib/getxyz.m b/matlab/unstable_examples/+imuSimulator/+lib/getxyz.m new file mode 100644 index 000000000..d2c70db3a --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/getxyz.m @@ -0,0 +1,14 @@ +function [c] = getxyz(poses, j) +% The function extract the Cartesian variables from pose (pose.p = positions, +% pose.R = rotations). In particular, if there are T poses, +% - getxyz(pose, 1) estracts the vector x \in R^T, +% - getxyz(pose, 2) estracts the vector y \in R^T, +% - getxyz(pose, 3) estracts the vector z \in R^T. + +L = length(poses); +c = []; +for i=1:L % for each pose + c = [c poses(i).p(j)]; +end + +c = c(:); % column vector \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m b/matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m new file mode 100644 index 000000000..1bababd33 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/plot_trajectory.m @@ -0,0 +1,19 @@ +function [] = plot_trajectory(pose, step, color, plot_name,a,b,c) +% Plot a collection of poses: positions are connected by a solid line +% of color "color" and it is shown a ref frame for each pose. +% Plot_name defines the name of the created figure. + +x = getxyz(pose,1); +y = getxyz(pose,2); +z = getxyz(pose,3); +plot3(x,y,z,color) +n = length(x)-1; +hold on +for i=1:step:n+1 + ref_frame_plot(pose(i).R,pose(i).p,a,b,c) +end +title(plot_name); +xlabel('x') +ylabel('y') +zlabel('z') +view(3) \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m b/matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m new file mode 100644 index 000000000..9c73d1c69 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/ref_frame_plot.m @@ -0,0 +1,54 @@ +function ref_frame_plot(Ref,OriginRef,rhoRatio,stemRatio,axsize) +% ref_frame plot a 3D representation of a reference frame +% given by three unit vectors +% +% ref_frame(Ref,DimSpace,OriginRef) +% Ref is a 3 x 3 orthogonal matrix representing the unit vectors +% of the reference frame to be drawn +% DimSpace is a 3 x 2 matrix with min an max dimensions of the space +% [xmin xmax; ymin ymax; zmin zmax] +% default value = [-1,5 +1.5] for all dimensions +% OriginRef is the reference frame origin point +% default value = [0 0 0]' + +% Copright (C) Basilio Bona 2007 + +n=nargin; +if n == 1 + OriginRef=[0 0 0]'; + colorcode=['r','g','b']; + rhoRatio=0.05; + stemRatio=0.75; + axsize=1; +end +if n == 2 + colorcode=['r','g','b']; + rhoRatio=0.05; + stemRatio=0.75; + axsize=1; +end +if n == 3 + colorcode=['r','g','b']; + stemRatio=0.75; + axsize=1; +end + +if n == 4 + colorcode=['r','g','b']; + axsize=1; +end + +if n == 5 + colorcode=['r','g','b']; +end + + +% xproj=DimSpace(1,2); yproj=DimSpace(2,2); zproj=DimSpace(3,1); + +%hold off; +arrow3d(OriginRef, axsize*Ref(:,1), colorcode(1), stemRatio, rhoRatio); +arrow3d(OriginRef, axsize*Ref(:,2), colorcode(2), stemRatio, rhoRatio); +arrow3d(OriginRef, axsize*Ref(:,3), colorcode(3), stemRatio, rhoRatio); +axis equal; xlabel('X'); ylabel('Y'); zlabel('Z'); +% lighting phong; +% camlight left; diff --git a/matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m b/matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m new file mode 100644 index 000000000..4380d9d5d --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/rotatePoints.m @@ -0,0 +1,82 @@ +function rotatedData = rotatePoints(alignmentVector, originalData) + +% rotatedData = rotatePoints(alignmentVector, originalData) %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% +% Rotate the 'originalData' in the form of Nx2 or Nx3 about the origin by aligning the x-axis with the alignment vector +% +% Rdata = rotatePoints([1,2,-1], [Xpts(:), Ypts(:), Zpts(:)]) - rotate the (X,Y,Z)pts in 3D with respect to the vector [1,2,-1] +% +% Rotating using spherical components can be done by first converting using [dX,dY,dZ] = cart2sph(theta, phi, rho); alignmentVector = [dX,dY,dZ]; +% +% Example: +% %% Rotate the point [3,4,-7] with respect to the following: +% %%%% Original associated vector is always [1,0,0] +% %%%% Calculate the appropriate rotation requested with respect to the x-axis. For example, if only a rotation about the z-axis is +% %%%% sought, alignmentVector = [2,1,0] %% Note that the z-component is zero +% rotData = rotatePoints(alignmentVector, [3,4,-7]); +% +% Author: Shawn Arseneau +% Created: Feb.2, 2006 +% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + + alignmentDim = numel(alignmentVector); + DOF = size(originalData,2); %---- DOF = Degrees of Freedom (i.e. 2 for two dimensional and 3 for three dimensional data) + + if alignmentDim~=DOF + error('Alignment vector does not agree with originalData dimensions'); + end + if DOF<2 || DOF>3 + error('rotatePoints only does rotation in two or three dimensions'); + end + + + if DOF==2 % 2D rotation... + [rad_theta, rho] = cart2pol(alignmentVector(1), alignmentVector(2)); + deg_theta = -1 * rad_theta * (180/pi); + ctheta = cosd(deg_theta); stheta = sind(deg_theta); + + Rmatrix = [ctheta, -1.*stheta;... + stheta, ctheta]; + rotatedData = originalData*Rmatrix; + + else % 3D rotation... + [rad_theta, rad_phi, rho] = cart2sph(alignmentVector(1), alignmentVector(2), alignmentVector(3)); + rad_theta = rad_theta * -1; + deg_theta = rad_theta * (180/pi); + deg_phi = rad_phi * (180/pi); + ctheta = cosd(deg_theta); stheta = sind(deg_theta); + Rz = [ctheta, -1.*stheta, 0;... + stheta, ctheta, 0;... + 0, 0, 1]; %% First rotate as per theta around the Z axis + rotatedData = originalData*Rz; + + [rotX, rotY, rotZ] = sph2cart(-1* (rad_theta+(pi/2)), 0, 1); %% Second rotation corresponding to phi + rotationAxis = [rotX, rotY, rotZ]; + u = rotationAxis(:)/norm(rotationAxis); %% Code extract from rotate.m from MATLAB + cosPhi = cosd(deg_phi); + sinPhi = sind(deg_phi); + invCosPhi = 1 - cosPhi; + x = u(1); + y = u(2); + z = u(3); + Rmatrix = [cosPhi+x^2*invCosPhi x*y*invCosPhi-z*sinPhi x*z*invCosPhi+y*sinPhi; ... + x*y*invCosPhi+z*sinPhi cosPhi+y^2*invCosPhi y*z*invCosPhi-x*sinPhi; ... + x*z*invCosPhi-y*sinPhi y*z*invCosPhi+x*sinPhi cosPhi+z^2*invCosPhi]'; + + rotatedData = rotatedData*Rmatrix; + end + + + + + + + + + + + + + + diff --git a/matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m b/matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m new file mode 100644 index 000000000..6735ad6e5 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/+lib/uth2rot.m @@ -0,0 +1,14 @@ +function R = uth2rot(u,teta) + +% Uso: R = uth2rot(u,teta) +% +% calcola la matrice R +% a partire da asse u ed angolo di rotazione teta (in rad) + +S=antisim(u); +t=teta; + +n=norm(u); + +R = eye(3) + sin(t)/n*S + (1-cos(t))/n^2*S^2; + diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison.m b/matlab/unstable_examples/+imuSimulator/IMUComparison.m new file mode 100644 index 000000000..68e20bb25 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison.m @@ -0,0 +1,146 @@ +import gtsam.*; + +deltaT = 0.001; +summarizedDeltaT = 0.1; +timeElapsed = 1; +times = 0:deltaT:timeElapsed; + +omega = [0;0;2*pi]; +velocity = [1;0;0]; + +summaryTemplate = gtsam.ImuFactorPreintegratedMeasurements( ... + gtsam.imuBias.ConstantBias([0;0;0], [0;0;0]), ... + 1e-3 * eye(3), 1e-3 * eye(3), 1e-3 * eye(3)); + +%% Set initial conditions for the true trajectory and for the estimates +% (one estimate is obtained by integrating in the body frame, the other +% by integrating in the navigation frame) +% Initial state (body) +currentPoseGlobal = Pose3; +currentVelocityGlobal = velocity; +% Initial state estimate (integrating in navigation frame) +currentPoseGlobalIMUnav = currentPoseGlobal; +currentVelocityGlobalIMUnav = currentVelocityGlobal; +% Initial state estimate (integrating in the body frame) +currentPoseGlobalIMUbody = currentPoseGlobal; +currentVelocityGlobalIMUbody = currentVelocityGlobal; + +%% Prepare data structures for actual trajectory and estimates +% Actual trajectory +positions = zeros(3, length(times)+1); +positions(:,1) = currentPoseGlobal.translation.vector; +poses(1).p = positions(:,1); +poses(1).R = currentPoseGlobal.rotation.matrix; + +% Trajectory estimate (integrated in the navigation frame) +positionsIMUnav = zeros(3, length(times)+1); +positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector; +posesIMUnav(1).p = positionsIMUnav(:,1); +posesIMUnav(1).R = poses(1).R; + +% Trajectory estimate (integrated in the body frame) +positionsIMUbody = zeros(3, length(times)+1); +positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector; +posesIMUbody(1).p = positionsIMUbody(:,1); +posesIMUbody(1).R = poses(1).R; + +%% Solver object +isamParams = ISAM2Params; +isamParams.setRelinearizeSkip(1); +isam = gtsam.ISAM2(isamParams); + +initialValues = Values; +initialValues.insert(symbol('x',0), currentPoseGlobal); +initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal)); +initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0])); +initialFactors = NonlinearFactorGraph; +initialFactors.add(PriorFactorPose3(symbol('x',0), ... + currentPoseGlobal, noiseModel.Isotropic.Sigma(6, 1.0))); +initialFactors.add(PriorFactorLieVector(symbol('v',0), ... + LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, 1.0))); +initialFactors.add(PriorFactorConstantBias(symbol('b',0), ... + imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, 1.0))); + +%% Main loop +i = 2; +lastSummaryTime = 0; +lastSummaryIndex = 0; +currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate); +for t = times + %% Create the actual trajectory, using the velocities and + % accelerations in the inertial frame to compute the positions + [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ... + currentPoseGlobal, omega, velocity, velocity, deltaT); + + %% Simulate IMU measurements, considering Coriolis effect + % (in this simple example we neglect gravity and there are no other forces acting on the body) + acc_omega = imuSimulator.calculateIMUMeas_coriolis( ... + omega, omega, velocity, velocity, deltaT); + + %% Accumulate preintegrated measurement + currentSummarizedMeasurement.integrateMeasurement(acc_omega(1:3), acc_omega(4:6), deltaT); + + %% Update solver + if t - lastSummaryTime >= summarizedDeltaT + % Create IMU factor + initialFactors.add(ImuFactor( ... + symbol('x',lastSummaryIndex), symbol('v',lastSummaryIndex), ... + symbol('x',lastSummaryIndex+1), symbol('v',lastSummaryIndex+1), ... + symbol('b',0), currentSummarizedMeasurement, [0;0;1], [0;0;0], ... + noiseModel.Isotropic.Sigma(9, 1e-6))); + + % Predict movement in a straight line (bad initialization) + if lastSummaryIndex > 0 + initialPose = isam.calculateEstimate(symbol('x',lastSummaryIndex)) ... + .compose(Pose3(Rot3, Point3( velocity * t - lastSummaryTime) )); + initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex)); + else + initialPose = Pose3; + initialVel = LieVector(velocity); + end + initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose); + initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel); + + % Update solver + isam.update(initialFactors, initialValues); + initialFactors = NonlinearFactorGraph; + initialValues = Values; + + lastSummaryIndex = lastSummaryIndex + 1; + lastSummaryTime = t; + currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate); + end + + %% Integrate in the body frame + [ currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody ] = imuSimulator.integrateIMUTrajectory_bodyFrame( ... + currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody, acc_omega, deltaT, velocity); + + %% Integrate in the navigation frame + [ currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav ] = imuSimulator.integrateIMUTrajectory_navFrame( ... + currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT); + + %% Store data in some structure for statistics and plots + positions(:,i) = currentPoseGlobal.translation.vector; + positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector; + positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector; + % - + poses(i).p = positions(:,i); + posesIMUbody(i).p = positionsIMUbody(:,i); + posesIMUnav(i).p = positionsIMUnav(:,i); + % - + poses(i).R = currentPoseGlobal.rotation.matrix; + posesIMUbody(i).R = currentPoseGlobalIMUbody.rotation.matrix; + posesIMUnav(i).R = currentPoseGlobalIMUnav.rotation.matrix; + i = i + 1; +end + +figure(1) +hold on; +plot(positions(1,:), positions(2,:), '-b'); +plot(positionsIMUbody(1,:), positionsIMUbody(2,:), '-r'); +plot(positionsIMUnav(1,:), positionsIMUnav(2,:), ':k'); +plot3DTrajectory(isam.calculateEstimate, 'g-'); +axis equal; +legend('true trajectory', 'traj integrated in body', 'traj integrated in nav') + + diff --git a/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m new file mode 100644 index 000000000..c589bea32 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/IMUComparison_with_cov.m @@ -0,0 +1,128 @@ +close all +clc + +import gtsam.*; + +deltaT = 0.001; +summarizedDeltaT = 0.1; +timeElapsed = 1; +times = 0:deltaT:timeElapsed; + +omega = [0;0;2*pi]; +velocity = [1;0;0]; + +g = [0;0;0]; +cor_v = [0;0;0]; + +summaryTemplate = gtsam.ImuFactorPreintegratedMeasurements( ... + gtsam.imuBias.ConstantBias([0;0;0], [0;0;0]), ... + 1e-3 * eye(3), 1e-3 * eye(3), 1e-3 * eye(3)); + +%% Set initial conditions for the true trajectory and for the estimates +% (one estimate is obtained by integrating in the body frame, the other +% by integrating in the navigation frame) +% Initial state (body) +currentPoseGlobal = Pose3; +currentVelocityGlobal = velocity; + +%% Prepare data structures for actual trajectory and estimates +% Actual trajectory +positions = zeros(3, length(times)+1); +positions(:,1) = currentPoseGlobal.translation.vector; +poses(1).p = positions(:,1); +poses(1).R = currentPoseGlobal.rotation.matrix; + +%% Solver object +isamParams = ISAM2Params; +isamParams.setRelinearizeSkip(1); +isam = gtsam.ISAM2(isamParams); + +sigma_init_x = 1.0; +sigma_init_v = 1.0; +sigma_init_b = 1.0; + +initialValues = Values; +initialValues.insert(symbol('x',0), currentPoseGlobal); +initialValues.insert(symbol('v',0), LieVector(currentVelocityGlobal)); +initialValues.insert(symbol('b',0), imuBias.ConstantBias([0;0;0],[0;0;0])); +initialFactors = NonlinearFactorGraph; +% Prior on initial pose +initialFactors.add(PriorFactorPose3(symbol('x',0), ... + currentPoseGlobal, noiseModel.Isotropic.Sigma(6, sigma_init_x))); +% Prior on initial velocity +initialFactors.add(PriorFactorLieVector(symbol('v',0), ... + LieVector(currentVelocityGlobal), noiseModel.Isotropic.Sigma(3, sigma_init_v))); +% Prior on initial bias +initialFactors.add(PriorFactorConstantBias(symbol('b',0), ... + imuBias.ConstantBias([0;0;0],[0;0;0]), noiseModel.Isotropic.Sigma(6, sigma_init_b))); + +%% Main loop +i = 2; +lastSummaryTime = 0; +lastSummaryIndex = 0; +currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate); +for t = times + %% Create the ground truth trajectory, using the velocities and accelerations in the inertial frame to compute the positions + [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ... + currentPoseGlobal, omega, velocity, velocity, deltaT); + + %% Simulate IMU measurements, considering Coriolis effect + % (in this simple example we neglect gravity and there are no other forces acting on the body) + acc_omega = imuSimulator.calculateIMUMeas_coriolis( ... + omega, omega, velocity, velocity, deltaT); + + %% Accumulate preintegrated measurement + currentSummarizedMeasurement.integrateMeasurement(acc_omega(1:3), acc_omega(4:6), deltaT); + + %% Update solver + if t - lastSummaryTime >= summarizedDeltaT + + % Create IMU factor + initialFactors.add(ImuFactor( ... + symbol('x',lastSummaryIndex), symbol('v',lastSummaryIndex), ... + symbol('x',lastSummaryIndex+1), symbol('v',lastSummaryIndex+1), ... + symbol('b',0), currentSummarizedMeasurement, g, cor_v, ... + noiseModel.Isotropic.Sigma(9, 1e-6))); + + % Predict movement in a straight line (bad initialization) + if lastSummaryIndex > 0 + initialPose = isam.calculateEstimate(symbol('x',lastSummaryIndex)) ... + .compose(Pose3(Rot3, Point3( velocity * t - lastSummaryTime) )); + initialVel = isam.calculateEstimate(symbol('v',lastSummaryIndex)); + else + initialPose = Pose3; + initialVel = LieVector(velocity); + end + initialValues.insert(symbol('x',lastSummaryIndex+1), initialPose); + initialValues.insert(symbol('v',lastSummaryIndex+1), initialVel); + + key_pose = symbol('x',lastSummaryIndex+1); + + % Update solver + isam.update(initialFactors, initialValues); + initialFactors = NonlinearFactorGraph; + initialValues = Values; + + isam.calculateEstimate(key_pose); + M = isam.marginalCovariance(key_pose); + + lastSummaryIndex = lastSummaryIndex + 1; + lastSummaryTime = t; + currentSummarizedMeasurement = ImuFactorPreintegratedMeasurements(summaryTemplate); + end + + %% Store data in some structure for statistics and plots + positions(:,i) = currentPoseGlobal.translation.vector; + i = i + 1; +end + +figure(1) +hold on; +plot(positions(1,:), positions(2,:), '-b'); +plot3DTrajectory(isam.calculateEstimate, 'g-'); + + +axis equal; +legend('true trajectory', 'traj integrated in body', 'traj integrated in nav') + + diff --git a/matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m b/matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m new file mode 100644 index 000000000..bcc163eba --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/LatLonHRad_to_ECEF.m @@ -0,0 +1,26 @@ +function pos_ECEF = LatLonHRad_to_ECEF(LatLonH) +% convert latitude, longitude, height to XYZ in ECEF coordinates +% LatLonH(1) : latitude in radian +% LatLonH(2) : longitude in radian +% LatLonH(3) : height in meter +% +% Source: A. Chatfield, "Fundamentals of High Accuracy Inertial +% Navigation", 1997. pp. 10 Eq 1.18 +% + +% constants +a = 6378137.0; %m +e_sqr =0.006694379990141317; +% b = 6356752.3142; % m + +%RAD_PER_DEG = pi/180; +phi = LatLonH(1);%*RAD_PER_DEG; +lambda = LatLonH(2);%*RAD_PER_DEG; +h = LatLonH(3); + +RN = a/sqrt(1 - e_sqr*sin(phi)^2); + +pos_ECEF = zeros(3,1); +pos_ECEF(1) = (RN + h )*cos(phi)*cos(lambda); +pos_ECEF(2) = (RN + h )*cos(phi)*sin(lambda); +pos_ECEF(3) = (RN*(1-e_sqr) + h)*sin(phi) ; \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/OdometryExample3D.m b/matlab/unstable_examples/+imuSimulator/OdometryExample3D.m new file mode 100644 index 000000000..d28d3c2cb --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/OdometryExample3D.m @@ -0,0 +1,68 @@ +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% 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 +% +% @brief Example of a simple 2D localization example +% @author Frank Dellaert +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Copied Original file. Modified by David Jensen to use Pose3 instead of +% Pose2. Everything else is the same. + +import gtsam.* + +%% Assumptions +% - Robot poses are facing along the X axis (horizontal, to the right in 2D) +% - The robot moves 2 meters each step +% - The robot is on a grid, moving 2 meters each step + +%% Create the graph (defined in pose2SLAM.h, derived from NonlinearFactorGraph) +graph = NonlinearFactorGraph; + +%% Add a Gaussian prior on pose x_1 +priorMean = Pose3();%Pose3.Expmap([0.0; 0.0; 0.0; 0.0; 0.0; 0.0]); % prior mean is at origin +priorNoise = noiseModel.Diagonal.Sigmas([0.3; 0.3; 0.3; 0.1; 0.1; 0.1]); % 30cm std on x,y, 0.1 rad on theta +graph.add(PriorFactorPose3(1, priorMean, priorNoise)); % add directly to graph + +%% Add two odometry factors +odometry = Pose3.Expmap([0.0; 0.0; 0.0; 2.0; 0.0; 0.0]); % create a measurement for both factors (the same in this case) +odometryNoise = noiseModel.Diagonal.Sigmas([0.2; 0.2; 0.2; 0.1; 0.1; 0.1]); % 20cm std on x,y, 0.1 rad on theta +graph.add(BetweenFactorPose3(1, 2, odometry, odometryNoise)); +graph.add(BetweenFactorPose3(2, 3, odometry, odometryNoise)); + +%% print +graph.print(sprintf('\nFactor graph:\n')); + +%% Initialize to noisy points +initialEstimate = Values; +%initialEstimate.insert(1, Pose3.Expmap([0.2; 0.1; 0.1; 0.5; 0.0; 0.0])); +%initialEstimate.insert(2, Pose3.Expmap([-0.2; 0.1; -0.1; 2.3; 0.1; 0.1])); +%initialEstimate.insert(3, Pose3.Expmap([0.1; -0.1; 0.1; 4.1; 0.1; -0.1])); +%initialEstimate.print(sprintf('\nInitial estimate:\n ')); + +for i=1:3 + deltaPosition = 0.5*rand(3,1) + [1;0;0]; % create random vector with mean = [1 0 0] and sigma = 0.5 + deltaRotation = 0.1*rand(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] and sigma = 0.1 (rad) + deltaPose = Pose3.Expmap([deltaRotation; deltaPosition]); + currentPose = currentPose.compose(deltaPose); + initialEstimate.insert(i, currentPose); +end + +%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd +optimizer = LevenbergMarquardtOptimizer(graph, initialEstimate); +result = optimizer.optimizeSafely(); +result.print(sprintf('\nFinal result:\n ')); + +%% Plot trajectory and covariance ellipses +cla; +hold on; + +plot3DTrajectory(result, [], Marginals(graph, result)); + +axis([-0.6 4.8 -1 1]) +axis equal +view(2) diff --git a/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m b/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m new file mode 100644 index 000000000..c86e40a21 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/calculateIMUMeas_coriolis.m @@ -0,0 +1,14 @@ +function [ acc_omega ] = calculateIMUMeas_coriolis(omega1Body, omega2Body, velocity1Body, velocity2Body, deltaT) + +import gtsam.*; + +% gyro measured rotation rate +measuredOmega = omega1Body; + +% Acceleration measurement (in this simple toy example no other forces +% act on the body and the only acceleration is the centripetal Coriolis acceleration) +measuredAcc = Point3(cross(omega1Body, velocity1Body)).vector; +acc_omega = [ measuredAcc; measuredOmega ]; + +end + diff --git a/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m b/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m new file mode 100644 index 000000000..534b9365e --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/calculateIMUMeasurement.m @@ -0,0 +1,26 @@ +function [ acc_omega ] = calculateIMUMeasurement(omega1Body, omega2Body, velocity1Body, velocity2Body, deltaT, imuFrame) +%CALCULATEIMUMEASUREMENT Calculate measured body frame acceleration in +%frame 1 and measured angular rates in frame 1. + +import gtsam.*; + +% Calculate gyro measured rotation rate by transforming body rotation rate +% into the IMU frame. +measuredOmega = imuFrame.rotation.unrotate(Point3(omega1Body)).vector; + +% Transform both velocities into IMU frame, accounting for the velocity +% induced by rigid body rotation on a lever arm (Coriolis effect). +velocity1inertial = imuFrame.rotation.unrotate( ... + Point3(velocity1Body + cross(omega1Body, imuFrame.translation.vector))).vector; + +imu2in1 = Rot3.Expmap(measuredOmega * deltaT); +velocity2inertial = imu2in1.rotate(imuFrame.rotation.unrotate( ... + Point3(velocity2Body + cross(omega2Body, imuFrame.translation.vector)))).vector; + +% Acceleration in IMU frame +measuredAcc = (velocity2inertial - velocity1inertial) / deltaT; + +acc_omega = [ measuredAcc; measuredOmega ]; + +end + diff --git a/matlab/unstable_examples/+imuSimulator/coriolisExample.m b/matlab/unstable_examples/+imuSimulator/coriolisExample.m new file mode 100644 index 000000000..35d27aa73 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/coriolisExample.m @@ -0,0 +1,508 @@ +%% coriolisExample.m +% Author(s): David Jensen (david.jensen@gtri.gatech.edu) +% This script demonstrates the relationship between object motion in inertial and rotating reference frames. +% For this example, we consider a fixed (inertial) reference frame located at the center of the earth and initially +% coincident with the rotating ECEF reference frame (X towards 0 longitude, Y towards 90 longitude, Z towards 90 latitude), +% which rotates with the earth. +% A body is moving in the positive Z-direction and positive Y-direction with respect to the fixed reference frame. +% Its position is plotted in both the fixed and rotating reference frames to simulate how an observer in each frame would +% experience the body's motion. + +import gtsam.*; + +addpath(genpath('./Libraries')) + +% Check for an external configuarion. This is useful for setting up +% multiple tests +if ~exist('externalCoriolisConfiguration', 'var') + clc + clear all + close all + %% General configuration + navFrameRotating = 1; % 0 = perform navigation in the fixed frame + % 1 = perform navigation in the rotating frame + IMU_type = 1; % IMU type 1 or type 2 + useRealisticValues = 1; % use reaslist values for initial position and earth rotation + record_movie = 0; % 0 = do not record movie + % 1 = record movie of the trajectories. One + % frame per time step (15 fps) + incrementalPlotting = 1; % turn incremental plotting on and off. Turning plotting off increases + % speed for batch testing + estimationEnabled = 1; + + %% Scenario Configuration + deltaT = 0.01; % timestep + timeElapsed = 5; % Total elapsed time + times = 0:deltaT:timeElapsed; + + % Initial Conditions + omegaEarthSeconds = [0;0;7.292115e-5]; % Earth Rotation rate (rad/s) + radiusEarth = 6378.1*1000; % radius of Earth is 6,378.1 km + + if useRealisticValues == 1 + omegaRotatingFrame = omegaEarthSeconds; % rotation of the moving frame wrt fixed frame + omegaFixed = [0;0;0]; % constant rotation rate measurement + accelFixed = [-0.5;0.5;2]; % constant acceleration measurement + g = [0;0;0]; % Gravity + % initial position at some [longitude, latitude] location on earth's + % surface (approximating Earth as a sphere) + initialLongitude = 45; % longitude in degrees + initialLatitude = 30; % latitude in degrees + initialAltitude = 0; % Altitude above Earth's surface in meters + [x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude); + initialPosition = [x; y; z]; + initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame, + % (ignoring the velocity due to the earth's rotation) + else + omegaRotatingFrame = [0;0;pi/5]; % rotation of the moving frame wrt fixed frame + omegaFixed = [0;0;0]; % constant rotation rate measurement + accelFixed = [0.5;0.5;0.5]; % constant acceleration measurement + g = [0;0;0]; % Gravity + initialPosition = [1; 0; 0];% initial position in both frames + initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation) + end + + if navFrameRotating == 0 + omegaCoriolisIMU = [0;0;0]; + else + omegaCoriolisIMU = omegaRotatingFrame; + end +end + + +% From Wikipedia Angular Velocity page, dr/dt = W*r, where r is +% position vector and W is angular velocity tensor +% We add the initial velocity in the rotating frame because they +% are the same frame at t=0, so no transformation is needed +angularVelocityTensor = [ 0 -omegaRotatingFrame(3) omegaRotatingFrame(2); + omegaRotatingFrame(3) 0 -omegaRotatingFrame(1); + -omegaRotatingFrame(2) omegaRotatingFrame(1) 0 ]; +initialVelocityFixedFrame = angularVelocityTensor * initialPosition + initialVelocity; +initialVelocityRotatingFrame = initialVelocity; + +% +currentRotatingFrame = Pose3; % rotating frame initially coincides with fixed frame at t=0 +currentPoseFixedGT = Pose3(Rot3, Point3(initialPosition)); +currentPoseRotatingGT = currentPoseFixedGT; % frames coincide for t=0 +currentVelocityFixedGT = initialVelocityFixedFrame; +currentVelocityRotatingGT = initialVelocityRotatingFrame; +% +epsBias = 1e-20; +sigma_init_x = noiseModel.Isotropic.Sigma(6, 1e-10); +sigma_init_v = noiseModel.Isotropic.Sigma(3, 1e-10); +sigma_init_b = noiseModel.Isotropic.Sigma(6, epsBias); + +% Imu metadata +zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); % bias is not of interest and is set to zero +IMU_metadata.AccelerometerSigma = 1e-5; +IMU_metadata.GyroscopeSigma = 1e-7; +IMU_metadata.IntegrationSigma = 1e-10; +IMU_metadata.BiasAccelerometerSigma = epsBias; +IMU_metadata.BiasGyroscopeSigma = epsBias; +IMU_metadata.BiasAccOmegaInit = epsBias; + +%% Initialize storage variables +positionsInFixedGT = zeros(3, length(times)); +velocityInFixedGT = zeros(3, length(times)); + +positionsInRotatingGT = zeros(3, length(times)); +velocityInRotatingGT = zeros(3, length(times)); + +positionsEstimates = zeros(3,length(times)); +velocitiesEstimates = zeros(3,length(times)); + +rotationsErrorVectors = zeros(3,length(times)); % Rotating/Fixed frame selected later + +changePoseRotatingFrame = Pose3.Expmap([omegaRotatingFrame*deltaT; 0; 0; 0]); % rotation of the rotating frame at each time step +h = figure; + +% Solver object +isamParams = ISAM2Params; +isamParams.setFactorization('CHOLESKY'); +isamParams.setRelinearizeSkip(10); +isam = gtsam.ISAM2(isamParams); +newFactors = NonlinearFactorGraph; +newValues = Values; + +% Video recording object +if record_movie == 1 + writerObj = VideoWriter('trajectories.avi'); + writerObj.Quality = 100; + writerObj.FrameRate = 15; %10; + open(writerObj); + set(gca,'nextplot','replacechildren'); + set(gcf,'Renderer','zbuffer'); +end + +%% Print Info about the test +fprintf('\n-------------------------------------------------\n'); +if navFrameRotating == 0 + fprintf('Performing navigation in the Fixed frame.\n'); +else + fprintf('Performing navigation in the Rotating frame.\n'); +end +fprintf('Estimation Enabled = %d\n', estimationEnabled); +fprintf('IMU_type = %d\n', IMU_type); +fprintf('Record Movie = %d\n', record_movie); +fprintf('Realistic Values = %d\n', useRealisticValues); +fprintf('deltaT = %f\n', deltaT); +fprintf('timeElapsed = %f\n', timeElapsed); +fprintf('omegaRotatingFrame = [%f %f %f]\n', omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +fprintf('omegaCoriolisIMU = [%f %f %f]\n', omegaCoriolisIMU(1), omegaCoriolisIMU(2), omegaCoriolisIMU(3)); +fprintf('omegaFixed = [%f %f %f]\n', omegaFixed(1), omegaFixed(2), omegaFixed(3)); +fprintf('accelFixed = [%f %f %f]\n', accelFixed(1), accelFixed(2), accelFixed(3)); +fprintf('Initial Velocity = [%f %f %f]\n', initialVelocity(1), initialVelocity(2), initialVelocity(3)); +if(exist('initialLatitude', 'var') && exist('initialLongitude', 'var')) + fprintf('Initial Position\n\t[Long, Lat] = [%f %f] degrees\n\tEFEC = [%f %f %f]\n', ... + initialLongitude, initialLatitude, initialPosition(1), initialPosition(2), initialPosition(3)); +else + fprintf('Initial Position = [%f %f %f]\n', initialPosition(1), initialPosition(2), initialPosition(3)); +end +fprintf('\n'); + +%% Main loop: iterate through the ground truth trajectory, add factors +% and values to the factor graph, and perform inference +for i = 1:length(times) + t = times(i); + + % Create keys for current state + currentPoseKey = symbol('x', i); + currentVelKey = symbol('v', i); + currentBiasKey = symbol('b', i); + + %% Set priors on the first iteration + if(i == 1) + % known initial conditions + currentPoseEstimate = currentPoseFixedGT; + if navFrameRotating == 1 + currentVelocityEstimate = LieVector(currentVelocityRotatingGT); + else + currentVelocityEstimate = LieVector(currentVelocityFixedGT); + end + + % Set Priors + newValues.insert(currentPoseKey, currentPoseEstimate); + newValues.insert(currentVelKey, currentVelocityEstimate); + newValues.insert(currentBiasKey, zeroBias); + % Initial values, same for IMU types 1 and 2 + newFactors.add(PriorFactorPose3(currentPoseKey, currentPoseEstimate, sigma_init_x)); + newFactors.add(PriorFactorLieVector(currentVelKey, currentVelocityEstimate, sigma_init_v)); + newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, sigma_init_b)); + + % Store data + positionsInFixedGT(:,1) = currentPoseFixedGT.translation.vector; + velocityInFixedGT(:,1) = currentVelocityFixedGT; + positionsInRotatingGT(:,1) = currentPoseRotatingGT.translation.vector; + %velocityInRotatingGT(:,1) = currentPoseRotatingGT.velocity.vector; + positionsEstimates(:,i) = currentPoseEstimate.translation.vector; + velocitiesEstimates(:,i) = currentVelocityEstimate.vector; + currentRotatingFrameForPlot(1).p = currentRotatingFrame.translation.vector; + currentRotatingFrameForPlot(1).R = currentRotatingFrame.rotation.matrix; + else + + %% Create ground truth trajectory + % Update the position and velocity + % x_t = x_0 + v_0*dt + 1/2*a_0*dt^2 + % v_t = v_0 + a_0*dt + currentPositionFixedGT = Point3(currentPoseFixedGT.translation.vector ... + + currentVelocityFixedGT * deltaT + 0.5 * accelFixed * deltaT * deltaT); + currentVelocityFixedGT = currentVelocityFixedGT + accelFixed * deltaT; + + currentPoseFixedGT = Pose3(Rot3, currentPositionFixedGT); % constant orientation + + % Rotate pose in fixed frame to get pose in rotating frame + previousPositionRotatingGT = currentPoseRotatingGT.translation.vector; + currentRotatingFrame = currentRotatingFrame.compose(changePoseRotatingFrame); + inverseCurrentRotatingFrame = (currentRotatingFrame.inverse); + currentPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentPoseFixedGT); + currentPositionRotatingGT = currentPoseRotatingGT.translation.vector; + + % Get velocity in rotating frame by treating it like a position and using compose + % Maybe Luca knows a better way to do this within GTSAM. + %currentVelocityPoseFixedGT = Pose3(Rot3, Point3(currentVelocityFixedGT-initialVelocityFixedFrame)); + %currentVelocityPoseRotatingGT = inverseCurrentRotatingFrame.compose(currentVelocityPoseFixedGT); + + %currentRot3RotatingGT = currentRotatingFrame.rotation(); + %currentVelocityRotatingGT = currentRot3RotatingGT.unrotate(Point3(currentVelocityFixedGT)); + % TODO: check if initial velocity in rotating frame is correct + currentVelocityRotatingGT = (currentPositionRotatingGT-previousPositionRotatingGT)/deltaT; + %currentVelocityRotatingGT = (currentPositionRotatingGT - previousPositionRotatingGT ... + % - 0.5 * accelFixed * deltaT * deltaT) / deltaT + accelFixed * deltaT; + + % Store GT (ground truth) poses + positionsInFixedGT(:,i) = currentPoseFixedGT.translation.vector; + velocityInFixedGT(:,i) = currentVelocityFixedGT; + positionsInRotatingGT(:,i) = currentPoseRotatingGT.translation.vector; + velocityInRotatingGT(:,i) = currentVelocityRotatingGT; + currentRotatingFrameForPlot(i).p = currentRotatingFrame.translation.vector; + currentRotatingFrameForPlot(i).R = currentRotatingFrame.rotation.matrix; + + %% Estimate trajectory in rotating frame using GTSAM (ground truth measurements) + % Instantiate preintegrated measurements class + if IMU_type == 1 + currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ... + zeroBias, ... + IMU_metadata.AccelerometerSigma.^2 * eye(3), ... + IMU_metadata.GyroscopeSigma.^2 * eye(3), ... + IMU_metadata.IntegrationSigma.^2 * eye(3)); + elseif IMU_type == 2 + currentSummarizedMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements( ... + zeroBias, ... + IMU_metadata.AccelerometerSigma.^2 * eye(3), ... + IMU_metadata.GyroscopeSigma.^2 * eye(3), ... + IMU_metadata.IntegrationSigma.^2 * eye(3), ... + IMU_metadata.BiasAccelerometerSigma.^2 * eye(3), ... + IMU_metadata.BiasGyroscopeSigma.^2 * eye(3), ... + IMU_metadata.BiasAccOmegaInit.^2 * eye(6)); + else + error('imuSimulator:coriolisExample:IMU_typeNotFound', ... + 'IMU_type = %d does not exist.\nAvailable IMU types are 1 and 2\n', IMU_type); + end + + % Add measurement + currentSummarizedMeasurement.integrateMeasurement(accelFixed, omegaFixed, deltaT); + + % Add factors to graph + if IMU_type == 1 + newFactors.add(ImuFactor( ... + currentPoseKey-1, currentVelKey-1, ... + currentPoseKey, currentVelKey, ... + currentBiasKey-1, currentSummarizedMeasurement, g, omegaCoriolisIMU)); + newFactors.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, zeroBias, ... + noiseModel.Isotropic.Sigma(6, epsBias))); + newFactors.add(PriorFactorConstantBias(currentBiasKey, zeroBias, ... + noiseModel.Isotropic.Sigma(6, epsBias))); + elseif IMU_type == 2 + newFactors.add(CombinedImuFactor( ... + currentPoseKey-1, currentVelKey-1, ... + currentPoseKey, currentVelKey, ... + currentBiasKey-1, currentBiasKey, ... + currentSummarizedMeasurement, g, omegaCoriolisIMU, ... + noiseModel.Isotropic.Sigma(15, epsBias))); + % TODO: prior on biases? + end + + % Add values to the graph. Use the current pose and velocity + % estimates as to values when interpreting the next pose and + % velocity estimates + %ImuFactor.Predict(currentPoseEstimate, currentVelocityEstimate, pose_j, vel_j, zeroBias, currentSummarizedMeasurement, g, omegaCoriolisIMU); + newValues.insert(currentPoseKey, currentPoseEstimate); + newValues.insert(currentVelKey, currentVelocityEstimate); + newValues.insert(currentBiasKey, zeroBias); + + %newFactors.print(''); newValues.print(''); + + %% Solve factor graph + if(i > 1 && estimationEnabled) + isam.update(newFactors, newValues); + newFactors = NonlinearFactorGraph; + newValues = Values; + + % Get the new pose, velocity, and bias estimates + currentPoseEstimate = isam.calculateEstimate(currentPoseKey); + currentVelocityEstimate = isam.calculateEstimate(currentVelKey); + currentBias = isam.calculateEstimate(currentBiasKey); + + positionsEstimates(:,i) = currentPoseEstimate.translation.vector; + velocitiesEstimates(:,i) = currentVelocityEstimate.vector; + biasEstimates(:,i) = currentBias.vector; + + % In matrix form: R_error = R_gt'*R_estimate + % Perform Logmap on the rotation matrix to get a vector + if navFrameRotating == 1 + rotationError = Rot3(currentPoseRotatingGT.rotation.matrix' * currentPoseEstimate.rotation.matrix); + else + rotationError = Rot3(currentPoseFixedGT.rotation.matrix' * currentPoseEstimate.rotation.matrix); + end + + rotationsErrorVectors(:,i) = Rot3.Logmap(rotationError); + end + end + + %% incremental plotting for animation (ground truth) + if incrementalPlotting == 1 + figure(h) + %plot_trajectory(currentRotatingFrameForPlot(i),1, '-k', 'Rotating Frame',0.1,0.75,1) + %hold on; + plot3(positionsInFixedGT(1,1:i), positionsInFixedGT(2,1:i), positionsInFixedGT(3,1:i),'r'); + hold on; + plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr'); + plot3(positionsInFixedGT(1,i), positionsInFixedGT(2,i), positionsInFixedGT(3,i), 'or'); + + plot3(positionsInRotatingGT(1,1:i), positionsInRotatingGT(2,1:i), positionsInRotatingGT(3,1:i), 'g'); + plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg'); + plot3(positionsInRotatingGT(1,i), positionsInRotatingGT(2,i), positionsInRotatingGT(3,i), 'og'); + + if(estimationEnabled) + plot3(positionsEstimates(1,1:i), positionsEstimates(2,1:i), positionsEstimates(3,1:i), 'b'); + plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb'); + plot3(positionsEstimates(1,i), positionsEstimates(2,i), positionsEstimates(3,i), 'ob'); + end + + hold off; + xlabel('X axis') + ylabel('Y axis') + zlabel('Z axis') + axis equal + grid on; + %pause(0.1); + + % record frames for movie + if record_movie == 1 + frame = getframe(gcf); + writeVideo(writerObj,frame); + end + end +end + +if record_movie == 1 + close(writerObj); +end + +% Calculate trajectory length +trajectoryLengthEstimated = 0; +trajectoryLengthFixedFrameGT = 0; +trajectoryLengthRotatingFrameGT = 0; +for i = 2:length(positionsEstimates) + trajectoryLengthEstimated = trajectoryLengthEstimated + norm(positionsEstimates(:,i) - positionsEstimates(:,i-1)); + trajectoryLengthFixedFrameGT = trajectoryLengthFixedFrameGT + norm(positionsInFixedGT(:,i) - positionsInFixedGT(:,i-1)); + trajectoryLengthRotatingFrameGT = trajectoryLengthRotatingFrameGT + norm(positionsInRotatingGT(:,i) - positionsInRotatingGT(:,i-1)); +end + +%% Print and plot error results +% Calculate errors for appropriate navigation scheme +if navFrameRotating == 0 + axisPositionsError = positionsInFixedGT - positionsEstimates; + axisVelocityError = velocityInFixedGT - velocitiesEstimates; +else + axisPositionsError = positionsInRotatingGT - positionsEstimates; + axisVelocityError = velocityInRotatingGT - velocitiesEstimates; +end + +% Plot individual axis position errors +figure +plot(times, axisPositionsError); +plotTitle = sprintf('Axis Error in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... + IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +title(plotTitle); +xlabel('Time [s]'); +ylabel('Error (ground_truth - estimate) [m]'); +legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST'); + +% Plot 3D position error +figure +positionError3D = sqrt(axisPositionsError(1,:).^2+axisPositionsError(2,:).^2 + axisPositionsError(3,:).^2); +plot(times, positionError3D); +plotTitle = sprintf('3D Error in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... + IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +title(plotTitle); +xlabel('Time [s]'); +ylabel('3D error [meters]'); + +% Plot 3D position error +if navFrameRotating == 0 + trajLen = trajectoryLengthFixedFrameGT; +else + trajLen = trajectoryLengthRotatingFrameGT; +end + +figure +plot(times, (positionError3D/trajLen)*100); +plotTitle = sprintf('3D Error normalized by distance in Estimated Position\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... + IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +title(plotTitle); +xlabel('Time [s]'); +ylabel('normalized 3D error [% of distance traveled]'); + +% Plot individual axis velocity errors +figure +plot(times, axisVelocityError); +plotTitle = sprintf('Axis Error in Estimated Velocity\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... + IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +title(plotTitle); +xlabel('Time [s]'); +ylabel('Error (ground_truth - estimate) [m/s]'); +legend('X axis', 'Y axis', 'Z axis', 'Location', 'NORTHWEST'); + +% Plot 3D velocity error +figure +velocityError3D = sqrt(axisVelocityError(1,:).^2+axisVelocityError(2,:).^2 + axisVelocityError(3,:).^2); +plot(times, velocityError3D); +plotTitle = sprintf('3D Error in Estimated Velocity\n(IMU type = %d, omega = [%.2f; %.2f; %.2f])', ... + IMU_type, omegaRotatingFrame(1), omegaRotatingFrame(2), omegaRotatingFrame(3)); +title(plotTitle); +xlabel('Time [s]'); +ylabel('3D error [meters/s]'); + +% Plot magnitude of rotation errors +figure +for i = 1:size(rotationsErrorVectors,2) + rotationsErrorMagnitudes(i) = norm(rotationsErrorVectors(:,i)); +end +plot(times,rotationsErrorMagnitudes); +title('Rotation Error'); +xlabel('Time [s]'); +ylabel('Error [rads]'); + + +% Text output for errors +if navFrameRotating == 0 + fprintf('Fixed Frame ground truth trajectory length is %f [m]\n', trajectoryLengthFixedFrameGT); + fprintf('Estimated trajectory length is %f [m]\n', trajectoryLengthEstimated); + fprintf('Final position error = %f [m](%.4f%% of ground truth trajectory length)\n', ... + norm(axisPositionsError(:,end)), norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100); +else + fprintf('Rotating Frame ground truth trajectory length is %f [m]\n', trajectoryLengthRotatingFrameGT); + fprintf('Estimated trajectory length is %f [m]\n', trajectoryLengthEstimated); + fprintf('Final position error = %f [m](%.4f%% of ground truth trajectory length)\n', ... + norm(axisPositionsError(:,end)), norm(axisPositionsError(:,end))/trajectoryLengthRotatingFrameGT*100); +end +fprintf('Final velocity error = [%f %f %f] [m/s]\n', axisVelocityError(1,end), axisVelocityError(2,end), axisVelocityError(3,end)); +fprintf('Final rotation error = %f [rads]\n', norm(rotationsErrorVectors(:,end))); + +%% Plot final trajectories +figure +[x,y,z] = sphere(30); +if useRealisticValues + mesh(radiusEarth*x,radiusEarth*y, radiusEarth*z) % where (a,b,c) is center of the sphere % sphere for reference +else + mesh(x,y,z); +end +hold on; +axis equal +% Ground truth trajectory in fixed reference frame +plot3(positionsInFixedGT(1,:), positionsInFixedGT(2,:), positionsInFixedGT(3,:),'r','lineWidth',4); +plot3(positionsInFixedGT(1,1), positionsInFixedGT(2,1), positionsInFixedGT(3,1), 'xr','lineWidth',4); +plot3(positionsInFixedGT(1,end), positionsInFixedGT(2,end), positionsInFixedGT(3,end), 'or','lineWidth',4); + +% Ground truth trajectory in rotating reference frame +plot3(positionsInRotatingGT(1,:), positionsInRotatingGT(2,:), positionsInRotatingGT(3,:), '-g','lineWidth',4); +plot3(positionsInRotatingGT(1,1), positionsInRotatingGT(2,1), positionsInRotatingGT(3,1), 'xg','lineWidth',4); +plot3(positionsInRotatingGT(1,end), positionsInRotatingGT(2,end), positionsInRotatingGT(3,end), 'og','lineWidth',4); + +% Estimates +if(estimationEnabled) + plot3(positionsEstimates(1,:), positionsEstimates(2,:), positionsEstimates(3,:), '-b','lineWidth',4); + plot3(positionsEstimates(1,1), positionsEstimates(2,1), positionsEstimates(3,1), 'xb','lineWidth',4); + plot3(positionsEstimates(1,end), positionsEstimates(2,end), positionsEstimates(3,end), 'ob','lineWidth',4); +end + +xlabel('X axis') +ylabel('Y axis') +zlabel('Z axis') +axis equal +grid on; +hold off; + +% TODO: logging rotation errors +%for all time steps +% Rerror = Rgt' * Restimated; +% % transforming rotation matrix to axis-angle representation +% vector_error = Rot3.Logmap(Rerror); +% norm(vector_error) +% +% axis angle: [u,theta], with norm(u)=1 +% vector_error = u * theta; + +% TODO: logging velocity errors +%velocities.. diff --git a/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m new file mode 100644 index 000000000..4d02fd03e --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/coriolisTestMonteCarlo.m @@ -0,0 +1,182 @@ +clc +clear all +close all + +% Flag to mark external configuration to the main test script +externalCoriolisConfiguration = 1; + +%% General configuration +navFrameRotating = 0; % 0 = perform navigation in the fixed frame + % 1 = perform navigation in the rotating frame +IMU_type = 1; % IMU type 1 or type 2 +useRealisticValues = 1; % use reaslist values for initial position and earth rotation +record_movie = 0; % 0 = do not record movie + % 1 = record movie of the trajectories. One + % frame per time step (15 fps) +incrementalPlotting = 0; +estimationEnabled = 1; + +%% Scenario Configuration +deltaT = 0.01; % timestep +timeElapsed = 5; % Total elapsed time +times = 0:deltaT:timeElapsed; + +% Initial Conditions +omegaEarthSeconds = [0;0;7.292115e-5]; % Earth Rotation rate (rad/s) +radiusEarth = 6378.1*1000; % radius of Earth is 6,378.1 km + +if useRealisticValues == 1 + omegaRotatingFrame = omegaEarthSeconds; % rotation of the moving frame wrt fixed frame + omegaFixed = [0;0;0]; % constant rotation rate measurement + accelFixed = [0.1;0;1]; % constant acceleration measurement + g = [0;0;0]; % Gravity + + initialLongitude = 45; % longitude in degrees + initialLatitude = 30; % latitude in degrees + initialAltitude = 0; % Altitude above Earth's surface in meters + [x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude); + initialPosition = [x; y; z]; + + initialVelocity = [0; 0; 0];% initial velocity of the body in the rotating frame, + % (ignoring the velocity due to the earth's rotation) + +else + omegaRotatingFrame = [0;0;pi/300]; % rotation of the moving frame wrt fixed frame + omegaFixed = [0;0;0]; % constant rotation rate measurement + accelFixed = [1;0;0]; % constant acceleration measurement + g = [0;0;0]; % Gravity + initialPosition = [0; 1; 0];% initial position in both frames + initialVelocity = [0;0;0]; % initial velocity in the rotating frame (ignoring the velocity due to the frame's rotation) +end + +%% +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% + +% Run tests with randomly generated initialPosition and accelFixed values +% For each initial position, a number of acceleration vectors are +% generated. For each (initialPosition, accelFixed) pair, the coriolis test +% is run for the following 3 scenarios +% - Navigation performed in fixed frame +% - Navigation performed in rotating frame, including the coriolis effect +% - Navigation performed in rotating frame, ignoring coriolis effect +% + +% Testing configuration +numPosTests = 1; +numAccelTests = 10; +totalNumTests = numPosTests * numAccelTests; + +% Storage variables + +posFinErrorFixed = zeros(numPosTests, numAccelTests); +rotFinErrorFixed = zeros(numPosTests, numAccelTests); +velFinErrorFixed = zeros(numPosTests, numAccelTests); + +posFinErrorRotCoriolis = zeros(numPosTests, numAccelTests); +rotFinErrorRotCoriolis = zeros(numPosTests, numAccelTests); +velFinErrorRotCoriolis = zeros(numPosTests, numAccelTests); + +posFinErrorRot = zeros(numPosTests, numAccelTests); +rotFinErrorRot = zeros(numPosTests, numAccelTests); +velFinErrorRot = zeros(numPosTests, numAccelTests); + + +numErrors = 0; +testIndPos = 1; +testIndAccel = 1; + +while testIndPos <= numPosTests + %generate a random initial position vector + initialLongitude = rand()*360 - 180; % longitude in degrees (-90 to 90) + initialLatitude = rand()*180 - 90; % latitude in degrees (-180 to 180) + initialAltitude = rand()*150; % Altitude above Earth's surface in meters (0-150) + [x, y, z] = sph2cart(initialLongitude * pi/180, initialLatitude * pi/180, radiusEarth + initialAltitude); + initialPosition = [x; y; z]; + + while testIndAccel <= numAccelTests + [testIndPos testIndAccel] + % generate a random acceleration vector + accelFixed = 2*rand(3,1)-ones(3,1); + + %lla = oldTestInfo(testIndPos,testIndAccel).initialPositionLLA; + %initialLongitude = lla(1); + %initialLatitude = lla(2); + %initialAltitude = lla(3); + %initialPosition = oldTestInfo(testIndPos, testIndAccel).initialPositionECEF; + + testInfo(testIndPos, testIndAccel).initialPositionLLA = [initialLongitude, initialLatitude, initialAltitude]; + testInfo(testIndPos, testIndAccel).initialPositionECEF = initialPosition; + testInfo(testIndPos, testIndAccel).accelFixed = accelFixed; + + try + omegaCoriolisIMU = [0;0;0]; + navFrameRotating = 0; + imuSimulator.coriolisExample + posFinErrorFixed(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajectoryLengthFixedFrameGT*100; + rotFinErrorFixed(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end)); + velFinErrorFixed(testIndPos, testIndAccel) = norm(axisVelocityError(:,end)); + testInfo(testIndPos, testIndAccel).fixedPositionError = axisPositionsError(:,end); + testInfo(testIndPos, testIndAccel).fixedVelocityError = axisVelocityError(:,end); + testInfo(testIndPos, testIndAccel).fixedRotationError = rotationsErrorVectors(:,end); + testInfo(testIndPos, testIndAccel).fixedEstTrajLength = trajectoryLengthEstimated; + testInfo(testIndPos, testIndAccel).trajLengthFixedFrameGT = trajectoryLengthFixedFrameGT; + testInfo(testIndPos, testIndAccel).trajLengthRotFrameGT = trajectoryLengthRotatingFrameGT; + + close all; + + % Run the same initial conditions but navigating in the rotating frame + % enable coriolis effect by setting: + omegaCoriolisIMU = omegaRotatingFrame; + navFrameRotating = 1; + imuSimulator.coriolisExample + posFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100; + rotFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end)); + velFinErrorRotCoriolis(testIndPos, testIndAccel) = norm(axisVelocityError(:,end)); + testInfo(testIndPos, testIndAccel).rotCoriolisPositionError = axisPositionsError(:,end); + testInfo(testIndPos, testIndAccel).rotCoriolisVelocityError = axisVelocityError(:,end); + testInfo(testIndPos, testIndAccel).rotCoriolisRotationError = rotationsErrorVectors(:,end); + testInfo(testIndPos, testIndAccel).rotCoriolisEstTrajLength = trajectoryLengthEstimated; + + close all; + + % Run the same initial conditions but navigating in the rotating frame + % disable coriolis effect by setting: + omegaCoriolisIMU = [0;0;0]; + navFrameRotating = 1; + imuSimulator.coriolisExample + posFinErrorRot(testIndPos, testIndAccel) = norm(axisPositionsError(:,end))/trajLen*100; + rotFinErrorRot(testIndPos, testIndAccel) = norm(rotationsErrorVectors(:,end)); + velFinErrorRot(testIndPos, testIndAccel) = norm(axisVelocityError(:,end)); + testInfo(testIndPos, testIndAccel).rotPositionError = axisPositionsError(:,end); + testInfo(testIndPos, testIndAccel).rotVelocityError = axisVelocityError(:,end); + testInfo(testIndPos, testIndAccel).rotRotationError = rotationsErrorVectors(:,end); + testInfo(testIndPos, testIndAccel).rotEstTrajLength = trajectoryLengthEstimated; + + close all; + catch + numErrors = numErrors + 1; + fprintf('*ERROR*'); + fprintf('%d tests cancelled due to errors\n', numErrors); + fprintf('restarting test with new accelFixed'); + testIndAccel = testIndAccel - 1; + end + testIndAccel = testIndAccel + 1; + end + testIndAccel = 1; + testIndPos = testIndPos + 1; +end +fprintf('\nTotal: %d tests cancelled due to errors\n', numErrors); + +mean(posFinErrorFixed); +max(posFinErrorFixed); +% box(posFinErrorFixed); + +mean(posFinErrorRotCoriolis); +max(posFinErrorRotCoriolis); +% box(posFinErrorRotCoriolis); + +mean(posFinErrorRot); +max(posFinErrorRot); +% box(posFinErrorRot); + + diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m new file mode 100644 index 000000000..2eddf75ee --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisBetween.m @@ -0,0 +1,325 @@ +import gtsam.*; + +% Test GTSAM covariances on a factor graph with: +% Between Factors +% IMU factors (type 1 and type 2) +% GPS prior factors on poses +% SmartProjectionPoseFactors +% Authors: Luca Carlone, David Jensen +% Date: 2014/4/6 + + +% Check for an extneral configuration, used when running multiple tests +if ~exist('externallyConfigured', 'var') + clc + clear all + close all + + saveResults = 0; + + %% Configuration + % General options + options.useRealData = 1; % controls whether or not to use the real data (if available) as the ground truth traj + options.includeBetweenFactors = 0; % if true, BetweenFactors will be added between consecutive poses + + options.includeIMUFactors = 1; % if true, IMU factors will be added between consecutive states (biases, poses, velocities) + options.imuFactorType = 1; % Set to 1 or 2 to use IMU type 1 or type 2 factors (will default to type 1) + options.imuNonzeroBias = 0; % if true, a nonzero bias is applied to IMU measurements + + options.includeCameraFactors = 1; % if true, SmartProjectionPose3Factors will be used with randomly generated landmarks + options.numberOfLandmarks = 1000; % Total number of visual landmarks (randomly generated in a box around the trajectory) + + options.includeGPSFactors = 0; % if true, GPS factors will be added as priors to poses + options.gpsStartPose = 100; % Pose number to start including GPS factors at + + options.trajectoryLength = 100;%209; % length of the ground truth trajectory + options.subsampleStep = 20; % number of poses to skip when using real data (to reduce computation on long trajectories) + + numMonteCarloRuns = 2; % number of Monte Carlo runs to perform + + % Noise values to be adjusted + sigma_ang = 1e-2; % std. deviation for rotational noise, typical 1e-2 + sigma_cart = 1e-1; % std. deviation for translational noise, typical 1e-1 + sigma_accel = 1e-3; % std. deviation for accelerometer noise, typical 1e-3 + sigma_gyro = 1e-5; % std. deviation for gyroscope noise, typical 1e-5 + sigma_accelBias = 1e-4; % std. deviation for added accelerometer constant bias, typical 1e-3 + sigma_gyroBias = 1e-6; % std. deviation for added gyroscope constant bias, typical 1e-5 + sigma_gps = 1e-4; % std. deviation for noise in GPS position measurements, typical 1e-4 + sigma_camera = 1; % std. deviation for noise in camera measurements (pixels) + + % Set log files + testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro) + folderName = 'results/' +else + fprintf('Tests have been externally configured.\n'); +end + +%% Between metadata +noiseVectorPose = [sigma_ang * ones(3,1); sigma_cart * ones(3,1)]; +noisePose = noiseModel.Diagonal.Sigmas(noiseVectorPose); + +%% Imu metadata +metadata.imu.epsBias = 1e-10; % was 1e-7 +metadata.imu.g = [0;0;0]; +metadata.imu.omegaCoriolis = [0;0;0]; +metadata.imu.IntegrationSigma = 1e-5; +metadata.imu.zeroBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1)); +metadata.imu.AccelerometerSigma = sigma_accel; +metadata.imu.GyroscopeSigma = sigma_gyro; +metadata.imu.BiasAccelerometerSigma = metadata.imu.epsBias; % noise on expected change in accelerometer bias over time +metadata.imu.BiasGyroscopeSigma = metadata.imu.epsBias; % noise on expected change in gyroscope bias over time +% noise on initial accelerometer and gyroscope biases +if options.imuNonzeroBias == 1 + metadata.imu.BiasAccOmegaInit = [sigma_accelBias * ones(3,1); sigma_gyroBias * ones(3,1)]; +else + metadata.imu.BiasAccOmegaInit = metadata.imu.epsBias * ones(6,1); +end + +noiseVel = noiseModel.Isotropic.Sigma(3, 1e-2); % was 0.1 +noiseBiasBetween = noiseModel.Diagonal.Sigmas([metadata.imu.BiasAccelerometerSigma * ones(3,1);... + metadata.imu.BiasGyroscopeSigma * ones(3,1)]); % between on biases +noisePriorBias = noiseModel.Diagonal.Sigmas(metadata.imu.BiasAccOmegaInit); + +noiseVectorAccel = metadata.imu.AccelerometerSigma * ones(3,1); +noiseVectorGyro = metadata.imu.GyroscopeSigma * ones(3,1); + +%% GPS metadata +noiseVectorGPS = sigma_gps * ones(3,1); +noiseGPS = noiseModel.Diagonal.Precisions([zeros(3,1); 1/sigma_gps^2 * ones(3,1)]); + +%% Camera metadata +metadata.camera.calibration = Cal3_S2(500,500,0,1920/2,1200/2); % Camera calibration +metadata.camera.xlims = [-100, 650]; % x limits on area for landmark creation +metadata.camera.ylims = [-100, 700]; % y limits on area for landmark creation +metadata.camera.zlims = [-30, 30]; % z limits on area for landmark creation +metadata.camera.visualRange = 100; % maximum distance from the camera that a landmark can be seen (meters) +metadata.camera.bodyPoseCamera = Pose3; % pose of camera in body +metadata.camera.CameraSigma = sigma_camera; +cameraMeasurementNoise = noiseModel.Isotropic.Sigma(2, metadata.camera.CameraSigma); +noiseVectorCamera = metadata.camera.CameraSigma .* ones(2,1); + +% Create landmarks and smart factors +if options.includeCameraFactors == 1 + for i = 1:options.numberOfLandmarks + metadata.camera.gtLandmarkPoints(i) = Point3( ... + [rand() * (metadata.camera.xlims(2)-metadata.camera.xlims(1)) + metadata.camera.xlims(1); ... + rand() * (metadata.camera.ylims(2)-metadata.camera.ylims(1)) + metadata.camera.ylims(1); ... + rand() * (metadata.camera.zlims(2)-metadata.camera.zlims(1)) + metadata.camera.zlims(1)]); + end +end + + +%% Create ground truth trajectory and measurements +[gtValues, gtMeasurements] = imuSimulator.covarianceAnalysisCreateTrajectory(options, metadata); + +%% Create ground truth graph +% Set up noise models +gtNoiseModels.noisePose = noisePose; +gtNoiseModels.noiseVel = noiseVel; +gtNoiseModels.noiseBiasBetween = noiseBiasBetween; +gtNoiseModels.noisePriorPose = noisePose; +gtNoiseModels.noisePriorBias = noisePriorBias; +gtNoiseModels.noiseGPS = noiseGPS; +gtNoiseModels.noiseCamera = cameraMeasurementNoise; + +% Set measurement noise to 0, because this is ground truth +gtMeasurementNoise.poseNoiseVector = zeros(6,1); +gtMeasurementNoise.imu.accelNoiseVector = zeros(3,1); +gtMeasurementNoise.imu.gyroNoiseVector = zeros(3,1); +gtMeasurementNoise.cameraNoiseVector = zeros(2,1); +gtMeasurementNoise.gpsNoiseVector = zeros(3,1); + +% Set IMU biases to zero +metadata.imu.accelConstantBiasVector = zeros(3,1); +metadata.imu.gyroConstantBiasVector = zeros(3,1); + +[gtGraph, projectionFactorSeenBy] = imuSimulator.covarianceAnalysisCreateFactorGraph( ... + gtMeasurements, ... % ground truth measurements + gtValues, ... % ground truth Values + gtNoiseModels, ... % noise models to use in this graph + gtMeasurementNoise, ... % noise to apply to measurements + options, ... % options for the graph (e.g. which factors to include) + metadata); % misc data necessary for factor creation + +%% Display, printing, and plotting of ground truth +%gtGraph.print(sprintf('\nGround Truth Factor graph:\n')); +%gtValues.print(sprintf('\nGround Truth Values:\n ')); + +figure(1) +hold on; + +if options.includeCameraFactors + b = [-1000 2000 -2000 2000 -30 30]; + for i = 1:size(metadata.camera.gtLandmarkPoints,2) + p = metadata.camera.gtLandmarkPoints(i).vector; + if(p(1) > b(1) && p(1) < b(2) && p(2) > b(3) && p(2) < b(4) && p(3) > b(5) && p(3) < b(6)) + plot3(p(1), p(2), p(3), 'k+'); + end + end + pointsToPlot = metadata.camera.gtLandmarkPoints(find(projectionFactorSeenBy > 0)); + for i = 1:length(pointsToPlot) + p = pointsToPlot(i).vector; + plot3(p(1), p(2), p(3), 'gs', 'MarkerSize', 10); + end +end +plot3DPoints(gtValues); +%plot3DTrajectory(gtValues, '-r', [], 1, Marginals(gtGraph, gtValues)); +plot3DTrajectory(gtValues, '-r'); + +axis equal + +% optimize +optimizer = GaussNewtonOptimizer(gtGraph, gtValues); +gtEstimate = optimizer.optimize(); +plot3DTrajectory(gtEstimate, '-k'); +% estimate should match gtValues if graph is correct. +fprintf('Error in ground truth graph at gtValues: %g \n', gtGraph.error(gtValues) ); +fprintf('Error in ground truth graph at gtEstimate: %g \n', gtGraph.error(gtEstimate) ); + +disp('Plotted ground truth') + +%% Monte Carlo Runs + +% Set up noise models +monteCarloNoiseModels.noisePose = noisePose; +monteCarloNoiseModels.noiseVel = noiseVel; +monteCarloNoiseModels.noiseBiasBetween = noiseBiasBetween; +monteCarloNoiseModels.noisePriorPose = noisePose; +monteCarloNoiseModels.noisePriorBias = noisePriorBias; +monteCarloNoiseModels.noiseGPS = noiseGPS; +monteCarloNoiseModels.noiseCamera = cameraMeasurementNoise; + +% Set measurement noise for monte carlo runs +monteCarloMeasurementNoise.poseNoiseVector = zeros(6,1); %noiseVectorPose; +monteCarloMeasurementNoise.imu.accelNoiseVector = noiseVectorAccel; +monteCarloMeasurementNoise.imu.gyroNoiseVector = noiseVectorGyro; +monteCarloMeasurementNoise.gpsNoiseVector = noiseVectorGPS; +monteCarloMeasurementNoise.cameraNoiseVector = noiseVectorCamera; + +for k=1:numMonteCarloRuns + fprintf('Monte Carlo Run %d...\n', k'); + + % Create a random bias for each run + if options.imuNonzeroBias == 1 + metadata.imu.accelConstantBiasVector = metadata.imu.BiasAccOmegaInit(1:3) .* randn(3,1); + metadata.imu.gyroConstantBiasVector = metadata.imu.BiasAccOmegaInit(4:6) .* randn(3,1); + %metadata.imu.accelConstantBiasVector = 1e-2 * ones(3,1); + %metadata.imu.gyroConstantBiasVector = 1e-3 * ones(3,1); + else + metadata.imu.accelConstantBiasVector = zeros(3,1); + metadata.imu.gyroConstantBiasVector = zeros(3,1); + end + + % Create a new graph using noisy measurements + [graph, projectionFactorSeenBy] = imuSimulator.covarianceAnalysisCreateFactorGraph( ... + gtMeasurements, ... % ground truth measurements + gtValues, ... % ground truth Values + monteCarloNoiseModels, ... % noise models to use in this graph + monteCarloMeasurementNoise, ... % noise to apply to measurements + options, ... % options for the graph (e.g. which factors to include) + metadata); % misc data necessary for factor creation + + %graph.print('graph') + + % optimize + optimizer = GaussNewtonOptimizer(graph, gtValues); + estimate = optimizer.optimize(); + figure(1) + plot3DTrajectory(estimate, '-b'); + + marginals = Marginals(graph, estimate); + + % for each pose in the trajectory + for i=0:options.trajectoryLength + % compute estimation errors + currentPoseKey = symbol('x', i); + gtPosition = gtValues.at(currentPoseKey).translation.vector; + estPosition = estimate.at(currentPoseKey).translation.vector; + estR = estimate.at(currentPoseKey).rotation.matrix; + errPosition = estPosition - gtPosition; + + % compute covariances: + cov = marginals.marginalCovariance(currentPoseKey); + covPosition = estR * cov(4:6,4:6) * estR'; + % compute NEES using (estimationError = estimatedValues - gtValues) and estimated covariances + NEES(k,i+1) = errPosition' * inv(covPosition) * errPosition; % distributed according to a Chi square with n = 3 dof + end + + figure(2) + hold on + plot(NEES(k,:),'-b','LineWidth',1.5) +end +%% +ANEES = mean(NEES); +plot(ANEES,'-r','LineWidth',2) +plot(3*ones(size(ANEES,2),1),'k--'); % Expectation(ANEES) = number of dof +box on +set(gca,'Fontsize',16) +title('NEES and ANEES'); +if saveResults + saveas(gcf,horzcat(folderName,'runs-',testName,'.fig'),'fig'); + saveas(gcf,horzcat(folderName,'runs-',testName,'.png'),'png'); +end + +%% +figure(1) +box on +set(gca,'Fontsize',16) +title('Ground truth and estimates for each MC runs'); +if saveResults + saveas(gcf,horzcat(folderName,'gt-',testName,'.fig'),'fig'); + saveas(gcf,horzcat(folderName,'gt-',testName,'.png'),'png'); +end + +%% Let us compute statistics on the overall NEES +n = 3; % position vector dimension +N = numMonteCarloRuns; % number of runs +alpha = 0.01; % confidence level + +% mean_value = n*N; % mean value of the Chi-square distribution +% (we divide by n * N and for this reason we expect ANEES around 1) +r1 = chi2inv(alpha, n * N) / (n * N); +r2 = chi2inv(1-alpha, n * N) / (n * N); + +% output here +fprintf(1, 'r1 = %g\n', r1); +fprintf(1, 'r2 = %g\n', r2); + +figure(3) +hold on +plot(ANEES/n,'-b','LineWidth',2) +plot(ones(size(ANEES,2),1),'r-'); +plot(r1*ones(size(ANEES,2),1),'k-.'); +plot(r2*ones(size(ANEES,2),1),'k-.'); +box on +set(gca,'Fontsize',16) +title('NEES normalized by dof VS bounds'); +if saveResults + saveas(gcf,horzcat(folderName,'ANEES-',testName,'.fig'),'fig'); + saveas(gcf,horzcat(folderName,'ANEES-',testName,'.png'),'png'); + logFile = horzcat(folderName,'log-',testName); + save(logFile) +end + +%% NEES COMPUTATION (Bar-Shalom 2001, Section 5.4) +% the nees for a single experiment (i) is defined as +% NEES_i = xtilda' * inv(P) * xtilda, +% where xtilda in R^n is the estimation +% error, and P is the covariance estimated by the approach we want to test +% +% Average NEES. Given N Monte Carlo simulations, i=1,...,N, the average +% NEES is: +% ANEES = sum(NEES_i)/N +% The quantity N*ANEES is distributed according to a Chi-square +% distribution with N*n degrees of freedom. +% +% For the single run case, N=1, therefore NEES = ANEES is distributed +% according to a chi-square distribution with n degrees of freedom (e.g. n=3 +% if we are testing a position estimate) +% Therefore its mean should be n (difficult to see from a single run) +% and, with probability alpha, it should hold: +% +% NEES in [r1, r2] +% +% where r1 and r2 are built from the Chi-square distribution + diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m new file mode 100644 index 000000000..00ae4b9c2 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateFactorGraph.m @@ -0,0 +1,199 @@ +function [ graph projectionFactorSeenBy] = covarianceAnalysisCreateFactorGraph( measurements, values, noiseModels, measurementNoise, options, metadata) +% Create a factor graph based on provided measurements, values, and noises. +% Used for covariance analysis scripts. +% 'options' contains fields for including various factor types. +% 'metadata' is a storage variable for miscellaneous factor-specific values +% Authors: Luca Carlone, David Jensen +% Date: 2014/04/16 + +import gtsam.*; + +graph = NonlinearFactorGraph; + +% Iterate through the trajectory +for i=0:length(measurements) + % Get the current pose + currentPoseKey = symbol('x', i); + currentPose = values.at(currentPoseKey); + + if i==0 + %% first time step, add priors + % Pose prior (poses used for all factors) + noisyInitialPoseVector = Pose3.Logmap(currentPose) + measurementNoise.poseNoiseVector .* randn(6,1); + initialPose = Pose3.Expmap(noisyInitialPoseVector); + graph.add(PriorFactorPose3(currentPoseKey, initialPose, noiseModels.noisePose)); + + % IMU velocity and bias priors + if options.includeIMUFactors == 1 + currentVelKey = symbol('v', 0); + currentVel = values.at(currentVelKey).vector; + graph.add(PriorFactorLieVector(currentVelKey, LieVector(currentVel), noiseModels.noiseVel)); + + currentBiasKey = symbol('b', 0); + currentBias = values.at(currentBiasKey); + graph.add(PriorFactorConstantBias(currentBiasKey, currentBias, noiseModels.noisePriorBias)); + end + + %% Create a SmartProjectionFactor for each landmark + projectionFactorSeenBy = []; + if options.includeCameraFactors == 1 + for j=1:options.numberOfLandmarks + SmartProjectionFactors(j) = SmartProjectionPose3Factor(0.01); + % Use constructor with default values, but express the pose of the + % camera as a 90 degree rotation about the X axis +% SmartProjectionFactors(j) = SmartProjectionPose3Factor( ... +% 1, ... % rankTol +% -1, ... % linThreshold +% false, ... % manageDegeneracy +% false, ... % enableEPI +% metadata.camera.bodyPoseCamera); % Pose of camera in body frame + end + projectionFactorSeenBy = zeros(options.numberOfLandmarks,1); + end + + else + + %% Add Between factors + if options.includeBetweenFactors == 1 + prevPoseKey = symbol('x', i-1); + % Create the noisy pose estimate + deltaPose = Pose3.Expmap(measurements(i).deltaVector ... + + (measurementNoise.poseNoiseVector .* randn(6,1))); % added noise + % Add the between factor to the graph + graph.add(BetweenFactorPose3(prevPoseKey, currentPoseKey, deltaPose, noiseModels.noisePose)); + end % end of Between pose factor creation + + %% Add IMU factors + if options.includeIMUFactors == 1 + % Update keys + currentVelKey = symbol('v', i); % not used if includeIMUFactors is false + currentBiasKey = symbol('b', i); % not used if includeIMUFactors is false + + if options.imuFactorType == 1 + use2ndOrderIntegration = true; + % Initialize preintegration + imuMeasurement = gtsam.ImuFactorPreintegratedMeasurements(... + metadata.imu.zeroBias, ... + metadata.imu.AccelerometerSigma.^2 * eye(3), ... + metadata.imu.GyroscopeSigma.^2 * eye(3), ... + metadata.imu.IntegrationSigma.^2 * eye(3), ... + use2ndOrderIntegration); + % Generate IMU measurements with noise + for j=1:length(measurements(i).imu) % all measurements to preintegrate + accelNoise = (measurementNoise.imu.accelNoiseVector .* randn(3,1)); + imuAccel = measurements(i).imu(j).accel ... + + accelNoise ... % added noise + + metadata.imu.accelConstantBiasVector; % constant bias + + gyroNoise = (measurementNoise.imu.gyroNoiseVector .* randn(3,1)); + imuGyro = measurements(i).imu(j).gyro ... + + gyroNoise ... % added noise + + metadata.imu.gyroConstantBiasVector; % constant bias + + % Used for debugging + %fprintf(' A: (meas)[%f %f %f] + (noise)[%f %f %f] + (bias)[%f %f %f] = [%f %f %f]\n', ... + % measurements(i).imu(j).accel(1), measurements(i).imu(j).accel(2), measurements(i).imu(j).accel(3), ... + % accelNoise(1), accelNoise(2), accelNoise(3), ... + % metadata.imu.accelConstantBiasVector(1), metadata.imu.accelConstantBiasVector(2), metadata.imu.accelConstantBiasVector(3), ... + % imuAccel(1), imuAccel(2), imuAccel(3)); + %fprintf(' G: (meas)[%f %f %f] + (noise)[%f %f %f] + (bias)[%f %f %f] = [%f %f %f]\n', ... + % measurements(i).imu(j).gyro(1), measurements(i).imu(j).gyro(2), measurements(i).imu(j).gyro(3), ... + % gyroNoise(1), gyroNoise(2), gyroNoise(3), ... + % metadata.imu.gyroConstantBiasVector(1), metadata.imu.gyroConstantBiasVector(2), metadata.imu.gyroConstantBiasVector(3), ... + % imuGyro(1), imuGyro(2), imuGyro(3)); + + % Preintegrate + imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT); + end + %imuMeasurement.print('imuMeasurement'); + + % Add Imu factor + graph.add(ImuFactor(currentPoseKey-1, currentVelKey-1, currentPoseKey, currentVelKey, ... + currentBiasKey-1, imuMeasurement, metadata.imu.g, metadata.imu.omegaCoriolis)); + % Add between factor on biases + graph.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, metadata.imu.zeroBias, ... + noiseModels.noiseBiasBetween)); + end + + if options.imuFactorType == 2 + use2ndOrderIntegration = true; + % Initialize preintegration + imuMeasurement = gtsam.CombinedImuFactorPreintegratedMeasurements(... + metadata.imu.zeroBias, ... + metadata.imu.AccelerometerSigma.^2 * eye(3), ... + metadata.imu.GyroscopeSigma.^2 * eye(3), ... + metadata.imu.IntegrationSigma.^2 * eye(3), ... + metadata.imu.BiasAccelerometerSigma.^2 * eye(3), ... % how bias changes over time + metadata.imu.BiasGyroscopeSigma.^2 * eye(3), ... % how bias changes over time + diag(metadata.imu.BiasAccOmegaInit.^2), ... % prior on bias + use2ndOrderIntegration); + % Generate IMU measurements with noise + for j=1:length(measurements(i).imu) % all measurements to preintegrate + imuAccel = measurements(i).imu(j).accel ... + + (measurementNoise.imu.accelNoiseVector .* randn(3,1))... % added noise + + metadata.imu.accelConstantBiasVector; % constant bias + imuGyro = measurements(i).imu(j).gyro ... + + (measurementNoise.imu.gyroNoiseVector .* randn(3,1))... % added noise + + metadata.imu.gyroConstantBiasVector; % constant bias + + % Preintegrate + imuMeasurement.integrateMeasurement(imuAccel, imuGyro, measurements(i).imu(j).deltaT); + end + + % Add Imu factor + graph.add(CombinedImuFactor( ... + currentPoseKey-1, currentVelKey-1, ... + currentPoseKey, currentVelKey, ... + currentBiasKey-1, currentBiasKey, ... + imuMeasurement, ... + metadata.imu.g, metadata.imu.omegaCoriolis)); + end + + end % end of IMU factor creation + + %% Build Camera Factors + if options.includeCameraFactors == 1 + for j = 1:length(measurements(i).landmarks) + cameraMeasurmentNoise = measurementNoise.cameraNoiseVector .* randn(2,1); + cameraPixelMeasurement = measurements(i).landmarks(j).vector; + % Only add the measurement if it is in the image frame (based on calibration) + if(cameraPixelMeasurement(1) > 0 && cameraPixelMeasurement(2) > 0 ... + && cameraPixelMeasurement(1) < 2*metadata.camera.calibration.px ... + && cameraPixelMeasurement(2) < 2*metadata.camera.calibration.py) + cameraPixelMeasurement = cameraPixelMeasurement + cameraMeasurmentNoise; + SmartProjectionFactors(j).add( ... + Point2(cameraPixelMeasurement), ... + currentPoseKey, noiseModels.noiseCamera, ... + metadata.camera.calibration); + projectionFactorSeenBy(j) = projectionFactorSeenBy(j)+1; + end + end + end % end of Camera factor creation + + %% Add GPS factors + if options.includeGPSFactors == 1 && i >= options.gpsStartPose + gpsPosition = measurements(i).gpsPositionVector ... + + measurementNoise.gpsNoiseVector .* randn(3,1); + graph.add(PriorFactorPose3(currentPoseKey, ... + Pose3(currentPose.rotation, Point3(gpsPosition)), ... + noiseModels.noiseGPS)); + end % end of GPS factor creation + + end % end of else (i=0) + +end % end of for over trajectory + +%% Add Camera Factors to the graph +% Only factors for landmarks that have been viewed at least once are added +% to the graph +%[find(projectionFactorSeenBy ~= 0) projectionFactorSeenBy(find(projectionFactorSeenBy ~= 0))] +if options.includeCameraFactors == 1 + for j = 1:options.numberOfLandmarks + if projectionFactorSeenBy(j) > 0 + graph.add(SmartProjectionFactors(j)); + end + end +end + +end % end of function + diff --git a/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m new file mode 100644 index 000000000..2cceea753 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/covarianceAnalysisCreateTrajectory.m @@ -0,0 +1,150 @@ +function [values, measurements] = covarianceAnalysisCreateTrajectory( options, metadata ) +% Create a trajectory for running covariance analysis scripts. +% 'options' contains fields for including various factor types and setting trajectory length +% 'metadata' is a storage variable for miscellaneous factor-specific values +% Authors: Luca Carlone, David Jensen +% Date: 2014/04/16 + +import gtsam.*; + +values = Values; + +warning('Rotating Pose inside getPoseFromGtScenario! TODO: Use a body_P_sensor transform in the factors') + + +if options.useRealData == 1 + %% Create a ground truth trajectory from Real data (if available) + fprintf('\nUsing real data as ground truth\n'); + gtScenario = load('truth_scen2.mat', 'Time', 'Lat', 'Lon', 'Alt', 'Roll', 'Pitch', 'Heading',... + 'VEast', 'VNorth', 'VUp'); + + % Limit the trajectory length + options.trajectoryLength = min([length(gtScenario.Lat)/options.subsampleStep options.trajectoryLength]); + fprintf('Scenario Ind: '); + + for i=0:options.trajectoryLength + scenarioInd = options.subsampleStep * i + 1; + fprintf('%d, ', scenarioInd); + if (mod(i,12) == 0) fprintf('\n'); end + + %% Generate the current pose + currentPoseKey = symbol('x', i); + currentPose = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd); + + %% FOR TESTING - this is currently moved to getPoseFromGtScenario + %currentPose = currentPose.compose(metadata.camera.bodyPoseCamera); + %currentPose = currentPose.compose(Pose3.Expmap([-pi/2;0;0;0;0;0])); + + % add to values + values.insert(currentPoseKey, currentPose); + + %% gt Between measurements + if options.includeBetweenFactors == 1 && i > 0 + prevPose = values.at(currentPoseKey - 1); + deltaPose = prevPose.between(currentPose); + measurements(i).deltaVector = Pose3.Logmap(deltaPose); + end + + %% gt IMU measurements + if options.includeIMUFactors == 1 + currentVelKey = symbol('v', i); + currentBiasKey = symbol('b', i); + deltaT = 1; % amount of time between IMU measurements + if i == 0 + currentVel = [0 0 0]'; + else + % integrate & store intermediate measurements + for j=1:options.subsampleStep % we integrate all the intermediate measurements + previousScenarioInd = options.subsampleStep * (i-1) + 1; + scenarioIndIMU1 = previousScenarioInd+j-1; + scenarioIndIMU2 = previousScenarioInd+j; + IMUPose1 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU1); + IMUPose2 = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioIndIMU2); + IMURot1 = IMUPose1.rotation.matrix; + + IMUdeltaPose = IMUPose1.between(IMUPose2); + IMUdeltaPoseVector = Pose3.Logmap(IMUdeltaPose); + IMUdeltaRotVector = IMUdeltaPoseVector(1:3); + IMUdeltaPositionVector = IMUPose2.translation.vector - IMUPose1.translation.vector; % translation in absolute frame + + measurements(i).imu(j).deltaT = deltaT; + + % gyro rate: Logmap(R_i' * R_i+1) / deltaT + measurements(i).imu(j).gyro = IMUdeltaRotVector./deltaT; + + % deltaPij += deltaVij * deltaT + 0.5 * deltaRij.matrix() * biasHat.correctAccelerometer(measuredAcc) * deltaT*deltaT; + % acc = (deltaPosition - initialVel * dT) * (2/dt^2) + measurements(i).imu(j).accel = IMURot1' * (IMUdeltaPositionVector - currentVel.*deltaT).*(2/(deltaT*deltaT)); + + % Update velocity + currentVel = currentVel + IMURot1 * measurements(i).imu(j).accel * deltaT; + end + end + + % Add Values: velocity and bias + values.insert(currentVelKey, LieVector(currentVel)); + values.insert(currentBiasKey, metadata.imu.zeroBias); + end + + %% gt GPS measurements + if options.includeGPSFactors == 1 && i > 0 + gpsPositionVector = imuSimulator.getPoseFromGtScenario(gtScenario,scenarioInd).translation.vector; + measurements(i).gpsPositionVector = gpsPositionVector; + end + + %% gt Camera measurements + if options.includeCameraFactors == 1 && i > 0 + % Create the camera based on the current pose and the pose of the + % camera in the body + cameraPose = currentPose.compose(metadata.camera.bodyPoseCamera); + camera = SimpleCamera(cameraPose, metadata.camera.calibration); + %camera = SimpleCamera(currentPose, metadata.camera.calibration); + + % Record measurements if the landmark is within visual range + for j=1:length(metadata.camera.gtLandmarkPoints) + distanceToLandmark = camera.pose.range(metadata.camera.gtLandmarkPoints(j)); + if distanceToLandmark < metadata.camera.visualRange + try + z = camera.project(metadata.camera.gtLandmarkPoints(j)); + measurements(i).landmarks(j) = z; + catch + % point is probably out of the camera's view + end + end + end + end + + end + fprintf('\n'); +else + error('Please use RealData') + %% Create a random trajectory as ground truth + currentPose = Pose3; % initial pose % initial pose + + unsmooth_DP = 0.5; % controls smoothness on translation norm + unsmooth_DR = 0.1; % controls smoothness on rotation norm + + fprintf('\nCreating a random ground truth trajectory\n'); + currentPoseKey = symbol('x', 0); + values.insert(currentPoseKey, currentPose); + + for i=1:options.trajectoryLength + % Update the pose key + currentPoseKey = symbol('x', i); + + % Generate the new measurements + gtDeltaPosition = unsmooth_DP*randn(3,1) + [20;0;0]; % create random vector with mean = [20 0 0] + gtDeltaRotation = unsmooth_DR*randn(3,1) + [0;0;0]; % create random rotation with mean [0 0 0] + measurements.deltaMatrix(i,:) = [gtDeltaRotation; gtDeltaPosition]; + + % Create the next pose + deltaPose = Pose3.Expmap(measurements.deltaMatrix(i,:)'); + currentPose = currentPose.compose(deltaPose); + + % Add the current pose as a value + values.insert(currentPoseKey, currentPose); + end % end of random trajectory creation +end % end of else + +end % end of function + diff --git a/matlab/unstable_examples/+imuSimulator/ct2ENU.m b/matlab/unstable_examples/+imuSimulator/ct2ENU.m new file mode 100644 index 000000000..b7257b664 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/ct2ENU.m @@ -0,0 +1,55 @@ +function [dx,dy,dz]=ct2ENU(dX,dY,dZ,lat,lon) +% CT2LG Converts CT coordinate differences to local geodetic. +% Local origin at lat,lon,h. If lat,lon are vectors, dx,dy,dz +% are referenced to orgin at lat,lon of same index. If +% astronomic lat,lon input, output is in local astronomic +% system. Vectorized in both dx,dy,dz and lat,lon. See also +% LG2CT. +% Version: 2011-02-19 +% Useage: [dx,dy,dz]=ct2lg(dX,dY,dZ,lat,lon) +% Input: dX - vector of X coordinate differences in CT +% dY - vector of Y coordinate differences in CT +% dZ - vector of Z coordinate differences in CT +% lat - lat(s) of local system origin (rad); may be vector +% lon - lon(s) of local system origin (rad); may be vector +% Output: dx - vector of x coordinates in local system (east) +% dy - vector of y coordinates in local system (north) +% dz - vector of z coordinates in local system (up) + +% Copyright (c) 2011, Michael R. Craymer +% All rights reserved. +% Email: mike@craymer.com + +if nargin ~= 5 + warning('Incorrect number of input arguements'); + return +end + +n=length(dX); +if length(lat)==1 + lat=ones(n,1)*lat; + lon=ones(n,1)*lon; +end +R=zeros(3,3,n); + +R(1,1,:)=-sin(lat').*cos(lon'); +R(1,2,:)=-sin(lat').*sin(lon'); +R(1,3,:)=cos(lat'); +R(2,1,:)=sin(lon'); +R(2,2,:)=-cos(lon'); +R(2,3,:)=zeros(1,n); +R(3,1,:)=cos(lat').*cos(lon'); +R(3,2,:)=cos(lat').*sin(lon'); +R(3,3,:)=sin(lat'); + +RR=reshape(R(1,:,:),3,n); +dx_temp=sum(RR'.*[dX dY dZ],2); +RR=reshape(R(2,:,:),3,n); +dy_temp=sum(RR'.*[dX dY dZ],2); +RR=reshape(R(3,:,:),3,n); +dz=sum(RR'.*[dX dY dZ],2); + +dx = -dy_temp; +dy = dx_temp; + + diff --git a/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m new file mode 100644 index 000000000..42dc1db60 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/getPoseFromGtScenario.m @@ -0,0 +1,39 @@ +function currentPose = getPoseFromGtScenario(gtScenario,scenarioInd) +% gtScenario contains vectors (Lat, Lon, Alt, Roll, Pitch, Yaw) +% The function picks the index 'scenarioInd' in those vectors and +% computes the corresponding pose by +% 1) Converting (Lat,Lon,Alt) to local coordinates +% 2) Converting (Roll,Pitch,Yaw) to a rotation matrix + +import gtsam.*; + +Org_lat = gtScenario.Lat(1); +Org_lon = gtScenario.Lon(1); +initialPositionECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(1); gtScenario.Lon(1); gtScenario.Alt(1)]); + +gtECEF = imuSimulator.LatLonHRad_to_ECEF([gtScenario.Lat(scenarioInd); gtScenario.Lon(scenarioInd); gtScenario.Alt(scenarioInd)]); +% truth in ENU +dX = gtECEF(1) - initialPositionECEF(1); +dY = gtECEF(2) - initialPositionECEF(2); +dZ = gtECEF(3) - initialPositionECEF(3); +[xlt, ylt, zlt] = imuSimulator.ct2ENU(dX, dY, dZ,Org_lat, Org_lon); + +gtPosition = Point3([xlt, ylt, zlt]'); +% use the gtsam.Rot3.Ypr constructor (yaw, pitch, roll) from the ground truth data +% yaw = measured positively to the right +% pitch = measured positively up +% roll = measured positively to right +% Assumes vehice X forward, Y right, Z down +% +% In the gtScenario data +% heading (yaw) = measured positively to the left from Y-axis +% pitch = +% roll = +% Coordinate frame is Y forward, X is right, Z is up +gtBodyRotation = Rot3.Ypr(-gtScenario.Heading(scenarioInd), gtScenario.Pitch(scenarioInd), gtScenario.Roll(scenarioInd)); +currentPose = Pose3(gtBodyRotation, gtPosition); + +%% Rotate the pose +currentPose = currentPose.compose(Pose3.Expmap([-pi/2;0;0;0;0;0])); + +end \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m new file mode 100644 index 000000000..3f72f1215 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory.m @@ -0,0 +1,17 @@ +function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory( ... + initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT) +%INTEGRATETRAJECTORY Integrate one trajectory step from IMU measurement + +import gtsam.*; + +imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT); +accelGlobal = initialPoseGlobal.rotation().rotate(Point3(acc_omegaIMU(1:3))).vector; + +finalPosition = Point3(initialPoseGlobal.translation.vector ... + + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT); +finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT; +finalRotation = initialPoseGlobal.rotation.compose(imu2in1); +finalPose = Pose3(finalRotation, finalPosition); + +end + diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m new file mode 100644 index 000000000..50b223060 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_bodyFrame.m @@ -0,0 +1,26 @@ +function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory_bodyFrame( ... + initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT, velocity1Body) + +% Before integrating in the body frame we need to compensate for the Coriolis +% effect +acc_body = acc_omegaIMU(1:3) - Point3(cross(acc_omegaIMU(4:6), velocity1Body)).vector; +% after compensating for coriolis this will be essentially zero +% since we are moving at constant body velocity + +import gtsam.*; +%% Integrate in the body frame +% Integrate rotations +imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT); +% Integrate positions +finalPositionBody = velocity1Body * deltaT + 0.5 * acc_body * deltaT * deltaT; +finalVelocityBody = velocity1Body + acc_body * deltaT; + +%% Express the integrated quantities in the global frame +finalVelocityGlobal = initialVelocityGlobal + (initialPoseGlobal.rotation().rotate(Point3(finalVelocityBody)).vector() ); +finalPosition = initialPoseGlobal.translation().vector() + initialPoseGlobal.rotation().rotate( Point3(finalPositionBody)).vector() ; +finalRotation = initialPoseGlobal.rotation.compose(imu2in1); +% Include position and rotation in a pose +finalPose = Pose3(finalRotation, Point3(finalPosition) ); + +end + diff --git a/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m new file mode 100644 index 000000000..b919520ac --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/integrateIMUTrajectory_navFrame.m @@ -0,0 +1,21 @@ +function [ finalPose, finalVelocityGlobal ] = integrateIMUTrajectory_navFrame( ... + initialPoseGlobal, initialVelocityGlobal, acc_omegaIMU, deltaT) +%INTEGRATETRAJECTORY Integrate one trajectory step from IMU measurement + +import gtsam.*; +% Integrate rotations +imu2in1 = Rot3.Expmap(acc_omegaIMU(4:6) * deltaT); +finalRotation = initialPoseGlobal.rotation.compose(imu2in1); + +intermediateRotation = initialPoseGlobal.rotation.compose( Rot3.Expmap(acc_omegaIMU(4:6) * deltaT/2 )); +% Integrate positions (equation (1) in Lupton) +accelGlobal = intermediateRotation.rotate(Point3(acc_omegaIMU(1:3))).vector; +finalPosition = Point3(initialPoseGlobal.translation.vector ... + + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT); +finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT; + +% Include position and rotation in a pose +finalPose = Pose3(finalRotation, finalPosition); + +end + diff --git a/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m b/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m new file mode 100644 index 000000000..e51b622b0 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/integrateTrajectory.m @@ -0,0 +1,24 @@ +function [ finalPose, finalVelocityGlobal ] = integrateTrajectory( ... + initialPose, omega1Body, velocity1Body, velocity2Body, deltaT) +%INTEGRATETRAJECTORY Integrate one trajectory step + +import gtsam.*; +% Rotation: R^1_2 +body2in1 = Rot3.Expmap(omega1Body * deltaT); +% Velocity 2 in frame 1: v^1_2 = R^1_2 v^2_2 +velocity2inertial = body2in1.rotate(Point3(velocity2Body)).vector; +% Acceleration: a^1 = (v^1_2 - v^1_1)/dt +accelBody1 = (velocity2inertial - velocity1Body) / deltaT; + +% Velocity 1 in frame W: v^W_1 = R^W_1 v^1_1 +initialVelocityGlobal = initialPose.rotation().rotate(Point3(velocity1Body)).vector; +% Acceleration in frame W: a^W = R^W_1 a^1 +accelGlobal = initialPose.rotation().rotate(Point3(accelBody1)).vector; + +finalPosition = Point3(initialPose.translation.vector + initialVelocityGlobal * deltaT + 0.5 * accelGlobal * deltaT * deltaT); +finalVelocityGlobal = initialVelocityGlobal + accelGlobal * deltaT; +finalRotation = initialPose.rotation.compose(body2in1); +finalPose = Pose3(finalRotation, finalPosition); + +end + diff --git a/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m b/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m new file mode 100644 index 000000000..697b49a6a --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/runConsistencyTests.m @@ -0,0 +1,173 @@ +% use this script to easily run and save results for multiple consistency +% tests without having to pay attention to the computer every 5 minutes + +import gtsam.*; + +resultsDir = 'results/' +if (~exist(resultsDir, 'dir')) + mkdir(resultsDir); +end + +testOptions = [ ... + % 1 2 3 4 5 6 7 8 9 10 11 12 + % RealData? Between? IMU? IMUType Bias? Camera? #LndMrk GPS? StrtPose TrajLength Subsample #MCRuns + %1 0 1 2 0 0 100 0 100 209 20 100 ;... % 1 + %1 0 1 2 0 0 100 0 100 209 20 100 ;... % 2 + % 1 0 1 2 0 0 100 0 100 209 20 100 ;... % 3 + 1 0 1 2 0 1 100 0 100 209 20 20 ;... % 4 + 1 0 1 2 0 1 100 0 100 209 20 20 ;... % 5 + 1 0 1 2 0 0 100 0 100 209 20 20 ];%... % 6 + % 1 0 1 2 0 0 100 0 100 209 20 100 ;... % 7 + %1 0 1 2 0 0 100 0 100 209 20 1 ;... % 8 + %1 0 1 2 0 0 100 0 100 209 20 1 ]; % 9 + +noises = [ ... + % 1 2 3 4 5 6 7 8 + % sigma_ang sigma_cart sigma_accel sigma_gyro sigma_accelBias sigma_gyroBias sigma_gps sigma_camera + %1e-2 1e-1 1e-3 1e-5 0 0 1e-4 1;... % 1 + %1e-2 1e-1 1e-2 1e-5 0 0 1e-4 1;... % 2 + % 1e-2 1e-1 1e-1 1e-5 0 0 1e-4 1;... % 3 + 1e-2 1e-1 1e-3 1e-4 0 0 1e-4 1;... % 4 + 1e-2 1e-1 1e-3 1e-3 0 0 1e-4 1;... % 5 + 1e-2 1e-1 1e-3 1e-2 0 0 1e-4 1];%... % 6 + % 1e-2 1e-1 1e-3 1e-1 0 0 1e-4 1;... % 7 + %1e-2 1e-1 1e-3 1e-2 1e-3 1e-5 1e-4 1;... % 8 + %1e-2 1e-1 1e-3 1e-2 1e-4 1e-6 1e-4 1]; % 9 + +if(size(testOptions,1) ~= size(noises,1)) + error('testOptions and noises do not have same number of rows'); +end + +% Set flag so the script knows there is an external configuration +externallyConfigured = 1; + +% Set the flag to save the results +saveResults = 0; + +errorRuns = []; + +% Go through tests +for i = 1:size(testOptions,1) + % Clean up from last test + close all; + %clc; + + % Set up variables for test + options.useRealData = testOptions(i,1); + options.includeBetweenFactors = testOptions(i,2); + options.includeIMUFactors = testOptions(i,3); + options.imuFactorType = testOptions(i,4); + options.imuNonzeroBias = testOptions(i,5); + options.includeCameraFactors = testOptions(i,6); + options.numberOfLandmarks = testOptions(i,7); + options.includeGPSFactors = testOptions(i,8); + options.gpsStartPose = testOptions(i,9); + options.trajectoryLength = testOptions(i,10); + options.subsampleStep = testOptions(i,11); + numMonteCarloRuns = testOptions(i,12); + + sigma_ang = noises(i,1); + sigma_cart = noises(i,2); + sigma_accel = noises(i,3); + sigma_gyro = noises(i,4); + sigma_accelBias = noises(i,5); + sigma_gyroBias = noises(i,6); + sigma_gps = noises(i,7); + sigma_camera = noises(i,8); + + % Create folder name + f_between = ''; + f_imu = ''; + f_bias = ''; + f_gps = ''; + f_camera = ''; + f_runs = ''; + + if (options.includeBetweenFactors == 1); + f_between = 'between_'; + end + if (options.includeIMUFactors == 1) + f_imu = sprintf('imu%d_', options.imuFactorType); + if (options.imuNonzeroBias == 1); + f_bias = sprintf('bias_a%1.2g_g%1.2g_', sigma_accelBias, sigma_gyroBias); + end + end + if (options.includeGPSFactors == 1); + f_between = sprintf('gps_%d_', gpsStartPose); + end + if (options.includeCameraFactors == 1) + f_camera = sprintf('camera_%d_', options.numberOfLandmarks); + end + f_runs = sprintf('mc%d', numMonteCarloRuns); + + folderName = [resultsDir f_between f_imu f_bias f_gps f_camera f_runs '/']; + + % make folder if it doesnt exist + if (~exist(folderName, 'dir')) + mkdir(folderName); + end + + testName = sprintf('sa-%1.2g-sc-%1.2g-sacc-%1.2g-sg-%1.2g',sigma_ang,sigma_cart,sigma_accel,sigma_gyro); + + % Run the test + fprintf('Test %d\n\tResults will be saved to:\n\t%s\n\trunning...\n', i, folderName); + fprintf('Test Name: %s\n', testName); + + try + imuSimulator.covarianceAnalysisBetween; + catch + errorRuns = [errorRuns i]; + fprintf('\n*****\n Something went wrong, most likely indeterminant linear system error.\n'); + disp('Test Options:\n'); + disp(testOptions(i,:)); + disp('Noises'); + disp(noises(i,:)); + fprintf('\n*****\n\n'); + end +end + +% Print error summary +fprintf('*************************\n'); +fprintf('%d Runs failed due to errors (data not collected for failed runs)\n', length(errorRuns)); +for i = 1:length(errorRuns) + k = errorRuns(i); + fprintf('\nTest %d:\n', k); + fprintf(' options.useRealData = %d\n', testOptions(k,1)); + fprintf(' options.includeBetweenFactors = %d\n', testOptions(k,2)); + fprintf(' options.includeIMUFactors = %d\n', testOptions(k,3)); + fprintf(' options.imuFactorType = %d\n', testOptions(k,4)); + fprintf(' options.imuNonzeroBias = %d\n', testOptions(k,5)); + fprintf(' options.includeCameraFactors = %d\n', testOptions(k,6)); + fprintf(' numberOfLandmarks = %d\n', testOptions(k,7)); + fprintf(' options.includeGPSFactors = %d\n', testOptions(k,8)); + fprintf(' options.gpsStartPose = %d\n', testOptions(k,9)); + fprintf(' options.trajectoryLength = %d\n', testOptions(k,10)); + fprintf(' options.subsampleStep = %d\n', testOptions(k,11)); + fprintf(' numMonteCarloRuns = %d\n', testOptions(k,12)); + fprintf('\n'); + fprintf(' sigma_ang = %f\n', noises(i,1)); + fprintf(' sigma_cart = %f\n', noises(i,2)); + fprintf(' sigma_accel = %f\n', noises(i,3)); + fprintf(' sigma_gyro = %f\n', noises(i,4)); + fprintf(' sigma_accelBias = %f\n', noises(i,5)); + fprintf(' sigma_gyroBias = %f\n', noises(i,6)); + fprintf(' sigma_gps = %f\n', noises(i,7)); +end + + + + + + + + + + + + + + + + + + diff --git a/matlab/unstable_examples/+imuSimulator/test1onestep.m b/matlab/unstable_examples/+imuSimulator/test1onestep.m new file mode 100644 index 000000000..883569849 --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/test1onestep.m @@ -0,0 +1,48 @@ +import gtsam.*; + +deltaT = 0.01; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Constant global velocity w/ lever arm +disp('--------------------------------------------------------'); +disp('Constant global velocity w/ lever arm'); +omega = [0;0;0.1]; +velocity = [1;0;0]; +R = Rot3.Expmap(omega * deltaT); + +velocity2body = R.unrotate(Point3(velocity)).vector; +acc_omegaExpected = [-0.01; 0; 0; 0; 0; 0.1]; +acc_omegaActual = imuSimulator.calculateIMUMeasurement(omega, omega, velocity, velocity2body, deltaT, Pose3(Rot3, Point3([1;0;0]))); +disp('IMU measurement discrepancy:'); +disp(acc_omegaActual - acc_omegaExpected); + +initialPose = Pose3; +finalPoseExpected = Pose3(Rot3.Expmap(omega * deltaT), Point3(velocity * deltaT)); +finalVelocityExpected = velocity; +[ finalPoseActual, finalVelocityActual ] = imuSimulator.integrateTrajectory(initialPose, omega, velocity, velocity2body, deltaT); +disp('Final pose discrepancy:'); +disp(finalPoseExpected.between(finalPoseActual).matrix); +disp('Final velocity discrepancy:'); +disp(finalVelocityActual - finalVelocityExpected); + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Constant body velocity w/ lever arm +disp('--------------------------------------------------------'); +disp('Constant body velocity w/ lever arm'); +omega = [0;0;0.1]; +velocity = [1;0;0]; + +acc_omegaExpected = [-0.01; 0.1; 0; 0; 0; 0.1]; +acc_omegaActual = imuSimulator.calculateIMUMeasurement(omega, omega, velocity, velocity, deltaT, Pose3(Rot3, Point3([1;0;0]))); +disp('IMU measurement discrepancy:'); +disp(acc_omegaActual - acc_omegaExpected); + +initialPose = Pose3; +initialVelocity = velocity; +finalPoseExpected = Pose3.Expmap([ omega; velocity ] * deltaT); +finalVelocityExpected = finalPoseExpected.rotation.rotate(Point3(velocity)).vector; +[ finalPoseActual, finalVelocityActual ] = imuSimulator.integrateTrajectory(initialPose, omega, velocity, velocity, deltaT); +disp('Final pose discrepancy:'); +disp(finalPoseExpected.between(finalPoseActual).matrix); +disp('Final velocity discrepancy:'); +disp(finalVelocityActual - finalVelocityExpected); \ No newline at end of file diff --git a/matlab/unstable_examples/+imuSimulator/test2constglobal.m b/matlab/unstable_examples/+imuSimulator/test2constglobal.m new file mode 100644 index 000000000..19956d08a --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/test2constglobal.m @@ -0,0 +1,34 @@ +import gtsam.*; + +deltaT = 0.01; +timeElapsed = 1000; + +times = 0:deltaT:timeElapsed; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Constant global velocity w/ lever arm +disp('--------------------------------------------------------'); +disp('Constant global velocity w/ lever arm'); +omega = [0;0;0.1]; +velocity = [1;0;0]; + +% Initial state +currentPoseGlobal = Pose3; +currentVelocityGlobal = velocity; + +% Positions +positions = zeros(3, length(times)+1); + +i = 2; +for t = times + velocity1body = currentPoseGlobal.rotation.unrotate(Point3(currentVelocityGlobal)).vector; + R = Rot3.Expmap(omega * deltaT); + velocity2body = currentPoseGlobal.rotation.compose(R).unrotate(Point3(currentVelocityGlobal)).vector; + [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory(currentPoseGlobal, omega, velocity1body, velocity2body, deltaT); + + positions(:,i) = currentPoseGlobal.translation.vector; + i = i + 1; +end + +figure; +plot(positions(1,:), positions(2,:), '.-'); diff --git a/matlab/unstable_examples/+imuSimulator/test3constbody.m b/matlab/unstable_examples/+imuSimulator/test3constbody.m new file mode 100644 index 000000000..b3ee2edfc --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/test3constbody.m @@ -0,0 +1,96 @@ +clc +clear all +close all + +import gtsam.*; + +addpath(genpath('./Libraries')) + +deltaT = 0.01; +timeElapsed = 25; + +times = 0:deltaT:timeElapsed; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +% Constant body velocity w/ lever arm +disp('--------------------------------------------------------'); +disp('Constant body velocity w/ lever arm'); +omega = [0;0;0.1]; +velocity = [1;0;0]; +RIMUinBody = Rot3.Rz(-pi/2); +% RIMUinBody = eye(3) +IMUinBody = Pose3(RIMUinBody, Point3([1;0;0])); + +% Initial state (body) +currentPoseGlobal = Pose3; +currentVelocityGlobal = velocity; +% Initial state (IMU) +currentPoseGlobalIMU = Pose3; %currentPoseGlobal.compose(IMUinBody); +%currentVelocityGlobalIMU = IMUinBody.rotation.unrotate(Point3(velocity)).vector; % no Coriolis here? +currentVelocityGlobalIMU = IMUinBody.rotation.unrotate( ... + Point3(velocity + cross(omega, IMUinBody.translation.vector))).vector; + + +% Positions +% body trajectory +positions = zeros(3, length(times)+1); +positions(:,1) = currentPoseGlobal.translation.vector; +poses(1).p = positions(:,1); +poses(1).R = currentPoseGlobal.rotation.matrix; + +% Expected IMU trajectory (from body trajectory and lever arm) +positionsIMUe = zeros(3, length(times)+1); +positionsIMUe(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector; +posesIMUe(1).p = positionsIMUe(:,1); +posesIMUe(1).R = poses(1).R * IMUinBody.rotation.matrix; + +% Integrated IMU trajectory (from IMU measurements) +positionsIMU = zeros(3, length(times)+1); +positionsIMU(:,1) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector; +posesIMU(1).p = positionsIMU(:,1); +posesIMU(1).R = IMUinBody.compose(currentPoseGlobalIMU).rotation.matrix; + +i = 2; +for t = times + [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ... + currentPoseGlobal, omega, velocity, velocity, deltaT); + + acc_omega = imuSimulator.calculateIMUMeasurement( ... + omega, omega, velocity, velocity, deltaT, IMUinBody); + + [ currentPoseGlobalIMU, currentVelocityGlobalIMU ] = imuSimulator.integrateIMUTrajectory( ... + currentPoseGlobalIMU, currentVelocityGlobalIMU, acc_omega, deltaT); + + % Store data in some structure for statistics and plots + positions(:,i) = currentPoseGlobal.translation.vector; + positionsIMUe(:,i) = currentPoseGlobal.translation.vector + currentPoseGlobal.rotation.matrix * IMUinBody.translation.vector; + positionsIMU(:,i) = IMUinBody.compose(currentPoseGlobalIMU).translation.vector; + + poses(i).p = positions(:,i); + posesIMUe(i).p = positionsIMUe(:,i); + posesIMU(i).p = positionsIMU(:,i); + + poses(i).R = currentPoseGlobal.rotation.matrix; + posesIMUe(i).R = poses(i).R * IMUinBody.rotation.matrix; + posesIMU(i).R = IMUinBody.compose(currentPoseGlobalIMU).rotation.matrix; + i = i + 1; +end + + +figure(1) +plot_trajectory(poses, 50, '-k', 'body trajectory',0.1,0.75,1) +hold on +plot_trajectory(posesIMU, 50, '-r', 'imu trajectory',0.1,0.75,1) + +figure(2) +hold on; +plot(positions(1,:), positions(2,:), '-b'); +plot(positionsIMU(1,:), positionsIMU(2,:), '-r'); +plot(positionsIMUe(1,:), positionsIMUe(2,:), ':k'); +axis equal; + +disp('Mismatch between final integrated IMU position and expected IMU position') +disp(norm(posesIMUe(end).p - posesIMU(end).p)) +disp('Mismatch between final integrated IMU orientation and expected IMU orientation') +disp(norm(posesIMUe(end).R - posesIMU(end).R)) + diff --git a/matlab/unstable_examples/+imuSimulator/test4circle.m b/matlab/unstable_examples/+imuSimulator/test4circle.m new file mode 100644 index 000000000..22ee175dd --- /dev/null +++ b/matlab/unstable_examples/+imuSimulator/test4circle.m @@ -0,0 +1,97 @@ +clc +clear all +close all + +import gtsam.*; + +deltaT = 0.001; +timeElapsed = 1; +times = 0:deltaT:timeElapsed; + +%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% +disp('--------------------------------------------------------'); +disp('Integration in body frame VS integration in navigation frame'); +disp('TOY EXAMPLE:'); +disp('- Body moves along a circular trajectory with constant rotation rate -omega- and constant -velocity- (in the body frame)'); +disp('- We compare two integration schemes: integating in the navigation frame (similar to Lupton approach) VS integrating in the body frame') +disp('- Navigation frame is assumed inertial for simplicity') +omega = [0;0;2*pi]; +velocity = [1;0;0]; + +%% Set initial conditions for the true trajectory and for the estimates +% (one estimate is obtained by integrating in the body frame, the other +% by integrating in the navigation frame) +% Initial state (body) +currentPoseGlobal = Pose3; +currentVelocityGlobal = velocity; +% Initial state estimate (integrating in navigation frame) +currentPoseGlobalIMUnav = currentPoseGlobal; +currentVelocityGlobalIMUnav = currentVelocityGlobal; +% Initial state estimate (integrating in the body frame) +currentPoseGlobalIMUbody = currentPoseGlobal; +currentVelocityGlobalIMUbody = currentVelocityGlobal; + +%% Prepare data structures for actual trajectory and estimates +% Actual trajectory +positions = zeros(3, length(times)+1); +positions(:,1) = currentPoseGlobal.translation.vector; +poses(1).p = positions(:,1); +poses(1).R = currentPoseGlobal.rotation.matrix; + +% Trajectory estimate (integrated in the navigation frame) +positionsIMUnav = zeros(3, length(times)+1); +positionsIMUnav(:,1) = currentPoseGlobalIMUbody.translation.vector; +posesIMUnav(1).p = positionsIMUnav(:,1); +posesIMUnav(1).R = poses(1).R; + +% Trajectory estimate (integrated in the body frame) +positionsIMUbody = zeros(3, length(times)+1); +positionsIMUbody(:,1) = currentPoseGlobalIMUbody.translation.vector; +posesIMUbody(1).p = positionsIMUbody(:,1); +posesIMUbody(1).R = poses(1).R; + +%% Main loop +i = 2; +for t = times + %% Create the actual trajectory, using the velocities and + % accelerations in the inertial frame to compute the positions + [ currentPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ... + currentPoseGlobal, omega, velocity, velocity, deltaT); + + %% Simulate IMU measurements, considering Coriolis effect + % (in this simple example we neglect gravity and there are no other forces acting on the body) + acc_omega = imuSimulator.calculateIMUMeas_coriolis( ... + omega, omega, velocity, velocity, deltaT); + + %% Integrate in the body frame + [ currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody ] = imuSimulator.integrateIMUTrajectory_bodyFrame( ... + currentPoseGlobalIMUbody, currentVelocityGlobalIMUbody, acc_omega, deltaT, velocity); + + %% Integrate in the navigation frame + [ currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav ] = imuSimulator.integrateIMUTrajectory_navFrame( ... + currentPoseGlobalIMUnav, currentVelocityGlobalIMUnav, acc_omega, deltaT); + + %% Store data in some structure for statistics and plots + positions(:,i) = currentPoseGlobal.translation.vector; + positionsIMUbody(:,i) = currentPoseGlobalIMUbody.translation.vector; + positionsIMUnav(:,i) = currentPoseGlobalIMUnav.translation.vector; + % - + poses(i).p = positions(:,i); + posesIMUbody(i).p = positionsIMUbody(:,i); + posesIMUnav(i).p = positionsIMUnav(:,i); + % - + poses(i).R = currentPoseGlobal.rotation.matrix; + posesIMUbody(i).R = currentPoseGlobalIMUbody.rotation.matrix; + posesIMUnav(i).R = currentPoseGlobalIMUnav.rotation.matrix; + i = i + 1; +end + +figure(1) +hold on; +plot(positions(1,:), positions(2,:), '-b'); +plot(positionsIMUbody(1,:), positionsIMUbody(2,:), '-r'); +plot(positionsIMUnav(1,:), positionsIMUnav(2,:), ':k'); +axis equal; +legend('true trajectory', 'traj integrated in body', 'traj integrated in nav') + + diff --git a/tests/testPCGSolver.cpp b/tests/testPCGSolver.cpp new file mode 100644 index 000000000..38a40521a --- /dev/null +++ b/tests/testPCGSolver.cpp @@ -0,0 +1,157 @@ +/* ---------------------------------------------------------------------------- + + * 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 testPCGSolver.cpp + * @brief Unit tests for PCGSolver class + * @author Yong-Dian Jian + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include +#include // for operator += +using namespace boost::assign; + +#include +#include + +using namespace std; +using namespace gtsam; + +const double tol = 1e-3; + +using symbol_shorthand::X; +using symbol_shorthand::L; + +/* ************************************************************************* */ +TEST( PCGSolver, llt ) { + Matrix R = (Matrix(3,3) << + 1., -1., -1., + 0., 2., -1., + 0., 0., 1.); + Matrix AtA = R.transpose() * R; + + Vector Rvector = (Vector(9) << 1., -1., -1., + 0., 2., -1., + 0., 0., 1.); +// Vector Rvector = (Vector(6) << 1., -1., -1., +// 2., -1., +// 1.); + + Vector b = (Vector(3) << 1., 2., 3.); + + Vector x = (Vector(3) << 6.5, 2.5, 3.) ; + + /* test cholesky */ + Matrix Rhat = AtA.llt().matrixL().transpose(); + EXPECT(assert_equal(R, Rhat, 1e-5)); + + /* test backward substitution */ + Vector xhat = Rhat.triangularView().solve(b); + EXPECT(assert_equal(x, xhat, 1e-5)); + + /* test in-place back substitution */ + xhat = b; + Rhat.triangularView().solveInPlace(xhat); + EXPECT(assert_equal(x, xhat, 1e-5)); + + /* test triangular matrix map */ + Eigen::Map Radapter(Rvector.data(), 3, 3); + xhat = Radapter.transpose().triangularView().solve(b); + EXPECT(assert_equal(x, xhat, 1e-5)); + +} + +/* ************************************************************************* */ +TEST( PCGSolver, dummy ) +{ + LevenbergMarquardtParams paramsPCG; + paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; + PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + pcg->preconditioner_ = boost::make_shared(); + paramsPCG.iterativeParams = pcg; + + NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); + + Point2 x0(10,10); + Values c0; + c0.insert(X(1), x0); + + Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize(); + + DOUBLES_EQUAL(0,fg.error(actualPCG),tol); +} + +/* ************************************************************************* */ +TEST( PCGSolver, blockjacobi ) +{ + LevenbergMarquardtParams paramsPCG; + paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; + PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + pcg->preconditioner_ = boost::make_shared(); + paramsPCG.iterativeParams = pcg; + + NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); + + Point2 x0(10,10); + Values c0; + c0.insert(X(1), x0); + + Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize(); + + DOUBLES_EQUAL(0,fg.error(actualPCG),tol); +} + +/* ************************************************************************* */ +TEST( PCGSolver, subgraph ) +{ + LevenbergMarquardtParams paramsPCG; + paramsPCG.linearSolverType = LevenbergMarquardtParams::Iterative; + PCGSolverParameters::shared_ptr pcg = boost::make_shared(); + pcg->preconditioner_ = boost::make_shared(); + paramsPCG.iterativeParams = pcg; + + NonlinearFactorGraph fg = example::createReallyNonlinearFactorGraph(); + + Point2 x0(10,10); + Values c0; + c0.insert(X(1), x0); + + Values actualPCG = LevenbergMarquardtOptimizer(fg, c0, paramsPCG).optimize(); + + DOUBLES_EQUAL(0,fg.error(actualPCG),tol); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +