diff --git a/.cproject b/.cproject index 1783efa98..62330cdbc 100644 --- a/.cproject +++ b/.cproject @@ -518,6 +518,22 @@ true true + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + make -j2 @@ -568,6 +584,7 @@ make + tests/testBayesTree.run true false @@ -575,6 +592,7 @@ make + testBinaryBayesNet.run true false @@ -622,6 +640,7 @@ make + testSymbolicBayesNet.run true false @@ -629,6 +648,7 @@ make + tests/testSymbolicFactor.run true false @@ -636,6 +656,7 @@ make + testSymbolicFactorGraph.run true false @@ -651,267 +672,12 @@ make + tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j5 - testPoseRTV.run - true - true - true - - - make - -j5 - testIMUSystem.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 - -j5 - testCal3Unified.run - true - true - true - - - make - -j5 - testBTree.run - true - true - true - - - make - -j5 - testDSF.run - true - true - true - - - make - -j5 - testDSFVector.run - true - true - true - - - make - -j5 - testFixedVector.run - true - true - true - - - make - -j5 - testDSFMap.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 @@ -928,30 +694,6 @@ true true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all - true - true - true - make -j2 @@ -976,90 +718,74 @@ true true - + make -j5 - schedulingExample.run + testAHRS.run true true true - + make -j5 - testCSP.run + testImuFactor.run true true true - + make -j5 - testScheduler.run + testInvDepthFactor3.run true true true - + make -j5 - schedulingQuals12.run + testMultiProjectionFactor.run true true true - + make -j5 - testSudoku.run + testPoseRotationPrior.run true true true - + make -j5 - schedulingQuals13.run + testPoseTranslationPrior.run true true true - + make -j5 - testWrap.run + testReferenceFrameFactor.run true true true - + make - -j2 - check + -j5 + testSmartProjectionFactor.run true true true - + make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - clean + -j5 + testTSAMFactors.run true true true @@ -1160,379 +886,22 @@ true true - + make -j5 - testInvDepthFactor3.run + testCombinedImuFactor.run true true true - + make -j5 - testPoseTranslationPrior.run - true - true - true - - - make - -j5 - testPoseRotationPrior.run - true - true - true - - - make - -j5 - testReferenceFrameFactor.run - true - true - true - - - make - -j5 - testAHRS.run - true - true - true - - - make - -j8 testImuFactor.run true true true - - make - -j5 - testMultiProjectionFactor.run - true - true - true - - - make - -j5 - testSmartProjectionFactor.run - true - true - true - - - make - -j5 - testTSAMFactors.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 - -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 @@ -1639,6 +1008,7 @@ make + testErrors.run true false @@ -1684,22 +1054,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j5 - testParticleFactor.run - true - true - true - make -j2 @@ -1876,54 +1230,6 @@ 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 @@ -1932,82 +1238,42 @@ true true - + make -j5 - testAntiFactor.run + testBTree.run true true true - + make -j5 - testBetweenFactor.run + testDSF.run true true true - + make -j5 - testDataset.run + testDSFMap.run true true true - + make -j5 - testEssentialMatrixFactor.run + testDSFVector.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 - testImuFactor.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run + testFixedVector.run true true true @@ -2094,7 +1360,6 @@ make - testSimulated2DOriented.run true false @@ -2134,7 +1399,6 @@ make - testSimulated2D.run true false @@ -2142,7 +1406,6 @@ make - testSimulated3D.run true false @@ -2196,246 +1459,6 @@ false true - - make - -j2 - SimpleRotation.run - true - true - true - - - make - -j5 - CameraResectioning.run - true - true - true - - - make - -j5 - PlanarSLAMExample.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - easyPoint2KalmanFilter.run - true - true - true - - - make - -j2 - elaboratePoint2KalmanFilter.run - true - true - true - - - make - -j5 - Pose2SLAMExample.run - true - true - true - - - make - -j2 - Pose2SLAMwSPCG_easy.run - true - true - true - - - make - -j5 - UGM_small.run - true - true - true - - - make - -j5 - LocalizationExample.run - true - true - true - - - make - -j5 - OdometryExample.run - true - true - true - - - make - -j5 - RangeISAMExample_plaza2.run - true - true - true - - - make - -j5 - SelfCalibrationExample.run - true - true - true - - - make - -j5 - SFMExample.run - true - true - true - - - make - -j5 - VisualISAMExample.run - true - true - true - - - make - -j5 - VisualISAM2Example.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graphviz.run - true - true - true - - - make - -j5 - Pose2SLAMExample_graph.run - true - true - true - - - make - -j5 - SFMExample_bal.run - true - true - true - - - make - -j5 - Pose2SLAMExample_lago.run - true - true - true - - - make - -j5 - Pose2SLAMExample_g2o.run - true - true - true - - - make - -j5 - testLago.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testWhiteNoiseFactor.run - true - true - true - - - make - -j5 - timeLago.run - true - true - true - - - make - -j4 - testImuFactor.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - - tests/testGaussianISAM2 - true - false - true - make -j2 @@ -2452,102 +1475,6 @@ true true - - make - -j2 - testRot3.run - true - true - true - - - make - -j2 - testRot2.run - true - true - true - - - make - -j2 - testPose3.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - testPose2.run - true - true - true - - - make - -j2 - testCal3_S2.run - true - true - true - - - make - -j2 - testSimpleCamera.run - true - true - true - - - make - -j2 - testHomography2.run - true - true - true - - - make - -j2 - testCalibratedCamera.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPoint2.run - true - true - true - make -j3 @@ -2749,6 +1676,7 @@ cpack + -G DEB true false @@ -2756,6 +1684,7 @@ cpack + -G RPM true false @@ -2763,6 +1692,7 @@ cpack + -G TGZ true false @@ -2770,6 +1700,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2943,30 +1874,6 @@ true true - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - wrap - true - true - true - make -j2 @@ -3006,6 +1913,1125 @@ 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 + testCal3Unified.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + clean all + true + true + true + + + make + -j5 + testIMUSystem.run + true + true + true + + + make + -j5 + testPoseRTV.run + true + true + true + + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + + make + -j5 + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteMarginals.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 + testWrap.run + true + true + true + + + make + -j5 + schedulingExample.run + true + true + true + + + make + -j5 + schedulingQuals12.run + true + true + true + + + make + -j5 + schedulingQuals13.run + true + true + true + + + make + -j5 + testCSP.run + true + true + true + + + make + -j5 + testScheduler.run + true + true + true + + + make + -j5 + testSudoku.run + true + true + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j2 + testVSLAMGraph + 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 + -j5 + testParticleFactor.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 + SimpleRotation.run + true + true + true + + + make + -j5 + CameraResectioning.run + true + true + true + + + make + -j5 + PlanarSLAMExample.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + easyPoint2KalmanFilter.run + true + true + true + + + make + -j2 + elaboratePoint2KalmanFilter.run + true + true + true + + + make + -j5 + Pose2SLAMExample.run + true + true + true + + + make + -j2 + Pose2SLAMwSPCG_easy.run + true + true + true + + + make + -j5 + UGM_small.run + true + true + true + + + make + -j5 + LocalizationExample.run + true + true + true + + + make + -j5 + OdometryExample.run + true + true + true + + + make + -j5 + RangeISAMExample_plaza2.run + true + true + true + + + make + -j5 + SelfCalibrationExample.run + true + true + true + + + make + -j5 + SFMExample.run + true + true + true + + + make + -j5 + VisualISAMExample.run + true + true + true + + + make + -j5 + VisualISAM2Example.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graphviz.run + true + true + true + + + make + -j5 + Pose2SLAMExample_graph.run + true + true + true + + + make + -j5 + SFMExample_bal.run + true + true + true + + + make + -j5 + Pose2SLAMExample_lago.run + true + true + true + + + make + -j5 + Pose2SLAMExample_g2o.run + true + true + true + + + make + -j5 + SFMExample_SmartFactor.run + true + true + true + + + make + -j5 + testLago.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j5 + testOrdering.run + true + true + true + + + make + -j5 + testValues.run + true + true + true + + + make + -j5 + testWhiteNoiseFactor.run + true + true + true + + + make + -j4 + testImuFactor.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 + timeLago.run + true + true + true + + + make + -j5 + timePose3.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + tests/testGaussianISAM2 + true + false + true + + + make + -j2 + testRot3.run + true + true + true + + + make + -j2 + testRot2.run + true + true + true + + + make + -j2 + testPose3.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + + make + -j2 + testPose2.run + true + true + true + + + make + -j2 + testCal3_S2.run + true + true + true + + + make + -j2 + testSimpleCamera.run + true + true + true + + + make + -j2 + testHomography2.run + true + true + true + + + make + -j2 + testCalibratedCamera.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testPoint2.run + true + true + true + + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + wrap + true + true + true + diff --git a/CMakeLists.txt b/CMakeLists.txt index da8c4e81a..004ded5e4 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -46,7 +46,6 @@ endif() # Set up options # Configurable Options -option(GTSAM_BUILD_TIMING "Enable/Disable building of timing scripts" OFF) # These do not currently work if(GTSAM_UNSTABLE_AVAILABLE) option(GTSAM_BUILD_UNSTABLE "Enable/Disable libgtsam_unstable" ON) endif() @@ -304,6 +303,9 @@ add_subdirectory(tests) # Build examples add_subdirectory(examples) +# Build timing +add_subdirectory(timing) + # Matlab toolbox if (GTSAM_INSTALL_MATLAB_TOOLBOX) add_subdirectory(matlab) @@ -361,7 +363,7 @@ message(STATUS "================ Configuration Options ======================" message(STATUS "Build flags ") print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ") print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all' ") -print_config_flag(${GTSAM_BUILD_TIMING} "Build Timing scripts ") +print_config_flag(${GTSAM_BUILD_TIMING_ALWAYS} "Build timing scripts with 'make all'") if (DOXYGEN_FOUND) print_config_flag(${GTSAM_BUILD_DOCS} "Build Docs ") endif() diff --git a/cmake/GtsamTesting.cmake b/cmake/GtsamTesting.cmake index 19f487256..2e505c11e 100644 --- a/cmake/GtsamTesting.cmake +++ b/cmake/GtsamTesting.cmake @@ -38,21 +38,46 @@ endmacro() # # Add scripts that will serve as examples of how to use the library. A list of files or # glob patterns is specified, and one executable will be created for each matching .cpp -# file. These executables will not be installed. They are build with 'make all' if +# file. These executables will not be installed. They are built with 'make all' if # GTSAM_BUILD_EXAMPLES_ALWAYS is enabled. They may also be built with 'make examples'. # # Usage example: # gtsamAddExamplesGlob("*.cpp" "BrokenExample.cpp" "gtsam;GeographicLib") # # Arguments: -# globPatterns: The list of files or glob patterns from which to create unit tests, with -# one test created for each cpp file. e.g. "*.cpp", or +# globPatterns: The list of files or glob patterns from which to create examples, with +# one program created for each cpp file. e.g. "*.cpp", or # "A*.cpp;B*.cpp;MyExample.cpp". # excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass # an empty string "" if nothing needs to be excluded. # linkLibraries: The list of libraries to link to. macro(gtsamAddExamplesGlob globPatterns excludedFiles linkLibraries) - gtsamAddExamplesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}") + gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "examples" ${GTSAM_BUILD_EXAMPLES_ALWAYS}) +endmacro() + + +############################################################################### +# Macro: +# +# gtsamAddTimingGlob(globPatterns excludedFiles linkLibraries) +# +# Add scripts that time aspects of the library. A list of files or +# glob patterns is specified, and one executable will be created for each matching .cpp +# file. These executables will not be installed. They are not built with 'make all', +# but they may be built with 'make timing'. +# +# Usage example: +# gtsamAddTimingGlob("*.cpp" "DisabledTimingScript.cpp" "gtsam;GeographicLib") +# +# Arguments: +# globPatterns: The list of files or glob patterns from which to create programs, with +# one program created for each cpp file. e.g. "*.cpp", or +# "A*.cpp;B*.cpp;MyExample.cpp". +# excludedFiles: A list of files or globs to exclude, e.g. "C*.cpp;BrokenExample.cpp". Pass +# an empty string "" if nothing needs to be excluded. +# linkLibraries: The list of libraries to link to. +macro(gtsamAddTimingGlob globPatterns excludedFiles linkLibraries) + gtsamAddExesGlob_impl("${globPatterns}" "${excludedFiles}" "${linkLibraries}" "timing" ${GTSAM_BUILD_TIMING_ALWAYS}) endmacro() @@ -63,6 +88,7 @@ enable_testing() option(GTSAM_BUILD_TESTS "Enable/Disable building of tests" ON) option(GTSAM_BUILD_EXAMPLES_ALWAYS "Build examples with 'make all' (build with 'make examples' if not)" ON) +option(GTSAM_BUILD_TIMING_ALWAYS "Build timing scripts with 'make all' (build with 'make timing' if not" OFF) # Add option for combining unit tests if(MSVC OR XCODE_VERSION) @@ -80,6 +106,9 @@ endif() # Add examples target add_custom_target(examples) +# Add timing target +add_custom_target(timing) + # Include obsolete macros - will be removed in the near future include(GtsamTestingObsolete) @@ -180,7 +209,7 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries) endmacro() -macro(gtsamAddExamplesGlob_impl globPatterns excludedFiles linkLibraries) +macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName buildWithAll) # Get all script files file(GLOB script_files ${globPatterns}) @@ -220,20 +249,22 @@ macro(gtsamAddExamplesGlob_impl globPatterns excludedFiles linkLibraries) target_link_libraries(${script_name} ${linkLibraries}) # Add target dependencies - add_dependencies(examples ${script_name}) + add_dependencies(${groupName} ${script_name}) if(NOT MSVC AND NOT XCODE_VERSION) add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name}) endif() # Add TOPSRCDIR set_property(SOURCE ${script_src} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"") - - if(NOT GTSAM_BUILD_EXAMPLES_ALWAYS) + + # Exclude from all or not - note weird variable assignment because we're in a macro + set(buildWithAll_on ${buildWithAll}) + if(NOT buildWithAll_on) # Exclude from 'make all' and 'make install' - set_target_properties(${target_name} PROPERTIES EXCLUDE_FROM_ALL ON) + set_target_properties("${script_name}" PROPERTIES EXCLUDE_FROM_ALL ON) endif() # Configure target folder (for MSVC and Xcode) - set_property(TARGET ${script_name} PROPERTY FOLDER "Examples") + set_property(TARGET ${script_name} PROPERTY FOLDER "${groupName}") endforeach() endmacro() diff --git a/examples/SFMExample_SmartFactor.cpp b/examples/SFMExample_SmartFactor.cpp index 904919d42..b3c5ee5fe 100644 --- a/examples/SFMExample_SmartFactor.cpp +++ b/examples/SFMExample_SmartFactor.cpp @@ -14,6 +14,7 @@ * @brief A structure-from-motion problem on a simulated dataset, using smart projection factor * @author Duy-Nguyen Ta * @author Jing Dong + * @author Frank Dellaert */ /** @@ -28,11 +29,6 @@ // Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y). #include -// Each variable in the system (poses and landmarks) must be identified with a unique key. -// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1). -// Here we will use Symbols -#include - // In GTSAM, measurement functions are represented as 'factors'. // The factor we used here is SmartProjectionPoseFactor. Every smart factor represent a single landmark, // The SmartProjectionPoseFactor only optimize the pose of camera, not the calibration, @@ -65,8 +61,8 @@ using namespace std; using namespace gtsam; // Make the typename short so it looks much cleaner -typedef gtsam::SmartProjectionPoseFactor - SmartFactor; +typedef gtsam::SmartProjectionPoseFactor SmartFactor; /* ************************************************************************* */ int main(int argc, char* argv[]) { @@ -75,93 +71,87 @@ int main(int argc, char* argv[]) { Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); // Define the camera observation noise model - noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v + noiseModel::Isotropic::shared_ptr measurementNoise = + noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v - // Create the set of ground-truth landmarks + // Create the set of ground-truth landmarks and poses vector points = createPoints(); - - // Create the set of ground-truth poses vector poses = createPoses(); // Create a factor graph NonlinearFactorGraph graph; - // A vector saved all Smart factors (for get landmark position after optimization) - vector smartfactors_ptr; - // Simulated measurements from each camera pose, adding them to the factor graph - for (size_t i = 0; i < points.size(); ++i) { + for (size_t j = 0; j < points.size(); ++j) { // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements. SmartFactor::shared_ptr smartfactor(new SmartFactor()); - for (size_t j = 0; j < poses.size(); ++j) { + for (size_t i = 0; i < poses.size(); ++i) { // generate the 2D measurement - SimpleCamera camera(poses[j], *K); - Point2 measurement = camera.project(points[i]); + SimpleCamera camera(poses[i], *K); + Point2 measurement = camera.project(points[j]); - // call add() function to add measurment into a single factor, here we need to add: + // call add() function to add measurement into a single factor, here we need to add: // 1. the 2D measurement // 2. the corresponding camera's key // 3. camera noise model // 4. camera calibration - smartfactor->add(measurement, Symbol('x', j), measurementNoise, K); + smartfactor->add(measurement, i, measurementNoise, K); } - // save smartfactors to get landmark position - smartfactors_ptr.push_back(smartfactor); - // insert the smart factor in the graph graph.push_back(smartfactor); } // Add a prior on pose x0. This indirectly specifies where the origin is. - noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw - graph.push_back(PriorFactor(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph + // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw + noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas( + (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); + graph.push_back(PriorFactor(0, poses[0], poseNoise)); - // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained - // Here we add a prior on the second pose x1, so this will fix the scale by indicating the distance between x0 and x1. - // Because these two are fixed, the rest poses will be alse fixed. - graph.push_back(PriorFactor(Symbol('x', 1), poses[1], poseNoise)); // add directly to graph + // Because the structure-from-motion problem has a scale ambiguity, the problem is + // still under-constrained. Here we add a prior on the second pose x1, so this will + // fix the scale by indicating the distance between x0 and x1. + // Because these two are fixed, the rest of the poses will be also be fixed. + graph.push_back(PriorFactor(1, poses[1], poseNoise)); // add directly to graph graph.print("Factor Graph:\n"); - // Create the data structure to hold the initial estimate to the solution + // Create the initial estimate to the solution // Intentionally initialize the variables off from the ground truth Values initialEstimate; + Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)); for (size_t i = 0; i < poses.size(); ++i) - initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); + initialEstimate.insert(i, poses[i].compose(delta)); initialEstimate.print("Initial Estimates:\n"); // Optimize the graph and print results Values result = DoglegOptimizer(graph, initialEstimate).optimize(); result.print("Final results:\n"); - - // Notice: Smart factor represent the 3D point as a factor, not a variable. + // A smart factor represent the 3D point inside the factor, not as a variable. // The 3D position of the landmark is not explicitly calculated by the optimizer. - // If you do want to output the landmark's 3D position, you should use the internal function point() - // of the smart factor to get the 3D point. + // To obtain the landmark's 3D position, we use the "point" method of the smart factor. Values landmark_result; - for (size_t i = 0; i < points.size(); ++i) { + for (size_t j = 0; j < points.size(); ++j) { - // The output of point() is in boost::optional, since sometimes - // the triangulation opterations inside smart factor will encounter degeneracy. - // Check the manual of boost::optional for more details for the usages. + // The output of point() is in boost::optional, as sometimes + // the triangulation operation inside smart factor will encounter degeneracy. boost::optional point; - // here we use the saved smart factors to call, pass in all optimized pose to calculate landmark positions - point = smartfactors_ptr.at(i)->point(result); - - // ignore if boost::optional return NULL - if (point) - landmark_result.insert(Symbol('l', i), *point); + // The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first + SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast(graph[j]); + if (smart) { + point = smart->point(result); + if (point) // ignore if boost::optional return NULL + landmark_result.insert(j, *point); + } } landmark_result.print("Landmark results:\n"); - return 0; } /* ************************************************************************* */ diff --git a/examples/SFMExample_bal.cpp b/examples/SFMExample_bal.cpp index 9d453fe5f..43177dc42 100644 --- a/examples/SFMExample_bal.cpp +++ b/examples/SFMExample_bal.cpp @@ -43,8 +43,7 @@ int main (int argc, char* argv[]) { // Load the SfM data from file SfM_data mydata; - const bool success = readBAL(filename, mydata); - assert(success); + assert(readBAL(filename, mydata)); cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras(); // Create a factor graph diff --git a/gtsam/base/CMakeLists.txt b/gtsam/base/CMakeLists.txt index 50145846e..66d3ec721 100644 --- a/gtsam/base/CMakeLists.txt +++ b/gtsam/base/CMakeLists.txt @@ -7,9 +7,3 @@ install(FILES ${base_headers_tree} DESTINATION include/gtsam/base/treeTraversal) # Build tests add_subdirectory(tests) - -# Build timing scripts -if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(base "gtsam" "gtsam" "${base_excluded_files}") -endif(GTSAM_BUILD_TIMING) - diff --git a/gtsam/discrete/CMakeLists.txt b/gtsam/discrete/CMakeLists.txt index 3c5602080..d78dff34f 100644 --- a/gtsam/discrete/CMakeLists.txt +++ b/gtsam/discrete/CMakeLists.txt @@ -6,9 +6,3 @@ install(FILES ${discrete_headers} DESTINATION include/gtsam/discrete) # Add all tests add_subdirectory(tests) - -# Build timing scripts -#if (GTSAM_BUILD_TIMING) -# gtsam_add_timing(discrete "${discrete_local_libs}") -#endif(GTSAM_BUILD_TIMING) - diff --git a/gtsam/geometry/CMakeLists.txt b/gtsam/geometry/CMakeLists.txt index f72f965ea..dabdde45c 100644 --- a/gtsam/geometry/CMakeLists.txt +++ b/gtsam/geometry/CMakeLists.txt @@ -4,9 +4,3 @@ install(FILES ${geometry_headers} DESTINATION include/gtsam/geometry) # Build tests add_subdirectory(tests) - -# Build timing scripts -if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(geometry "gtsam" "gtsam" "${geometry_excluded_files}") -endif(GTSAM_BUILD_TIMING) - diff --git a/gtsam/geometry/EssentialMatrix.h b/gtsam/geometry/EssentialMatrix.h index 3c9568c3d..32b966261 100644 --- a/gtsam/geometry/EssentialMatrix.h +++ b/gtsam/geometry/EssentialMatrix.h @@ -112,6 +112,16 @@ public: return E_; } + /// Return epipole in image_a , as Unit3 to allow for infinity + inline const Unit3& epipole_a() const { + return aTb_; // == direction() + } + + /// Return epipole in image_b, as Unit3 to allow for infinity + inline Unit3 epipole_b() const { + return aRb_.unrotate(aTb_); // == rotation.unrotate(direction()) + } + /** * @brief takes point in world coordinates and transforms it to pose with |t|==1 * @param p point in world coordinates diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 19eb87b5f..709b95181 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -275,11 +275,12 @@ public: if (P.z() <= 0) throw CheiralityException(); #endif + double d = 1.0 / P.z(); + const double u = P.x() * d, v = P.y() * d; if (Dpoint) { - double d = 1.0 / P.z(), d2 = d * d; - *Dpoint = (Matrix(2, 3) << d, 0.0, -P.x() * d2, 0.0, d, -P.y() * d2); + *Dpoint = (Matrix(2, 3) << d, 0.0, -u * d, 0.0, d, -v * d); } - return Point2(P.x() / P.z(), P.y() / P.z()); + return Point2(u, v); } /// Project a point into the image and check depth diff --git a/gtsam/geometry/Unit3.cpp b/gtsam/geometry/Unit3.cpp index db4c8da83..ad6a41bde 100644 --- a/gtsam/geometry/Unit3.cpp +++ b/gtsam/geometry/Unit3.cpp @@ -58,7 +58,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) { } /* ************************************************************************* */ -Matrix Unit3::basis() const { +const Matrix& Unit3::basis() const { // Return cached version if exists if (B_.rows() == 3) diff --git a/gtsam/geometry/Unit3.h b/gtsam/geometry/Unit3.h index 39e2c9799..8d2c024c0 100644 --- a/gtsam/geometry/Unit3.h +++ b/gtsam/geometry/Unit3.h @@ -84,7 +84,7 @@ public: * It is a 3*2 matrix [b1 b2] composed of two orthogonal directions * tangent to the sphere at the current direction. */ - Matrix basis() const; + const Matrix& basis() const; /// Return skew-symmetric associated with 3D point on unit sphere Matrix skew() const; diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 9ad30bc41..4d34bbe3d 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -144,9 +144,9 @@ TEST (EssentialMatrix, FromPose3_b) { Matrix actualH; Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3); Point3 c1Tc2(0.4, 0.5, 0.6); - EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2)); + EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); Pose3 pose(c1Rc2, c1Tc2); // Pose between two cameras - EXPECT(assert_equal(trueE, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); + EXPECT(assert_equal(E, EssentialMatrix::FromPose3(pose, actualH), 1e-8)); Matrix expectedH = numericalDerivative11( boost::bind(EssentialMatrix::FromPose3, _1, boost::none), pose); EXPECT(assert_equal(expectedH, actualH, 1e-5)); @@ -161,6 +161,35 @@ TEST (EssentialMatrix, streaming) { EXPECT(assert_equal(expected, actual)); } +//************************************************************************* +TEST (EssentialMatrix, epipoles) { + // Create an E + Rot3 c1Rc2 = Rot3::ypr(0.1, -0.2, 0.3); + Point3 c1Tc2(0.4, 0.5, 0.6); + EssentialMatrix E(c1Rc2, Unit3(c1Tc2)); + + // Calculate expected values through SVD + Matrix U, V; + Vector S; + gtsam::svd(E.matrix(), U, S, V); + + // check rank 2 constraint + CHECK(fabs(S(2))<1e-10); + + // check epipoles not at infinity + CHECK(fabs(U(2,2))>1e-10 && fabs(V(2,2))>1e-10); + + // Check epipoles + + // Epipole in image 1 is just E.direction() + Unit3 e1(U(0, 2), U(1, 2), U(2, 2)); + EXPECT(assert_equal(e1, E.epipole_a())); + + // Epipole in image 2 is E.rotation().unrotate(E.direction()) + Unit3 e2(V(0, 2), V(1, 2), V(2, 2)); + EXPECT(assert_equal(e2, E.epipole_b())); +} + /* ************************************************************************* */ int main() { TestResult tr; diff --git a/gtsam/inference/CMakeLists.txt b/gtsam/inference/CMakeLists.txt index d5c37d976..c3df3a989 100644 --- a/gtsam/inference/CMakeLists.txt +++ b/gtsam/inference/CMakeLists.txt @@ -4,9 +4,3 @@ install(FILES ${inference_headers} DESTINATION include/gtsam/inference) # Build tests add_subdirectory(tests) - -# Build timing scripts -if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(inference "gtsam" "gtsam" "${inference_excluded_files}") -endif(GTSAM_BUILD_TIMING) - diff --git a/gtsam/linear/CMakeLists.txt b/gtsam/linear/CMakeLists.txt index 29eb60b93..084c27057 100644 --- a/gtsam/linear/CMakeLists.txt +++ b/gtsam/linear/CMakeLists.txt @@ -4,8 +4,3 @@ install(FILES ${linear_headers} DESTINATION include/gtsam/linear) # Build tests add_subdirectory(tests) - -# Build timing scripts -if (GTSAM_BUILD_TIMING) - gtsam_add_subdir_timing(linear "gtsam" "gtsam" "${linear_excluded_files}") -endif(GTSAM_BUILD_TIMING) diff --git a/gtsam/linear/IterativeSolver.h b/gtsam/linear/IterativeSolver.h index a7787d15a..3c397c9e9 100644 --- a/gtsam/linear/IterativeSolver.h +++ b/gtsam/linear/IterativeSolver.h @@ -104,6 +104,7 @@ namespace gtsam { 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>(); } diff --git a/gtsam/linear/SubgraphPreconditioner.h b/gtsam/linear/SubgraphPreconditioner.h index 4c65d14ca..86bee2188 100644 --- a/gtsam/linear/SubgraphPreconditioner.h +++ b/gtsam/linear/SubgraphPreconditioner.h @@ -157,8 +157,6 @@ namespace gtsam { Weights weights(const GaussianFactorGraph &gfg) const; SubgraphBuilderParameters parameters_; - private: - SubgraphBuilder() {} }; /*******************************************************************************************/ diff --git a/gtsam/linear/tests/timeSLAMlike.cpp b/gtsam/linear/tests/timeSLAMlike.cpp deleted file mode 100644 index 479c03940..000000000 --- a/gtsam/linear/tests/timeSLAMlike.cpp +++ /dev/null @@ -1,144 +0,0 @@ -/* ---------------------------------------------------------------------------- - - * 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 timeSLAMlike.cpp - * @brief Times solving of random SLAM-like graphs - * @author Richard Roberts - * @date Aug 30, 2010 - */ - -#include -#include - -#include -#include -#include -#include -#include - -using namespace gtsam; -using namespace std; -using namespace boost::lambda; - -typedef EliminationTree GaussianEliminationTree; - -static boost::variate_generator > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0)); - -bool _pair_compare(const pair& a1, const pair& a2) { return a1.first < a2.first; } - -int main(int argc, char *argv[]) { - - size_t vardim = 3; - size_t blockdim = 3; - int nVars = 500; - size_t blocksPerVar = 5; - size_t varsPerBlock = 2; - size_t varSpread = 10; - - size_t nTrials = 10; - - double blockbuild, blocksolve; - - cout << "\n" << nVars << " variables of dimension " << vardim << ", " << - blocksPerVar << " blocks for each variable, blocks of dimension " << blockdim << " measure " << varsPerBlock << " variables\n"; - cout << nTrials << " trials\n"; - - boost::variate_generator > ri(boost::mt19937(), boost::uniform_int<>(-varSpread, varSpread)); - - ///////////////////////////////////////////////////////////////////////////// - // Timing test with blockwise Gaussian factor graphs - - { - // Build GFG's - cout << "Building SLAM-like Gaussian factor graphs... "; - cout.flush(); - boost::timer timer; - timer.restart(); - vector blockGfgs; - blockGfgs.reserve(nTrials); - for(size_t trial=0; trial > terms; terms.reserve(varsPerBlock); - if(c == 0 && d == 0) - // If it's the first factor, just make a prior - terms.push_back(make_pair(0, eye(vardim))); - else if(c != 0) { - // Generate a random Gaussian factor - for(size_t h=0; h nVars-1 || find_if(terms.begin(), terms.end(), - boost::bind(&pair::first, boost::lambda::_1) == Index(var)) != terms.end()); - Matrix A(blockdim, vardim); - for(size_t j=0; j #include #include +#include namespace gtsam { @@ -292,6 +293,77 @@ namespace gtsam { return flag_bump_up_near_zero_probs_; } + /* ************************************************************************* */ + SharedGaussian get_model_inlier() const { + return model_inlier_; + } + + /* ************************************************************************* */ + SharedGaussian get_model_outlier() const { + return model_outlier_; + } + + /* ************************************************************************* */ + Matrix get_model_inlier_cov() const { + return (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); + } + + /* ************************************************************************* */ + Matrix get_model_outlier_cov() const { + return (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); + } + + /* ************************************************************************* */ + void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){ + /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories + * (note these are given in the E step, where indicator probabilities are calculated). + * + * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the + * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). + * + * TODO: improve efficiency (info form) + */ + + const T& p1 = values.at(key1_); + const T& p2 = values.at(key2_); + + Matrix H1, H2; + T hx = p1.between(p2, H1, H2); // h(x) + + // get joint covariance of the involved states + std::vector Keys; + Keys.push_back(key1_); + Keys.push_back(key2_); + Marginals marginals( graph, values, Marginals::QR ); + JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys); + Matrix cov1 = joint_marginal12(key1_, key1_); + Matrix cov2 = joint_marginal12(key2_, key2_); + Matrix cov12 = joint_marginal12(key1_, key2_); + + Matrix H; + H.resize(H1.rows(), H1.rows()+H2.rows()); + H << H1, H2; // H = [H1 H2] + + Matrix joint_cov; + joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols()); + joint_cov << cov1, cov12, + cov12.transpose(), cov2; + + Matrix cov_state = H*joint_cov*H.transpose(); + + // model_inlier_->print("before:"); + + // update inlier and outlier noise models + Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); + model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state); + + Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); + model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state); + + // model_inlier_->print("after:"); + // std::cout<<"covRinlier + cov_state: "< #include +#include using namespace std; using namespace gtsam; + // Disabled this test because it is currently failing - remove the lines "#if 0" and "#endif" below // to reenable the test. #if 0 @@ -251,6 +253,49 @@ TEST( BetweenFactorEM, CaseStudy) } } + +///* ************************************************************************** */ +TEST (BetweenFactorEM, updateNoiseModel ) { + gtsam::Key key1(1); + gtsam::Key key2(2); + + gtsam::Pose2 p1(10.0, 15.0, 0.1); + gtsam::Pose2 p2(15.0, 15.0, 0.3); + gtsam::Pose2 noise(0.5, 0.4, 0.01); + gtsam::Pose2 rel_pose_ideal = p1.between(p2); + gtsam::Pose2 rel_pose_msr = rel_pose_ideal.compose(noise); + + SharedGaussian model_inlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 1.5, 2.5, 4.05))); + SharedGaussian model_outlier(noiseModel::Diagonal::Sigmas( (gtsam::Vector(3) << 50.0, 50.0, 10.0))); + + gtsam::Values values; + values.insert(key1, p1); + values.insert(key2, p2); + + double prior_outlier = 0.0; + double prior_inlier = 1.0; + + BetweenFactorEM f(key1, key2, rel_pose_msr, model_inlier, model_outlier, + prior_inlier, prior_outlier); + + SharedGaussian model = SharedGaussian(noiseModel::Isotropic::Sigma(3, 1e2)); + + NonlinearFactorGraph graph; + graph.push_back(gtsam::PriorFactor(key1, p1, model)); + graph.push_back(gtsam::PriorFactor(key2, p2, model)); + + f.updateNoiseModels(values, graph); + + SharedGaussian model_inlier_new = f.get_model_inlier(); + SharedGaussian model_outlier_new = f.get_model_outlier(); + + model_inlier->print("model_inlier:"); + model_outlier->print("model_outlier:"); + model_inlier_new->print("model_inlier_new:"); + model_outlier_new->print("model_outlier_new:"); +} + + #endif /* ************************************************************************* */ diff --git a/gtsam_unstable/timing/CMakeLists.txt b/gtsam_unstable/timing/CMakeLists.txt new file mode 100644 index 000000000..b5130ae92 --- /dev/null +++ b/gtsam_unstable/timing/CMakeLists.txt @@ -0,0 +1 @@ +gtsamAddTimingGlob("*.cpp" "" "gtsam_unstable") diff --git a/gtsam_unstable/base/tests/timeDSFvariants.cpp b/gtsam_unstable/timing/timeDSFvariants.cpp similarity index 100% rename from gtsam_unstable/base/tests/timeDSFvariants.cpp rename to gtsam_unstable/timing/timeDSFvariants.cpp diff --git a/gtsam_unstable/base/tests/timeDSFvariants.xlsx b/gtsam_unstable/timing/timeDSFvariants.xlsx similarity index 100% rename from gtsam_unstable/base/tests/timeDSFvariants.xlsx rename to gtsam_unstable/timing/timeDSFvariants.xlsx diff --git a/gtsam_unstable/base/tests/timeDSFvariants2.xlsx b/gtsam_unstable/timing/timeDSFvariants2.xlsx similarity index 100% rename from gtsam_unstable/base/tests/timeDSFvariants2.xlsx rename to gtsam_unstable/timing/timeDSFvariants2.xlsx diff --git a/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp similarity index 86% rename from gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp rename to gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp index 5132df658..0d514abcd 100644 --- a/gtsam_unstable/slam/tests/timeInertialNavFactor_GlobalVelocity.cpp +++ b/gtsam_unstable/timing/timeInertialNavFactor_GlobalVelocity.cpp @@ -20,9 +20,9 @@ #include #include #include -#include #include #include +#include using namespace std; using namespace gtsam; @@ -32,7 +32,7 @@ gtsam::Rot3 world_R_ECEF( 0.85173, -0.52399, 0, 0.41733, 0.67835, -0.60471); -gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); +gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); /* ************************************************************************* */ @@ -54,16 +54,16 @@ int main() { gtsam::Key BiasKey1(31); double measurement_dt(0.1); - Vector world_g((Vec(3) << 0.0, 0.0, 9.81)); - Vector world_rho((Vec(3) << 0.0, -1.5724e-05, 0.0)); // NED system - gtsam::Vector ECEF_omega_earth((Vec(3) << 0.0, 0.0, 7.292115e-5)); + Vector world_g((Vector(3) << 0.0, 0.0, 9.81)); + Vector world_rho((Vector(3) << 0.0, -1.5724e-05, 0.0)); // NED system + gtsam::Vector ECEF_omega_earth((Vector(3) << 0.0, 0.0, 7.292115e-5)); gtsam::Vector world_omega_earth(world_R_ECEF.matrix() * ECEF_omega_earth); SharedGaussian model(noiseModel::Isotropic::Sigma(9, 0.1)); // Second test: zero angular motion, some acceleration - generated in matlab - Vector measurement_acc((Vec(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); - Vector measurement_gyro((Vec(3) << 0.1, 0.2, 0.3)); + Vector measurement_acc((Vector(3) << 6.501390843381716, -6.763926150509185, -2.300389940090343)); + Vector measurement_gyro((Vector(3) << 0.1, 0.2, 0.3)); InertialNavFactor_GlobalVelocity f(PoseKey1, VelKey1, BiasKey1, PoseKey2, VelKey2, measurement_acc, measurement_gyro, measurement_dt, world_g, world_rho, world_omega_earth, model); @@ -72,7 +72,7 @@ int main() { -0.652537293, 0.709880342, 0.265075427); Point3 t1(2.0,1.0,3.0); Pose3 Pose1(R1, t1); - LieVector Vel1(3,0.5,-0.5,0.4); + LieVector Vel1 = Vector((Vector(3) << 0.5,-0.5,0.4)); Rot3 R2(0.473618898, 0.119523052, 0.872582019, 0.609241153, 0.67099888, -0.422594037, -0.636011287, 0.731761397, 0.244979388); @@ -99,7 +99,7 @@ int main() { GaussianFactorGraph graph; gttic_(LinearizeTiming); for(size_t i = 0; i < 100000; ++i) { - GaussianFactor::shared_ptr g = f.linearize(values, ordering); + GaussianFactor::shared_ptr g = f.linearize(values); graph.push_back(g); } gttoc_(LinearizeTiming); diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index cf81dc762..008dbb78d 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -14,16 +14,3 @@ if(MSVC) set_property(SOURCE "${CMAKE_CURRENT_SOURCE_DIR}/testSerializationSLAM.cpp" APPEND PROPERTY COMPILE_FLAGS "/bigobj") endif() - -# Build timing scripts -if (GTSAM_BUILD_TIMING) - # Subdirectory target for timing - does not actually execute the scripts - add_custom_target(timing.tests) - set(is_test FALSE) - - # Build grouped benchmarks - gtsam_add_grouped_scripts("tests" # Use subdirectory as group label - "time*.cpp" timing "Timing Benchmark" # Standard for all timing scripts - "${tests_full_libs}" "${tests_full_libs}" "${tests_exclude}" # Pass in linking and exclusion lists - ${is_test}) -endif (GTSAM_BUILD_TIMING) diff --git a/timing/CMakeLists.txt b/timing/CMakeLists.txt new file mode 100644 index 000000000..fc16d3ac8 --- /dev/null +++ b/timing/CMakeLists.txt @@ -0,0 +1,3 @@ +gtsamAddTimingGlob("*.cpp" "" "gtsam") + +target_link_libraries(timeGaussianFactorGraph CppUnitLite) diff --git a/tests/timeBatch.cpp b/timing/timeBatch.cpp similarity index 100% rename from tests/timeBatch.cpp rename to timing/timeBatch.cpp diff --git a/gtsam/geometry/tests/timeCalibratedCamera.cpp b/timing/timeCalibratedCamera.cpp similarity index 96% rename from gtsam/geometry/tests/timeCalibratedCamera.cpp rename to timing/timeCalibratedCamera.cpp index 1833579d9..76813c455 100644 --- a/gtsam/geometry/tests/timeCalibratedCamera.cpp +++ b/timing/timeCalibratedCamera.cpp @@ -27,11 +27,11 @@ int main() { int n = 100000; - const Pose3 pose1((Matrix)(Mat(3,3) << + const Pose3 pose1(Matrix3((Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. - ), + )), Point3(0,0,0.5)); const CalibratedCamera camera(pose1); diff --git a/gtsam/linear/tests/timeFactorOverhead.cpp b/timing/timeFactorOverhead.cpp similarity index 78% rename from gtsam/linear/tests/timeFactorOverhead.cpp rename to timing/timeFactorOverhead.cpp index 6e5d6b9ad..97c40e6f4 100644 --- a/gtsam/linear/tests/timeFactorOverhead.cpp +++ b/timing/timeFactorOverhead.cpp @@ -16,30 +16,29 @@ * @date Aug 20, 2010 */ +#include +#include #include #include -#include +#include #include -#include #include using namespace gtsam; using namespace std; -typedef EliminationTree GaussianEliminationTree; - static boost::variate_generator > rg(boost::mt19937(), boost::uniform_real<>(0.0, 1.0)); int main(int argc, char *argv[]) { - Index key = 0; + Key key = 0; size_t vardim = 2; size_t blockdim = 1; - size_t nBlocks = 2000; + size_t nBlocks = 4000; - size_t nTrials = 10; + size_t nTrials = 20; double blockbuild, blocksolve, combbuild, combsolve; @@ -54,8 +53,7 @@ int main(int argc, char *argv[]) { // Build GFG's cout << "Building blockwise Gaussian factor graphs... "; cout.flush(); - boost::timer timer; - timer.restart(); + gttic_(blockbuild); vector blockGfgs; blockGfgs.reserve(nTrials); for(size_t trial=0; trial(key, A, b, noise)); } } - blockbuild = timer.elapsed(); + gttoc_(blockbuild); + tictoc_getNode(blockbuildNode, blockbuild); + blockbuild = blockbuildNode->secs(); cout << blockbuild << " s" << endl; // Solve GFG's cout << "Solving blockwise Gaussian factor graphs... "; cout.flush(); - timer.restart(); + gttic_(blocksolve); for(size_t trial=0; trialeliminate(&EliminateQR)); - VectorValues soln(optimize(*gbn)); + GaussianBayesNet::shared_ptr gbn = blockGfgs[trial].eliminateSequential(); + VectorValues soln = gbn->optimize(); } - blocksolve = timer.elapsed(); + gttoc_(blocksolve); + tictoc_getNode(blocksolveNode, blocksolve); + blocksolve = blocksolveNode->secs(); cout << blocksolve << " s" << endl; } @@ -97,8 +99,7 @@ int main(int argc, char *argv[]) { // Build GFG's cout << "Building combined-factor Gaussian factor graphs... "; cout.flush(); - boost::timer timer; - timer.restart(); + gttic_(combbuild); vector combGfgs; for(size_t trial=0; trial(key, Acomb, bcomb, + noiseModel::Isotropic::Sigma(blockdim*nBlocks, 1.0))); } - combbuild = timer.elapsed(); + gttoc(combbuild); + tictoc_getNode(combbuildNode, combbuild); + combbuild = combbuildNode->secs(); cout << combbuild << " s" << endl; // Solve GFG's cout << "Solving combined-factor Gaussian factor graphs... "; cout.flush(); - timer.restart(); + gttic_(combsolve); for(size_t trial=0; trialeliminate(&EliminateQR)); - VectorValues soln(optimize(*gbn)); + GaussianBayesNet::shared_ptr gbn = combGfgs[trial].eliminateSequential(); + VectorValues soln = gbn->optimize(); } - combsolve = timer.elapsed(); + gttoc_(combsolve); + tictoc_getNode(combsolveNode, combsolve); + combsolve = combsolveNode->secs(); cout << combsolve << " s" << endl; } diff --git a/gtsam/linear/tests/timeGaussianFactor.cpp b/timing/timeGaussianFactor.cpp similarity index 92% rename from gtsam/linear/tests/timeGaussianFactor.cpp rename to timing/timeGaussianFactor.cpp index 0ffdd1cfa..261fbac48 100644 --- a/gtsam/linear/tests/timeGaussianFactor.cpp +++ b/timing/timeGaussianFactor.cpp @@ -22,7 +22,7 @@ using namespace std; #include -#include // for operator += in Ordering +#include #include #include @@ -33,7 +33,7 @@ using namespace std; using namespace gtsam; using namespace boost::assign; -static const Index _x1_=1, _x2_=2, _l1_=3; +static const Key _x1_=1, _x2_=2, _l1_=3; /* * Alex's Machine @@ -53,7 +53,7 @@ static const Index _x1_=1, _x2_=2, _l1_=3; int main() { // create a linear factor - Matrix Ax2 = (Mat(8, 2) << + Matrix Ax2 = (Matrix(8, 2) << // x2 -5., 0., +0.,-5., @@ -65,7 +65,7 @@ int main() +0.,10. ); - Matrix Al1 = (Mat(8, 10) << + Matrix Al1 = (Matrix(8, 10) << // l1 5., 0.,1.,2.,3.,4.,5.,6.,7.,8., 0., 5.,1.,2.,3.,4.,5.,6.,7.,8., @@ -77,7 +77,7 @@ int main() 0., 0.,1.,2.,3.,4.,5.,6.,7.,8. ); - Matrix Ax1 = (Mat(8, 2) << + Matrix Ax1 = (Matrix(8, 2) << // x1 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8., 0.00, 0.,1.,2.,3.,4.,5.,6.,7.,8., @@ -108,7 +108,8 @@ int main() JacobianFactor::shared_ptr factor; for(int i = 0; i < n; i++) - conditional = JacobianFactor(combined).eliminateFirst(); + boost::tie(conditional, factor) = + JacobianFactor(combined).eliminate(Ordering(boost::assign::list_of(_x2_))); long timeLog2 = clock(); double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; diff --git a/tests/timeGaussianFactorGraph.cpp b/timing/timeGaussianFactorGraph.cpp similarity index 91% rename from tests/timeGaussianFactorGraph.cpp rename to timing/timeGaussianFactorGraph.cpp index 00d24bd13..a3157729e 100644 --- a/tests/timeGaussianFactorGraph.cpp +++ b/timing/timeGaussianFactorGraph.cpp @@ -29,10 +29,10 @@ using namespace boost::assign; /* ************************************************************************* */ // Create a Kalman smoother for t=1:T and optimize double timeKalmanSmoother(int T) { - pair smoother_ordering = createSmoother(T); - GaussianFactorGraph& smoother(smoother_ordering.first); + GaussianFactorGraph smoother = createSmoother(T); clock_t start = clock(); - GaussianSequentialSolver(smoother).optimize(); + // Keys will come out sorted since keys() returns a set + smoother.optimize(Ordering(smoother.keys())); clock_t end = clock (); double dif = (double)(end - start) / CLOCKS_PER_SEC; return dif; @@ -40,12 +40,10 @@ double timeKalmanSmoother(int T) { /* ************************************************************************* */ // Create a planar factor graph and optimize -// todo: use COLAMD ordering again (removed when linear baked-in ordering added) double timePlanarSmoother(int N, bool old = true) { - boost::tuple pg = planarGraph(N); - GaussianFactorGraph& fg(pg.get<0>()); + GaussianFactorGraph fg = planarGraph(N).get<0>(); clock_t start = clock(); - GaussianSequentialSolver(fg).optimize(); + fg.optimize(); clock_t end = clock (); double dif = (double)(end - start) / CLOCKS_PER_SEC; return dif; @@ -53,12 +51,10 @@ double timePlanarSmoother(int N, bool old = true) { /* ************************************************************************* */ // Create a planar factor graph and eliminate -// todo: use COLAMD ordering again (removed when linear baked-in ordering added) double timePlanarSmootherEliminate(int N, bool old = true) { - boost::tuple pg = planarGraph(N); - GaussianFactorGraph& fg(pg.get<0>()); + GaussianFactorGraph fg = planarGraph(N).get<0>(); clock_t start = clock(); - GaussianSequentialSolver(fg).eliminate(); + fg.eliminateMultifrontal(); clock_t end = clock (); double dif = (double)(end - start) / CLOCKS_PER_SEC; return dif; diff --git a/tests/timeIncremental.cpp b/timing/timeIncremental.cpp similarity index 100% rename from tests/timeIncremental.cpp rename to timing/timeIncremental.cpp diff --git a/gtsam/slam/tests/timeLago.cpp b/timing/timeLago.cpp similarity index 100% rename from gtsam/slam/tests/timeLago.cpp rename to timing/timeLago.cpp diff --git a/gtsam/base/tests/timeMatrix.cpp b/timing/timeMatrix.cpp similarity index 99% rename from gtsam/base/tests/timeMatrix.cpp rename to timing/timeMatrix.cpp index 2cd0a8b19..7e1c1b5be 100644 --- a/gtsam/base/tests/timeMatrix.cpp +++ b/timing/timeMatrix.cpp @@ -189,7 +189,7 @@ double timeColumn(size_t reps) { */ double timeHouseholder(size_t reps) { // create a matrix - Matrix Abase = Mat(4, 7) << + Matrix Abase = (Matrix(4, 7) << -5, 0, 5, 0, 0, 0, -1, 00, -5, 0, 5, 0, 0, 1.5, 10, 0, 0, 0,-10, 0, 2, diff --git a/gtsam/base/tests/timeMatrixOps.cpp b/timing/timeMatrixOps.cpp similarity index 100% rename from gtsam/base/tests/timeMatrixOps.cpp rename to timing/timeMatrixOps.cpp diff --git a/gtsam/geometry/tests/timePinholeCamera.cpp b/timing/timePinholeCamera.cpp similarity index 87% rename from gtsam/geometry/tests/timePinholeCamera.cpp rename to timing/timePinholeCamera.cpp index de1fa1279..8e3e12e22 100644 --- a/gtsam/geometry/tests/timePinholeCamera.cpp +++ b/timing/timePinholeCamera.cpp @@ -28,7 +28,7 @@ int main() { int n = 1000000; - const Pose3 pose1((Matrix)(Mat(3,3) << + const Pose3 pose1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. @@ -53,6 +53,10 @@ int main() // After Cal3DS2 fix: 0.12231 musecs/call // Cal3Bundler: 0.12000 musecs/call // Cal3Bundler fix: 0.14637 musecs/call + // June 24 2014, Macbook Pro 2.3GHz Core i7 + // GTSAM 3.1: 0.04295 musecs/call + // After project fix: 0.04193 musecs/call + { long timeLog = clock(); for(int i = 0; i < n; i++) @@ -70,6 +74,9 @@ int main() // After Cal3DS2 fix: 3.2857 musecs/call // Cal3Bundler: 2.6556 musecs/call // Cal3Bundler fix: 2.1613 musecs/call + // June 24 2014, Macbook Pro 2.3GHz Core i7 + // GTSAM 3.1: 0.2322 musecs/call + // After project fix: 0.2094 musecs/call { Matrix Dpose, Dpoint; long timeLog = clock(); @@ -88,6 +95,9 @@ int main() // After Cal3DS2 fix: 3.4483 musecs/call // Cal3Bundler: 2.5112 musecs/call // Cal3Bundler fix: 2.0946 musecs/call + // June 24 2014, Macbook Pro 2.3GHz Core i7 + // GTSAM 3.1: 0.2294 musecs/call + // After project fix: 0.2093 musecs/call { Matrix Dpose, Dpoint, Dcal; long timeLog = clock(); diff --git a/gtsam/geometry/tests/timePose2.cpp b/timing/timePose2.cpp similarity index 99% rename from gtsam/geometry/tests/timePose2.cpp rename to timing/timePose2.cpp index 28044068c..94bd78ef9 100644 --- a/gtsam/geometry/tests/timePose2.cpp +++ b/timing/timePose2.cpp @@ -58,7 +58,7 @@ Pose2 Pose2betweenOptimized(const Pose2& r1, const Pose2& r2, if (H1) { double dt1 = -s2 * x + c2 * y; double dt2 = -c2 * x - s2 * y; - *H1 = (Mat(3,3) << + *H1 = (Matrix(3,3) << -c, -s, dt1, s, -c, dt2, 0.0, 0.0,-1.0); diff --git a/gtsam/geometry/tests/timePose3.cpp b/timing/timePose3.cpp similarity index 90% rename from gtsam/geometry/tests/timePose3.cpp rename to timing/timePose3.cpp index 13e630706..fa8ef7b6c 100644 --- a/gtsam/geometry/tests/timePose3.cpp +++ b/timing/timePose3.cpp @@ -37,8 +37,8 @@ int main() double norm=sqrt(1.0+16.0+4.0); double x=1.0/norm, y=4.0/norm, z=2.0/norm; - Vector v = (Vec(6) << x, y, z, 0.1, 0.2, -0.1); - Pose3 T = Pose3::Expmap((Vec(6) << 0.1, 0.1, 0.2, 0.1, 0.4, 0.2)), T2 = T.retract(v); + Vector v = (Vector(6) << x, y, z, 0.1, 0.2, -0.1); + Pose3 T = Pose3::Expmap((Vector(6) << 0.1, 0.1, 0.2, 0.1, 0.4, 0.2)), T2 = T.retract(v); Matrix H1,H2; TEST(retract, T.retract(v)) diff --git a/gtsam/geometry/tests/timeRot2.cpp b/timing/timeRot2.cpp similarity index 100% rename from gtsam/geometry/tests/timeRot2.cpp rename to timing/timeRot2.cpp diff --git a/gtsam/geometry/tests/timeRot3.cpp b/timing/timeRot3.cpp similarity index 100% rename from gtsam/geometry/tests/timeRot3.cpp rename to timing/timeRot3.cpp diff --git a/gtsam/geometry/tests/timeStereoCamera.cpp b/timing/timeStereoCamera.cpp similarity index 96% rename from gtsam/geometry/tests/timeStereoCamera.cpp rename to timing/timeStereoCamera.cpp index bfe4b5aaa..dce622720 100644 --- a/gtsam/geometry/tests/timeStereoCamera.cpp +++ b/timing/timeStereoCamera.cpp @@ -27,7 +27,7 @@ int main() { int n = 100000; - const Pose3 pose1((Matrix)(Mat(3,3) << + const Pose3 pose1((Matrix)(Matrix(3,3) << 1., 0., 0., 0.,-1., 0., 0., 0.,-1. diff --git a/gtsam/base/tests/timeTest.cpp b/timing/timeTest.cpp similarity index 100% rename from gtsam/base/tests/timeTest.cpp rename to timing/timeTest.cpp diff --git a/gtsam/base/tests/timeVirtual.cpp b/timing/timeVirtual.cpp similarity index 100% rename from gtsam/base/tests/timeVirtual.cpp rename to timing/timeVirtual.cpp diff --git a/gtsam/base/tests/timeVirtual2.cpp b/timing/timeVirtual2.cpp similarity index 100% rename from gtsam/base/tests/timeVirtual2.cpp rename to timing/timeVirtual2.cpp diff --git a/tests/timeiSAM2Chain.cpp b/timing/timeiSAM2Chain.cpp similarity index 100% rename from tests/timeiSAM2Chain.cpp rename to timing/timeiSAM2Chain.cpp