12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Merged gtborg/gtsam into develop

release/4.3a0
Stephen Camp 2014-06-03 10:04:19 -04:00
parent a5c938f8ca e60f21a22f
commit 917c9c46c8
44 changed files with 1512 additions and 1147 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

298
.cproject
View File

@ -568,6 +568,7 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -575,6 +576,7 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -622,6 +624,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -629,6 +632,7 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -636,6 +640,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -651,6 +656,7 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -728,46 +734,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testValues.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testOrdering.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testKey.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLinearContainerFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testLinearContainerFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testWhiteNoiseFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments>
<buildTarget>testWhiteNoiseFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1114,6 +1080,7 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1159,6 +1126,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testParticleFactor.run" path="build/gtsam_unstable/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testParticleFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1239,14 +1214,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testParticleFactor.run" path="build/gtsam_unstable/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testParticleFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1351,6 +1318,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testImuFactor.run" path="build/gtsam/navigation" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testImuFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCombinedImuFactor.run" path="build/gtsam/navigation" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testCombinedImuFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1433,7 +1416,6 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1473,7 +1455,6 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1481,7 +1462,6 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1495,22 +1475,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testImuFactor.run" path="build/gtsam/navigation" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testImuFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCombinedImuFactor.run" path="build/gtsam/navigation" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testCombinedImuFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1768,6 +1732,7 @@
</target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1775,6 +1740,7 @@
</target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1782,6 +1748,7 @@
</target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1789,6 +1756,7 @@
</target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2217,70 +2185,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGeneralSFMFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testProjectionFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testProjectionFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGeneralSFMFactor_Cal3Bundler.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGeneralSFMFactor_Cal3Bundler.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testAntiFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments>
<buildTarget>testAntiFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBetweenFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments>
<buildTarget>testBetweenFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDataset.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDataset.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEssentialMatrixFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testEssentialMatrixFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRotateFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testRotateFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2547,6 +2451,7 @@
</target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2554,6 +2459,7 @@
</target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2561,6 +2467,7 @@
</target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2678,6 +2585,70 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testAntiFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testAntiFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBetweenFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBetweenFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDataset.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDataset.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEssentialMatrixFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testEssentialMatrixFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGeneralSFMFactor_Cal3Bundler.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGeneralSFMFactor_Cal3Bundler.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGeneralSFMFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGeneralSFMFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testProjectionFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testProjectionFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRotateFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testRotateFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2830,6 +2801,70 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMExample_lago.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>Pose2SLAMExample_lago.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMExample_g2o.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>Pose2SLAMExample_g2o.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLago.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testLago.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLinearContainerFactor.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testLinearContainerFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testOrdering.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testOrdering.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testValues.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testValues.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testWhiteNoiseFactor.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testWhiteNoiseFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeLago.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeLago.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
@ -2848,7 +2883,6 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>

4
.gitignore vendored
View File

@ -1,4 +1,6 @@
/build*
/doc*
*.pyc
*.DS_Store
*.DS_Store
/examples/Data/dubrovnik-3-7-pre-rewritten.txt
/examples/Data/pose2example-rewritten.txt

9
CMakeLists.txt
View File

@ -4,7 +4,7 @@ cmake_minimum_required(VERSION 2.6)
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 3)
set (GTSAM_VERSION_MINOR 0)
set (GTSAM_VERSION_MINOR 1)
set (GTSAM_VERSION_PATCH 0)
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
@ -273,6 +273,13 @@ if(MSVC)
add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344) # Disable non-DLL-exported base class and other warnings
endif()
# GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8)
add_definitions(-Wno-unused-local-typedefs)
endif()
endif()
if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS)
endif()

80
examples/Data/dubrovnik-3-7-pre-rewritten.txt
View File

@ -1,80 +0,0 @@
3 7 19
0 0 -385.989990234375 387.1199951171875
1 0 -38.439998626708984375 492.1199951171875
2 0 -667.91998291015625 123.1100006103515625
0 1 383.8800048828125 -15.299989700317382812
1 1 559.75 -106.15000152587890625
0 2 591.54998779296875 136.44000244140625
1 2 863.8599853515625 -23.469970703125
2 2 494.720001220703125 112.51999664306640625
0 3 592.5 125.75
1 3 861.08001708984375 -35.219970703125
2 3 498.540008544921875 101.55999755859375
0 4 348.720001220703125 558.3800048828125
1 4 776.030029296875 483.529998779296875
2 4 7.7800288200378417969 326.350006103515625
0 5 14.010009765625 96.420013427734375
1 5 207.1300048828125 118.3600006103515625
0 6 202.7599945068359375 340.989990234375
1 6 543.18011474609375 294.80999755859375
2 6 -58.419979095458984375 110.8300018310546875
0.29656188120312942935
-0.035318354384285870207
0.31252101755032046793
0.47230274932665988752
-0.3572340863744113415
-2.0517704282499575896
1430.031982421875
-7.5572756941255647689e-08
3.2377570134516087119e-14
0.28532097381985194184
-0.27699838370789808817
0.048601169984112867206
-1.2598695987143850861
-0.049063798188844320869
-1.9586867140445654023
1432.137451171875
-7.3171918302250560373e-08
3.1759419042137054801e-14
0.057491325683772541433
0.34853090049579965592
0.47985129303736057116
8.1963904289063389541
6.5146840788718787252
-3.8392804395897406344
1572.047119140625
-1.5962623223231275915e-08
-1.6507904730136101212e-14
-11.317351620610928364
3.3594874875767186673
-42.755222607849105998
4.2648515634753199066
-8.4629358700849355301
-22.252086323427270997
10.996977688149536689
-9.2123370180278048025
-29.206739014051372294
10.935342607054865383
-9.4338917557810741954
-29.112263909175499776
15.714024935401759819
1.3745079651566265433
-59.286834979937104606
-1.3624227800805182031
-4.1979357415396094666
-21.034430148188398846
6.7690173115899296974
-4.7352452433700786827
-53.605307875695892506

23
examples/Data/pose2example-rewritten.txt
View File

@ -1,23 +0,0 @@
VERTEX_SE2 0 0 0 0
VERTEX_SE2 1 1.03039 0.01135 -0.081596
VERTEX_SE2 2 2.03614 -0.129733 -0.301887
VERTEX_SE2 3 3.0151 -0.442395 -0.345514
VERTEX_SE2 4 3.34395 0.506678 1.21471
VERTEX_SE2 5 3.68449 1.46405 1.18379
VERTEX_SE2 6 4.06463 2.41478 1.17633
VERTEX_SE2 7 4.42978 3.30018 1.25917
VERTEX_SE2 8 4.12888 2.32148 -1.82539
VERTEX_SE2 9 3.88465 1.32751 -1.95302
VERTEX_SE2 10 3.53107 0.388263 -2.14893
EDGE_SE2 0 1 1.03039 0.01135 -0.081596 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 1 2 1.0139 -0.058639 -0.220291 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 2 3 1.02765 -0.007456 -0.043627 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 3 4 -0.012016 1.00436 1.56023 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 4 5 1.01603 0.014565 -0.03093 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 5 6 1.02389 0.006808 -0.007452 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 6 7 0.957734 0.003159 0.082836 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 7 8 -1.02382 -0.013668 -3.08456 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 8 9 1.02344 0.013984 -0.127624 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 9 10 1.00335 0.02225 -0.195918 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 5 9 0.033943 0.032439 3.07364 44.7214 0 0 44.7214 0 30.9017
EDGE_SE2 3 10 0.04402 0.988477 -1.55351 44.7214 0 0 44.7214 0 30.9017

45
examples/Pose2SLAMExample_g2o.cpp
View File

@ -18,46 +18,45 @@
* @author Luca Carlone
*/
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Key.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <fstream>
#include <sstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
int main(const int argc, const char *argv[]){
// Read graph from file
string g2oFile;
if (argc < 2)
std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl;
const string g2oFile = argv[1];
g2oFile = "../../examples/Data/noisyToyGraph.txt";
else
g2oFile = argv[1];
NonlinearFactorGraph graph;
Values initial;
readG2o(g2oFile, graph, initial);
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
boost::tie(graph, initial) = readG2o(g2oFile);
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = graph;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonOptimizer optimizer(graphWithPrior, initial); // , parameters);
GaussNewtonOptimizer optimizer(graphWithPrior, *initial);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(outputFile, graph, result);
std::cout << "done! " << std::endl;
if (argc < 3) {
result.print("result");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, result, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}

48
examples/Pose2SLAMExample_lago.cpp
View File

@ -12,51 +12,53 @@
/**
* @file Pose2SLAMExample_lago.cpp
* @brief A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem
* using LAGO (Linear Approximation for Graph Optimization). See class LagoInitializer.h
* using LAGO (Linear Approximation for Graph Optimization). See class lago.h
* Output is written on a file, in g2o format
* Syntax for the script is ./Pose2SLAMExample_lago input.g2o output.g2o
* @date May 15, 2014
* @author Luca Carlone
*/
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Key.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/lago.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/nonlinear/LagoInitializer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
#include <sstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
int main(const int argc, const char *argv[]){
// Read graph from file
string g2oFile;
if (argc < 2)
std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl;
const string g2oFile = argv[1];
g2oFile = "../../examples/Data/noisyToyGraph.txt";
else
g2oFile = argv[1];
NonlinearFactorGraph graph;
Values initial;
readG2o(g2oFile, graph, initial);
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
boost::tie(graph, initial) = readG2o(g2oFile);
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = graph;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
graphWithPrior.print();
std::cout << "Computing LAGO estimate" << std::endl;
Values estimateLago = initializeLago(graphWithPrior);
Values estimateLago = lago::initialize(graphWithPrior);
std::cout << "done!" << std::endl;
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(outputFile, graph, estimateLago);
std::cout << "done! " << std::endl;
if (argc < 3) {
estimateLago.print("estimateLago");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, estimateLago, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}

168
examples/SFMExample_SmartFactor.cpp Normal file
View File

@ -0,0 +1,168 @@
/* ----------------------------------------------------------------------------
* 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 SFMExample_SmartFactor.cpp
* @brief A structure-from-motion problem on a simulated dataset, using smart projection factor
* @author Duy-Nguyen Ta
* @author Jing Dong
*/
/**
* A structure-from-motion example with landmarks
* - The landmarks form a 10 meter cube
* - The robot rotates around the landmarks, always facing towards the cube
*/
// For loading the data
#include "SFMdata.h"
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
#include <gtsam/geometry/Point2.h>
// 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 <gtsam/inference/Symbol.h>
// 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,
// The calibration should be known.
#include <gtsam/slam/SmartProjectionPoseFactor.h>
// Also, we will initialize the robot at some location using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// Finally, once all of the factors have been added to our factor graph, we will want to
// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
// trust-region method known as Powell's Degleg
#include <gtsam/nonlinear/DoglegOptimizer.h>
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
// nonlinear functions around an initial linearization point, then solve the linear system
// to update the linearization point. This happens repeatedly until the solver converges
// to a consistent set of variable values. This requires us to specify an initial guess
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
#include <vector>
using namespace std;
using namespace gtsam;
// Make the typename short so it looks much cleaner
typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2>
SmartFactor;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
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
// Create the set of ground-truth landmarks
vector<Point3> points = createPoints();
// Create the set of ground-truth poses
vector<Pose3> poses = createPoses();
// Create a factor graph
NonlinearFactorGraph graph;
// A vector saved all Smart factors (for get landmark position after optimization)
vector<SmartFactor::shared_ptr> smartfactors_ptr;
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < points.size(); ++i) {
// 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) {
// generate the 2D measurement
SimpleCamera camera(poses[j], *K);
Point2 measurement = camera.project(points[i]);
// call add() function to add measurment 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);
}
// 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<Pose3>(Symbol('x', 0), poses[0], 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 poses will be alse fixed.
graph.push_back(PriorFactor<Pose3>(Symbol('x', 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
// Intentionally initialize the variables off from the ground truth
Values initialEstimate;
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.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.
// 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.
Values landmark_result;
for (size_t i = 0; i < points.size(); ++i) {
// The output of point() is in boost::optional<gtsam::Point3>, since sometimes
// the triangulation opterations inside smart factor will encounter degeneracy.
// Check the manual of boost::optional for more details for the usages.
boost::optional<Point3> 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);
}
landmark_result.print("Landmark results:\n");
return 0;
}
/* ************************************************************************* */

74
examples/StereoVOExample_large.cpp
View File

@ -25,47 +25,43 @@
*/
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Key.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/StereoFactor.h>
#include <gtsam/slam/dataset.h>
#include <string>
#include <fstream>
#include <iostream>
#include <sstream>
#include <string>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
Values initial_estimate;
NonlinearFactorGraph graph;
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
Values initial_estimate = Values();
vector<double> read_vector;
string read_string, parse_string;
string data_folder = "C:/Users/Stephen/Documents/Borg/gtsam/Examples/Data/";
string calibration_loc = data_folder + "VO_calibration.txt";
string pose_loc = data_folder + "VO_camera_poses_large.txt";
string factor_loc = data_folder + "VO_stereo_factors_large.txt";
string calibration_loc = findExampleDataFile("VO_calibration.txt");
string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
//read camera calibration info from file
double fx,fy,s,u,v,b;
ifstream calibration_file(calibration_loc);
// focal lengths fx, fy, skew s, principal point u0, v0, baseline b
double fx, fy, s, u0, v0, b;
ifstream calibration_file(calibration_loc.c_str());
cout << "Reading calibration info" << endl;
calibration_file >> fx >> fy >> s >> u >> v >> b;
calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
//create stereo camera calibration object
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u,v,b));
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b));
ifstream pose_file(pose_loc);
ifstream pose_file(pose_loc.c_str());
cout << "Reading camera poses" << endl;
int pose_id;
MatrixRowMajor m(4,4);
@ -77,30 +73,36 @@ int main(int argc, char** argv){
initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
}
double x, l, uL, uR, v, X, Y, Z;
ifstream factor_file(factor_loc);
// camera and landmark keys
size_t x, l;
// pixel coordinates uL, uR, v (same for left/right images due to rectification)
// landmark coordinates X, Y, Z in camera frame, resulting from triangulation
double uL, uR, v, X, Y, Z;
ifstream factor_file(factor_loc.c_str());
cout << "Reading stereo factors" << endl;
//read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
graph.push_back(
GenericStereoFactor<Pose3,Point3>(StereoPoint2(uL, uR, v), model,
Symbol('x', x), Symbol('l', l), K));
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
if(!initial_estimate.exists(Symbol('l',l))){
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
//transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space
Point3 worldPoint = camPose.transform_from(Point3(X,Y,Z));
initial_estimate.insert(Symbol('l',l),worldPoint);
}
graph.push_back(
GenericStereoFactor<Pose3, Point3>(StereoPoint2(uL, uR, v), model,
Symbol('x', x), Symbol('l', l), K));
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
if (!initial_estimate.exists(Symbol('l', l))) {
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
//transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space
Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z));
initial_estimate.insert(Symbol('l', l), worldPoint);
}
}
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
first_pose.print("Check estimate poses:\n");
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to solve the initial factor graph estimate
//create Levenberg-Marquardt optimizer to optimize the factor graph
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
Values result = optimizer.optimize();
@ -109,4 +111,4 @@ int main(int argc, char** argv){
pose_values.print("Final camera poses:\n");
return 0;
}
}

7
gtsam.h
View File

@ -2249,6 +2249,13 @@ pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
gtsam::noiseModel::Base* model);
void save2D(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
string filename);
pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
void writeG2o(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& estimate, string filename);
//*************************************************************************
// Navigation

3
gtsam/3rdparty/CCOLAMD/Source/ccolamd.c vendored
View File

@ -617,11 +617,12 @@
#include "ccolamd.h"
#include <stdlib.h>
#include <math.h>
#include <limits.h>
#ifdef MATLAB_MEX_FILE
#include <stdint.h>
typedef uint16_t char16_t;
#include "mex.h"
#include "matrix.h"
#endif

3
gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c vendored
View File

@ -13,6 +13,9 @@
#ifndef NPRINT
#ifdef MATLAB_MEX_FILE
#include <stdlib.h>
#include <stdint.h>
typedef uint16_t char16_t;
#include "mex.h"
int (*ccolamd_printf) (const char *, ...) = mexPrintf ;
#else

0
gtsam_unstable/geometry/TriangulationFactor.h → gtsam/geometry/TriangulationFactor.h
View File

2
gtsam_unstable/geometry/tests/testTriangulation.cpp → gtsam/geometry/tests/testTriangulation.cpp
View File

@ -16,7 +16,7 @@
* Author: cbeall3
*/
#include <gtsam_unstable/geometry/triangulation.h>
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <CppUnitLite/TestHarness.h>

0
gtsam/geometry/tests/testSphere2.cpp → gtsam/geometry/tests/testUnit3.cpp
View File

2
gtsam_unstable/geometry/triangulation.cpp → gtsam/geometry/triangulation.cpp
View File

@ -16,7 +16,7 @@
* @author Chris Beall
*/
#include <gtsam_unstable/geometry/triangulation.h>
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

8
gtsam_unstable/geometry/triangulation.h → gtsam/geometry/triangulation.h
View File

@ -18,8 +18,8 @@
#pragma once
#include <gtsam_unstable/base/dllexport.h>
#include <gtsam_unstable/geometry/TriangulationFactor.h>
#include <gtsam/geometry/TriangulationFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/PriorFactor.h>
@ -52,7 +52,7 @@ public:
* @param rank_tol SVD rank tolerance
* @return Triangulated Point3
*/
GTSAM_UNSTABLE_EXPORT Point3 triangulateDLT(
GTSAM_EXPORT Point3 triangulateDLT(
const std::vector<Matrix>& projection_matrices,
const std::vector<Point2>& measurements, double rank_tol);
@ -120,7 +120,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
* @param landmarkKey to refer to landmark
* @return refined Point3
*/
GTSAM_UNSTABLE_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
const Values& values, Key landmarkKey);
/**

60
gtsam/inference/VariableIndex-inl.h
View File

@ -23,43 +23,35 @@ namespace gtsam {
/* ************************************************************************* */
template<class FG>
void VariableIndex::augment(const FG& factors, boost::optional<const FastVector<size_t>&> newFactorIndices)
{
void VariableIndex::augment(const FG& factors,
boost::optional<const FastVector<size_t>&> newFactorIndices) {
gttic(VariableIndex_augment);
// Augment index for each factor
for(size_t i = 0; i < factors.size(); ++i)
{
if(factors[i])
{
for (size_t i = 0; i < factors.size(); ++i) {
if (factors[i]) {
const size_t globalI =
newFactorIndices ?
(*newFactorIndices)[i] :
nFactors_;
BOOST_FOREACH(const Key key, *factors[i])
{
newFactorIndices ? (*newFactorIndices)[i] : nFactors_;
BOOST_FOREACH(const Key key, *factors[i]) {
index_[key].push_back(globalI);
++ nEntries_;
++nEntries_;
}
}
// Increment factor count even if factors are null, to keep indices consistent
if(newFactorIndices)
{
if((*newFactorIndices)[i] >= nFactors_)
if (newFactorIndices) {
if ((*newFactorIndices)[i] >= nFactors_)
nFactors_ = (*newFactorIndices)[i] + 1;
}
else
{
++ nFactors_;
} else {
++nFactors_;
}
}
}
/* ************************************************************************* */
template<typename ITERATOR, class FG>
void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors)
{
void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor,
const FG& factors) {
gttic(VariableIndex_remove);
// NOTE: We intentionally do not decrement nFactors_ because the factor
@ -68,17 +60,20 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG&
// one greater than the highest-numbered factor referenced in a VariableIndex.
ITERATOR factorIndex = firstFactor;
size_t i = 0;
for( ; factorIndex != lastFactor; ++factorIndex, ++i) {
if(i >= factors.size())
throw std::invalid_argument("Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
if(factors[i]) {
for (; factorIndex != lastFactor; ++factorIndex, ++i) {
if (i >= factors.size())
throw std::invalid_argument(
"Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
if (factors[i]) {
BOOST_FOREACH(Key j, *factors[i]) {
Factors& factorEntries = internalAt(j);
Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), *factorIndex);
if(entry == factorEntries.end())
throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
Factors::iterator entry = std::find(factorEntries.begin(),
factorEntries.end(), *factorIndex);
if (entry == factorEntries.end())
throw std::invalid_argument(
"Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
factorEntries.erase(entry);
-- nEntries_;
--nEntries_;
}
}
}
@ -87,10 +82,11 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG&
/* ************************************************************************* */
template<typename ITERATOR>
void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) {
for(ITERATOR key = firstKey; key != lastKey; ++key) {
for (ITERATOR key = firstKey; key != lastKey; ++key) {
KeyMap::iterator entry = index_.find(*key);
if(!entry->second.empty())
throw std::invalid_argument("Asking to remove variables from the variable index that are not unused");
if (!entry->second.empty())
throw std::invalid_argument(
"Asking to remove variables from the variable index that are not unused");
index_.erase(entry);
}
}

2
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -70,7 +70,7 @@ namespace gtsam {
vector<size_t> dims_accumulated;
dims_accumulated.resize(dims.size()+1,0);
dims_accumulated[0]=0;
for (int i=1; i<dims_accumulated.size(); i++)
for (size_t i=1; i<dims_accumulated.size(); i++)
dims_accumulated[i] = dims_accumulated[i-1]+dims[i-1];
return dims_accumulated;
}

39
gtsam/linear/NoiseModel.cpp
View File

@ -49,7 +49,7 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) {
/* ************************************************************************* */
// check *above the diagonal* for non-zero entries
static boost::optional<Vector> checkIfDiagonal(const Matrix M) {
boost::optional<Vector> checkIfDiagonal(const Matrix M) {
size_t m = M.rows(), n = M.cols();
// check all non-diagonal entries
bool full = false;
@ -74,23 +74,46 @@ static boost::optional<Vector> checkIfDiagonal(const Matrix M) {
/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
size_t m = R.rows(), n = R.cols();
if (m != n) throw invalid_argument("Gaussian::SqrtInformation: R not square");
if (m != n)
throw invalid_argument("Gaussian::SqrtInformation: R not square");
boost::optional<Vector> diagonal = boost::none;
if (smart)
diagonal = checkIfDiagonal(R);
if (diagonal) return Diagonal::Sigmas(reciprocal(*diagonal),true);
else return shared_ptr(new Gaussian(R.rows(),R));
if (diagonal)
return Diagonal::Sigmas(reciprocal(*diagonal), true);
else
return shared_ptr(new Gaussian(R.rows(), R));
}
/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) {
Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) {
size_t m = M.rows(), n = M.cols();
if (m != n)
throw invalid_argument("Gaussian::Information: R not square");
boost::optional<Vector> diagonal = boost::none;
if (smart)
diagonal = checkIfDiagonal(M);
if (diagonal)
return Diagonal::Precisions(*diagonal, true);
else {
Matrix R = RtR(M);
return shared_ptr(new Gaussian(R.rows(), R));
}
}
/* ************************************************************************* */
Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance,
bool smart) {
size_t m = covariance.rows(), n = covariance.cols();
if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square");
if (m != n)
throw invalid_argument("Gaussian::Covariance: covariance not square");
boost::optional<Vector> variances = boost::none;
if (smart)
variances = checkIfDiagonal(covariance);
if (variances) return Diagonal::Variances(*variances,true);
else return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
if (variances)
return Diagonal::Variances(*variances, true);
else
return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
}
/* ************************************************************************* */

10
gtsam/linear/NoiseModel.h
View File

@ -164,6 +164,13 @@ namespace gtsam {
*/
static shared_ptr SqrtInformation(const Matrix& R, bool smart = true);
/**
* A Gaussian noise model created by specifying an information matrix.
* @param M The information matrix
* @param smart check if can be simplified to derived class
*/
static shared_ptr Information(const Matrix& M, bool smart = true);
/**
* A Gaussian noise model created by specifying a covariance matrix.
* @param covariance The square covariance Matrix
@ -864,6 +871,9 @@ namespace gtsam {
ar & boost::serialization::make_nvp("noise_", const_cast<NoiseModel::shared_ptr&>(noise_));
}
};
// Helper function
GTSAM_EXPORT boost::optional<Vector> checkIfDiagonal(const Matrix M);
} // namespace noiseModel

11
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -285,6 +285,17 @@ TEST(NoiseModel, SmartSqrtInformation2 )
EXPECT(assert_equal(*expected,*actual));
}
/* ************************************************************************* */
TEST(NoiseModel, SmartInformation )
{
bool smart = true;
gtsam::SharedGaussian expected = Unit::Isotropic::Variance(3,2);
Matrix M = 0.5*eye(3);
EXPECT(checkIfDiagonal(M));
gtsam::SharedGaussian actual = Gaussian::Information(M, smart);
EXPECT(assert_equal(*expected,*actual));
}
/* ************************************************************************* */
TEST(NoiseModel, SmartCovariance )
{

7
gtsam/navigation/MagFactor.h
View File

@ -54,8 +54,11 @@ public:
static Point3 unrotate(const Rot2& R, const Point3& p,
boost::optional<Matrix&> HR = boost::none) {
Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR);
if (HR)
*HR = HR->col(2);
if (HR) {
// assign to temporary first to avoid error in Win-Debug mode
Matrix H = HR->col(2);
*HR = H;
}
return q;
}

100
gtsam/nonlinear/LagoInitializer.h
View File

@ -1,100 +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 LagoInitializer.h
* @brief Initialize Pose2 in a factor graph using LAGO
* (Linear Approximation for Graph Optimization). see papers:
*
* L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate
* approximation for planar pose graph optimization, IJRR, 2014.
*
* L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation
* for graph-based simultaneous localization and mapping, RSS, 2011.
*
* @param graph: nonlinear factor graph (can include arbitrary factors but we assume
* that there is a subgraph involving Pose2 and betweenFactors). Also in the current
* version we assume that there is an odometric spanning path (x0->x1, x1->x2, etc)
* and a prior on x0. This assumption can be relaxed by using the extra argument
* useOdometricPath = false, although this part of code is not stable yet.
* @return Values: initial guess from LAGO (only pose2 are initialized)
*
* @author Luca Carlone
* @author Frank Dellaert
* @date May 14, 2014
*/
#pragma once
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
namespace gtsam {
typedef std::map<Key,double> key2doubleMap;
const Key keyAnchor = symbol('Z',9999999);
noiseModel::Diagonal::shared_ptr priorOrientationNoise = noiseModel::Diagonal::Variances((Vector(1) << 1e-8));
noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
/* This function computes the cumulative orientation (without wrapping) wrt the root of a spanning tree (tree)
* for a node (nodeKey). The function starts at the nodes and moves towards the root
* summing up the (directed) rotation measurements. Relative measurements are encoded in "deltaThetaMap"
* The root is assumed to have orientation zero.
*/
GTSAM_EXPORT double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree,
const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap);
/* This function computes the cumulative orientations (without wrapping)
* for all node wrt the root (root has zero orientation)
*/
GTSAM_EXPORT key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
const PredecessorMap<Key>& tree);
/* Given a factor graph "g", and a spanning tree "tree", the function selects the nodes belonging to the tree and to g,
* and stores the factor slots corresponding to edges in the tree and to chordsIds wrt this tree
* Also it computes deltaThetaMap which is a fast way to encode relative orientations along the tree:
* for a node key2, s.t. tree[key2]=key1, the values deltaThetaMap[key2] is the relative orientation theta[key2]-theta[key1]
*/
GTSAM_EXPORT void getSymbolicGraph(
/*OUTPUTS*/ std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds, key2doubleMap& deltaThetaMap,
/*INPUTS*/ const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g);
/* Retrieves the deltaTheta and the corresponding noise model from a BetweenFactor<Pose2> */
GTSAM_EXPORT void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta);
/* Linear factor graph with regularized orientation measurements */
GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const std::vector<size_t>& spanningTreeIds, const std::vector<size_t>& chordsIds,
const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
/* Selects the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */
GTSAM_EXPORT NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph);
/* Returns the orientations of a graph including only BetweenFactors<Pose2> */
GTSAM_EXPORT VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true);
/* LAGO: Returns the orientations of the Pose2 in a generic factor graph */
GTSAM_EXPORT VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true);
/* Returns the values for the Pose2 in a generic factor graph */
GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true);
/* Only corrects the orientation part in initialGuess */
GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess);
} // end of namespace gtsam

2
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -244,7 +244,7 @@ void LevenbergMarquardtOptimizer::iterate() {
try {
delta = solve(dampedSystem, state_.values, params_);
systemSolvedSuccessfully = true;
} catch (IndeterminantLinearSystemException& e) {
} catch (IndeterminantLinearSystemException) {
systemSolvedSuccessfully = false;
}

0
gtsam_unstable/slam/ImplicitSchurFactor.h → gtsam/slam/ImplicitSchurFactor.h
View File

0
gtsam_unstable/slam/JacobianFactorQ.h → gtsam/slam/JacobianFactorQ.h
View File

2
gtsam_unstable/slam/JacobianFactorQR.h → gtsam/slam/JacobianFactorQR.h
View File

@ -6,7 +6,7 @@
*/
#pragma once
#include <gtsam_unstable/slam/JacobianSchurFactor.h>
#include <gtsam/slam/JacobianSchurFactor.h>
namespace gtsam {
/**

2
gtsam_unstable/slam/JacobianFactorSVD.h → gtsam/slam/JacobianFactorSVD.h
View File

@ -5,7 +5,7 @@
*/
#pragma once
#include "gtsam_unstable/slam/JacobianSchurFactor.h"
#include "gtsam/slam/JacobianSchurFactor.h"
namespace gtsam {
/**

0
gtsam_unstable/slam/JacobianSchurFactor.h → gtsam/slam/JacobianSchurFactor.h
View File

0
gtsam_unstable/slam/RegularHessianFactor.h → gtsam/slam/RegularHessianFactor.h
View File

4
gtsam_unstable/slam/SmartFactorBase.h → gtsam/slam/SmartFactorBase.h
View File

@ -325,7 +325,7 @@ public:
const Cameras& cameras, const Point3& point,
const double lambda = 0.0) const {
int numKeys = this->keys_.size();
size_t numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
lambda);
@ -352,7 +352,7 @@ public:
Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
Vector s = svd.singularValues();
// Enull = zeros(2 * numKeys, 2 * numKeys - 3);
int numKeys = this->keys_.size();
size_t numKeys = this->keys_.size();
Enull = svd.matrixU().block(0, 3, 2 * numKeys, 2 * numKeys - 3); // last 2m-3 columns
return f;

7
gtsam_unstable/slam/SmartProjectionFactor.h → gtsam/slam/SmartProjectionFactor.h
View File

@ -21,11 +21,10 @@
#include "SmartFactorBase.h"
#include <gtsam_unstable/geometry/triangulation.h>
#include <gtsam/geometry/triangulation.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
#include <gtsam_unstable/geometry/triangulation.h>
#include <boost/optional.hpp>
#include <boost/make_shared.hpp>
@ -54,7 +53,7 @@ public:
double f;
};
enum linearizationType {
enum LinearizationMode {
HESSIAN, JACOBIAN_SVD, JACOBIAN_Q
};
@ -263,7 +262,7 @@ public:
try {
Point2 reprojectionError(camera.project(point_) - zi);
totalReprojError += reprojectionError.vector().norm();
} catch (CheiralityException& e) {
} catch (CheiralityException) {
cheiralityException_ = true;
}
i += 1;

32
gtsam_unstable/slam/SmartProjectionPoseFactor.h → gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -41,9 +41,8 @@ template<class POSE, class LANDMARK, class CALIBRATION>
class SmartProjectionPoseFactor: public SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> {
protected:
linearizationType linearizeTo_;
LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
// Known calibration
std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
public:
@ -69,7 +68,7 @@ public:
SmartProjectionPoseFactor(const double rankTol = 1,
const double linThreshold = -1, const bool manageDegeneracy = false,
const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none,
linearizationType linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}
@ -80,7 +79,7 @@ public:
/**
* add a new measurement and pose key
* @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKey is the index corresponding to the camera observing the same landmark
* @param poseKey is key corresponding to the camera observing the same landmark
* @param noise_i is the measurement noise
* @param K_i is the (known) camera calibration
*/
@ -92,8 +91,11 @@ public:
}
/**
* add a new measurements and pose keys
* Variant of the previous one in which we include a set of measurements
* Variant of the previous one in which we include a set of measurements
* @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
* @param noises vector of measurement noises
* @param Ks vector of calibration objects
*/
void add(std::vector<Point2> measurements, std::vector<Key> poseKeys,
std::vector<SharedNoiseModel> noises,
@ -105,8 +107,11 @@ public:
}
/**
* add a new measurements and pose keys
* Variant of the previous one in which we include a set of measurements with the same noise and calibration
* @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
* @param poseKeys vector of keys corresponding to the camera observing the same landmark
* @param noise measurement noise (same for all measurements)
* @param K the (known) camera calibration (same for all measurements)
*/
void add(std::vector<Point2> measurements, std::vector<Key> poseKeys,
const SharedNoiseModel noise, const boost::shared_ptr<CALIBRATION> K) {
@ -141,7 +146,12 @@ public:
return 6 * this->keys_.size();
}
// Collect all cameras
/**
* Collect all cameras involved in this factor
* @param values Values structure which must contain camera poses corresponding
* to keys involved in this factor
* @return vector of Values
*/
typename Base::Cameras cameras(const Values& values) const {
typename Base::Cameras cameras;
size_t i=0;
@ -154,7 +164,9 @@ public:
}
/**
* linear factor on the poses
* Linearize to Gaussian Factor
* @param values Values structure which must contain camera poses for this factor
* @return
*/
virtual boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const {
@ -184,7 +196,7 @@ public:
}
/** return the calibration object */
inline const boost::shared_ptr<CALIBRATION> calibration() const {
inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
return K_all_;
}

694
gtsam/slam/dataset.cpp
View File

File diff suppressed because it is too large Load Diff

129
gtsam/slam/dataset.h
View File

@ -35,7 +35,7 @@ namespace gtsam {
/**
* Find the full path to an example dataset distributed with gtsam. The name
* may be specified with or without a file extension - if no extension is
* give, this function first looks for the .graph extension, then .txt. We
* given, this function first looks for the .graph extension, then .txt. We
* first check the gtsam source tree for the file, followed by the installed
* example dataset location. Both the source tree and installed locations
* are obtained from CMake during compilation.
@ -44,8 +44,30 @@ namespace gtsam {
* search process described above.
*/
GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
/**
* Creates a temporary file name that needs to be ignored in .gitingnore
* for checking read-write oprations
*/
GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
#endif
/// Indicates how noise parameters are stored in file
enum NoiseFormat {
NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33
NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr
NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix !
NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33
};
/// Robust kernel type to wrap around quadratic noise model
enum KernelFunctionType {
KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
};
/// Return type for load functions
typedef std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> GraphAndValues;
/**
* Load TORO 2D Graph
* @param dataset/model pair as constructed by [dataset]
@ -53,31 +75,57 @@ GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
* @param addNoise add noise to the edges
* @param smart try to reduce complexity of covariance to cheapest model
*/
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
std::pair<std::string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset,
int maxID = 0, bool addNoise = false, bool smart = true);
GTSAM_EXPORT GraphAndValues load2D(
std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0,
bool addNoise = false,
bool smart = true, //
NoiseFormat noiseFormat = NoiseFormatGRAPH,
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
/**
* Load TORO 2D Graph
* Load TORO/G2O style graph files
* @param filename
* @param model optional noise model to use instead of one specified by file
* @param maxID if non-zero cut out vertices >= maxID
* @param addNoise add noise to the edges
* @param smart try to reduce complexity of covariance to cheapest model
* @param noiseFormat how noise parameters are stored
* @param kernelFunctionType whether to wrap the noise model in a robust kernel
* @return graph and initial values
*/
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
const std::string& filename,
boost::optional<gtsam::SharedDiagonal> model = boost::optional<
noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false,
bool smart = true);
GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
SharedNoiseModel model = SharedNoiseModel(), int maxID = 0, bool addNoise =
false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatGRAPH, //
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
const std::string& filename,
gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0);
/// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel
GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename,
noiseModel::Base::shared_ptr& model, int maxID = 0);
/** save 2d graph */
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config,
const noiseModel::Diagonal::shared_ptr model, const std::string& filename);
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
const Values& config, const noiseModel::Diagonal::shared_ptr model,
const std::string& filename);
/**
* @brief This function parses a g2o file and stores the measurements into a
* NonlinearFactorGraph and the initial guess in a Values structure
* @param filename The name of the g2o file
* @param kernelFunctionType whether to wrap the noise model in a robust kernel
* @return graph and initial values
*/
GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile,
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
/**
* @brief This function writes a g2o file from
* NonlinearFactorGraph and a Values structure
* @param filename The name of the g2o file to write
* @param graph NonlinearFactor graph storing the measurements
* @param estimate Values
*/
GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
const Values& estimate, const std::string& filename);
/**
* Load TORO 3D Graph
@ -85,27 +133,31 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config
GTSAM_EXPORT bool load3D(const std::string& filename);
/// A measurement with its camera index
typedef std::pair<size_t,gtsam::Point2> SfM_Measurement;
typedef std::pair<size_t, Point2> SfM_Measurement;
/// Define the structure for the 3D points
struct SfM_Track
{
gtsam::Point3 p; ///< 3D position of the point
float r,g,b; ///< RGB color of the 3D point
struct SfM_Track {
Point3 p; ///< 3D position of the point
float r, g, b; ///< RGB color of the 3D point
std::vector<SfM_Measurement> measurements; ///< The 2D image projections (id,(u,v))
size_t number_measurements() const { return measurements.size();}
size_t number_measurements() const {
return measurements.size();
}
};
/// Define the structure for the camera poses
typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> SfM_Camera;
typedef PinholeCamera<Cal3Bundler> SfM_Camera;
/// Define the structure for SfM data
struct SfM_data
{
std::vector<SfM_Camera> cameras; ///< Set of cameras
struct SfM_data {
std::vector<SfM_Camera> cameras; ///< Set of cameras
std::vector<SfM_Track> tracks; ///< Sparse set of points
size_t number_cameras() const { return cameras.size();} ///< The number of camera poses
size_t number_tracks() const { return tracks.size();} ///< The number of reconstructed 3D points
size_t number_cameras() const {
return cameras.size();
} ///< The number of camera poses
size_t number_tracks() const {
return tracks.size();
} ///< The number of reconstructed 3D points
};
/**
@ -117,25 +169,6 @@ struct SfM_data
*/
GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data);
/**
* @brief This function parses a g2o file and stores the measurements into a
* NonlinearFactorGraph and the initial guess in a Values structure
* @param filename The name of the g2o file
* @param graph NonlinearFactor graph storing the measurements (EDGE_SE2). NOTE: information matrix is assumed diagonal.
* @return initial Values containing the initial guess (VERTEX_SE2)
*/
enum kernelFunctionType { QUADRATIC, HUBER, TUKEY };
GTSAM_EXPORT bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction = QUADRATIC);
/**
* @brief This function writes a g2o file from
* NonlinearFactorGraph and a Values structure
* @param filename The name of the g2o file to write
* @param graph NonlinearFactor graph storing the measurements (EDGE_SE2)
* @return estimate Values containing the values (VERTEX_SE2)
*/
GTSAM_EXPORT bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate);
/**
* @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
* SfM_data structure
@ -165,7 +198,8 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data);
* assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
* @return true if the parsing was successful, false otherwise
*/
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, const SfM_data &data, Values& values);
GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
const SfM_data &data, Values& values);
/**
* @brief This function converts an openGL camera pose to an GTSAM camera pose
@ -208,5 +242,4 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfM_data& db);
*/
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_data& db);
} // namespace gtsam

258
gtsam/nonlinear/LagoInitializer.cpp → gtsam/slam/lago.cpp
View File

@ -10,38 +10,60 @@
* -------------------------------------------------------------------------- */
/**
* @file LagoInitializer.h
* @file lago.h
* @author Luca Carlone
* @author Frank Dellaert
* @date May 14, 2014
*/
#include <gtsam/nonlinear/LagoInitializer.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/lago.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/base/timing.h>
#include <boost/math/special_functions.hpp>
namespace gtsam {
using namespace std;
static Matrix I = eye(1);
static Matrix I3 = eye(3);
namespace gtsam {
namespace lago {
static const Matrix I = eye(1);
static const Matrix I3 = eye(3);
static const Key keyAnchor = symbol('Z', 9999999);
static const noiseModel::Diagonal::shared_ptr priorOrientationNoise =
noiseModel::Diagonal::Sigmas((Vector(1) << 0));
static const noiseModel::Diagonal::shared_ptr priorPose2Noise =
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
/* ************************************************************************* */
double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree,
const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap) {
/**
* Compute the cumulative orientation (without wrapping) wrt the root of a
* spanning tree (tree) for a node (nodeKey). The function starts at the nodes and
* moves towards the root summing up the (directed) rotation measurements.
* Relative measurements are encoded in "deltaThetaMap".
* The root is assumed to have orientation zero.
*/
static double computeThetaToRoot(const Key nodeKey,
const PredecessorMap<Key>& tree, const key2doubleMap& deltaThetaMap,
const key2doubleMap& thetaFromRootMap) {
double nodeTheta = 0;
Key key_child = nodeKey; // the node
Key key_parent = 0; // the initialization does not matter
while(1){
while (1) {
// We check if we reached the root
if(tree.at(key_child)==key_child) // if we reached the root
if (tree.at(key_child) == key_child) // if we reached the root
break;
// we sum the delta theta corresponding to the edge parent->child
nodeTheta += deltaThetaMap.at(key_child);
// we get the parent
key_parent = tree.at(key_child); // the parent
// we check if we connected to some part of the tree we know
if(thetaFromRootMap.find(key_parent)!=thetaFromRootMap.end()){
if (thetaFromRootMap.find(key_parent) != thetaFromRootMap.end()) {
nodeTheta += thetaFromRootMap.at(key_parent);
break;
}
@ -55,53 +77,55 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
const PredecessorMap<Key>& tree) {
key2doubleMap thetaToRootMap;
key2doubleMap::const_iterator it;
// Orientation of the roo
thetaToRootMap.insert(std::pair<Key, double>(keyAnchor, 0.0));
thetaToRootMap.insert(pair<Key, double>(keyAnchor, 0.0));
// for all nodes in the tree
for(it = deltaThetaMap.begin(); it != deltaThetaMap.end(); ++it ){
BOOST_FOREACH(const key2doubleMap::value_type& it, deltaThetaMap) {
// compute the orientation wrt root
Key nodeKey = it->first;
Key nodeKey = it.first;
double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap,
thetaToRootMap);
thetaToRootMap.insert(std::pair<Key, double>(nodeKey, nodeTheta));
thetaToRootMap.insert(pair<Key, double>(nodeKey, nodeTheta));
}
return thetaToRootMap;
}
/* ************************************************************************* */
void getSymbolicGraph(
/*OUTPUTS*/ std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds, key2doubleMap& deltaThetaMap,
/*INPUTS*/ const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g){
/*OUTPUTS*/vector<size_t>& spanningTreeIds, vector<size_t>& chordsIds,
key2doubleMap& deltaThetaMap,
/*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g) {
// Get keys for which you want the orientation
size_t id=0;
size_t id = 0;
// Loop over the factors
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g){
if (factor->keys().size() == 2){
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g) {
if (factor->keys().size() == 2) {
Key key1 = factor->keys()[0];
Key key2 = factor->keys()[1];
// recast to a between
boost::shared_ptr< BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor);
if (!pose2Between) continue;
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
if (!pose2Between)
continue;
// get the orientation - measured().theta();
double deltaTheta = pose2Between->measured().theta();
// insert (directed) orientations in the map "deltaThetaMap"
bool inTree=false;
if(tree.at(key1)==key2){ // key2 -> key1
deltaThetaMap.insert(std::pair<Key, double>(key1, -deltaTheta));
bool inTree = false;
if (tree.at(key1) == key2) { // key2 -> key1
deltaThetaMap.insert(pair<Key, double>(key1, -deltaTheta));
inTree = true;
} else if(tree.at(key2)==key1){ // key1 -> key2
deltaThetaMap.insert(std::pair<Key, double>(key2, deltaTheta));
} else if (tree.at(key2) == key1) { // key1 -> key2
deltaThetaMap.insert(pair<Key, double>(key2, deltaTheta));
inTree = true;
}
// store factor slot, distinguishing spanning tree edges from chordsIds
if(inTree == true)
if (inTree == true)
spanningTreeIds.push_back(id);
else // it's a chord!
else
// it's a chord!
chordsIds.push_back(id);
}
id++;
@ -109,14 +133,16 @@ void getSymbolicGraph(
}
/* ************************************************************************* */
void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
// Retrieve the deltaTheta and the corresponding noise model from a BetweenFactor<Pose2>
static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta) {
// Get the relative rotation measurement from the between factor
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
if (!pose2Between)
throw std::invalid_argument("buildLinearOrientationGraph: invalid between factor!");
throw invalid_argument(
"buildLinearOrientationGraph: invalid between factor!");
deltaTheta = (Vector(1) << pose2Between->measured().theta());
// Retrieve the noise model for the relative rotation
@ -124,114 +150,127 @@ void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonalModel)
throw std::invalid_argument("buildLinearOrientationGraph: invalid noise model "
throw invalid_argument("buildLinearOrientationGraph: invalid noise model "
"(current version assumes diagonal noise model)!");
Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2) ); // std on the angular measurement
Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2)); // std on the angular measurement
model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta);
}
/* ************************************************************************* */
GaussianFactorGraph buildLinearOrientationGraph(const std::vector<size_t>& spanningTreeIds, const std::vector<size_t>& chordsIds,
const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree){
GaussianFactorGraph buildLinearOrientationGraph(
const vector<size_t>& spanningTreeIds, const vector<size_t>& chordsIds,
const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot,
const PredecessorMap<Key>& tree) {
GaussianFactorGraph lagoGraph;
Vector deltaTheta;
noiseModel::Diagonal::shared_ptr model_deltaTheta;
// put original measurements in the spanning tree
BOOST_FOREACH(const size_t& factorId, spanningTreeIds){
BOOST_FOREACH(const size_t& factorId, spanningTreeIds) {
const FastVector<Key>& keys = g[factorId]->keys();
Key key1 = keys[0], key2 = keys[1];
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta));
lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta);
}
// put regularized measurements in the chordsIds
BOOST_FOREACH(const size_t& factorId, chordsIds){
BOOST_FOREACH(const size_t& factorId, chordsIds) {
const FastVector<Key>& keys = g[factorId]->keys();
Key key1 = keys[0], key2 = keys[1];
getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
double key1_DeltaTheta_key2 = deltaTheta(0);
///std::cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << std::endl;
double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord
double k = boost::math::round(k2pi_noise/(2*M_PI));
//if (k2pi_noise - 2*k*M_PI > 1e-5) std::cout << k2pi_noise - 2*k*M_PI << std::endl; // for debug
Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*M_PI);
lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta));
///cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << endl;
double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1)
- orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord
double k = boost::math::round(k2pi_noise / (2 * M_PI));
//if (k2pi_noise - 2*k*M_PI > 1e-5) cout << k2pi_noise - 2*k*M_PI << endl; // for debug
Vector deltaThetaRegularized = (Vector(1)
<< key1_DeltaTheta_key2 - 2 * k * M_PI);
lagoGraph.add(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta);
}
// prior on the anchor orientation
lagoGraph.add(JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise));
lagoGraph.add(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise);
return lagoGraph;
}
/* ************************************************************************* */
NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph){
// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) {
gttic(lago_buildPose2graph);
NonlinearFactorGraph pose2Graph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph){
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) {
// recast to a between on Pose2
boost::shared_ptr< BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor);
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
if (pose2Between)
pose2Graph.add(pose2Between);
// recast PriorFactor<Pose2> to BetweenFactor<Pose2>
boost::shared_ptr< PriorFactor<Pose2> > pose2Prior =
boost::dynamic_pointer_cast< PriorFactor<Pose2> >(factor);
boost::shared_ptr<PriorFactor<Pose2> > pose2Prior =
boost::dynamic_pointer_cast<PriorFactor<Pose2> >(factor);
if (pose2Prior)
pose2Graph.add(BetweenFactor<Pose2>(keyAnchor, pose2Prior->keys()[0],
pose2Prior->prior(), pose2Prior->get_noiseModel()));
pose2Graph.add(
BetweenFactor<Pose2>(keyAnchor, pose2Prior->keys()[0],
pose2Prior->prior(), pose2Prior->get_noiseModel()));
}
return pose2Graph;
}
/* ************************************************************************* */
PredecessorMap<Key> findOdometricPath(const NonlinearFactorGraph& pose2Graph) {
static PredecessorMap<Key> findOdometricPath(
const NonlinearFactorGraph& pose2Graph) {
PredecessorMap<Key> tree;
Key minKey;
bool minUnassigned = true;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2Graph){
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2Graph) {
Key key1 = std::min(factor->keys()[0], factor->keys()[1]);
Key key2 = std::max(factor->keys()[0], factor->keys()[1]);
if(minUnassigned){
if (minUnassigned) {
minKey = key1;
minUnassigned = false;
}
if( key2 - key1 == 1){ // consecutive keys
if (key2 - key1 == 1) { // consecutive keys
tree.insert(key2, key1);
if(key1 < minKey)
if (key1 < minKey)
minKey = key1;
}
}
tree.insert(minKey,keyAnchor);
tree.insert(keyAnchor,keyAnchor); // root
tree.insert(minKey, keyAnchor);
tree.insert(keyAnchor, keyAnchor); // root
return tree;
}
/* ************************************************************************* */
VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){
// Return the orientations of a graph including only BetweenFactors<Pose2>
static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
bool useOdometricPath) {
gttic(lago_computeOrientations);
// Find a minimum spanning tree
PredecessorMap<Key> tree;
if (useOdometricPath)
tree = findOdometricPath(pose2Graph);
else
tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, BetweenFactor<Pose2> >(pose2Graph);
tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(pose2Graph);
// Create a linear factor graph (LFG) of scalars
key2doubleMap deltaThetaMap;
std::vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
std::vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph);
// temporary structure to correct wraparounds along loops
key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree);
// regularize measurements and plug everything in a factor graph
GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree);
GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds,
chordsIds, pose2Graph, orientationsToRoot, tree);
// Solve the LFG
VectorValues orientationsLago = lagoGraph.optimize();
@ -240,70 +279,79 @@ VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, boo
}
/* ************************************************************************* */
VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath) {
VectorValues initializeOrientations(const NonlinearFactorGraph& graph,
bool useOdometricPath) {
// We "extract" the Pose2 subgraph of the original graph: this
// is done to properly model priors and avoiding operating on a larger graph
NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
// Get orientations from relative orientation measurements
return computeLagoOrientations(pose2Graph, useOdometricPath);
return computeOrientations(pose2Graph, useOdometricPath);
}
/* ************************************************************************* */
Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) {
Values computePoses(const NonlinearFactorGraph& pose2graph,
VectorValues& orientationsLago) {
gttic(lago_computePoses);
// Linearized graph on full poses
GaussianFactorGraph linearPose2graph;
// We include the linear version of each between factor
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2graph){
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2graph) {
boost::shared_ptr< BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor);
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
if(pose2Between){
if (pose2Between) {
Key key1 = pose2Between->keys()[0];
double theta1 = orientationsLago.at(key1)(0);
double s1 = sin(theta1); double c1 = cos(theta1);
double s1 = sin(theta1);
double c1 = cos(theta1);
Key key2 = pose2Between->keys()[1];
double theta2 = orientationsLago.at(key2)(0);
double linearDeltaRot = theta2 - theta1 - pose2Between->measured().theta();
double linearDeltaRot = theta2 - theta1
- pose2Between->measured().theta();
linearDeltaRot = Rot2(linearDeltaRot).theta(); // to normalize
double dx = pose2Between->measured().x();
double dy = pose2Between->measured().y();
Vector globalDeltaCart = (Vector(2) << c1*dx - s1*dy, s1*dx + c1*dy);
Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot );// rhs
Matrix J1 = - I3;
J1(0,2) = s1*dx + c1*dy;
J1(1,2) = -c1*dx + s1*dy;
Vector globalDeltaCart = //
(Vector(2) << c1 * dx - s1 * dy, s1 * dx + c1 * dy);
Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs
Matrix J1 = -I3;
J1(0, 2) = s1 * dx + c1 * dy;
J1(1, 2) = -c1 * dx + s1 * dy;
// Retrieve the noise model for the relative rotation
boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(pose2Between->get_noiseModel());
boost::dynamic_pointer_cast<noiseModel::Diagonal>(
pose2Between->get_noiseModel());
linearPose2graph.add(JacobianFactor(key1, J1, key2, I3, b, diagonalModel));
}else{
throw std::invalid_argument("computeLagoPoses: cannot manage non between factor here!");
linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel);
} else {
throw invalid_argument(
"computeLagoPoses: cannot manage non between factor here!");
}
}
// add prior
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
linearPose2graph.add(JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0,0.0,0.0), priorModel));
linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0),
priorPose2Noise);
// optimize
VectorValues posesLago = linearPose2graph.optimize();
// put into Values structure
Values initialGuessLago;
for(VectorValues::const_iterator it = posesLago.begin(); it != posesLago.end(); ++it ){
Key key = it->first;
if (key != keyAnchor){
Vector poseVector = posesLago.at(key);
Pose2 poseLago = Pose2(poseVector(0),poseVector(1),orientationsLago.at(key)(0)+poseVector(2));
BOOST_FOREACH(const VectorValues::value_type& it, posesLago) {
Key key = it.first;
if (key != keyAnchor) {
const Vector& poseVector = it.second;
Pose2 poseLago = Pose2(poseVector(0), poseVector(1),
orientationsLago.at(key)(0) + poseVector(2));
initialGuessLago.insert(key, poseLago);
}
}
@ -311,37 +359,41 @@ Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& or
}
/* ************************************************************************* */
Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath) {
Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) {
gttic(lago_initialize);
// We "extract" the Pose2 subgraph of the original graph: this
// is done to properly model priors and avoiding operating on a larger graph
NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
// Get orientations from relative orientation measurements
VectorValues orientationsLago = computeLagoOrientations(pose2Graph, useOdometricPath);
VectorValues orientationsLago = computeOrientations(pose2Graph,
useOdometricPath);
// Compute the full poses
return computeLagoPoses(pose2Graph, orientationsLago);
return computePoses(pose2Graph, orientationsLago);
}
/* ************************************************************************* */
Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess) {
Values initialize(const NonlinearFactorGraph& graph,
const Values& initialGuess) {
Values initialGuessLago;
// get the orientation estimates from LAGO
VectorValues orientations = initializeOrientationsLago(graph);
VectorValues orientations = initializeOrientations(graph);
// for all nodes in the tree
for(VectorValues::const_iterator it = orientations.begin(); it != orientations.end(); ++it ){
Key key = it->first;
if (key != keyAnchor){
Pose2 pose = initialGuess.at<Pose2>(key);
Vector orientation = orientations.at(key);
Pose2 poseLago = Pose2(pose.x(),pose.y(),orientation(0));
BOOST_FOREACH(const VectorValues::value_type& it, orientations) {
Key key = it.first;
if (key != keyAnchor) {
const Pose2& pose = initialGuess.at<Pose2>(key);
const Vector& orientation = it.second;
Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0));
initialGuessLago.insert(key, poseLago);
}
}
return initialGuessLago;
}
} // end of namespace lago
} // end of namespace gtsam

86
gtsam/slam/lago.h Normal file
View File

@ -0,0 +1,86 @@
/* ----------------------------------------------------------------------------
* 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 lago.h
* @brief Initialize Pose2 in a factor graph using LAGO
* (Linear Approximation for Graph Optimization). see papers:
*
* L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate
* approximation for planar pose graph optimization, IJRR, 2014.
*
* L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation
* for graph-based simultaneous localization and mapping, RSS, 2011.
*
* @param graph: nonlinear factor graph (can include arbitrary factors but we assume
* that there is a subgraph involving Pose2 and betweenFactors). Also in the current
* version we assume that there is an odometric spanning path (x0->x1, x1->x2, etc)
* and a prior on x0. This assumption can be relaxed by using the extra argument
* useOdometricPath = false, although this part of code is not stable yet.
* @return Values: initial guess from LAGO (only pose2 are initialized)
*
* @author Luca Carlone
* @author Frank Dellaert
* @date May 14, 2014
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph.h>
namespace gtsam {
namespace lago {
typedef std::map<Key, double> key2doubleMap;
/**
* Compute the cumulative orientations (without wrapping)
* for all nodes wrt the root (root has zero orientation).
*/
GTSAM_EXPORT key2doubleMap computeThetasToRoot(
const key2doubleMap& deltaThetaMap, const PredecessorMap<Key>& tree);
/**
* Given a factor graph "g", and a spanning tree "tree", select the nodes belonging
* to the tree and to g, and stores the factor slots corresponding to edges in the
* tree and to chordsIds wrt this tree.
* Also it computes deltaThetaMap which is a fast way to encode relative
* orientations along the tree: for a node key2, s.t. tree[key2]=key1,
* the value deltaThetaMap[key2] is relative orientation theta[key2]-theta[key1]
*/
GTSAM_EXPORT void getSymbolicGraph(
/*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds,
key2doubleMap& deltaThetaMap,
/*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g);
/** Linear factor graph with regularized orientation measurements */
GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
const std::vector<size_t>& spanningTreeIds,
const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
/** LAGO: Return the orientations of the Pose2 in a generic factor graph */
GTSAM_EXPORT VectorValues initializeOrientations(
const NonlinearFactorGraph& graph, bool useOdometricPath = true);
/** Return the values for the Pose2 in a generic factor graph */
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
bool useOdometricPath = true);
/** Only correct the orientation part in initialGuess */
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
const Values& initialGuess);
} // end of namespace lago
} // end of namespace gtsam

77
gtsam/slam/tests/testDataset.cpp
View File

@ -40,18 +40,21 @@ TEST(dataSet, findExampleDataFile) {
}
/* ************************************************************************* */
//TEST( dataSet, load2D)
//{
// ///< The structure where we will save the SfM data
// const string filename = findExampleDataFile("smallGraph.g2o");
// boost::tie(graph,initialGuess) = load2D(filename, boost::none, 10000,
// false, false);
//// print
////
//// print
////
//// EXPECT(assert_equal(expected,actual,12));
//}
TEST( dataSet, load2D)
{
///< The structure where we will save the SfM data
const string filename = findExampleDataFile("w100.graph");
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
boost::tie(graph, initial) = load2D(filename);
EXPECT_LONGS_EQUAL(300,graph->size());
EXPECT_LONGS_EQUAL(100,initial->size());
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3);
BetweenFactor<Pose2> expected(1, 0, Pose2(-0.99879,0.0417574,-0.00818381), model);
BetweenFactor<Pose2>::shared_ptr actual = boost::dynamic_pointer_cast<
BetweenFactor<Pose2> >(graph->at(0));
EXPECT(assert_equal(expected, *actual));
}
/* ************************************************************************* */
TEST( dataSet, Balbianello)
@ -78,9 +81,9 @@ TEST( dataSet, Balbianello)
TEST( dataSet, readG2o)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph actualGraph;
Values actualValues;
readG2o(g2oFile, actualGraph, actualValues);
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile);
Values expectedValues;
expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000));
@ -94,7 +97,7 @@ TEST( dataSet, readG2o)
expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391));
expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016));
expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934));
EXPECT(assert_equal(expectedValues,actualValues,1e-5));
EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
NonlinearFactorGraph expectedGraph;
@ -110,16 +113,16 @@ TEST( dataSet, readG2o)
expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
}
/* ************************************************************************* */
TEST( dataSet, readG2oHuber)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph actualGraph;
Values actualValues;
readG2o(g2oFile, actualGraph, actualValues, HUBER);
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeHUBER);
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel);
@ -137,16 +140,16 @@ TEST( dataSet, readG2oHuber)
expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
}
/* ************************************************************************* */
TEST( dataSet, readG2oTukey)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph actualGraph;
Values actualValues;
readG2o(g2oFile, actualGraph, actualValues, TUKEY);
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeTUKEY);
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
@ -164,25 +167,25 @@ TEST( dataSet, readG2oTukey)
expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
}
/* ************************************************************************* */
TEST( dataSet, writeG2o)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph expectedGraph;
Values expectedValues;
readG2o(g2oFile, expectedGraph, expectedValues);
NonlinearFactorGraph::shared_ptr expectedGraph;
Values::shared_ptr expectedValues;
boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile);
const string filenameToWrite = findExampleDataFile("pose2example-rewritten");
writeG2o(filenameToWrite, expectedGraph, expectedValues);
const string filenameToWrite = createRewrittenFileName(g2oFile);
writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
NonlinearFactorGraph actualGraph;
Values actualValues;
readG2o(filenameToWrite, actualGraph, actualValues);
EXPECT(assert_equal(expectedValues,actualValues,1e-5));
EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite);
EXPECT(assert_equal(*expectedValues,*actualValues,1e-5));
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-5));
}
/* ************************************************************************* */
@ -249,7 +252,7 @@ TEST( dataSet, writeBAL_Dubrovnik)
readBAL(filenameToRead, readData);
// Write readData to file filenameToWrite
const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten");
const string filenameToWrite = createRewrittenFileName(filenameToRead);
CHECK(writeBAL(filenameToWrite, readData));
// Read what we wrote
@ -311,7 +314,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
}
// Write values and readData to a file
const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten");
const string filenameToWrite = createRewrittenFileName(filenameToRead);
writeBALfromValues(filenameToWrite, readData, value);
// Read the file we wrote

152
gtsam/nonlinear/tests/testLagoInitializer.cpp → gtsam/slam/tests/testLago.cpp
View File

@ -19,27 +19,21 @@
* @date May 14, 2014
*/
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/lago.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LagoInitializer.h>
#include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/math/constants/constants.hpp>
#include <cmath>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3);
static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3);
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
namespace simple {
@ -54,10 +48,10 @@ namespace simple {
// x0
//
Pose2 pose0 = Pose2(0.000000, 0.000000, 0.000000);
Pose2 pose1 = Pose2(1.000000, 1.000000, 1.570796);
Pose2 pose2 = Pose2(0.000000, 2.000000, 3.141593);
Pose2 pose3 = Pose2(-1.000000, 1.000000, 4.712389);
static Pose2 pose0 = Pose2(0.000000, 0.000000, 0.000000);
static Pose2 pose1 = Pose2(1.000000, 1.000000, 1.570796);
static Pose2 pose2 = Pose2(0.000000, 2.000000, 3.141593);
static Pose2 pose3 = Pose2(-1.000000, 1.000000, 4.712389);
NonlinearFactorGraph graph() {
NonlinearFactorGraph g;
@ -77,10 +71,10 @@ TEST( Lago, checkSTandChords ) {
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g);
key2doubleMap deltaThetaMap;
lago::key2doubleMap deltaThetaMap;
vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
DOUBLES_EQUAL(spanningTreeIds[0], 0, 1e-6); // factor 0 is the first in the ST (0->1)
DOUBLES_EQUAL(spanningTreeIds[1], 3, 1e-6); // factor 3 is the second in the ST(2->0)
@ -100,19 +94,19 @@ TEST( Lago, orientationsOverSpanningTree ) {
EXPECT_LONGS_EQUAL(tree[x2], x0);
EXPECT_LONGS_EQUAL(tree[x3], x0);
key2doubleMap expected;
lago::key2doubleMap expected;
expected[x0]= 0;
expected[x1]= M_PI/2; // edge x0->x1 (consistent with edge (x0,x1))
expected[x2]= -M_PI; // edge x0->x2 (traversed backwards wrt edge (x2,x0))
expected[x3]= -M_PI/2; // edge x0->x3 (consistent with edge (x0,x3))
key2doubleMap deltaThetaMap;
lago::key2doubleMap deltaThetaMap;
vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
key2doubleMap actual;
actual = computeThetasToRoot(deltaThetaMap, tree);
lago::key2doubleMap actual;
actual = lago::computeThetasToRoot(deltaThetaMap, tree);
DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6);
DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6);
DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6);
@ -125,14 +119,14 @@ TEST( Lago, regularizedMeasurements ) {
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g);
key2doubleMap deltaThetaMap;
lago::key2doubleMap deltaThetaMap;
vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree);
lago::key2doubleMap orientationsToRoot = lago::computeThetasToRoot(deltaThetaMap, tree);
GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree);
GaussianFactorGraph lagoGraph = lago::buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree);
std::pair<Matrix,Vector> actualAb = lagoGraph.jacobian();
// jacobian corresponding to the orientation measurements (last entry is the prior on the anchor and is disregarded)
Vector actual = (Vector(5) << actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4));
@ -147,25 +141,25 @@ TEST( Lago, regularizedMeasurements ) {
/* *************************************************************************** */
TEST( Lago, smallGraphVectorValues ) {
bool useOdometricPath = false;
VectorValues initialGuessLago = initializeOrientationsLago(simple::graph(), useOdometricPath);
VectorValues initial = lago::initializeOrientations(simple::graph(), useOdometricPath);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
TEST( Lago, smallGraphVectorValuesSP ) {
VectorValues initialGuessLago = initializeOrientationsLago(simple::graph());
VectorValues initial = lago::initializeOrientations(simple::graph());
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
@ -173,26 +167,26 @@ TEST( Lago, multiplePosePriors ) {
bool useOdometricPath = false;
NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Pose2>(x1, simple::pose1, model));
VectorValues initialGuessLago = initializeOrientationsLago(g, useOdometricPath);
VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
TEST( Lago, multiplePosePriorsSP ) {
NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Pose2>(x1, simple::pose1, model));
VectorValues initialGuessLago = initializeOrientationsLago(g);
VectorValues initial = lago::initializeOrientations(g);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
@ -200,26 +194,26 @@ TEST( Lago, multiplePoseAndRotPriors ) {
bool useOdometricPath = false;
NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model));
VectorValues initialGuessLago = initializeOrientationsLago(g, useOdometricPath);
VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
TEST( Lago, multiplePoseAndRotPriorsSP ) {
NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model));
VectorValues initialGuessLago = initializeOrientationsLago(g);
VectorValues initial = lago::initializeOrientations(g);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
@ -233,7 +227,7 @@ TEST( Lago, smallGraphValues ) {
initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.y(),0.0));
// lago does not touch the Cartesian part and only fixed the orientations
Values actual = initializeLago(simple::graph(), initialGuess);
Values actual = lago::initialize(simple::graph(), initialGuess);
// we are in a noiseless case
Values expected;
@ -249,7 +243,7 @@ TEST( Lago, smallGraphValues ) {
TEST( Lago, smallGraph2 ) {
// lago does not touch the Cartesian part and only fixed the orientations
Values actual = initializeLago(simple::graph());
Values actual = lago::initialize(simple::graph());
// we are in a noiseless case
Values expected;
@ -264,17 +258,17 @@ TEST( Lago, smallGraph2 ) {
/* *************************************************************************** */
TEST( Lago, largeGraphNoisy_orientations ) {
NonlinearFactorGraph g;
Values initial;
string inputFile = findExampleDataFile("noisyToyGraph");
readG2o(inputFile, g, initial);
NonlinearFactorGraph::shared_ptr g;
Values::shared_ptr initial;
boost::tie(g, initial) = readG2o(inputFile);
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = g;
NonlinearFactorGraph graphWithPrior = *g;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
VectorValues actualVV = initializeOrientationsLago(graphWithPrior);
VectorValues actualVV = lago::initializeOrientations(graphWithPrior);
Values actual;
Key keyAnc = symbol('Z',9999999);
for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){
@ -285,40 +279,40 @@ TEST( Lago, largeGraphNoisy_orientations ) {
actual.insert(key, poseLago);
}
}
NonlinearFactorGraph gmatlab;
Values expected;
string matlabFile = findExampleDataFile("orientationsNoisyToyGraph");
readG2o(matlabFile, gmatlab, expected);
NonlinearFactorGraph::shared_ptr gmatlab;
Values::shared_ptr expected;
boost::tie(gmatlab, expected) = readG2o(matlabFile);
BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){
BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){
Key k = key_val.key;
EXPECT(assert_equal(expected.at<Pose2>(k), actual.at<Pose2>(k), 1e-5));
EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-5));
}
}
/* *************************************************************************** */
TEST( Lago, largeGraphNoisy ) {
NonlinearFactorGraph g;
Values initial;
string inputFile = findExampleDataFile("noisyToyGraph");
readG2o(inputFile, g, initial);
NonlinearFactorGraph::shared_ptr g;
Values::shared_ptr initial;
boost::tie(g, initial) = readG2o(inputFile);
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = g;
NonlinearFactorGraph graphWithPrior = *g;
noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
Values actual = initializeLago(graphWithPrior);
Values actual = lago::initialize(graphWithPrior);
NonlinearFactorGraph gmatlab;
Values expected;
string matlabFile = findExampleDataFile("optimizedNoisyToyGraph");
readG2o(matlabFile, gmatlab, expected);
NonlinearFactorGraph::shared_ptr gmatlab;
Values::shared_ptr expected;
boost::tie(gmatlab, expected) = readG2o(matlabFile);
BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){
BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){
Key k = key_val.key;
EXPECT(assert_equal(expected.at<Pose2>(k), actual.at<Pose2>(k), 1e-2));
EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-2));
}
}

6
gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp → gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -52,9 +52,9 @@ using symbol_shorthand::X;
using symbol_shorthand::L;
// tests data
Symbol x1('X', 1);
Symbol x2('X', 2);
Symbol x3('X', 3);
static Symbol x1('X', 1);
static Symbol x2('X', 2);
static Symbol x3('X', 3);
static Key poseKey1(x1);
static Point2 measurement1(323.0, 240.0);

82
gtsam/slam/tests/timeLago.cpp Normal file
View File

@ -0,0 +1,82 @@
/* ----------------------------------------------------------------------------
* 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 timeVirtual.cpp
* @brief Time the overhead of using virtual destructors and methods
* @author Richard Roberts
* @date Dec 3, 2010
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/lago.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/linear/Sampler.h>
#include <gtsam/base/timing.h>
#include <iostream>
using namespace std;
using namespace gtsam;
int main(int argc, char *argv[]) {
size_t trials = 1;
// read graph
Values::shared_ptr solution;
NonlinearFactorGraph::shared_ptr g;
string inputFile = findExampleDataFile("w10000");
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
boost::tie(g, solution) = load2D(inputFile, model);
// add noise to create initial estimate
Values initial;
Sampler sampler(42u);
Values::ConstFiltered<Pose2> poses = solution->filter<Pose2>();
SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0));
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& it, poses)
initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise)));
// Add prior on the pose having index (key) = 0
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8));
g->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
// LAGO
for (size_t i = 0; i < trials; i++) {
{
gttic_(lago);
gttic_(init);
Values lagoInitial = lago::initialize(*g);
gttoc_(init);
gttic_(refine);
GaussNewtonOptimizer optimizer(*g, lagoInitial);
Values result = optimizer.optimize();
gttoc_(refine);
}
{
gttic_(optimize);
GaussNewtonOptimizer optimizer(*g, initial);
Values result = optimizer.optimize();
}
tictoc_finishedIteration_();
}
tictoc_print_();
return 0;
}

121
gtsam_unstable/examples/SmartProjectionFactorExample.cpp Normal file
View File

@ -0,0 +1,121 @@
/* ----------------------------------------------------------------------------
* 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 SmartProjectionFactorExample.cpp
* @brief A stereo visual odometry example
* @date May 30, 2014
* @author Stephen Camp
* @author Chris Beall
*/
/**
* A smart projection factor example based on stereo data, throwing away the
* measurement from the right camera
* -robot starts at origin
* -moves forward, taking periodic stereo measurements
* -makes monocular observations of many landmarks
*/
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearEquality.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/SmartProjectionPoseFactor.h>
#include <string>
#include <fstream>
#include <iostream>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
typedef SmartProjectionPoseFactor<Pose3, Point3, Cal3_S2> SmartFactor;
Values initial_estimate;
NonlinearFactorGraph graph;
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,1);
string calibration_loc = findExampleDataFile("VO_calibration.txt");
string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
//read camera calibration info from file
// focal lengths fx, fy, skew s, principal point u0, v0, baseline b
cout << "Reading calibration info" << endl;
ifstream calibration_file(calibration_loc.c_str());
double fx, fy, s, u0, v0, b;
calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
const Cal3_S2::shared_ptr K(new Cal3_S2(fx, fy, s, u0, v0));
cout << "Reading camera poses" << endl;
ifstream pose_file(pose_loc.c_str());
int pose_id;
MatrixRowMajor m(4,4);
//read camera pose parameters and use to make initial estimates of camera poses
while (pose_file >> pose_id) {
for (int i = 0; i < 16; i++) {
pose_file >> m.data()[i];
}
initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
}
// camera and landmark keys
size_t x, l;
// pixel coordinates uL, uR, v (same for left/right images due to rectification)
// landmark coordinates X, Y, Z in camera frame, resulting from triangulation
double uL, uR, v, X, Y, Z;
ifstream factor_file(factor_loc.c_str());
cout << "Reading stereo factors" << endl;
//read stereo measurements and construct smart factors
SmartFactor::shared_ptr factor(new SmartFactor());
size_t current_l = 3; // hardcoded landmark ID from first measurement
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
if(current_l != l) {
graph.push_back(factor);
factor = SmartFactor::shared_ptr(new SmartFactor());
current_l = l;
}
factor->add(Point2(uL,v), Symbol('x',x), model, K);
}
Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
//constrain the first pose such that it cannot change from its original value during optimization
// NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
// QR is much slower than Cholesky, but numerically more stable
graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to optimize the factor graph
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
Values result = optimizer.optimize();
cout << "Final result sample:" << endl;
Values pose_values = result.filter<Pose3>();
pose_values.print("Final camera poses:\n");
return 0;
}

2
wrap/FileWriter.cpp
View File

@ -28,7 +28,7 @@ void FileWriter::emit(bool add_header, bool force_overwrite) const {
bool file_exists = true;
try {
existing_contents = file_contents(filename_.c_str(), add_header);
} catch (CantOpenFile& e) {
} catch (CantOpenFile) {
file_exists = false;
}