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

Merge remote-tracking branch 'origin/release/3.2.0' into feature/quadratic_programming 

This should make merging in develop easier, and it also helps me understand what changed.
I mostly avoided conflicts by keeping Duy's versions of:

Conflicts:
	gtsam/3rdparty/metis-5.1.0/CMakeLists.txt
	gtsam/linear/JacobianFactor-inl.h
	gtsam/linear/NoiseModel.cpp
	gtsam/nonlinear/NonlinearFactor.h

and a number of other files. In particular, I did not upgrade Eigen or remove metis.

The following unit tests fail in this branch:

The following tests FAILED:
	  2 - testWrap (Failed)
	 85 - testGeneralSFMFactor (SEGFAULT)
	142 - testIMUSystem (Failed)
	178 - testTSAMFactors (Failed)
release/4.3a0
dellaert 2014-11-22 15:18:09 +01:00
parent a222194b26 f1d46d0a78
commit a9e3545a29
120 changed files with 148846 additions and 3011 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

202
.cproject
View File

@ -1,19 +1,17 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?> <?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?> <?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings"> <storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544"> <cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC"> <storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC">
<externalSettings/> <externalSettings/>
<extensions> <extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions> </extensions>
</storageModule> </storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> <storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -62,13 +60,13 @@
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890" moduleId="org.eclipse.cdt.core.settings" name="Timing"> <storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890" moduleId="org.eclipse.cdt.core.settings" name="Timing">
<externalSettings/> <externalSettings/>
<extensions> <extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions> </extensions>
</storageModule> </storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> <storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -118,13 +116,13 @@
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" moduleId="org.eclipse.cdt.core.settings" name="fast"> <storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" moduleId="org.eclipse.cdt.core.settings" name="fast">
<externalSettings/> <externalSettings/>
<extensions> <extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions> </extensions>
</storageModule> </storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> <storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -790,18 +788,18 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGaussianFactorGraphUnordered.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>tests/testPose3.run</buildTarget> <buildTarget>testGaussianFactorGraphUnordered.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGaussianBayesNetUnordered.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>all</buildTarget> <buildTarget>testGaussianBayesNetUnordered.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -1008,8 +1006,8 @@
</target> </target>
<target name="schedulingQuals13.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="schedulingQuals13.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments/>
<buildTarget>schedulingQuals13.run</buildTarget> <buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -1238,7 +1236,47 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testGaussianISAM2.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.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> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testGaussianISAM2.run</buildTarget> <buildTarget>testGaussianISAM2.run</buildTarget>
@ -1318,7 +1356,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="timing.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVSLAMConfig.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>timing.tests</buildTarget> <buildTarget>timing.tests</buildTarget>
@ -1352,16 +1397,14 @@
</target> </target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildTarget>testSimulated2D.run</buildTarget>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildTarget>testSimulated3D.run</buildTarget>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -1374,47 +1417,47 @@
<useDefaultCommand>false</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testGaussianISAM.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testGaussianISAM.run</buildTarget> <buildTarget>testEliminationTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testDoglegOptimizer.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testInference.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testDoglegOptimizer.run</buildTarget> <buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testNonlinearFactorGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testKey.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testNonlinearFactorGraph.run</buildTarget> <buildTarget>testKey.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testIterative.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicBayesTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j1</buildArguments>
<buildTarget>testIterative.run</buildTarget> <buildTarget>testSymbolicBayesTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testSubgraphSolver.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSymbolicSequentialSolver.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j1</buildArguments>
<buildTarget>testSubgraphSolver.run</buildTarget> <buildTarget>testSymbolicSequentialSolver.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testGaussianFactorGraphB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testGaussianFactorGraphB.run</buildTarget> <buildTarget>testGaussianFactorGraphB.run</buildTarget>
@ -1624,8 +1667,40 @@
</target> </target>
<target name="testDSFVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testDSFVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>VERBOSE=1</buildArguments>
<buildTarget>testDSFVector.run</buildTarget> <buildTarget>wrap_gtsam</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<runAllBuilders>true</runAllBuilders>
</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>
<runAllBuilders>true</runAllBuilders>
</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>
<runAllBuilders>true</runAllBuilders>
</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> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -2299,18 +2374,26 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="SelfCalibrationExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments/>
<buildTarget>SelfCalibrationExample.run</buildTarget> <buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="SFMExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="SFMExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments/>
<buildTarget>SFMExample.run</buildTarget> <buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -2497,7 +2580,31 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="check.geometry" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGPSFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGPSFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussMarkov1stOrderFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussMarkov1stOrderFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testImplicitSchurFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testImplicitSchurFactor.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> <buildCommand>make</buildCommand>
<buildArguments>-j2 VERBOSE=1</buildArguments> <buildArguments>-j2 VERBOSE=1</buildArguments>
<buildTarget>check.geometry</buildTarget> <buildTarget>check.geometry</buildTarget>
@ -2779,8 +2886,7 @@
</target> </target>
<target name="check.nonlinear_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check.nonlinear_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j6 -j8</buildArguments> <buildTarget>tests/testGaussianISAM2</buildTarget>
<buildTarget>check.nonlinear_unstable</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>

6
.gitignore vendored
View File

@ -1,6 +1,6 @@
/build* /build*
*.pyc *.pyc
*.DS_Store *.DS_Store
/debug/ /examples/Data/dubrovnik-3-7-pre-rewritten.txt
*.txt.user /examples/Data/pose2example-rewritten.txt
/release/ /examples/Data/pose3example-rewritten.txt

17
CMakeLists.txt
View File

@ -2,9 +2,15 @@
project(GTSAM CXX C) project(GTSAM CXX C)
cmake_minimum_required(VERSION 2.6) cmake_minimum_required(VERSION 2.6)
# new feature to Cmake Version > 2.8.12
# Mac ONLY. Define Relative Path on Mac OS
if(NOT DEFINED CMAKE_MACOSX_RPATH)
set(CMAKE_MACOSX_RPATH 0)
endif()
# Set the version number for the library # Set the version number for the library
set (GTSAM_VERSION_MAJOR 3) set (GTSAM_VERSION_MAJOR 3)
set (GTSAM_VERSION_MINOR 1) set (GTSAM_VERSION_MINOR 2)
set (GTSAM_VERSION_PATCH 0) set (GTSAM_VERSION_PATCH 0)
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") 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}") set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
@ -123,6 +129,11 @@ else()
endif() endif()
if(${Boost_VERSION} EQUAL 105600)
message("Ignoring Boost restriction on optional lvalue assignment from rvalues")
add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES)
endif()
############################################################################### ###############################################################################
# Find TBB # Find TBB
find_package(TBB) find_package(TBB)
@ -169,9 +180,9 @@ endif()
############################################################################### ###############################################################################
# Find OpenMP (if we're also using MKL) # Find OpenMP (if we're also using MKL)
if(GTSAM_WITH_EIGEN_MKL AND GTSAM_USE_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL) find_package(OpenMP) # do this here to generate correct message if disabled
find_package(OpenMP)
if(GTSAM_WITH_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP AND GTSAM_USE_EIGEN_MKL)
if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP) if(OPENMP_FOUND AND GTSAM_USE_EIGEN_MKL AND GTSAM_WITH_EIGEN_MKL_OPENMP)
set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")

1
README.md
View File

@ -1,5 +1,6 @@
README - Georgia Tech Smoothing and Mapping library README - Georgia Tech Smoothing and Mapping library
=================================================== ===================================================
Version: Pre-Release 3.2.0
What is GTSAM? What is GTSAM?
-------------- --------------

25
cmake/FindMKL.cmake
View File

@ -58,6 +58,7 @@ FIND_PATH(MKL_ROOT_DIR
/opt/intel/mkl/*/ /opt/intel/mkl/*/
/opt/intel/cmkl/ /opt/intel/cmkl/
/opt/intel/cmkl/*/ /opt/intel/cmkl/*/
/opt/intel/*/mkl/
/Library/Frameworks/Intel_MKL.framework/Versions/Current/lib/universal /Library/Frameworks/Intel_MKL.framework/Versions/Current/lib/universal
"C:/Program Files (x86)/Intel/ComposerXE-2011/mkl" "C:/Program Files (x86)/Intel/ComposerXE-2011/mkl"
"C:/Program Files (x86)/Intel/Composer XE 2013/mkl" "C:/Program Files (x86)/Intel/Composer XE 2013/mkl"
@ -136,13 +137,16 @@ ELSE() # UNIX and macOS
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/ ${MKL_ROOT_DIR}/lib/
) )
FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY # MKL on Mac OS doesn't ship with GNU thread versions, only Intel versions (see above)
mkl_gnu_thread IF(NOT APPLE)
PATHS FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR} mkl_gnu_thread
${MKL_ROOT_DIR}/lib/ PATHS
) ${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
${MKL_ROOT_DIR}/lib/
)
ENDIF()
# Intel Libraries # Intel Libraries
IF("${MKL_ARCH_DIR}" STREQUAL "32") IF("${MKL_ARCH_DIR}" STREQUAL "32")
@ -226,7 +230,12 @@ ELSE() # UNIX and macOS
endforeach() endforeach()
endforeach() endforeach()
SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES}) IF(APPLE)
SET(MKL_LIBRARIES ${MKL_LP_INTELTHREAD_LIBRARIES})
ELSE()
SET(MKL_LIBRARIES ${MKL_LP_GNUTHREAD_LIBRARIES})
ENDIF()
MARK_AS_ADVANCED(MKL_CORE_LIBRARY MKL_LP_LIBRARY MKL_ILP_LIBRARY MARK_AS_ADVANCED(MKL_CORE_LIBRARY MKL_LP_LIBRARY MKL_ILP_LIBRARY
MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY) MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY)
ENDIF() ENDIF()

1
examples/Data/VO_calibration00.txt Normal file
View File

@ -0,0 +1 @@
718.856 718.856 0.0 607.1928 185.2157 0.5371657189

1
examples/Data/VO_calibration00s.txt Normal file
View File

@ -0,0 +1 @@
718.856 718.856 0.0 607.1928 185.2157 0.5371657189

135
examples/Data/VO_camera_poses00.txt Normal file
View File

@ -0,0 +1,135 @@
0 1 0 0 0 0 1 0 0 -0 0 1 0 0 0 0 1
1 0.99999 -0.00268679 -0.00354618 6.43221e-05 0.00267957 0.999994 -0.00204036 -0.0073023 0.00355164 0.00203084 0.999992 0.676456 0 0 0 1
2 0.999969 -0.00120771 -0.00772489 -0.0100328 0.00117985 0.999993 -0.003611 -0.0111185 0.00772919 0.00360178 0.999964 1.37125 0 0 0 1
3 0.999931 -0.00128098 -0.0117006 -0.0237327 0.00122052 0.999986 -0.00517227 -0.0136538 0.0117071 0.00515763 0.999918 2.08563 0 0 0 1
4 0.99986 5.79321e-05 -0.0167106 -0.0402272 -0.000155312 0.999983 -0.00582618 -0.0194327 0.01671 0.00582796 0.999843 2.81528 0 0 0 1
5 0.999772 -0.00118366 -0.0213077 -0.0572378 0.0010545 0.999981 -0.00607208 -0.0278191 0.0213145 0.00604822 0.999755 3.56204 0 0 0 1
6 0.999662 0.000544425 -0.0259946 -0.081545 -0.000735472 0.999973 -0.00734051 -0.0358844 0.0259899 0.00735714 0.999635 4.32265 0 0 0 1
7 0.999513 0.0032602 -0.0310324 -0.112137 -0.0035101 0.999962 -0.00800188 -0.0447209 0.0310051 0.00810691 0.999486 5.09668 0 0 0 1
8 0.999361 0.00349173 -0.0355658 -0.143594 -0.00372162 0.999973 -0.00639979 -0.0532611 0.0355425 0.00652807 0.999347 5.88701 0 0 0 1
9 0.999185 0.00268131 -0.040271 -0.176401 -0.0028332 0.999989 -0.00371493 -0.0632884 0.0402606 0.003826 0.999182 6.6897 0 0 0 1
10 0.99903 0.00226305 -0.0439747 -0.211687 -0.00231163 0.999997 -0.00105382 -0.072362 0.0439722 0.00115445 0.999032 7.50361 0 0 0 1
11 0.998896 0.00366482 -0.0468376 -0.254125 -0.00374515 0.999992 -0.00162734 -0.0820263 0.0468312 0.00180096 0.998901 8.32333 0 0 0 1
12 0.998775 0.00304285 -0.0493866 -0.295424 -0.00313866 0.999993 -0.00186268 -0.0885739 0.0493806 0.00201541 0.998778 9.15211 0 0 0 1
13 0.998682 7.09894e-05 -0.0513155 -0.334647 -0.000203775 0.999997 -0.00258241 -0.0938889 0.0513152 0.00258946 0.998679 9.98839 0 0 0 1
14 0.998565 -8.82523e-05 -0.0535542 -0.380835 -9.36659e-06 0.999998 -0.00182255 -0.10173 0.0535542 0.00182044 0.998563 10.832 0 0 0 1
15 0.998481 -0.00146793 -0.0550718 -0.429135 0.0013525 0.999997 -0.00213307 -0.111427 0.0550748 0.00205535 0.99848 11.687 0 0 0 1
16 0.998373 0.000738731 -0.0570218 -0.483426 -0.000993083 0.99999 -0.00443241 -0.122139 0.0570179 0.00448183 0.998363 12.5483 0 0 0 1
17 0.998285 0.00120595 -0.0585258 -0.540056 -0.00162301 0.999974 -0.00707907 -0.132598 0.0585158 0.00716191 0.998261 13.4179 0 0 0 1
18 0.998165 0.00516151 -0.060337 -0.6023 -0.00570195 0.999945 -0.00878826 -0.143753 0.0602883 0.00911617 0.998139 14.2952 0 0 0 1
19 0.998101 0.00610094 -0.0612993 -0.66308 -0.00663017 0.999942 -0.00843386 -0.157854 0.0612443 0.00882427 0.998084 15.1802 0 0 0 1
20 0.998014 0.0052997 -0.0627662 -0.722045 -0.00574767 0.999959 -0.0069587 -0.172847 0.0627268 0.00730564 0.998004 16.074 0 0 0 1
21 0.99792 0.00591748 -0.0641975 -0.78346 -0.00627924 0.999966 -0.00543487 -0.186221 0.0641631 0.00582667 0.997922 16.9738 0 0 0 1
22 0.997857 0.00547694 -0.0651993 -0.845347 -0.00584101 0.999968 -0.00539455 -0.199741 0.0651677 0.00576382 0.997858 17.8786 0 0 0 1
23 0.997737 0.00536917 -0.0670282 -0.908218 -0.00579979 0.999964 -0.0062316 -0.212775 0.0669924 0.00660624 0.997732 18.7877 0 0 0 1
24 0.997663 0.00386695 -0.0682185 -0.971291 -0.00435203 0.999966 -0.00696344 -0.226442 0.0681893 0.00724406 0.997646 19.7046 0 0 0 1
25 0.997629 0.00410637 -0.0687004 -1.03663 -0.00448288 0.999976 -0.00532714 -0.239555 0.0686769 0.00562249 0.997623 20.6257 0 0 0 1
26 0.997617 0.00588773 -0.0687501 -1.10557 -0.0060349 0.99998 -0.00193325 -0.254273 0.0687373 0.00234355 0.997632 21.55 0 0 0 1
27 0.997662 0.00693766 -0.0679906 -1.17297 -0.00682806 0.999975 0.0018442 -0.26563 0.0680017 -0.00137565 0.997684 22.4875 0 0 0 1
28 0.997774 0.00579785 -0.0664343 -1.23728 -0.00550265 0.999974 0.00462554 -0.271962 0.0664594 -0.00424968 0.99778 23.4285 0 0 0 1
29 0.997872 0.00589563 -0.0649408 -1.30214 -0.00556012 0.99997 0.00534586 -0.277922 0.0649704 -0.0049734 0.997875 24.3732 0 0 0 1
30 0.997958 0.00627024 -0.0635595 -1.36462 -0.00612984 0.999978 0.00240374 -0.285335 0.0635732 -0.00200922 0.997975 25.314 0 0 0 1
31 0.998004 0.00714074 -0.0627411 -1.42783 -0.00731158 0.99997 -0.00249375 -0.293171 0.0627215 0.00294751 0.998027 26.2605 0 0 0 1
32 0.99808 0.0063692 -0.0616159 -1.48954 -0.00671918 0.999962 -0.00547459 -0.302321 0.0615787 0.00587809 0.998085 27.2168 0 0 0 1
33 0.99813 0.00376787 -0.0610159 -1.54654 -0.00404632 0.999982 -0.0044408 -0.313516 0.0609981 0.00467938 0.998127 28.1829 0 0 0 1
34 0.998113 0.00193972 -0.0613743 -1.60668 -0.00191171 0.999998 0.000515183 -0.324411 0.0613752 -0.000396881 0.998115 29.1626 0 0 0 1
35 0.99806 -0.0017885 -0.062228 -1.66532 0.00203402 0.99999 0.00388232 -0.335656 0.0622204 -0.00400136 0.998054 30.1428 0 0 0 1
36 0.997945 -0.00917543 -0.0634115 -1.72059 0.00939451 0.999951 0.00315749 -0.343316 0.0633794 -0.00374672 0.997982 31.1244 0 0 0 1
37 0.997825 -0.0112684 -0.0649459 -1.78049 0.011242 0.999937 -0.000771312 -0.350864 0.0649504 3.95099e-05 0.997888 32.1064 0 0 0 1
38 0.997739 -0.0110126 -0.0662983 -1.85007 0.0107254 0.999932 -0.00468596 -0.361068 0.0663454 0.00396429 0.997789 33.0886 0 0 0 1
39 0.997597 -0.00959503 -0.0686163 -1.92119 0.00924037 0.999942 -0.00548426 -0.373466 0.0686649 0.00483704 0.997628 34.0774 0 0 0 1
40 0.99755 -0.0095802 -0.0693031 -1.99331 0.00931271 0.999948 -0.00418184 -0.387047 0.0693396 0.00352619 0.997587 35.0736 0 0 0 1
41 0.997473 -0.00634387 -0.0707596 -2.0707 0.00626661 0.99998 -0.0013139 -0.403858 0.0707665 0.00086716 0.997493 36.0721 0 0 0 1
42 0.99739 -0.00624366 -0.0719343 -2.14553 0.00625582 0.99998 -5.62375e-05 -0.416888 0.0719332 -0.000393917 0.997409 37.0728 0 0 0 1
43 0.997312 -0.00473093 -0.0731254 -2.21909 0.00492848 0.999985 0.00252135 -0.428625 0.0731123 -0.00287497 0.99732 38.0643 0 0 0 1
44 0.997318 -0.00467696 -0.0730348 -2.29215 0.00509473 0.999972 0.00553481 -0.440023 0.0730068 -0.00589206 0.997314 39.0618 0 0 0 1
45 0.997274 0.00138304 -0.0737801 -2.37574 -0.000811217 0.999969 0.00777971 -0.447869 0.0737886 -0.00769865 0.997244 40.0548 0 0 0 1
46 0.997262 0.00149131 -0.0739326 -2.45529 -0.000969511 0.999974 0.00709318 -0.454763 0.0739413 -0.00700208 0.997238 41.0557 0 0 0 1
47 0.997266 0.00175929 -0.0738699 -2.53081 -0.00136899 0.999985 0.00533379 -0.460519 0.0738782 -0.00521809 0.997254 42.0518 0 0 0 1
48 0.997253 0.00408494 -0.0739555 -2.61212 -0.00386552 0.999988 0.00310988 -0.469863 0.0739673 -0.00281546 0.997257 43.0493 0 0 0 1
49 0.997185 0.00365371 -0.0748884 -2.68799 -0.00342799 0.999989 0.00314243 -0.47951 0.0748991 -0.00287687 0.997187 44.0473 0 0 0 1
50 0.997077 0.00181435 -0.0763845 -2.76071 -0.00149292 0.99999 0.00426495 -0.487845 0.0763915 -0.00413845 0.997069 45.0403 0 0 0 1
51 0.997018 0.00246727 -0.0771352 -2.84117 -0.00206285 0.999984 0.00532227 -0.499132 0.0771471 -0.00514727 0.997006 46.0244 0 0 0 1
52 0.996991 0.00504805 -0.0773507 -2.92304 -0.00493379 0.999986 0.00166824 -0.510863 0.0773581 -0.00128158 0.997003 46.994 0 0 0 1
53 0.996911 0.00581773 -0.0783264 -3.00373 -0.00604061 0.999978 -0.00260888 -0.521193 0.0783095 0.00307396 0.996924 47.9551 0 0 0 1
54 0.996846 0.00678413 -0.0790757 -3.08343 -0.00711636 0.999967 -0.00392044 -0.534186 0.0790465 0.00447081 0.996861 48.9236 0 0 0 1
55 0.996843 0.00557268 -0.0792034 -3.16262 -0.00562268 0.999984 -0.000408328 -0.54901 0.0791999 0.000852374 0.996858 49.9005 0 0 0 1
56 0.996831 0.00375007 -0.0794568 -3.23868 -0.00354655 0.99999 0.00270227 -0.563036 0.0794661 -0.0024119 0.996835 50.8752 0 0 0 1
57 0.996805 0.00190455 -0.0798474 -3.31582 -0.00164885 0.999993 0.00326822 -0.574113 0.0798531 -0.00312612 0.996802 51.8394 0 0 0 1
58 0.996782 -0.00124932 -0.0801505 -3.39153 0.00141878 0.999997 0.0020573 -0.586659 0.0801477 -0.00216439 0.996781 52.8005 0 0 0 1
59 0.996745 -0.0038025 -0.0805262 -3.4676 0.0038689 0.999992 0.000668539 -0.59892 0.080523 -0.00097791 0.996752 53.7575 0 0 0 1
60 0.996643 -0.00519016 -0.0817059 -3.54489 0.00535256 0.999984 0.00176869 -0.60864 0.0816955 -0.00220009 0.996655 54.708 0 0 0 1
61 0.996534 -0.0079249 -0.0828082 -3.62139 0.00842977 0.999948 0.00574894 -0.618858 0.0827583 -0.00642707 0.996549 55.6588 0 0 0 1
62 0.996473 -0.00854289 -0.0834829 -3.69959 0.00945654 0.9999 0.0105549 -0.624401 0.0833844 -0.0113071 0.996453 56.6119 0 0 0 1
63 0.996447 -0.00664747 -0.083957 -3.78502 0.00773966 0.99989 0.0126902 -0.629769 0.0838633 -0.0132949 0.996389 57.5607 0 0 0 1
64 0.996335 -0.00522633 -0.0853755 -3.8689 0.00597793 0.999946 0.00855017 -0.636709 0.0853262 -0.0090292 0.996312 58.4941 0 0 0 1
65 0.996221 -0.00343661 -0.0867892 -3.95276 0.00350579 0.999994 0.000644619 -0.644008 0.0867865 -0.000946448 0.996226 59.4131 0 0 0 1
66 0.996144 -0.00149623 -0.0877201 -4.03806 0.00112725 0.99999 -0.00425562 -0.655271 0.0877256 0.00414033 0.996136 60.3236 0 0 0 1
67 0.996055 0.00375138 -0.0886573 -4.12895 -0.00406723 0.999986 -0.00338223 -0.671324 0.0886434 0.00372948 0.996056 61.2274 0 0 0 1
68 0.995922 0.00719305 -0.0899263 -4.21985 -0.0073202 0.999973 -0.00108421 -0.691307 0.089916 0.00173807 0.995948 62.125 0 0 0 1
69 0.99582 0.00967277 -0.0908194 -4.30702 -0.00966905 0.999953 0.000481019 -0.708494 0.0908198 0.000399128 0.995867 63.0134 0 0 0 1
70 0.995713 0.0102896 -0.0919182 -4.39131 -0.0103098 0.999947 0.000255248 -0.721276 0.091916 0.000693502 0.995767 63.8776 0 0 0 1
71 0.99554 0.0119225 -0.0935844 -4.477 -0.0118725 0.999929 0.00109156 -0.734766 0.0935908 2.43836e-05 0.995611 64.7307 0 0 0 1
72 0.995397 0.0126524 -0.0950024 -4.56121 -0.0125521 0.99992 0.00165348 -0.749039 0.0950157 -0.000453392 0.995476 65.5703 0 0 0 1
73 0.995256 0.0126635 -0.0964665 -4.64297 -0.0125254 0.999919 0.00203772 -0.761909 0.0964846 -0.00081977 0.995334 66.3938 0 0 0 1
74 0.995133 0.0127023 -0.0977168 -4.72623 -0.0124698 0.999918 0.00298947 -0.7711 0.0977468 -0.00175641 0.99521 67.2017 0 0 0 1
75 0.994948 0.015548 -0.0991814 -4.81287 -0.0150604 0.999871 0.00566291 -0.780301 0.0992566 -0.0041406 0.995053 67.9995 0 0 0 1
76 0.994794 0.0171065 -0.100462 -4.90076 -0.0162095 0.999821 0.009738 -0.788037 0.100611 -0.00805885 0.994893 68.7922 0 0 0 1
77 0.994568 0.0201446 -0.102122 -4.98771 -0.0190681 0.999752 0.0115061 -0.794955 0.102328 -0.00949629 0.994705 69.5653 0 0 0 1
78 0.99437 0.0229894 -0.10344 -5.07707 -0.0223435 0.999723 0.00739861 -0.804841 0.103581 -0.00504575 0.994608 70.3178 0 0 0 1
79 0.99423 0.0228747 -0.1048 -5.16223 -0.0229899 0.999736 0.000108822 -0.81531 0.104774 0.00230114 0.994493 71.0475 0 0 0 1
80 0.994077 0.0219938 -0.106431 -5.24094 -0.0227304 0.999725 -0.00571252 -0.825441 0.106276 0.00809789 0.994304 71.7584 0 0 0 1
81 0.994023 0.0228054 -0.106762 -5.32437 -0.0236929 0.999694 -0.00705161 -0.831667 0.106569 0.00953897 0.99426 72.4548 0 0 0 1
82 0.99386 0.0255543 -0.107653 -5.40648 -0.0260808 0.999654 -0.00348505 -0.846106 0.107527 0.00627133 0.994182 73.1337 0 0 0 1
83 0.993702 0.0257681 -0.109048 -5.48436 -0.02605 0.99966 -0.00116096 -0.865059 0.108981 0.00399435 0.994036 73.7942 0 0 0 1
84 0.99367 0.0225468 -0.110051 -5.5561 -0.0231761 0.999722 -0.00444219 -0.879535 0.10992 0.00696462 0.993916 74.4324 0 0 0 1
85 0.993802 0.0143509 -0.110234 -5.61528 -0.0155198 0.999832 -0.00975281 -0.89282 0.110075 0.0114032 0.993858 75.0504 0 0 0 1
86 0.993949 0.0102 -0.10937 -5.67796 -0.0118817 0.999821 -0.0147354 -0.904058 0.1092 0.0159457 0.993892 75.6535 0 0 0 1
87 0.994244 0.0126451 -0.106395 -5.74524 -0.014328 0.999784 -0.0150673 -0.916949 0.106181 0.016505 0.99421 76.2455 0 0 0 1
88 0.994592 0.0175824 -0.102356 -5.81375 -0.0189231 0.999747 -0.0121417 -0.930972 0.102117 0.0140129 0.994674 76.8222 0 0 0 1
89 0.995077 0.0159295 -0.0978149 -5.86699 -0.0169123 0.999814 -0.00922648 -0.942909 0.0976498 0.0108353 0.995162 77.3862 0 0 0 1
90 0.995665 0.0122046 -0.0922106 -5.90659 -0.0127358 0.999906 -0.00517412 -0.954487 0.0921387 0.00632606 0.995726 77.9355 0 0 0 1
91 0.996426 0.00781104 -0.084105 -5.94257 -0.00798227 0.999967 -0.0016998 -0.970792 0.0840889 0.00236507 0.996455 78.4692 0 0 0 1
92 0.997233 0.0114593 -0.0734453 -5.98049 -0.0116369 0.99993 -0.00198965 -0.981019 0.0734174 0.00283882 0.997297 78.9883 0 0 0 1
93 0.998165 0.0165636 -0.0582361 -6.0154 -0.0167456 0.999856 -0.0026387 -0.99003 0.058184 0.00360906 0.998299 79.4952 0 0 0 1
95 0.999635 0.0200255 -0.0181151 -6.02981 -0.0200623 0.999797 -0.00185529 -1.00502 0.0180742 0.00221804 0.999834 80.4727 0 0 0 1
97 0.999162 0.015548 0.037857 -5.96801 -0.0155684 0.999879 0.000243918 -1.02389 -0.0378486 -0.000833085 0.999283 81.4025 0 0 0 1
99 0.993959 0.0151454 0.108698 -5.84553 -0.0154328 0.999879 0.0018028 -1.04109 -0.108657 -0.00346942 0.994073 82.2952 0 0 0 1
101 0.980499 0.0151504 0.195937 -5.64466 -0.0157106 0.999876 0.00130478 -1.05761 -0.195893 -0.00435763 0.980616 83.1489 0 0 0 1
103 0.954186 0.0182833 0.298656 -5.36588 -0.0179595 0.999831 -0.00382887 -1.08348 -0.298675 -0.00171027 0.954353 83.9397 0 0 0 1
105 0.910736 0.0194893 0.412529 -4.99648 -0.0175815 0.99981 -0.00842014 -1.10057 -0.412615 0.000415655 0.910905 84.6633 0 0 0 1
107 0.848724 0.0183908 0.528517 -4.54701 -0.0135972 0.999824 -0.0129557 -1.12003 -0.528662 0.00380946 0.848824 85.2983 0 0 0 1
109 0.772259 0.0170098 0.63508 -4.0183 -0.0106749 0.999848 -0.0137989 -1.14244 -0.635218 0.00387688 0.772323 85.8601 0 0 0 1
111 0.684256 0.0156411 0.729074 -3.42903 -0.0102231 0.999877 -0.0118561 -1.16079 -0.72917 0.000659179 0.684332 86.3474 0 0 0 1
113 0.590745 0.011826 0.806772 -2.77931 -0.000173089 0.999894 -0.0145301 -1.17886 -0.806858 0.00844396 0.590684 86.7443 0 0 0 1
115 0.496173 0.0169181 0.868059 -2.10955 -0.00504039 0.999849 -0.0166057 -1.20339 -0.868209 0.00386392 0.496183 87.0847 0 0 0 1
117 0.408192 0.0165355 0.912746 -1.40862 0.00231553 0.999814 -0.0191484 -1.2249 -0.912893 0.00992974 0.408078 87.3396 0 0 0 1
119 0.333443 0.00543386 0.942754 -0.671521 0.0223493 0.999657 -0.0136666 -1.23662 -0.942505 0.025627 0.333208 87.522 0 0 0 1
121 0.269054 0.0173163 0.962969 0.0638526 0.00961829 0.99974 -0.0206648 -1.26307 -0.963077 0.0148221 0.268818 87.7199 0 0 0 1
123 0.214897 0.0233915 0.976357 0.843046 0.00677025 0.999653 -0.0254398 -1.30009 -0.976613 0.0120771 0.214664 87.8763 0 0 0 1
125 0.171479 0.031054 0.984698 1.66216 0.0212619 0.999154 -0.0352125 -1.3179 -0.984958 0.0269747 0.170674 87.9743 0 0 0 1
127 0.134011 0.0386308 0.990227 2.52547 0.0207141 0.998912 -0.041773 -1.34147 -0.990763 0.0261097 0.133065 88.0809 0 0 0 1
129 0.10418 0.0310179 0.994075 3.44652 0.0195614 0.999256 -0.0332297 -1.39013 -0.994366 0.0229074 0.103496 88.1692 0 0 0 1
131 0.0794366 0.027788 0.996453 4.42556 0.0261822 0.999208 -0.0299521 -1.42776 -0.996496 0.0284686 0.0786462 88.2295 0 0 0 1
132 0.0693462 0.028443 0.997187 4.93885 0.0294969 0.999098 -0.0305488 -1.44582 -0.997156 0.0315324 0.0684447 88.2553 0 0 0 1
133 0.0615414 0.0290168 0.997683 5.46907 0.0316982 0.999016 -0.0310108 -1.46406 -0.997601 0.0335332 0.0605611 88.2814 0 0 0 1
134 0.0559347 0.029371 0.998002 6.0151 0.0334765 0.99895 -0.0312751 -1.48373 -0.997873 0.035159 0.0548927 88.307 0 0 0 1
135 0.0504312 0.0304374 0.998264 6.58025 0.0349281 0.99887 -0.0322204 -1.50267 -0.998117 0.0364923 0.0493112 88.3306 0 0 0 1
136 0.0445067 0.0311103 0.998525 7.16082 0.0355578 0.998832 -0.0327048 -1.52353 -0.998376 0.0369609 0.0433485 88.3531 0 0 0 1
137 0.040243 0.0311989 0.998703 7.76375 0.0381603 0.998735 -0.0327376 -1.54487 -0.998461 0.0394283 0.0390016 88.3716 0 0 0 1
138 0.0373982 0.0312027 0.998813 8.38568 0.0397152 0.998676 -0.0326855 -1.56772 -0.998511 0.0408905 0.0361095 88.3901 0 0 0 1
139 0.0343726 0.0307634 0.998936 9.02449 0.0406913 0.998654 -0.0321549 -1.59059 -0.99858 0.0417533 0.0330745 88.4092 0 0 0 1
140 0.0320861 0.0302694 0.999027 9.68038 0.0427798 0.998584 -0.03163 -1.61442 -0.998569 0.043753 0.0307457 88.4263 0 0 0 1
141 0.0316452 0.0299561 0.99905 10.3542 0.0473602 0.998383 -0.0314363 -1.63856 -0.998376 0.04831 0.0301753 88.4381 0 0 0 1
142 0.0327723 0.029714 0.999021 11.0457 0.0507142 0.998221 -0.0313539 -1.66282 -0.998175 0.0516921 0.031207 88.4556 0 0 0 1
143 0.0353027 0.0297602 0.998933 11.7546 0.0522842 0.998133 -0.0315841 -1.68678 -0.998008 0.0533435 0.0336808 88.4781 0 0 0 1
144 0.0392372 0.0297502 0.998787 12.4771 0.0547241 0.997993 -0.0318763 -1.71289 -0.99773 0.0559084 0.0375304 88.5062 0 0 0 1
145 0.0437096 0.0293188 0.998614 13.219 0.0550685 0.997979 -0.0317105 -1.73922 -0.997525 0.0563782 0.0420067 88.5387 0 0 0 1
146 0.0477725 0.0278103 0.998471 13.9764 0.0564652 0.997939 -0.0304971 -1.76499 -0.997261 0.0578358 0.0461037 88.5751 0 0 0 1
147 0.0518486 0.0263145 0.998308 14.7472 0.0562418 0.997989 -0.0292271 -1.79222 -0.99707 0.057662 0.0502644 88.6178 0 0 0 1
148 0.0560658 0.0242863 0.998132 15.5313 0.0531494 0.998214 -0.0272738 -1.82056 -0.997011 0.0545792 0.0546748 88.6693 0 0 0 1
149 0.0600218 0.0233355 0.997924 16.3271 0.0522059 0.998285 -0.0264839 -1.84733 -0.996831 0.0536871 0.0587006 88.7243 0 0 0 1
150 0.0641513 0.0243795 0.997642 17.1258 0.0492204 0.998408 -0.0275632 -1.87761 -0.996726 0.0508726 0.0628492 88.7821 0 0 0 1
151 0.0672583 0.028483 0.997329 17.929 0.0470717 0.998389 -0.0316877 -1.91204 -0.996625 0.0490772 0.0658092 88.842 0 0 0 1
152 0.0688453 0.0337446 0.997056 18.7357 0.0413971 0.99847 -0.0366509 -1.9468 -0.996768 0.0437985 0.067343 88.9041 0 0 0 1
153 0.0686545 0.0370247 0.996953 19.5482 0.0387033 0.99846 -0.0397459 -1.98038 -0.996889 0.0413142 0.0671158 88.9665 0 0 0 1

77
examples/Data/VO_camera_poses00s.txt Normal file
View File

@ -0,0 +1,77 @@
0 1 0 0 0 0 1 0 0 -0 0 1 0 0 0 0 1
1 0.99999 -0.00268679 -0.00354618 6.43221e-05 0.00267957 0.999994 -0.00204036 -0.0073023 0.00355164 0.00203084 0.999992 0.676456 0 0 0 1
2 0.999969 -0.00120771 -0.00772489 -0.0100328 0.00117985 0.999993 -0.003611 -0.0111185 0.00772919 0.00360178 0.999964 1.37125 0 0 0 1
3 0.999931 -0.00128098 -0.0117006 -0.0237327 0.00122052 0.999986 -0.00517227 -0.0136538 0.0117071 0.00515763 0.999918 2.08563 0 0 0 1
4 0.99986 5.79321e-05 -0.0167106 -0.0402272 -0.000155312 0.999983 -0.00582618 -0.0194327 0.01671 0.00582796 0.999843 2.81528 0 0 0 1
5 0.999772 -0.00118366 -0.0213077 -0.0572378 0.0010545 0.999981 -0.00607208 -0.0278191 0.0213145 0.00604822 0.999755 3.56204 0 0 0 1
6 0.999662 0.000544425 -0.0259946 -0.081545 -0.000735472 0.999973 -0.00734051 -0.0358844 0.0259899 0.00735714 0.999635 4.32265 0 0 0 1
7 0.999513 0.0032602 -0.0310324 -0.112137 -0.0035101 0.999962 -0.00800188 -0.0447209 0.0310051 0.00810691 0.999486 5.09668 0 0 0 1
8 0.999361 0.00349173 -0.0355658 -0.143594 -0.00372162 0.999973 -0.00639979 -0.0532611 0.0355425 0.00652807 0.999347 5.88701 0 0 0 1
9 0.999185 0.00268131 -0.040271 -0.176401 -0.0028332 0.999989 -0.00371493 -0.0632884 0.0402606 0.003826 0.999182 6.6897 0 0 0 1
10 0.99903 0.00226305 -0.0439747 -0.211687 -0.00231163 0.999997 -0.00105382 -0.072362 0.0439722 0.00115445 0.999032 7.50361 0 0 0 1
11 0.998896 0.00366482 -0.0468376 -0.254125 -0.00374515 0.999992 -0.00162734 -0.0820263 0.0468312 0.00180096 0.998901 8.32333 0 0 0 1
12 0.998775 0.00304285 -0.0493866 -0.295424 -0.00313866 0.999993 -0.00186268 -0.0885739 0.0493806 0.00201541 0.998778 9.15211 0 0 0 1
13 0.998682 7.09894e-05 -0.0513155 -0.334647 -0.000203775 0.999997 -0.00258241 -0.0938889 0.0513152 0.00258946 0.998679 9.98839 0 0 0 1
14 0.998565 -8.82523e-05 -0.0535542 -0.380835 -9.36659e-06 0.999998 -0.00182255 -0.10173 0.0535542 0.00182044 0.998563 10.832 0 0 0 1
15 0.998481 -0.00146793 -0.0550718 -0.429135 0.0013525 0.999997 -0.00213307 -0.111427 0.0550748 0.00205535 0.99848 11.687 0 0 0 1
16 0.998373 0.000738731 -0.0570218 -0.483426 -0.000993083 0.99999 -0.00443241 -0.122139 0.0570179 0.00448183 0.998363 12.5483 0 0 0 1
17 0.998285 0.00120595 -0.0585258 -0.540056 -0.00162301 0.999974 -0.00707907 -0.132598 0.0585158 0.00716191 0.998261 13.4179 0 0 0 1
18 0.998165 0.00516151 -0.060337 -0.6023 -0.00570195 0.999945 -0.00878826 -0.143753 0.0602883 0.00911617 0.998139 14.2952 0 0 0 1
19 0.998101 0.00610094 -0.0612993 -0.66308 -0.00663017 0.999942 -0.00843386 -0.157854 0.0612443 0.00882427 0.998084 15.1802 0 0 0 1
20 0.998014 0.0052997 -0.0627662 -0.722045 -0.00574767 0.999959 -0.0069587 -0.172847 0.0627268 0.00730564 0.998004 16.074 0 0 0 1
21 0.99792 0.00591748 -0.0641975 -0.78346 -0.00627924 0.999966 -0.00543487 -0.186221 0.0641631 0.00582667 0.997922 16.9738 0 0 0 1
22 0.997857 0.00547694 -0.0651993 -0.845347 -0.00584101 0.999968 -0.00539455 -0.199741 0.0651677 0.00576382 0.997858 17.8786 0 0 0 1
23 0.997737 0.00536917 -0.0670282 -0.908218 -0.00579979 0.999964 -0.0062316 -0.212775 0.0669924 0.00660624 0.997732 18.7877 0 0 0 1
24 0.997663 0.00386695 -0.0682185 -0.971291 -0.00435203 0.999966 -0.00696344 -0.226442 0.0681893 0.00724406 0.997646 19.7046 0 0 0 1
25 0.997629 0.00410637 -0.0687004 -1.03663 -0.00448288 0.999976 -0.00532714 -0.239555 0.0686769 0.00562249 0.997623 20.6257 0 0 0 1
26 0.997617 0.00588773 -0.0687501 -1.10557 -0.0060349 0.99998 -0.00193325 -0.254273 0.0687373 0.00234355 0.997632 21.55 0 0 0 1
27 0.997662 0.00693766 -0.0679906 -1.17297 -0.00682806 0.999975 0.0018442 -0.26563 0.0680017 -0.00137565 0.997684 22.4875 0 0 0 1
28 0.997774 0.00579785 -0.0664343 -1.23728 -0.00550265 0.999974 0.00462554 -0.271962 0.0664594 -0.00424968 0.99778 23.4285 0 0 0 1
29 0.997872 0.00589563 -0.0649408 -1.30214 -0.00556012 0.99997 0.00534586 -0.277922 0.0649704 -0.0049734 0.997875 24.3732 0 0 0 1
30 0.997958 0.00627024 -0.0635595 -1.36462 -0.00612984 0.999978 0.00240374 -0.285335 0.0635732 -0.00200922 0.997975 25.314 0 0 0 1
31 0.998004 0.00714074 -0.0627411 -1.42783 -0.00731158 0.99997 -0.00249375 -0.293171 0.0627215 0.00294751 0.998027 26.2605 0 0 0 1
32 0.99808 0.0063692 -0.0616159 -1.48954 -0.00671918 0.999962 -0.00547459 -0.302321 0.0615787 0.00587809 0.998085 27.2168 0 0 0 1
33 0.99813 0.00376787 -0.0610159 -1.54654 -0.00404632 0.999982 -0.0044408 -0.313516 0.0609981 0.00467938 0.998127 28.1829 0 0 0 1
34 0.998113 0.00193972 -0.0613743 -1.60668 -0.00191171 0.999998 0.000515183 -0.324411 0.0613752 -0.000396881 0.998115 29.1626 0 0 0 1
35 0.99806 -0.0017885 -0.062228 -1.66532 0.00203402 0.99999 0.00388232 -0.335656 0.0622204 -0.00400136 0.998054 30.1428 0 0 0 1
36 0.997945 -0.00917543 -0.0634115 -1.72059 0.00939451 0.999951 0.00315749 -0.343316 0.0633794 -0.00374672 0.997982 31.1244 0 0 0 1
37 0.997825 -0.0112684 -0.0649459 -1.78049 0.011242 0.999937 -0.000771312 -0.350864 0.0649504 3.95099e-05 0.997888 32.1064 0 0 0 1
38 0.997739 -0.0110126 -0.0662983 -1.85007 0.0107254 0.999932 -0.00468596 -0.361068 0.0663454 0.00396429 0.997789 33.0886 0 0 0 1
39 0.997597 -0.00959503 -0.0686163 -1.92119 0.00924037 0.999942 -0.00548426 -0.373466 0.0686649 0.00483704 0.997628 34.0774 0 0 0 1
40 0.99755 -0.0095802 -0.0693031 -1.99331 0.00931271 0.999948 -0.00418184 -0.387047 0.0693396 0.00352619 0.997587 35.0736 0 0 0 1
41 0.997473 -0.00634387 -0.0707596 -2.0707 0.00626661 0.99998 -0.0013139 -0.403858 0.0707665 0.00086716 0.997493 36.0721 0 0 0 1
42 0.99739 -0.00624366 -0.0719343 -2.14553 0.00625582 0.99998 -5.62375e-05 -0.416888 0.0719332 -0.000393917 0.997409 37.0728 0 0 0 1
43 0.997312 -0.00473093 -0.0731254 -2.21909 0.00492848 0.999985 0.00252135 -0.428625 0.0731123 -0.00287497 0.99732 38.0643 0 0 0 1
44 0.997318 -0.00467696 -0.0730348 -2.29215 0.00509473 0.999972 0.00553481 -0.440023 0.0730068 -0.00589206 0.997314 39.0618 0 0 0 1
45 0.997274 0.00138304 -0.0737801 -2.37574 -0.000811217 0.999969 0.00777971 -0.447869 0.0737886 -0.00769865 0.997244 40.0548 0 0 0 1
46 0.997262 0.00149131 -0.0739326 -2.45529 -0.000969511 0.999974 0.00709318 -0.454763 0.0739413 -0.00700208 0.997238 41.0557 0 0 0 1
47 0.997266 0.00175929 -0.0738699 -2.53081 -0.00136899 0.999985 0.00533379 -0.460519 0.0738782 -0.00521809 0.997254 42.0518 0 0 0 1
48 0.997253 0.00408494 -0.0739555 -2.61212 -0.00386552 0.999988 0.00310988 -0.469863 0.0739673 -0.00281546 0.997257 43.0493 0 0 0 1
49 0.997185 0.00365371 -0.0748884 -2.68799 -0.00342799 0.999989 0.00314243 -0.47951 0.0748991 -0.00287687 0.997187 44.0473 0 0 0 1
50 0.997077 0.00181435 -0.0763845 -2.76071 -0.00149292 0.99999 0.00426495 -0.487845 0.0763915 -0.00413845 0.997069 45.0403 0 0 0 1
51 0.997018 0.00246727 -0.0771352 -2.84117 -0.00206285 0.999984 0.00532227 -0.499132 0.0771471 -0.00514727 0.997006 46.0244 0 0 0 1
52 0.996991 0.00504805 -0.0773507 -2.92304 -0.00493379 0.999986 0.00166824 -0.510863 0.0773581 -0.00128158 0.997003 46.994 0 0 0 1
53 0.996911 0.00581773 -0.0783264 -3.00373 -0.00604061 0.999978 -0.00260888 -0.521193 0.0783095 0.00307396 0.996924 47.9551 0 0 0 1
54 0.996846 0.00678413 -0.0790757 -3.08343 -0.00711636 0.999967 -0.00392044 -0.534186 0.0790465 0.00447081 0.996861 48.9236 0 0 0 1
55 0.996843 0.00557268 -0.0792034 -3.16262 -0.00562268 0.999984 -0.000408328 -0.54901 0.0791999 0.000852374 0.996858 49.9005 0 0 0 1
56 0.996831 0.00375007 -0.0794568 -3.23868 -0.00354655 0.99999 0.00270227 -0.563036 0.0794661 -0.0024119 0.996835 50.8752 0 0 0 1
57 0.996805 0.00190455 -0.0798474 -3.31582 -0.00164885 0.999993 0.00326822 -0.574113 0.0798531 -0.00312612 0.996802 51.8394 0 0 0 1
58 0.996782 -0.00124932 -0.0801505 -3.39153 0.00141878 0.999997 0.0020573 -0.586659 0.0801477 -0.00216439 0.996781 52.8005 0 0 0 1
59 0.996745 -0.0038025 -0.0805262 -3.4676 0.0038689 0.999992 0.000668539 -0.59892 0.080523 -0.00097791 0.996752 53.7575 0 0 0 1
60 0.996643 -0.00519016 -0.0817059 -3.54489 0.00535256 0.999984 0.00176869 -0.60864 0.0816955 -0.00220009 0.996655 54.708 0 0 0 1
61 0.996534 -0.0079249 -0.0828082 -3.62139 0.00842977 0.999948 0.00574894 -0.618858 0.0827583 -0.00642707 0.996549 55.6588 0 0 0 1
62 0.996473 -0.00854289 -0.0834829 -3.69959 0.00945654 0.9999 0.0105549 -0.624401 0.0833844 -0.0113071 0.996453 56.6119 0 0 0 1
63 0.996447 -0.00664747 -0.083957 -3.78502 0.00773966 0.99989 0.0126902 -0.629769 0.0838633 -0.0132949 0.996389 57.5607 0 0 0 1
64 0.996335 -0.00522633 -0.0853755 -3.8689 0.00597793 0.999946 0.00855017 -0.636709 0.0853262 -0.0090292 0.996312 58.4941 0 0 0 1
65 0.996221 -0.00343661 -0.0867892 -3.95276 0.00350579 0.999994 0.000644619 -0.644008 0.0867865 -0.000946448 0.996226 59.4131 0 0 0 1
66 0.996144 -0.00149623 -0.0877201 -4.03806 0.00112725 0.99999 -0.00425562 -0.655271 0.0877256 0.00414033 0.996136 60.3236 0 0 0 1
67 0.996055 0.00375138 -0.0886573 -4.12895 -0.00406723 0.999986 -0.00338223 -0.671324 0.0886434 0.00372948 0.996056 61.2274 0 0 0 1
68 0.995922 0.00719305 -0.0899263 -4.21985 -0.0073202 0.999973 -0.00108421 -0.691307 0.089916 0.00173807 0.995948 62.125 0 0 0 1
69 0.99582 0.00967277 -0.0908194 -4.30702 -0.00966905 0.999953 0.000481019 -0.708494 0.0908198 0.000399128 0.995867 63.0134 0 0 0 1
70 0.995713 0.0102896 -0.0919182 -4.39131 -0.0103098 0.999947 0.000255248 -0.721276 0.091916 0.000693502 0.995767 63.8776 0 0 0 1
71 0.99554 0.0119225 -0.0935844 -4.477 -0.0118725 0.999929 0.00109156 -0.734766 0.0935908 2.43836e-05 0.995611 64.7307 0 0 0 1
72 0.995397 0.0126524 -0.0950024 -4.56121 -0.0125521 0.99992 0.00165348 -0.749039 0.0950157 -0.000453392 0.995476 65.5703 0 0 0 1
73 0.995256 0.0126635 -0.0964665 -4.64297 -0.0125254 0.999919 0.00203772 -0.761909 0.0964846 -0.00081977 0.995334 66.3938 0 0 0 1
74 0.995133 0.0127023 -0.0977168 -4.72623 -0.0124698 0.999918 0.00298947 -0.7711 0.0977468 -0.00175641 0.99521 67.2017 0 0 0 1
75 0.994948 0.015548 -0.0991814 -4.81287 -0.0150604 0.999871 0.00566291 -0.780301 0.0992566 -0.0041406 0.995053 67.9995 0 0 0 1
76 0.994794 0.0171065 -0.100462 -4.90076 -0.0162095 0.999821 0.009738 -0.788037 0.100611 -0.00805885 0.994893 68.7922 0 0 0 1

88781
examples/Data/VO_stereo_factors00.txt Normal file
View File

File diff suppressed because it is too large Load Diff

52544
examples/Data/VO_stereo_factors00s.txt Normal file
View File

File diff suppressed because it is too large Load Diff

71
examples/Data/pose3example-grid.txt Normal file
View File

@ -0,0 +1,71 @@
VERTEX_SE3:QUAT 0 1.63791e-12 7.56548e-14 -3.02811e-12 5.35657e-13 2.43616e-13 9.71152e-14 1
VERTEX_SE3:QUAT 1 1.01609 0.00274307 -0.0351514 -0.499545 0.247735 0.723569 -0.406854
VERTEX_SE3:QUAT 2 1.99996 0.0304956 -0.040662 0.403501 -0.294714 -0.4254 0.754563
VERTEX_SE3:QUAT 3 1.94371 1.06535 0.0118614 -0.0471731 -0.541615 0.820893 0.17482
VERTEX_SE3:QUAT 4 0.962753 0.999477 0.0211017 -0.19663 -0.66009 0.470743 0.551379
VERTEX_SE3:QUAT 5 -0.00956768 0.965396 -0.021854 -0.320221 -0.518368 0.47521 0.634766
VERTEX_SE3:QUAT 6 -0.0863793 1.97682 0.000531117 -0.0173439 -0.573793 -0.450627 0.683663
VERTEX_SE3:QUAT 7 0.918905 2.01556 -0.0139773 0.56169 -0.440513 0.199057 0.671438
VERTEX_SE3:QUAT 8 1.92094 2.05524 0.0469884 0.0073084 -0.372357 -0.467582 0.801663
VERTEX_SE3:QUAT 9 1.86182 2.05449 1.09237 0.0131731 -0.05784 0.0335652 0.997674
VERTEX_SE3:QUAT 10 0.880176 2.02406 1.00997 -0.39342 -0.287909 0.757918 0.433462
VERTEX_SE3:QUAT 11 -0.0960463 1.98653 0.995791 0.434103 -0.199044 0.585176 0.655367
VERTEX_SE3:QUAT 12 -0.0911401 0.997117 0.988217 -0.0925477 0.572872 0.537294 0.612019
VERTEX_SE3:QUAT 13 0.948316 1.02239 0.991745 0.142484 0.560062 0.750078 0.321578
VERTEX_SE3:QUAT 14 1.92631 1.08945 1.06749 0.23878 0.380837 0.796564 -0.404269
VERTEX_SE3:QUAT 15 1.95398 0.0777667 0.982353 -0.384392 0.58733 0.685207 -0.194366
VERTEX_SE3:QUAT 16 0.946032 0.0482667 0.952308 -0.218979 0.186315 -0.494185 0.820437
VERTEX_SE3:QUAT 17 -0.0625076 -0.034424 0.942171 0.514725 -0.185043 -0.44771 0.707371
VERTEX_SE3:QUAT 18 -0.083807 -0.0106666 1.9853 0.00792651 1.98919e-05 -0.00128106 0.999968
VERTEX_SE3:QUAT 19 0.918067 -0.000897795 1.92157 -0.342141 0.241241 -0.726975 0.544288
VERTEX_SE3:QUAT 20 1.90041 0.0323631 2.00636 0.412572 -0.0930131 -0.133075 0.896339
VERTEX_SE3:QUAT 21 1.84895 1.05013 2.0738 -0.580757 0.35427 0.729393 -0.0721062
VERTEX_SE3:QUAT 22 0.880221 1.00671 1.99021 0.147752 0.355662 0.917953 0.095058
VERTEX_SE3:QUAT 23 -0.0950872 1.00374 1.95013 -0.29909 -0.0578461 0.857019 0.415594
VERTEX_SE3:QUAT 24 -0.111581 1.97979 1.98762 0.565153 0.214463 -0.523058 0.600848
VERTEX_SE3:QUAT 25 0.837568 2.01589 2.03075 -0.284756 0.369992 0.875484 -0.124692
VERTEX_SE3:QUAT 26 1.82708 2.05081 2.07052 0.254696 0.250865 0.653216 0.667462
EDGE_SE3:QUAT 0 1 1.00497 0.002077 -0.015539 -0.508004 0.250433 0.711222 -0.416386 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 1 2 -0.200593 0.339956 -0.908079 -0.093598 0.151993 0.42829 0.885836 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 2 3 -0.922791 0.330629 -0.292682 0.365657 -0.051986 0.924849 -0.090813 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 3 4 0.893075 0.246476 0.331154 -0.285927 0.341221 -0.267609 0.854517 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 4 5 0.280674 0.244242 0.923726 0.035064 0.21101 0.083834 0.973251 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 5 6 0.955621 0.355669 -0.025152 -0.306713 0.131221 -0.781587 0.527096 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 6 7 -0.076631 0.636081 -0.771439 0.702021 0.326514 0.122181 0.620988 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 7 8 0.582761 -0.721177 -0.376875 -0.733841 -0.170725 -0.256653 0.605359 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 8 9 0.600312 0.298765 0.767014 0.057612 0.332574 0.486324 0.805956 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 9 10 -0.986649 0.03008 -0.008766 -0.362177 -0.253215 0.763748 0.470531 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 10 11 0.275109 0.534769 0.823463 0.450708 -0.472399 -0.432689 0.621677 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 11 12 -0.61882 0.024878 0.773748 0.0927029 0.786162 -0.21122 0.573359 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 12 13 -0.175537 -0.730832 0.634529 -0.018628 0.006375 0.428306 0.903419 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 13 14 -0.700208 -0.245198 0.637353 -0.035865 0.273394 0.645363 0.712374 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 14 15 0.373495 0.373768 -0.846199 0.400323 0.310362 -0.422222 0.751762 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 15 16 0.648588 0.157829 0.72252 0.781502 -0.210141 -0.501005 -0.30674 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 16 17 -0.390339 -0.702656 -0.572321 0.765815 0.055816 0.032478 0.63981 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 17 18 -0.261114 0.908685 0.421318 -0.501833 0.166567 0.448468 0.720622 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 18 19 1.00815 0.012634 -0.029822 -0.347007 0.205082 -0.740641 0.537569 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 19 20 -0.162376 0.581623 0.810804 0.628338 0.075411 0.650639 0.41973 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 20 21 -0.358942 0.627689 -0.704045 -0.469133 0.542456 0.530583 -0.451816 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 21 22 0.362417 0.298352 0.854822 0.004058 -0.696926 0.140345 0.703265 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 22 23 0.934942 0.020321 -0.358044 -0.445461 0.260916 -0.379862 0.767589 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 23 24 0.741887 -0.657659 0.215293 -0.584859 0.196138 0.688031 0.38221 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 24 25 0.300145 0.82011 -0.39974 0.46538 -0.593595 -0.202131 0.624668 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 25 26 -0.85591 0.022701 -0.510794 0.12929 -0.685192 -0.503707 0.509978 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 0 5 0.026721 0.990497 -0.007651 -0.317476 -0.510239 0.467341 0.648427 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 3 8 0.390516 -0.401461 -0.830724 0.503106 -0.367814 0.780584 0.047806 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 4 1 -0.813838 -0.446181 0.319175 0.224903 -0.031827 0.97265 0.048561 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 4 13 0.571273 -0.805401 0.077339 0.892031 0.329761 0.275468 0.140201 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 5 12 0.389794 -0.882655 0.268063 0.712423 0.550662 0.275339 0.33677 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 6 11 0.800298 0.505022 0.361738 0.739335 0.419366 0.443817 0.283801 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 10 13 -0.912531 0.430955 -0.018942 0.830493 -0.093519 0.272041 0.477001 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 12 23 -0.797606 0.437737 0.311476 -0.657137 -0.196625 0.136652 0.714728 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 13 22 -0.116836 0.952032 0.269398 -0.216437 0.086571 0.260965 0.936781 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 14 21 0.749295 0.373389 0.581641 0.253048 0.511007 -0.537262 0.621439 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 16 1 0.160985 0.555966 -0.811911 0.748057 0.122381 -0.369631 0.537407 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 18 23 0.028909 1.02689 -0.00265 -0.294167 -0.071607 0.850901 0.429308 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 19 16 -0.230711 0.750637 -0.607511 0.14647 -0.102538 0.297899 0.937704 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 20 15 -0.031986 -0.741129 -0.728721 -0.278926 0.731172 0.404675 -0.473103 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 22 19 -0.332601 0.704401 -0.687251 -0.372165 -0.054346 0.713024 0.591725 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 22 25 0.347067 -0.634646 0.657147 0.018567 0.476762 0.040939 0.877882 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 25 10 0.388971 -0.723981 -0.559653 -0.373459 -0.014654 -0.696123 0.612965 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400
EDGE_SE3:QUAT 26 21 -0.979482 -0.024822 0.043763 -0.326753 0.819942 0.292615 0.367837 2500 0 0 0 0 0 2500 0 0 0 0 2500 0 0 0 400 0 0 400 0 400

3
examples/Data/pose3example-offdiagonal-rewritten.txt Normal file
View File

@ -0,0 +1,3 @@
VERTEX_SE3:QUAT 0 0 0 0 0 0 0 1
VERTEX_SE3:QUAT 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423
EDGE_SE3:QUAT 0 1 1.00137 0.01539 0.004948 0.190253 0.283162 -0.392318 0.85423 10000 1 1 1 1 1 10000 2 2 2 2 10000 3 3 3 10000 4 4 10000 5 10000

3
examples/Data/pose3example-offdiagonal.txt Normal file
View File

@ -0,0 +1,3 @@
VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230
EDGE_SE3:QUAT 0 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 10000.000000 1.000000 1.000000 1.000000 1.000000 1.000000 10000.000000 2.000000 2.000000 2.000000 2.000000 10000.000000 3.000000 3.000000 3.000000 10000.000000 4.000000 4.000000 10000.000000 5.000000 10000.0000

11
examples/Data/pose3example.txt Normal file
View File

@ -0,0 +1,11 @@
VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 1.000000
VERTEX_SE3:QUAT 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230
VERTEX_SE3:QUAT 2 1.993500 0.023275 0.003793 -0.351729 -0.597838 0.584174 0.421446
VERTEX_SE3:QUAT 3 2.004291 1.024305 0.018047 0.331798 -0.200659 0.919323 0.067024
VERTEX_SE3:QUAT 4 0.999908 1.055073 0.020212 -0.035697 -0.462490 0.445933 0.765488
EDGE_SE3:QUAT 0 1 1.001367 0.015390 0.004948 0.190253 0.283162 -0.392318 0.854230 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 1 2 0.523923 0.776654 0.326659 0.311512 0.656877 -0.678505 0.105373 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 2 3 0.910927 0.055169 -0.411761 0.595795 -0.561677 0.079353 0.568551 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 3 4 0.775288 0.228798 -0.596923 -0.592077 0.303380 -0.513226 0.542221 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 1 4 -0.577841 0.628016 -0.543592 -0.125250 -0.534379 0.769122 0.327419 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000
EDGE_SE3:QUAT 3 0 -0.623267 0.086928 0.773222 0.104639 0.627755 0.766795 0.083672 10000.000000 0.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 0.000000 10000.000000 0.000000 0.000000 10000.000000 0.000000 10000.000000

11
examples/Data/simpleGraph10gradIter.txt Normal file
View File

@ -0,0 +1,11 @@
VERTEX_SE3:QUAT 0 0.000000 0.000000 0.000000 0.0008187 0.0011723 0.0895466 0.9959816
VERTEX_SE3:QUAT 1 0.000000 -0.000000 0.000000 0.0010673 0.0015636 0.1606931 0.9870026
VERTEX_SE3:QUAT 2 -0.388822 0.632954 0.001223 0.0029920 0.0014066 0.0258235 0.9996610
VERTEX_SE3:QUAT 3 -1.143204 0.050638 0.006026 -0.0012800 -0.0002767 -0.2850291 0.9585180
VERTEX_SE3:QUAT 4 -0.512416 0.486441 0.005171 0.0002681 0.0023574 0.0171476 0.9998502
EDGE_SE3:QUAT 1 2 1.000000 2.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
EDGE_SE3:QUAT 2 3 -0.000000 1.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
EDGE_SE3:QUAT 3 4 1.000000 1.000000 0.000000 0.0000000 0.0000000 0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
EDGE_SE3:QUAT 3 1 0.000001 2.000000 0.000000 0.0000000 0.0000000 1.0000000 0.0000002 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
EDGE_SE3:QUAT 1 4 -1.000000 1.000000 0.000000 0.0000000 0.0000000 -0.7071068 0.7071068 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000
EDGE_SE3:QUAT 0 1 0.000000 0.000000 0.000000 0.0000000 0.0000000 0.0000000 1.0000000 100.000000 0.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 0.000000 100.000000 0.000000 0.000000 100.000000 0.000000 100.000000

10
examples/LocalizationExample.cpp
View File

@ -120,15 +120,15 @@ int main(int argc, char** argv) {
// For simplicity, we will use the same noise model for each odometry factor // For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses // Create odometry (Between) factors between consecutive poses
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise)); graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise)); graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
// 2b. Add "GPS-like" measurements // 2b. Add "GPS-like" measurements
// We will use our custom UnaryFactor for this. // We will use our custom UnaryFactor for this.
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // 10cm std on x,y noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // 10cm std on x,y
graph.push_back(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise)); graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.push_back(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise)); graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.push_back(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise)); graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
graph.print("\nFactor Graph:\n"); // print graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution // 3. Create the data structure to hold the initialEstimate estimate to the solution

6
examples/OdometryExample.cpp
View File

@ -65,15 +65,15 @@ int main(int argc, char** argv) {
// A prior factor consists of a mean and a noise model (covariance matrix) // A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, priorMean, priorNoise)); graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
// Add odometry factors // Add odometry factors
Pose2 odometry(2.0, 0.0, 0.0); Pose2 odometry(2.0, 0.0, 0.0);
// For simplicity, we will use the same noise model for each odometry factor // For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses // Create odometry (Between) factors between consecutive poses
graph.push_back(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise)); graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise)); graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
graph.print("\nFactor Graph:\n"); // print graph.print("\nFactor Graph:\n"); // print
// Create the data structure to hold the initialEstimate estimate to the solution // Create the data structure to hold the initialEstimate estimate to the solution

12
examples/PlanarSLAMExample.cpp
View File

@ -81,13 +81,13 @@ int main(int argc, char** argv) {
// Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix) // Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.push_back(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph graph.add(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph
// Add two odometry factors // Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case) Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.push_back(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise)); graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
graph.push_back(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise)); graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
// Add Range-Bearing measurements to two different landmarks // Add Range-Bearing measurements to two different landmarks
// create a noise model for the landmark measurements // create a noise model for the landmark measurements
@ -101,9 +101,9 @@ int main(int argc, char** argv) {
range32 = 2.0; range32 = 2.0;
// Add Bearing-Range factors // Add Bearing-Range factors
graph.push_back(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise)); graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
graph.push_back(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise)); graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
graph.push_back(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise)); graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
// Print // Print
graph.print("Factor Graph:\n"); graph.print("Factor Graph:\n");

12
examples/Pose2SLAMExample.cpp
View File

@ -72,23 +72,23 @@ int main(int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin // 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix) // A prior factor consists of a mean and a noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise)); graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
// For simplicity, we will use the same noise model for odometry and loop closures // For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// 2b. Add odometry factors // 2b. Add odometry factors
// Create odometry (Between) factors between consecutive poses // Create odometry (Between) factors between consecutive poses
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model)); graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model)); graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model)); graph.add(BetweenFactor<Pose2>(3, 4, Pose2(2, 0, M_PI_2), model));
graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model)); graph.add(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
// 2c. Add the loop closure constraint // 2c. Add the loop closure constraint
// This factor encodes the fact that we have returned to the same pose. In real systems, // This factor encodes the fact that we have returned to the same pose. In real systems,
// these constraints may be identified in many ways, such as appearance-based techniques // these constraints may be identified in many ways, such as appearance-based techniques
// with camera images. We will use another Between Factor to enforce this constraint: // with camera images. We will use another Between Factor to enforce this constraint:
graph.push_back(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model)); graph.add(BetweenFactor<Pose2>(5, 2, Pose2(2, 0, M_PI_2), model));
graph.print("\nFactor Graph:\n"); // print graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution // 3. Create the data structure to hold the initialEstimate estimate to the solution

54
examples/Pose2SLAMExample_g2o.cpp
View File

@ -26,36 +26,72 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
int main(const int argc, const char *argv[]) { int main(const int argc, const char *argv[]) {
// Read graph from file string kernelType = "none";
string g2oFile; int maxIterations = 100; // default
if (argc < 2) string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
g2oFile = findExampleDataFile("noisyToyGraph.txt");
else
g2oFile = argv[1];
// Parse user's inputs
if (argc > 1){
g2oFile = argv[1]; // input dataset filename
// outputFile = g2oFile = argv[2]; // done later
}
if (argc > 3){
maxIterations = atoi(argv[3]); // user can specify either tukey or huber
}
if (argc > 4){
kernelType = argv[4]; // user can specify either tukey or huber
}
// reading file and creating factor graph
NonlinearFactorGraph::shared_ptr graph; NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial; Values::shared_ptr initial;
boost::tie(graph, initial) = readG2o(g2oFile); bool is3D = false;
if(kernelType.compare("none") == 0){
boost::tie(graph, initial) = readG2o(g2oFile,is3D);
}
if(kernelType.compare("huber") == 0){
std::cout << "Using robust kernel: huber " << std::endl;
boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER);
}
if(kernelType.compare("tukey") == 0){
std::cout << "Using robust kernel: tukey " << std::endl;
boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY);
}
// Add prior on the pose having index (key) = 0 // Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph; NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel)); graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
std::cout << "Adding prior on pose 0 " << std::endl;
GaussNewtonParams params;
params.setVerbosity("TERMINATION");
if (argc > 3) {
params.maxIterations = maxIterations;
std::cout << "User required to perform maximum " << params.maxIterations << " iterations "<< std::endl;
}
std::cout << "Optimizing the factor graph" << std::endl; std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonOptimizer optimizer(graphWithPrior, *initial); GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
Values result = optimizer.optimize(); Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl; std::cout << "Optimization complete" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "final error=" <<graph->error(result)<< std::endl;
if (argc < 3) { if (argc < 3) {
result.print("result"); result.print("result");
} else { } else {
const string outputFile = argv[2]; const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl; std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, result, outputFile); NonlinearFactorGraph::shared_ptr graphNoKernel;
Values::shared_ptr initial2;
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
writeG2o(*graphNoKernel, result, outputFile);
std::cout << "done! " << std::endl; std::cout << "done! " << std::endl;
} }
return 0; return 0;

89
examples/Pose3SLAMExample_changeKeys.cpp Normal file
View File

@ -0,0 +1,89 @@
/* ----------------------------------------------------------------------------
* 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 Pose3SLAMExample_initializePose3.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_changeKeys input.g2o rewritted.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3example.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
bool add = false;
Key firstKey = 8646911284551352320;
std::cout << "Using reference key: " << firstKey << std::endl;
if(add)
std::cout << "adding key " << std::endl;
else
std::cout << "subtracting key " << std::endl;
if (argc < 3) {
std::cout << "Please provide output file to write " << std::endl;
} else {
const string inputFileRewritten = argv[2];
std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
// Additional: rewrite input with simplified keys 0,1,...
Values simpleInitial;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
Key key;
if(add)
key = key_value.key + firstKey;
else
key = key_value.key - firstKey;
simpleInitial.insert(key, initial->at(key_value.key));
}
NonlinearFactorGraph simpleGraph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between){
Key key1, key2;
if(add){
key1 = pose3Between->key1() + firstKey;
key2 = pose3Between->key2() + firstKey;
}else{
key1 = pose3Between->key1() - firstKey;
key2 = pose3Between->key2() - firstKey;
}
NonlinearFactor::shared_ptr simpleFactor(
new BetweenFactor<Pose3>(key1, key2, pose3Between->measured(), pose3Between->get_noiseModel()));
simpleGraph.add(simpleFactor);
}
}
writeG2o(simpleGraph, simpleInitial, inputFileRewritten);
}
return 0;
}

74
examples/Pose3SLAMExample_g2o.cpp Normal file
View File

@ -0,0 +1,74 @@
/* ----------------------------------------------------------------------------
* 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 Pose3SLAMExample_initializePose3.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3example.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break;
}
std::cout << "Optimizing the factor graph" << std::endl;
GaussNewtonParams params;
params.setVerbosity("TERMINATION"); // this will show info about stopping conditions
GaussNewtonOptimizer optimizer(graphWithPrior, *initial, params);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "final error=" <<graph->error(result)<< 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;
}

68
examples/Pose3SLAMExample_initializePose3Chordal.cpp Normal file
View File

@ -0,0 +1,68 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file Pose3SLAMExample_initializePose3.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3example.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break;
}
std::cout << "Initializing Pose3 - chordal relaxation" << std::endl;
Values initialization = InitializePose3::initialize(graphWithPrior);
std::cout << "done!" << std::endl;
if (argc < 3) {
initialization.print("initialization");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, initialization, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}

72
examples/Pose3SLAMExample_initializePose3Gradient.cpp Normal file
View File

@ -0,0 +1,72 @@
/* ----------------------------------------------------------------------------
* 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 Pose3SLAMExample_initializePose3.cpp
* @brief A 3D Pose SLAM example that reads input from g2o, and initializes the Pose3 using InitializePose3
* Syntax for the script is ./Pose3SLAMExample_initializePose3 input.g2o output.g2o
* @date Aug 25, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = findExampleDataFile("pose3example.txt");
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
bool is3D = true;
boost::tie(graph, initial) = readG2o(g2oFile, is3D);
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));
break;
}
std::cout << "Initializing Pose3 - Riemannian gradient" << std::endl;
bool useGradient = true;
Values initialization = InitializePose3::initialize(graphWithPrior, *initial, useGradient);
std::cout << "done!" << std::endl;
std::cout << "initial error=" <<graph->error(*initial)<< std::endl;
std::cout << "initialization error=" <<graph->error(initialization)<< std::endl;
if (argc < 3) {
initialization.print("initialization");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, initialization, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}

16
examples/SolverComparer.cpp
View File

@ -13,6 +13,22 @@
* @brief Incremental and batch solving, timing, and accuracy comparisons * @brief Incremental and batch solving, timing, and accuracy comparisons
* @author Richard Roberts * @author Richard Roberts
* @date August, 2013 * @date August, 2013
*
* Here is an example. Below, to run in batch mode, we first generate an initialization in incremental mode.
*
* Solve in incremental and write to file w_inc:
* ./SolverComparer --incremental -d w10000 -o w_inc
*
* You can then perturb that initialization to get batch something to optimize.
* Read in w_inc, perturb it with noise of stddev 0.6, and write to w_pert:
* ./SolverComparer --perturb 0.6 -i w_inc -o w_pert
*
* Then optimize with batch, read in w_pert, solve in batch, and write to w_batch:
* ./SolverComparer --batch -d w10000 -i w_pert -o w_batch
*
* And finally compare solutions in w_inc and w_batch to check that batch converged to the global minimum
* ./SolverComparer --compare w_inc w_batch
*
*/ */
#include <gtsam/base/timing.h> #include <gtsam/base/timing.h>

43
examples/VisualISAMExample.cpp
View File

@ -14,6 +14,7 @@
* @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset * @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset
* This version uses iSAM to solve the problem incrementally * This version uses iSAM to solve the problem incrementally
* @author Duy-Nguyen Ta * @author Duy-Nguyen Ta
* @author Frank Dellaert
*/ */
/** /**
@ -61,7 +62,8 @@ int main(int argc, char* argv[]) {
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)); Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model // Define the camera observation noise model
noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v noiseModel::Isotropic::shared_ptr noise = //
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks // Create the set of ground-truth landmarks
vector<Point3> points = createPoints(); vector<Point3> points = createPoints();
@ -69,7 +71,8 @@ int main(int argc, char* argv[]) {
// Create the set of ground-truth poses // Create the set of ground-truth poses
vector<Pose3> poses = createPoses(); vector<Pose3> poses = createPoses();
// Create a NonlinearISAM object which will relinearize and reorder the variables every "relinearizeInterval" updates // Create a NonlinearISAM object which will relinearize and reorder the variables
// every "relinearizeInterval" updates
int relinearizeInterval = 3; int relinearizeInterval = 3;
NonlinearISAM isam(relinearizeInterval); NonlinearISAM isam(relinearizeInterval);
@ -82,32 +85,44 @@ int main(int argc, char* argv[]) {
// Add factors for each landmark observation // Add factors for each landmark observation
for (size_t j = 0; j < points.size(); ++j) { for (size_t j = 0; j < points.size(); ++j) {
// Create ground truth measurement
SimpleCamera camera(poses[i], *K); SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]); Point2 measurement = camera.project(points[j]);
graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); // Add measurement
graph.add(
GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, noise,
Symbol('x', i), Symbol('l', j), K));
} }
// Add an initial guess for the current pose
// Intentionally initialize the variables off from the ground truth // Intentionally initialize the variables off from the ground truth
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20)))); Pose3 noise(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
Pose3 initial_xi = poses[i].compose(noise);
// Add an initial guess for the current pose
initialEstimate.insert(Symbol('x', i), initial_xi);
// If this is the first iteration, add a prior on the first pose to set the coordinate frame // If this is the first iteration, add a prior on the first pose to set the coordinate frame
// and a prior on the first landmark to set the scale // and a prior on the first landmark to set the scale
// Also, as iSAM solves incrementally, we must wait until each is observed at least twice before // Also, as iSAM solves incrementally, we must wait until each is observed at least twice before
// adding it to iSAM. // adding it to iSAM.
if( i == 0) { if (i == 0) {
// Add a prior on pose x0 // Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); (Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)));
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Add a prior on landmark l0 // Add a prior on landmark l0
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1); noiseModel::Isotropic::shared_ptr pointNoise =
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph noiseModel::Isotropic::Sigma(3, 0.1);
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise));
// Add initial guesses to all observed landmarks // Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth Point3 noise(-0.25, 0.20, 0.15);
for (size_t j = 0; j < points.size(); ++j) for (size_t j = 0; j < points.size(); ++j) {
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15))); // Intentionally initialize the variables off from the ground truth
Point3 initial_lj = points[j].compose(noise);
initialEstimate.insert(Symbol('l', j), initial_lj);
}
} else { } else {
// Update iSAM with the new factors // Update iSAM with the new factors

3
gtsam/base/DerivedValue.h
View File

@ -16,8 +16,9 @@
*/ */
#pragma once #pragma once
#include <boost/make_shared.hpp>
#include <gtsam/base/Value.h> #include <gtsam/base/Value.h>
#include <boost/make_shared.hpp>
////////////////// //////////////////
// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR // The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR

8
gtsam/base/LieMatrix.h
View File

@ -19,9 +19,9 @@
#include <cstdarg> #include <cstdarg>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.h> #include <gtsam/base/Lie.h>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <gtsam/base/DerivedValue.h>
#include <boost/serialization/nvp.hpp> #include <boost/serialization/nvp.hpp>
namespace gtsam { namespace gtsam {
@ -40,9 +40,12 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
/** initialize from a normal matrix */ /** initialize from a normal matrix */
LieMatrix(const Matrix& v) : Matrix(v) {} LieMatrix(const Matrix& v) : Matrix(v) {}
// Currently TMP constructor causes ICE on MSVS 2013
#if (_MSC_VER < 1800)
/** initialize from a fixed size normal vector */ /** initialize from a fixed size normal vector */
template<int M, int N> template<int M, int N>
LieMatrix(const Eigen::Matrix<double, M, N>& v) : Matrix(v) {} LieMatrix(const Eigen::Matrix<double, M, N>& v) : Matrix(v) {}
#endif
/** constructor with size and initial data, row order ! */ /** constructor with size and initial data, row order ! */
LieMatrix(size_t m, size_t n, const double* const data) : LieMatrix(size_t m, size_t n, const double* const data) :
@ -82,6 +85,7 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
inline LieMatrix retract(const Vector& v) const { inline LieMatrix retract(const Vector& v) const {
if(v.size() != this->size()) if(v.size() != this->size())
throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size"); throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size");
return LieMatrix(*this + return LieMatrix(*this +
Eigen::Map<const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >( Eigen::Map<const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(
&v(0), this->rows(), this->cols())); &v(0), this->rows(), this->cols()));
@ -153,7 +157,7 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
result.data(), p.rows(), p.cols()) = p; result.data(), p.rows(), p.cols()) = p;
return result; return result;
} }
/// @} /// @}
private: private:

5
gtsam/base/LieVector.h
View File

@ -33,10 +33,13 @@ struct LieVector : public Vector, public DerivedValue<LieVector> {
/** initialize from a normal vector */ /** initialize from a normal vector */
LieVector(const Vector& v) : Vector(v) {} LieVector(const Vector& v) : Vector(v) {}
// Currently TMP constructor causes ICE on MSVS 2013
#if (_MSC_VER < 1800)
/** initialize from a fixed size normal vector */ /** initialize from a fixed size normal vector */
template<int N> template<int N>
LieVector(const Eigen::Matrix<double, N, 1>& v) : Vector(v) {} LieVector(const Eigen::Matrix<double, N, 1>& v) : Vector(v) {}
#endif
/** wrap a double */ /** wrap a double */
LieVector(double d) : Vector((Vector(1) << d)) {} LieVector(double d) : Vector((Vector(1) << d)) {}

38
gtsam/base/Value.h
View File

@ -36,18 +36,19 @@ namespace gtsam {
* Values can operate generically on Value objects, retracting or computing * Values can operate generically on Value objects, retracting or computing
* local coordinates for many Value objects of different types. * local coordinates for many Value objects of different types.
* *
* When you implement retract_(), localCoordinates_(), and equals_(), we * Inheriting from the DerivedValue class templated provides a generic implementation of
* suggest first implementing versions of these functions that work directly * the pure virtual functions retract_(), localCoordinates_(), and equals_(), eliminating
* with derived objects, then using the provided helper functions to * the need to implement these functions in your class. Note that you must inherit from
* implement the generic Value versions. This makes your implementation * DerivedValue templated on the class you are defining. For example you cannot define
* easier, and also improves performance in situations where the derived type * the following
* is in fact known, such as in most implementations of \c evaluateError() in * \code
* classes derived from NonlinearFactor. * class Rot3 : public DerivedValue<Point3>{ \\classdef }
* \endcode
* *
* Using the above practice, here is an example of implementing a typical * Using the above practice, here is an example of implementing a typical
* class derived from Value: * class derived from Value:
* \code * \code
class Rot3 : public Value { class GTSAM_EXPORT Rot3 : public DerivedValue<Rot3> {
public: public:
// Constructor, there is never a need to call the Value base class constructor. // Constructor, there is never a need to call the Value base class constructor.
Rot3() { ... } Rot3() { ... }
@ -74,27 +75,6 @@ namespace gtsam {
// Math to implement 3D rotation localCoordinates, e.g. logarithm map // Math to implement 3D rotation localCoordinates, e.g. logarithm map
return Vector(result); return Vector(result);
} }
// Equals implementing the generic Value interface (virtual, implements Value::equals_())
virtual bool equals_(const Value& other, double tol = 1e-9) const {
// Call our provided helper function to call your Rot3-specific
// equals with appropriate casting.
return CallDerivedEquals(this, other, tol);
}
// retract implementing the generic Value interface (virtual, implements Value::retract_())
virtual std::auto_ptr<Value> retract_(const Vector& delta) const {
// Call our provided helper function to call your Rot3-specific
// retract and do the appropriate casting and allocation.
return CallDerivedRetract(this, delta);
}
// localCoordinates implementing the generic Value interface (virtual, implements Value::localCoordinates_())
virtual Vector localCoordinates_(const Value& value) const {
// Call our provided helper function to call your Rot3-specific
// localCoordinates and do the appropriate casting.
return CallDerivedLocalCoordinates(this, value);
}
}; };
\endcode \endcode
*/ */

54
gtsam/base/Vector.cpp
View File

@ -30,55 +30,11 @@
#include <gtsam/base/Vector.h> #include <gtsam/base/Vector.h>
//#ifdef WIN32
//#include <Windows.h>
//#endif
using namespace std; using namespace std;
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
void odprintf_(const char *format, ostream& stream, ...) {
char buf[4096], *p = buf;
va_list args;
va_start(args, stream);
#ifdef WIN32
_vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#else
vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#endif
va_end(args);
//#ifdef WIN32
// OutputDebugString(buf);
//#else
stream << buf;
//#endif
}
/* ************************************************************************* */
void odprintf(const char *format, ...) {
char buf[4096], *p = buf;
va_list args;
va_start(args, format);
#ifdef WIN32
_vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#else
vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#endif
va_end(args);
//#ifdef WIN32
// OutputDebugString(buf);
//#else
cout << buf;
//#endif
}
/* ************************************************************************* */ /* ************************************************************************* */
bool zero(const Vector& v) { bool zero(const Vector& v) {
bool result = true; bool result = true;
@ -101,10 +57,12 @@ Vector delta(size_t n, size_t i, double value) {
/* ************************************************************************* */ /* ************************************************************************* */
void print(const Vector& v, const string& s, ostream& stream) { void print(const Vector& v, const string& s, ostream& stream) {
size_t n = v.size(); size_t n = v.size();
odprintf_("%s [", stream, s.c_str());
for(size_t i=0; i<n; i++) stream << s << "[";
odprintf_("%g%s", stream, v[i], (i<n-1 ? "; " : "")); for(size_t i=0; i<n; i++) {
odprintf_("];\n", stream); stream << setprecision(9) << v(i) << (i<n-1 ? "; " : "");
}
stream << "];" << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */

5
gtsam/base/Vector.h
View File

@ -41,11 +41,6 @@ typedef Eigen::Matrix<double, 6, 1> Vector6;
typedef Eigen::VectorBlock<Vector> SubVector; typedef Eigen::VectorBlock<Vector> SubVector;
typedef Eigen::VectorBlock<const Vector> ConstSubVector; typedef Eigen::VectorBlock<const Vector> ConstSubVector;
/**
* An auxiliary function to printf for Win32 compatibility, added by Kai
*/
GTSAM_EXPORT void odprintf(const char *format, ...);
/** /**
* Create vector initialized to a constant value * Create vector initialized to a constant value
* @param n is the size of the vector * @param n is the size of the vector

24
gtsam/base/tests/testMatrix.cpp
View File

@ -1127,6 +1127,12 @@ TEST( matrix, svd2 )
svd(sampleA, U, s, V); svd(sampleA, U, s, V);
// take care of sign ambiguity
if (U(0, 1) > 0) {
U = -U;
V = -V;
}
EXPECT(assert_equal(expectedU,U)); EXPECT(assert_equal(expectedU,U));
EXPECT(assert_equal(expected_s,s,1e-9)); EXPECT(assert_equal(expected_s,s,1e-9));
EXPECT(assert_equal(expectedV,V)); EXPECT(assert_equal(expectedV,V));
@ -1143,6 +1149,13 @@ TEST( matrix, svd3 )
Matrix expectedV = (Matrix(3, 2) << 0.,1.,0.,0.,-1.,0.); Matrix expectedV = (Matrix(3, 2) << 0.,1.,0.,0.,-1.,0.);
svd(sampleAt, U, s, V); svd(sampleAt, U, s, V);
// take care of sign ambiguity
if (U(0, 0) > 0) {
U = -U;
V = -V;
}
Matrix S = diag(s); Matrix S = diag(s);
Matrix t = U * S; Matrix t = U * S;
Matrix Vt = trans(V); Matrix Vt = trans(V);
@ -1176,6 +1189,17 @@ TEST( matrix, svd4 )
0.6723, 0.7403); 0.6723, 0.7403);
svd(A, U, s, V); svd(A, U, s, V);
// take care of sign ambiguity
if (U(0, 0) < 0) {
U.col(0) = -U.col(0);
V.col(0) = -V.col(0);
}
if (U(0, 1) < 0) {
U.col(1) = -U.col(1);
V.col(1) = -V.col(1);
}
Matrix reconstructed = U * diag(s) * trans(V); Matrix reconstructed = U * diag(s) * trans(V);
EXPECT(assert_equal(A, reconstructed, 1e-4)); EXPECT(assert_equal(A, reconstructed, 1e-4));

4
gtsam/base/types.h
View File

@ -299,6 +299,8 @@ namespace gtsam {
// Define some common g++ functions and macros we use that MSVC does not have // Define some common g++ functions and macros we use that MSVC does not have
#if (_MSC_VER < 1800)
#include <boost/math/special_functions/fpclassify.hpp> #include <boost/math/special_functions/fpclassify.hpp>
namespace std { namespace std {
template<typename T> inline int isfinite(T a) { template<typename T> inline int isfinite(T a) {
@ -309,6 +311,8 @@ namespace std {
return (int)boost::math::isinf(a); } return (int)boost::math::isinf(a); }
} }
#endif
#include <boost/math/constants/constants.hpp> #include <boost/math/constants/constants.hpp>
#ifndef M_PI #ifndef M_PI
#define M_PI (boost::math::constants::pi<double>()) #define M_PI (boost::math::constants::pi<double>())

2
gtsam/geometry/Cal3Bundler.h
View File

@ -53,6 +53,8 @@ public:
*/ */
Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0); Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0);
virtual ~Cal3Bundler() {}
/// @} /// @}
/// @name Testable /// @name Testable
/// @{ /// @{

146
gtsam/geometry/Cal3DS2.cpp
View File

@ -23,24 +23,9 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */
Cal3DS2::Cal3DS2(const Vector &v):
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
/* ************************************************************************* */
Matrix Cal3DS2::K() const {
return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
}
/* ************************************************************************* */
Vector Cal3DS2::vector() const {
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_);
}
/* ************************************************************************* */ /* ************************************************************************* */
void Cal3DS2::print(const std::string& s_) const { void Cal3DS2::print(const std::string& s_) const {
gtsam::print(K(), s_ + ".K"); Base::print(s_);
gtsam::print(Vector(k()), s_ + ".k");
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -52,135 +37,6 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
return true; return true;
} }
/* ************************************************************************* */
static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx,
double yy, double xy, double rr, double r4, double pnx, double pny,
const Eigen::Matrix<double, 2, 2>& DK) {
Eigen::Matrix<double, 2, 5> DR1;
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 4> DR2;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy;
Eigen::Matrix<double, 2, 9> D;
D << DR1, DK * DR2;
return D;
}
/* ************************************************************************* */
static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
double g, double k1, double k2, double p1, double p2,
const Eigen::Matrix<double, 2, 2>& DK) {
const double drdx = 2. * x;
const double drdy = 2. * y;
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
const double dgdy = k1 * drdy + k2 * 2. * rr * drdy;
// Dx = 2*p1*xy + p2*(rr+2*xx);
// Dy = 2*p2*xy + p1*(rr+2*yy);
const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x);
const double dDxdy = 2. * p1 * x + p2 * drdy;
const double dDydx = 2. * p2 * y + p1 * drdx;
const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
Eigen::Matrix<double, 2, 2> DR;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy;
return DK * DR;
}
/* ************************************************************************* */
Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);
// dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
// pi(:,i) = g * pn(:,i) + dp;
const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
// tangential component
const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy);
// Radial and tangential distortion applied
const double pnx = g * x + dx;
const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
// Derivative for calibration
if (H1)
*H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
// Derivative for points
if (H2)
*H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
// Regular uncalibrate after distortion
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
}
/* ************************************************************************* */
Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
// Use the following fixed point iteration to invert the radial distortion.
// pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
(1 / fy_) * (pi.y() - v0_));
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary
Point2 pn = invKPi;
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (uncalibrate(pn).distance(pi) <= tol) break;
const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr);
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
pn = (invKPi - Point2(dx, dy)) / g;
}
if ( iteration >= maxIterations )
throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
return pn;
}
/* ************************************************************************* */
Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
Eigen::Matrix<double, 2, 2> DK;
DK << fx_, s_, 0.0, fy_;
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
}
/* ************************************************************************* */
Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
const double pnx = g * x + dx;
const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK;
DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
}
/* ************************************************************************* */ /* ************************************************************************* */
Cal3DS2 Cal3DS2::retract(const Vector& d) const { Cal3DS2 Cal3DS2::retract(const Vector& d) const {
return Cal3DS2(vector() + d); return Cal3DS2(vector() + d);

86
gtsam/geometry/Cal3DS2.h
View File

@ -11,7 +11,7 @@
/** /**
* @file Cal3DS2.h * @file Cal3DS2.h
* @brief Calibration of a camera with radial distortion * @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base
* @date Feb 28, 2010 * @date Feb 28, 2010
* @author ydjian * @author ydjian
*/ */
@ -19,7 +19,7 @@
#pragma once #pragma once
#include <gtsam/base/DerivedValue.h> #include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Cal3DS2_Base.h>
namespace gtsam { namespace gtsam {
@ -37,34 +37,29 @@ namespace gtsam {
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn * pi = K*pn
*/ */
class GTSAM_EXPORT Cal3DS2 : public DerivedValue<Cal3DS2> { class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue<Cal3DS2> {
protected: typedef Cal3DS2_Base Base;
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
double k1_, k2_ ; // radial 2nd-order and 4th-order
double p1_, p2_ ; // tangential distortion
public: public:
Matrix K() const ;
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
Vector vector() const ;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
/// Default Constructor with only unit focal length /// Default Constructor with only unit focal length
Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {} Cal3DS2() : Base() {}
Cal3DS2(double fx, double fy, double s, double u0, double v0, Cal3DS2(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double p1 = 0.0, double p2 = 0.0) : double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {} Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {}
virtual ~Cal3DS2() {}
/// @} /// @}
/// @name Advanced Constructors /// @name Advanced Constructors
/// @{ /// @{
Cal3DS2(const Vector &v) ; Cal3DS2(const Vector &v) : Base(v) {}
/// @} /// @}
/// @name Testable /// @name Testable
@ -76,57 +71,6 @@ public:
/// assert equality up to a tolerance /// assert equality up to a tolerance
bool equals(const Cal3DS2& K, double tol = 10e-9) const; bool equals(const Cal3DS2& K, double tol = 10e-9) const;
/// @}
/// @name Standard Interface
/// @{
/// focal length x
inline double fx() const { return fx_;}
/// focal length x
inline double fy() const { return fy_;}
/// skew
inline double skew() const { return s_;}
/// image center in x
inline double px() const { return u0_;}
/// image center in y
inline double py() const { return v0_;}
/// First distortion coefficient
inline double k1() const { return k1_;}
/// Second distortion coefficient
inline double k2() const { return k2_;}
/// First tangential distortion coefficient
inline double p1() const { return p1_;}
/// Second tangential distortion coefficient
inline double p2() const { return p2_;}
/**
* convert intrinsic coordinates xy to (distorted) image coordinates uv
* @param p point in intrinsic coordinates
* @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates
Matrix D2d_intrinsic(const Point2& p) const ;
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix D2d_calibration(const Point2& p) const ;
/// @} /// @}
/// @name Manifold /// @name Manifold
/// @{ /// @{
@ -156,18 +100,10 @@ private:
{ {
ar & boost::serialization::make_nvp("Cal3DS2", ar & boost::serialization::make_nvp("Cal3DS2",
boost::serialization::base_object<Value>(*this)); boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(fx_); ar & boost::serialization::make_nvp("Cal3DS2",
ar & BOOST_SERIALIZATION_NVP(fy_); boost::serialization::base_object<Cal3DS2_Base>(*this));
ar & BOOST_SERIALIZATION_NVP(s_);
ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(p1_);
ar & BOOST_SERIALIZATION_NVP(p2_);
} }
/// @} /// @}
}; };

187
gtsam/geometry/Cal3DS2_Base.cpp Normal file
View File

@ -0,0 +1,187 @@
/* ----------------------------------------------------------------------------
* 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 Cal3DS2_Base.cpp
* @date Feb 28, 2010
* @author ydjian
*/
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Cal3DS2_Base.h>
namespace gtsam {
/* ************************************************************************* */
Cal3DS2_Base::Cal3DS2_Base(const Vector &v):
fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
/* ************************************************************************* */
Matrix Cal3DS2_Base::K() const {
return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0);
}
/* ************************************************************************* */
Vector Cal3DS2_Base::vector() const {
return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_);
}
/* ************************************************************************* */
void Cal3DS2_Base::print(const std::string& s_) const {
gtsam::print(K(), s_ + ".K");
gtsam::print(Vector(k()), s_ + ".k");
}
/* ************************************************************************* */
bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol)
return false;
return true;
}
/* ************************************************************************* */
static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx,
double yy, double xy, double rr, double r4, double pnx, double pny,
const Eigen::Matrix<double, 2, 2>& DK) {
Eigen::Matrix<double, 2, 5> DR1;
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 4> DR2;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy;
Eigen::Matrix<double, 2, 9> D;
D << DR1, DK * DR2;
return D;
}
/* ************************************************************************* */
static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
double g, double k1, double k2, double p1, double p2,
const Eigen::Matrix<double, 2, 2>& DK) {
const double drdx = 2. * x;
const double drdy = 2. * y;
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
const double dgdy = k1 * drdy + k2 * 2. * rr * drdy;
// Dx = 2*p1*xy + p2*(rr+2*xx);
// Dy = 2*p2*xy + p1*(rr+2*yy);
const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x);
const double dDxdy = 2. * p1 * x + p2 * drdy;
const double dDydx = 2. * p2 * y + p1 * drdx;
const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
Eigen::Matrix<double, 2, 2> DR;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy;
return DK * DR;
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);
// dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
// pi(:,i) = g * pn(:,i) + dp;
const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
// tangential component
const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy);
// Radial and tangential distortion applied
const double pnx = g * x + dx;
const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_;
// Derivative for calibration
if (H1)
*H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
// Derivative for points
if (H2)
*H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
// Regular uncalibrate after distortion
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
}
/* ************************************************************************* */
Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
// Use the following fixed point iteration to invert the radial distortion.
// pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t})
const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)),
(1 / fy_) * (pi.y() - v0_));
// initialize by ignoring the distortion at all, might be problematic for pixels around boundary
Point2 pn = invKPi;
// iterate until the uncalibrate is close to the actual pixel coordinate
const int maxIterations = 10;
int iteration;
for (iteration = 0; iteration < maxIterations; ++iteration) {
if (uncalibrate(pn).distance(pi) <= tol) break;
const double x = pn.x(), y = pn.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double g = (1 + k1_ * rr + k2_ * rr * rr);
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
pn = (invKPi - Point2(dx, dy)) / g;
}
if ( iteration >= maxIterations )
throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization");
return pn;
}
/* ************************************************************************* */
Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
Eigen::Matrix<double, 2, 2> DK;
DK << fx_, s_, 0.0, fy_;
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
}
/* ************************************************************************* */
Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x * x, yy = y * y, xy = x * y;
const double rr = xx + yy;
const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4);
const double dx = 2 * p1_ * xy + p2_ * (rr + 2 * xx);
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
const double pnx = g * x + dx;
const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK;
DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
}
}
/* ************************************************************************* */

158
gtsam/geometry/Cal3DS2_Base.h Normal file
View File

@ -0,0 +1,158 @@
/* ----------------------------------------------------------------------------
* 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 Cal3DS2.h
* @brief Calibration of a camera with radial distortion
* @date Feb 28, 2010
* @author ydjian
*/
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
/**
* @brief Calibration of a camera with radial distortion
* @addtogroup geometry
* \nosubgrouping
*
* Uses same distortionmodel as OpenCV, with
* http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
* but using only k1,k2,p1, and p2 coefficients.
* K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
* rr = Pn.x^2 + Pn.y^2
* \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn
*/
class GTSAM_EXPORT Cal3DS2_Base {
protected:
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
double k1_, k2_ ; // radial 2nd-order and 4th-order
double p1_, p2_ ; // tangential distortion
public:
Matrix K() const ;
Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
Vector vector() const ;
/// @name Standard Constructors
/// @{
/// Default Constructor with only unit focal length
Cal3DS2_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
Cal3DS2_Base(double fx, double fy, double s, double u0, double v0,
double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
/// @}
/// @name Advanced Constructors
/// @{
Cal3DS2_Base(const Vector &v) ;
/// @}
/// @name Testable
/// @{
/// print with optional string
void print(const std::string& s = "") const ;
/// assert equality up to a tolerance
bool equals(const Cal3DS2_Base& K, double tol = 10e-9) const;
/// @}
/// @name Standard Interface
/// @{
/// focal length x
inline double fx() const { return fx_;}
/// focal length x
inline double fy() const { return fy_;}
/// skew
inline double skew() const { return s_;}
/// image center in x
inline double px() const { return u0_;}
/// image center in y
inline double py() const { return v0_;}
/// First distortion coefficient
inline double k1() const { return k1_;}
/// Second distortion coefficient
inline double k2() const { return k2_;}
/// First tangential distortion coefficient
inline double p1() const { return p1_;}
/// Second tangential distortion coefficient
inline double p2() const { return p2_;}
/**
* convert intrinsic coordinates xy to (distorted) image coordinates uv
* @param p point in intrinsic coordinates
* @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates
Matrix D2d_intrinsic(const Point2& p) const ;
/// Derivative of uncalibrate wrpt the calibration parameters
Matrix D2d_calibration(const Point2& p) const ;
private:
/// @}
/// @name Advanced Interface
/// @{
/** Serialization function */
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);
ar & BOOST_SERIALIZATION_NVP(u0_);
ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_);
ar & BOOST_SERIALIZATION_NVP(p1_);
ar & BOOST_SERIALIZATION_NVP(p2_);
}
/// @}
};
}

14
gtsam/geometry/Cal3Unified.h
View File

@ -22,8 +22,8 @@
#pragma once #pragma once
#include <gtsam/geometry/Cal3DS2.h> #include <gtsam/geometry/Cal3DS2_Base.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/base/DerivedValue.h>
namespace gtsam { namespace gtsam {
@ -40,10 +40,10 @@ namespace gtsam {
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn * pi = K*pn
*/ */
class GTSAM_EXPORT Cal3Unified : public Cal3DS2 { class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue<Cal3Unified> {
typedef Cal3Unified This; typedef Cal3Unified This;
typedef Cal3DS2 Base; typedef Cal3DS2_Base Base;
private: private:
@ -90,7 +90,7 @@ public:
/** /**
* convert intrinsic coordinates xy to image coordinates uv * convert intrinsic coordinates xy to image coordinates uv
* @param p point in intrinsic coordinates * @param p point in intrinsic coordinates
* @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters * @param Dcal optional 2*10 Jacobian wrpt Cal3Unified parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates * @return point in image coordinates
*/ */
@ -135,7 +135,9 @@ private:
void serialize(Archive & ar, const unsigned int version) void serialize(Archive & ar, const unsigned int version)
{ {
ar & boost::serialization::make_nvp("Cal3Unified", ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Cal3DS2>(*this)); boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Cal3DS2_Base>(*this));
ar & BOOST_SERIALIZATION_NVP(xi_); ar & BOOST_SERIALIZATION_NVP(xi_);
} }

10
gtsam/geometry/Cal3_S2.h
View File

@ -165,6 +165,16 @@ public:
*/ */
Vector3 calibrate(const Vector3& p) const; Vector3 calibrate(const Vector3& p) const;
/// "Between", subtracts calibrations. between(p,q) == compose(inverse(p),q)
inline Cal3_S2 between(const Cal3_S2& q,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const {
if(H1) *H1 = -eye(5);
if(H2) *H2 = eye(5);
return Cal3_S2(q.fx_-fx_, q.fy_-fy_, q.s_-s_, q.u0_-u0_, q.v0_-v0_);
}
/// @} /// @}
/// @name Manifold /// @name Manifold
/// @{ /// @{

4
gtsam/geometry/Rot3M.cpp
View File

@ -240,7 +240,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
return retractCayley(omega); return retractCayley(omega);
} else if(mode == Rot3::SLOW_CAYLEY) { } else if(mode == Rot3::SLOW_CAYLEY) {
Matrix Omega = skewSymmetric(omega); Matrix Omega = skewSymmetric(omega);
return (*this)*Cayley<3>(-Omega/2); return (*this)*CayleyFixed<3>(-Omega/2);
} else { } else {
assert(false); assert(false);
exit(1); exit(1);
@ -269,7 +269,7 @@ Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const
// Create a fixed-size matrix // Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix()); Eigen::Matrix3d A(between(T).matrix());
// using templated version of Cayley // using templated version of Cayley
Eigen::Matrix3d Omega = Cayley<3>(A); Eigen::Matrix3d Omega = CayleyFixed<3>(A);
return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
} else { } else {
assert(false); assert(false);

32
gtsam/geometry/Rot3Q.cpp
View File

@ -21,6 +21,7 @@
#include <boost/math/constants/constants.hpp> #include <boost/math/constants/constants.hpp>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <cmath>
using namespace std; using namespace std;
@ -120,14 +121,31 @@ namespace gtsam {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation
Vector3 Rot3::Logmap(const Rot3& R) { Vector3 Rot3::Logmap(const Rot3& R) {
Eigen::AngleAxisd angleAxis(R.quaternion_); using std::acos;
if(angleAxis.angle() > M_PI) // Important: use the smallest possible using std::sqrt;
angleAxis.angle() -= 2.0*M_PI; // angle, e.g. no more than PI, to keep static const double twoPi = 2.0 * M_PI,
if(angleAxis.angle() < -M_PI) // error continuous. // define these compile time constants to avoid std::abs:
angleAxis.angle() += 2.0*M_PI; NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10;
return angleAxis.axis() * angleAxis.angle();
const Quaternion& q = R.quaternion_;
const double qw = q.w();
if (qw > NearlyOne) {
// Taylor expansion of (angle / s) at 1
return (2 - 2 * (qw - 1) / 3) * q.vec();
} else if (qw < NearlyNegativeOne) {
// Angle is zero, return zero vector
return Vector3::Zero();
} else {
// Normal, away from zero case
double angle = 2 * acos(qw), s = sqrt(1 - qw * qw);
// Important: convert to [-pi,pi] to keep error continuous
if (angle > M_PI)
angle -= twoPi;
else if (angle < -M_PI)
angle += twoPi;
return (angle / s) * q.vec();
}
} }
/* ************************************************************************* */ /* ************************************************************************* */

21
gtsam/geometry/Unit3.cpp
View File

@ -21,7 +21,16 @@
#include <gtsam/geometry/Unit3.h> #include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <boost/random/mersenne_twister.hpp> #include <boost/random/mersenne_twister.hpp>
#ifdef __clang__
# pragma clang diagnostic push
# pragma clang diagnostic ignored "-Wunused-variable"
#endif
#include <boost/random/uniform_on_sphere.hpp> #include <boost/random/uniform_on_sphere.hpp>
#ifdef __clang__
# pragma clang diagnostic pop
#endif
#include <boost/random/variate_generator.hpp> #include <boost/random/variate_generator.hpp>
#include <iostream> #include <iostream>
@ -58,11 +67,11 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
const Matrix& Unit3::basis() const { const Unit3::Matrix32& Unit3::basis() const {
// Return cached version if exists // Return cached version if exists
if (B_.rows() == 3) if (B_)
return B_; return *B_;
// Get the axis of rotation with the minimum projected length of the point // Get the axis of rotation with the minimum projected length of the point
Point3 axis; Point3 axis;
@ -83,9 +92,9 @@ const Matrix& Unit3::basis() const {
b2 = b2 / b2.norm(); b2 = b2 / b2.norm();
// Create the basis matrix // Create the basis matrix
B_ = Matrix(3, 2); B_.reset(Unit3::Matrix32());
B_ << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z(); (*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
return B_; return *B_;
} }
/* ************************************************************************* */ /* ************************************************************************* */

7
gtsam/geometry/Unit3.h
View File

@ -23,6 +23,7 @@
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/base/DerivedValue.h> #include <gtsam/base/DerivedValue.h>
#include <boost/random/mersenne_twister.hpp> #include <boost/random/mersenne_twister.hpp>
#include <boost/optional.hpp>
namespace gtsam { namespace gtsam {
@ -31,8 +32,10 @@ class GTSAM_EXPORT Unit3: public DerivedValue<Unit3> {
private: private:
typedef Eigen::Matrix<double,3,2> Matrix32;
Point3 p_; ///< The location of the point on the unit sphere Point3 p_; ///< The location of the point on the unit sphere
mutable Matrix B_; ///< Cached basis mutable boost::optional<Matrix32> B_; ///< Cached basis
public: public:
@ -84,7 +87,7 @@ public:
* It is a 3*2 matrix [b1 b2] composed of two orthogonal directions * It is a 3*2 matrix [b1 b2] composed of two orthogonal directions
* tangent to the sphere at the current direction. * tangent to the sphere at the current direction.
*/ */
const Matrix& basis() const; const Matrix32& basis() const;
/// Return skew-symmetric associated with 3D point on unit sphere /// Return skew-symmetric associated with 3D point on unit sphere
Matrix skew() const; Matrix skew() const;

16
gtsam/geometry/tests/testCal3Unified.cpp
View File

@ -19,6 +19,9 @@
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Cal3Unified.h> #include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Key.h>
using namespace gtsam; using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Cal3Unified) GTSAM_CONCEPT_TESTABLE_INST(Cal3Unified)
@ -97,6 +100,19 @@ TEST( Cal3Unified, retract)
CHECK(assert_equal(d,K.localCoordinates(actual),1e-9)); CHECK(assert_equal(d,K.localCoordinates(actual),1e-9));
} }
/* ************************************************************************* */
TEST( Cal3Unified, DerivedValue)
{
Values values;
Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10);
Key key = 1;
values.insert(key, cal);
Cal3Unified calafter = values.at<Cal3Unified>(key);
CHECK(assert_equal(cal,calafter,1e-9));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */ /* ************************************************************************* */

11
gtsam/geometry/tests/testCal3_S2.cpp
View File

@ -93,6 +93,17 @@ TEST( Cal3_S2, retract)
CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); CHECK(assert_equal(d,K.localCoordinates(actual),1e-7));
} }
/* ************************************************************************* */
TEST(Cal3_S2, between) {
Cal3_S2 k1(5, 5, 5, 5, 5), k2(5, 6, 7, 8, 9);
Matrix H1, H2;
EXPECT(assert_equal(Cal3_S2(0,1,2,3,4), k1.between(k2, H1, H2)));
EXPECT(assert_equal(-eye(5), H1));
EXPECT(assert_equal(eye(5), H2));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;

17
gtsam/geometry/tests/testEssentialMatrix.cpp
View File

@ -173,6 +173,12 @@ TEST (EssentialMatrix, epipoles) {
Vector S; Vector S;
gtsam::svd(E.matrix(), U, S, V); gtsam::svd(E.matrix(), U, S, V);
// take care of SVD sign ambiguity
if (U(0, 2) > 0) {
U = -U;
V = -V;
}
// check rank 2 constraint // check rank 2 constraint
CHECK(fabs(S(2))<1e-10); CHECK(fabs(S(2))<1e-10);
@ -182,8 +188,15 @@ TEST (EssentialMatrix, epipoles) {
// Check epipoles // Check epipoles
// Epipole in image 1 is just E.direction() // Epipole in image 1 is just E.direction()
Unit3 e1(U(0, 2), U(1, 2), U(2, 2)); Unit3 e1(-U(0, 2), -U(1, 2), -U(2, 2));
EXPECT(assert_equal(e1, E.epipole_a())); Unit3 actual = E.epipole_a();
EXPECT(assert_equal(e1, actual));
// take care of SVD sign ambiguity
if (V(0, 2) < 0) {
U = -U;
V = -V;
}
// Epipole in image 2 is E.rotation().unrotate(E.direction()) // Epipole in image 2 is E.rotation().unrotate(E.direction())
Unit3 e2(V(0, 2), V(1, 2), V(2, 2)); Unit3 e2(V(0, 2), V(1, 2), V(2, 2));

43
gtsam/geometry/tests/testPinholeCamera.cpp
View File

@ -183,25 +183,30 @@ TEST( PinholeCamera, Dproject)
} }
/* ************************************************************************* */ /* ************************************************************************* */
//static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) { static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const Cal3_S2& cal) {
// Point3 point(point2D.x(), point2D.y(), 1.0); return Camera(pose,cal).projectPointAtInfinity(point3D);
// return Camera(pose,cal).projectPointAtInfinity(point); }
//}
// TEST( PinholeCamera, Dproject_Infinity)
//TEST( PinholeCamera, Dproject_Infinity) {
//{ Matrix Dpose, Dpoint, Dcal;
// Matrix Dpose, Dpoint, Dcal; Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera
// Point2 point2D(-0.08,-0.08);
// Point3 point3D(point1.x(), point1.y(), 1.0); // test Projection
// Point2 result = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal);
// Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point2D, K); Point2 expected(-5.0, 5.0);
// Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point2D, K); CHECK(assert_equal(actual, expected, 1e-7));
// Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point2D, K);
// CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); // test Jacobians
// CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point3D, K);
// CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point3D, K);
//} Matrix numerical_point2x2 = numerical_point.block(0,0,2,2); // only the direction to the point matters
// Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point3D, K);
CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
CHECK(assert_equal(numerical_point2x2, Dpoint, 1e-7));
CHECK(assert_equal(numerical_cal, Dcal, 1e-7));
}
/* ************************************************************************* */ /* ************************************************************************* */
static Point2 project4(const Camera& camera, const Point3& point) { static Point2 project4(const Camera& camera, const Point3& point) {
return camera.project2(point); return camera.project2(point);

586
gtsam/geometry/tests/testRot3.cpp Normal file
View File

@ -0,0 +1,586 @@
/* ----------------------------------------------------------------------------
* 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 testRot3.cpp
* @brief Unit tests for Rot3 class - common between Matrix and Quaternion
* @author Alireza Fathi
* @author Frank Dellaert
*/
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <boost/math/constants/constants.hpp>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
GTSAM_CONCEPT_TESTABLE_INST(Rot3)
GTSAM_CONCEPT_LIE_INST(Rot3)
static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
static Point3 P(0.2, 0.7, -2.0);
static double error = 1e-9, epsilon = 0.001;
static const Matrix I3 = eye(3);
/* ************************************************************************* */
TEST( Rot3, constructor)
{
Rot3 expected(I3);
Point3 r1(1,0,0), r2(0,1,0), r3(0,0,1);
Rot3 actual(r1, r2, r3);
CHECK(assert_equal(actual,expected));
}
/* ************************************************************************* */
TEST( Rot3, constructor2)
{
Matrix R = (Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1);
Rot3 actual(R);
Rot3 expected(0, 1, 0, 1, 0, 0, 0, 0, -1);
CHECK(assert_equal(actual,expected));
}
/* ************************************************************************* */
TEST( Rot3, constructor3)
{
Rot3 expected(0, 1, 0, 1, 0, 0, 0, 0, -1);
Point3 r1(0,1,0), r2(1,0,0), r3(0,0,-1);
CHECK(assert_equal(expected,Rot3(r1,r2,r3)));
}
/* ************************************************************************* */
TEST( Rot3, transpose)
{
Point3 r1(0,1,0), r2(1,0,0), r3(0,0,-1);
Rot3 R(0, 1, 0, 1, 0, 0, 0, 0, -1);
CHECK(assert_equal(R.inverse(),Rot3(r1,r2,r3)));
}
/* ************************************************************************* */
TEST( Rot3, equals)
{
CHECK(R.equals(R));
Rot3 zero;
CHECK(!R.equals(zero));
}
/* ************************************************************************* */
// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
Rot3 slow_but_correct_rodriguez(const Vector& w) {
double t = norm_2(w);
Matrix J = skewSymmetric(w / t);
if (t < 1e-5) return Rot3();
Matrix R = I3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
return R;
}
/* ************************************************************************* */
TEST( Rot3, rodriguez)
{
Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0);
Vector w = (Vector(3) << epsilon, 0., 0.);
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez2)
{
Vector axis = (Vector(3) << 0., 1., 0.); // rotation around Y
double angle = 3.14 / 4.0;
Rot3 actual = Rot3::rodriguez(axis, angle);
Rot3 expected(0.707388, 0, 0.706825,
0, 1, 0,
-0.706825, 0, 0.707388);
CHECK(assert_equal(expected,actual,1e-5));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez3)
{
Vector w = (Vector(3) << 0.1, 0.2, 0.3);
Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w));
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez4)
{
Vector axis = (Vector(3) << 0., 0., 1.); // rotation around Z
double angle = M_PI/2.0;
Rot3 actual = Rot3::rodriguez(axis, angle);
double c=cos(angle),s=sin(angle);
Rot3 expected(c,-s, 0,
s, c, 0,
0, 0, 1);
CHECK(assert_equal(expected,actual,1e-5));
CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual,1e-5));
}
/* ************************************************************************* */
TEST( Rot3, retract)
{
Vector v = zero(3);
CHECK(assert_equal(R.retract(v), R));
}
/* ************************************************************************* */
TEST(Rot3, log)
{
static const double PI = boost::math::constants::pi<double>();
Vector w;
Rot3 R;
#define CHECK_OMEGA(X,Y,Z) \
w = (Vector(3) << (double)X, (double)Y, double(Z)); \
R = Rot3::rodriguez(w); \
EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12));
// Check zero
CHECK_OMEGA( 0, 0, 0)
// create a random direction:
double norm=sqrt(1.0+16.0+4.0);
double x=1.0/norm, y=4.0/norm, z=2.0/norm;
// Check very small rotation for Taylor expansion
// Note that tolerance above is 1e-12, so Taylor is pretty good !
double d = 0.0001;
CHECK_OMEGA( d, 0, 0)
CHECK_OMEGA( 0, d, 0)
CHECK_OMEGA( 0, 0, d)
CHECK_OMEGA(x*d, y*d, z*d)
// check normal rotation
d = 0.1;
CHECK_OMEGA( d, 0, 0)
CHECK_OMEGA( 0, d, 0)
CHECK_OMEGA( 0, 0, d)
CHECK_OMEGA(x*d, y*d, z*d)
// Check 180 degree rotations
CHECK_OMEGA( PI, 0, 0)
CHECK_OMEGA( 0, PI, 0)
CHECK_OMEGA( 0, 0, PI)
// Windows and Linux have flipped sign in quaternion mode
#if !defined(__APPLE__) && defined (GTSAM_USE_QUATERNIONS)
w = (Vector(3) << x*PI, y*PI, z*PI);
R = Rot3::rodriguez(w);
EXPECT(assert_equal(Vector(-w), Rot3::Logmap(R),1e-12));
#else
CHECK_OMEGA(x*PI,y*PI,z*PI)
#endif
// Check 360 degree rotations
#define CHECK_OMEGA_ZERO(X,Y,Z) \
w = (Vector(3) << (double)X, (double)Y, double(Z)); \
R = Rot3::rodriguez(w); \
EXPECT(assert_equal(zero(3), Rot3::Logmap(R)));
CHECK_OMEGA_ZERO( 2.0*PI, 0, 0)
CHECK_OMEGA_ZERO( 0, 2.0*PI, 0)
CHECK_OMEGA_ZERO( 0, 0, 2.0*PI)
CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI)
}
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){
return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) );
}
/* ************************************************************************* */
TEST( Rot3, rightJacobianExpMapSO3 )
{
// Linearization point
Vector thetahat = (Vector(3) << 0.1, 0, 0);
Matrix expectedJacobian = numericalDerivative11<Rot3, LieVector>(
boost::bind(&Rot3::Expmap, _1), thetahat);
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat);
CHECK(assert_equal(expectedJacobian, actualJacobian));
}
/* ************************************************************************* */
TEST( Rot3, rightJacobianExpMapSO3inverse )
{
// Linearization point
Vector thetahat = (Vector(3) << 0.1,0.1,0); ///< Current estimate of rotation rate bias
Vector deltatheta = (Vector(3) << 0, 0, 0);
Matrix expectedJacobian = numericalDerivative11<LieVector>(
boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta);
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat);
EXPECT(assert_equal(expectedJacobian, actualJacobian));
}
/* ************************************************************************* */
TEST(Rot3, manifold_expmap)
{
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
Rot3 origin;
// log behaves correctly
Vector d12 = gR1.localCoordinates(gR2, Rot3::EXPMAP);
CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::EXPMAP)));
Vector d21 = gR2.localCoordinates(gR1, Rot3::EXPMAP);
CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::EXPMAP)));
// Check that it is expmap
CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21)));
// Check that log(t1,t2)=-log(t2,t1)
CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
Vector d = (Vector(3) << 0.1, 0.2, 0.3);
// exp(-d)=inverse(exp(d))
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
Rot3 R2 = Rot3::Expmap (2 * d);
Rot3 R3 = Rot3::Expmap (3 * d);
Rot3 R5 = Rot3::Expmap (5 * d);
CHECK(assert_equal(R5,R2*R3));
CHECK(assert_equal(R5,R3*R2));
}
/* ************************************************************************* */
class AngularVelocity: public Point3 {
public:
AngularVelocity(const Point3& p) :
Point3(p) {
}
AngularVelocity(double wx, double wy, double wz) :
Point3(wx, wy, wz) {
}
};
AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) {
return X.cross(Y);
}
/* ************************************************************************* */
TEST(Rot3, BCH)
{
// Approximate exmap by BCH formula
AngularVelocity w1(0.2, -0.1, 0.1);
AngularVelocity w2(0.01, 0.02, -0.03);
Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector());
Rot3 R3 = R1 * R2;
Vector expected = Rot3::Logmap(R3);
Vector actual = BCH(w1, w2).vector();
CHECK(assert_equal(expected, actual,1e-5));
}
/* ************************************************************************* */
TEST( Rot3, rotate_derivatives)
{
Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
R.rotate(P, actualDrotate1a, actualDrotate2);
R.inverse().rotate(P, actualDrotate1b, boost::none);
Matrix numerical1 = numericalDerivative21(testing::rotate<Rot3,Point3>, R, P);
Matrix numerical2 = numericalDerivative21(testing::rotate<Rot3,Point3>, R.inverse(), P);
Matrix numerical3 = numericalDerivative22(testing::rotate<Rot3,Point3>, R, P);
EXPECT(assert_equal(numerical1,actualDrotate1a,error));
EXPECT(assert_equal(numerical2,actualDrotate1b,error));
EXPECT(assert_equal(numerical3,actualDrotate2, error));
}
/* ************************************************************************* */
TEST( Rot3, unrotate)
{
Point3 w = R * P;
Matrix H1,H2;
Point3 actual = R.unrotate(w,H1,H2);
CHECK(assert_equal(P,actual));
Matrix numerical1 = numericalDerivative21(testing::unrotate<Rot3,Point3>, R, w);
CHECK(assert_equal(numerical1,H1,error));
Matrix numerical2 = numericalDerivative22(testing::unrotate<Rot3,Point3>, R, w);
CHECK(assert_equal(numerical2,H2,error));
}
/* ************************************************************************* */
TEST( Rot3, compose )
{
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
Rot3 expected = R1 * R2;
Matrix actualH1, actualH2;
Rot3 actual = R1.compose(R2, actualH1, actualH2);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(testing::compose<Rot3>, R1,
R2, 1e-2);
CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(testing::compose<Rot3>, R1,
R2, 1e-2);
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
TEST( Rot3, inverse )
{
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 I;
Matrix actualH;
Rot3 actual = R.inverse(actualH);
CHECK(assert_equal(I,R*actual));
CHECK(assert_equal(I,actual*R));
CHECK(assert_equal((Matrix)actual.matrix(), R.transpose()));
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R);
CHECK(assert_equal(numericalH,actualH));
}
/* ************************************************************************* */
TEST( Rot3, between )
{
Rot3 r1 = Rot3::Rz(M_PI/3.0);
Rot3 r2 = Rot3::Rz(2.0*M_PI/3.0);
Matrix expectedr1 = (Matrix(3, 3) <<
0.5, -sqrt(3.0)/2.0, 0.0,
sqrt(3.0)/2.0, 0.5, 0.0,
0.0, 0.0, 1.0);
EXPECT(assert_equal(expectedr1, r1.matrix()));
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 origin;
EXPECT(assert_equal(R, origin.between(R)));
EXPECT(assert_equal(R.inverse(), R.between(origin)));
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
Rot3 expected = R1.inverse() * R2;
Matrix actualH1, actualH2;
Rot3 actual = R1.between(R2, actualH1, actualH2);
EXPECT(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(testing::between<Rot3> , R1, R2);
CHECK(assert_equal(numericalH1,actualH1, 1e-4));
Matrix numericalH2 = numericalDerivative22(testing::between<Rot3> , R1, R2);
CHECK(assert_equal(numericalH2,actualH2, 1e-4));
}
/* ************************************************************************* */
Vector w = (Vector(3) << 0.1, 0.27, -0.2);
// Left trivialization Derivative of exp(w) wrpt w:
// How does exp(w) change when w changes?
// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
// => y = log (exp(-w) * exp(w+dw))
Vector3 testDexpL(const Vector3& dw) {
return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
}
TEST( Rot3, dexpL) {
Matrix actualDexpL = Rot3::dexpL(w);
Matrix expectedDexpL = numericalDerivative11<LieVector>(testDexpL,
LieVector(zero(3)), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
Matrix actualDexpInvL = Rot3::dexpInvL(w);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
}
/* ************************************************************************* */
TEST( Rot3, xyz )
{
double t = 0.1, st = sin(t), ct = cos(t);
// Make sure all counterclockwise
// Diagrams below are all from from unchanging axis
// z
// | * Y=(ct,st)
// x----y
Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
CHECK(assert_equal(expected1,Rot3::Rx(t)));
// x
// | * Z=(ct,st)
// y----z
Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
CHECK(assert_equal(expected2,Rot3::Ry(t)));
// y
// | X=* (ct,st)
// z----x
Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
CHECK(assert_equal(expected3,Rot3::Rz(t)));
// Check compound rotation
Rot3 expected = Rot3::Rz(0.3) * Rot3::Ry(0.2) * Rot3::Rx(0.1);
CHECK(assert_equal(expected,Rot3::RzRyRx(0.1,0.2,0.3)));
}
/* ************************************************************************* */
TEST( Rot3, yaw_pitch_roll )
{
double t = 0.1;
// yaw is around z axis
CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t)));
// pitch is around y axis
CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t)));
// roll is around x axis
CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t)));
// Check compound rotation
Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3);
CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3)));
CHECK(assert_equal((Vector)(Vector(3) << 0.1, 0.2, 0.3),expected.ypr()));
}
/* ************************************************************************* */
TEST( Rot3, RQ)
{
// Try RQ on a pure rotation
Matrix actualK;
Vector actual;
boost::tie(actualK, actual) = RQ(R.matrix());
Vector expected = (Vector(3) << 0.14715, 0.385821, 0.231671);
CHECK(assert_equal(I3,actualK));
CHECK(assert_equal(expected,actual,1e-6));
// Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z]
CHECK(assert_equal(expected,R.xyz(),1e-6));
CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz()));
// Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r]
CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr()));
CHECK(assert_equal((Vector)(Vector(3) << 0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy()));
// Try ypr for pure yaw-pitch-roll matrices
CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.0,0.0),Rot3::yaw (0.1).ypr()));
CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.1,0.0),Rot3::pitch(0.1).ypr()));
CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
Matrix A = K * R.matrix();
boost::tie(actualK, actual) = RQ(A);
CHECK(assert_equal(K,actualK));
CHECK(assert_equal(expected,actual,1e-6));
}
/* ************************************************************************* */
TEST( Rot3, expmapStability ) {
Vector w = (Vector(3) << 78e-9, 5e-8, 97e-7);
double theta = w.norm();
double theta2 = theta*theta;
Rot3 actualR = Rot3::Expmap(w);
Matrix W = (Matrix(3, 3) << 0.0, -w(2), w(1),
w(2), 0.0, -w(0),
-w(1), w(0), 0.0 );
Matrix W2 = W*W;
Matrix Rmat = I3 + (1.0-theta2/6.0 + theta2*theta2/120.0
- theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ;
Rot3 expectedR( Rmat );
CHECK(assert_equal(expectedR, actualR, 1e-10));
}
/* ************************************************************************* */
TEST( Rot3, logmapStability ) {
Vector w = (Vector(3) << 1e-8, 0.0, 0.0);
Rot3 R = Rot3::Expmap(w);
// double tr = R.r1().x()+R.r2().y()+R.r3().z();
// std::cout.precision(5000);
// std::cout << "theta: " << w.norm() << std::endl;
// std::cout << "trace: " << tr << std::endl;
// R.print("R = ");
Vector actualw = Rot3::Logmap(R);
CHECK(assert_equal(w, actualw, 1e-15)); // this should be fixed for Quaternions!!!
}
/* ************************************************************************* */
TEST(Rot3, quaternion) {
// NOTE: This is also verifying the ability to convert Vector to Quaternion
Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
Rot3 R1 = Rot3((Matrix)(Matrix(3, 3) <<
0.271018623057411, 0.278786459830371, 0.921318086098018,
0.578529366719085, 0.717799701969298, -0.387385285854279,
-0.769319620053772, 0.637998195662053, 0.033250932803219));
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) <<
-0.207341903877828, 0.250149415542075, 0.945745528564780,
0.881304914479026, -0.371869043667957, 0.291573424846290,
0.424630407073532, 0.893945571198514, -0.143353873763946));
// Check creating Rot3 from quaternion
EXPECT(assert_equal(R1, Rot3(q1)));
EXPECT(assert_equal(R1, Rot3::quaternion(q1.w(), q1.x(), q1.y(), q1.z())));
EXPECT(assert_equal(R2, Rot3(q2)));
EXPECT(assert_equal(R2, Rot3::quaternion(q2.w(), q2.x(), q2.y(), q2.z())));
// Check converting Rot3 to quaterion
EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs())));
EXPECT(assert_equal(Vector(R2.toQuaternion().coeffs()), Vector(q2.coeffs())));
// Check that quaternion and Rot3 represent the same rotation
Point3 p1(1.0, 2.0, 3.0);
Point3 p2(8.0, 7.0, 9.0);
Point3 expected1 = R1*p1;
Point3 expected2 = R2*p2;
Point3 actual1 = Point3(q1*p1.vector());
Point3 actual2 = Point3(q2*p2.vector());
EXPECT(assert_equal(expected1, actual1));
EXPECT(assert_equal(expected2, actual2));
}
/* ************************************************************************* */
TEST( Rot3, Cayley ) {
Matrix A = skewSymmetric(1,2,-3);
Matrix Q = Cayley(A);
EXPECT(assert_equal(I3, trans(Q)*Q));
EXPECT(assert_equal(A, Cayley(Q)));
}
/* ************************************************************************* */
TEST( Rot3, stream)
{
Rot3 R;
std::ostringstream os;
os << R;
EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n");
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

544
gtsam/geometry/tests/testRot3M.cpp
View File

@ -13,15 +13,16 @@
* @file testRot3.cpp * @file testRot3.cpp
* @brief Unit tests for Rot3 class * @brief Unit tests for Rot3 class
* @author Alireza Fathi * @author Alireza Fathi
* @author Frank Dellaert
*/ */
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h> #include <gtsam/base/lieProxies.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
#include <boost/math/constants/constants.hpp> #include <boost/math/constants/constants.hpp>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -33,206 +34,10 @@ using namespace gtsam;
static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
static Point3 P(0.2, 0.7, -2.0); static Point3 P(0.2, 0.7, -2.0);
static double error = 1e-9, epsilon = 0.001;
static const Matrix I3 = eye(3); static const Matrix I3 = eye(3);
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, constructor) TEST(Rot3, manifold_cayley)
{
Rot3 expected(I3);
Vector r1(3), r2(3), r3(3);
r1(0) = 1;
r1(1) = 0;
r1(2) = 0;
r2(0) = 0;
r2(1) = 1;
r2(2) = 0;
r3(0) = 0;
r3(1) = 0;
r3(2) = 1;
Rot3 actual(r1, r2, r3);
CHECK(assert_equal(actual,expected));
}
/* ************************************************************************* */
TEST( Rot3, constructor2)
{
Matrix R = (Matrix(3, 3) << 11., 12., 13., 21., 22., 23., 31., 32., 33.);
Rot3 actual(R);
Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
CHECK(assert_equal(actual,expected));
}
/* ************************************************************************* */
TEST( Rot3, constructor3)
{
Rot3 expected(1, 2, 3, 4, 5, 6, 7, 8, 9);
Point3 r1(1, 4, 7), r2(2, 5, 8), r3(3, 6, 9);
CHECK(assert_equal(Rot3(r1,r2,r3),expected));
}
/* ************************************************************************* */
TEST( Rot3, transpose)
{
Rot3 R(1, 2, 3, 4, 5, 6, 7, 8, 9);
Point3 r1(1, 2, 3), r2(4, 5, 6), r3(7, 8, 9);
CHECK(assert_equal(R.inverse(),Rot3(r1,r2,r3)));
}
/* ************************************************************************* */
TEST( Rot3, equals)
{
CHECK(R.equals(R));
Rot3 zero;
CHECK(!R.equals(zero));
}
/* ************************************************************************* */
// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
Rot3 slow_but_correct_rodriguez(const Vector& w) {
double t = norm_2(w);
Matrix J = skewSymmetric(w / t);
if (t < 1e-5) return Rot3();
Matrix R = I3 + sin(t) * J + (1.0 - cos(t)) * (J * J);
return R;
}
/* ************************************************************************* */
TEST( Rot3, rodriguez)
{
Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0);
Vector w = (Vector(3) << epsilon, 0., 0.);
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez2)
{
Vector axis = (Vector(3) << 0., 1., 0.); // rotation around Y
double angle = 3.14 / 4.0;
Rot3 actual = Rot3::rodriguez(axis, angle);
Rot3 expected(0.707388, 0, 0.706825,
0, 1, 0,
-0.706825, 0, 0.707388);
CHECK(assert_equal(expected,actual,1e-5));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez3)
{
Vector w = (Vector(3) << 0.1, 0.2, 0.3);
Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w));
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez4)
{
Vector axis = (Vector(3) << 0., 0., 1.); // rotation around Z
double angle = M_PI/2.0;
Rot3 actual = Rot3::rodriguez(axis, angle);
double c=cos(angle),s=sin(angle);
Rot3 expected(c,-s, 0,
s, c, 0,
0, 0, 1);
CHECK(assert_equal(expected,actual,1e-5));
CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual,1e-5));
}
/* ************************************************************************* */
TEST( Rot3, expmap)
{
Vector v = zero(3);
CHECK(assert_equal(R.retract(v), R));
}
/* ************************************************************************* */
TEST(Rot3, log)
{
static const double PI = boost::math::constants::pi<double>();
Vector w;
Rot3 R;
#define CHECK_OMEGA(X,Y,Z) \
w = (Vector(3) << (double)X, (double)Y, double(Z)); \
R = Rot3::rodriguez(w); \
EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12));
// Check zero
CHECK_OMEGA( 0, 0, 0)
// create a random direction:
double norm=sqrt(1.0+16.0+4.0);
double x=1.0/norm, y=4.0/norm, z=2.0/norm;
// Check very small rotation for Taylor expansion
// Note that tolerance above is 1e-12, so Taylor is pretty good !
double d = 0.0001;
CHECK_OMEGA( d, 0, 0)
CHECK_OMEGA( 0, d, 0)
CHECK_OMEGA( 0, 0, d)
CHECK_OMEGA(x*d, y*d, z*d)
// check normal rotation
d = 0.1;
CHECK_OMEGA( d, 0, 0)
CHECK_OMEGA( 0, d, 0)
CHECK_OMEGA( 0, 0, d)
CHECK_OMEGA(x*d, y*d, z*d)
// Check 180 degree rotations
CHECK_OMEGA( PI, 0, 0)
CHECK_OMEGA( 0, PI, 0)
CHECK_OMEGA( 0, 0, PI)
CHECK_OMEGA(x*PI,y*PI,z*PI)
// Check 360 degree rotations
#define CHECK_OMEGA_ZERO(X,Y,Z) \
w = (Vector(3) << (double)X, (double)Y, double(Z)); \
R = Rot3::rodriguez(w); \
EXPECT(assert_equal(zero(3), Rot3::Logmap(R)));
CHECK_OMEGA_ZERO( 2.0*PI, 0, 0)
CHECK_OMEGA_ZERO( 0, 2.0*PI, 0)
CHECK_OMEGA_ZERO( 0, 0, 2.0*PI)
CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI)
}
Rot3 evaluateRotation(const Vector3 thetahat){
return Rot3::Expmap(thetahat);
}
Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){
return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) );
}
/* ************************************************************************* */
TEST( Rot3, rightJacobianExpMapSO3 )
{
// Linearization point
Vector3 thetahat; thetahat << 0.1, 0, 0;
Matrix expectedJacobian = numericalDerivative11<Rot3, LieVector>(boost::bind(&evaluateRotation, _1), LieVector(thetahat));
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat);
EXPECT(assert_equal(expectedJacobian, actualJacobian));
}
/* ************************************************************************* */
TEST( Rot3, rightJacobianExpMapSO3inverse )
{
// Linearization point
Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias
Vector3 deltatheta; deltatheta << 0, 0, 0;
Matrix expectedJacobian = numericalDerivative11<LieVector>(boost::bind(&evaluateLogRotation, thetahat, _1), LieVector(deltatheta));
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat);
EXPECT(assert_equal(expectedJacobian, actualJacobian));
}
/* ************************************************************************* */
TEST(Rot3, manifold_caley)
{ {
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
@ -260,7 +65,7 @@ TEST(Rot3, manifold_caley)
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Rot3, manifold_slow_caley) TEST(Rot3, manifold_slow_cayley)
{ {
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
@ -287,343 +92,6 @@ TEST(Rot3, manifold_slow_caley)
CHECK(assert_equal(R5,R3*R2)); CHECK(assert_equal(R5,R3*R2));
} }
/* ************************************************************************* */
TEST(Rot3, manifold_expmap)
{
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
Rot3 origin;
// log behaves correctly
Vector d12 = gR1.localCoordinates(gR2, Rot3::EXPMAP);
CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::EXPMAP)));
Vector d21 = gR2.localCoordinates(gR1, Rot3::EXPMAP);
CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::EXPMAP)));
// Check that it is expmap
CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21)));
// Check that log(t1,t2)=-log(t2,t1)
CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
Vector d = (Vector(3) << 0.1, 0.2, 0.3);
// exp(-d)=inverse(exp(d))
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
Rot3 R2 = Rot3::Expmap (2 * d);
Rot3 R3 = Rot3::Expmap (3 * d);
Rot3 R5 = Rot3::Expmap (5 * d);
CHECK(assert_equal(R5,R2*R3));
CHECK(assert_equal(R5,R3*R2));
}
/* ************************************************************************* */
class AngularVelocity: public Point3 {
public:
AngularVelocity(const Point3& p) :
Point3(p) {
}
AngularVelocity(double wx, double wy, double wz) :
Point3(wx, wy, wz) {
}
};
AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) {
return X.cross(Y);
}
/* ************************************************************************* */
TEST(Rot3, BCH)
{
// Approximate exmap by BCH formula
AngularVelocity w1(0.2, -0.1, 0.1);
AngularVelocity w2(0.01, 0.02, -0.03);
Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector());
Rot3 R3 = R1 * R2;
Vector expected = Rot3::Logmap(R3);
Vector actual = BCH(w1, w2).vector();
CHECK(assert_equal(expected, actual,1e-5));
}
/* ************************************************************************* */
TEST( Rot3, rotate_derivatives)
{
Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
R.rotate(P, actualDrotate1a, actualDrotate2);
R.inverse().rotate(P, actualDrotate1b, boost::none);
Matrix numerical1 = numericalDerivative21(testing::rotate<Rot3,Point3>, R, P);
Matrix numerical2 = numericalDerivative21(testing::rotate<Rot3,Point3>, R.inverse(), P);
Matrix numerical3 = numericalDerivative22(testing::rotate<Rot3,Point3>, R, P);
EXPECT(assert_equal(numerical1,actualDrotate1a,error));
EXPECT(assert_equal(numerical2,actualDrotate1b,error));
EXPECT(assert_equal(numerical3,actualDrotate2, error));
}
/* ************************************************************************* */
TEST( Rot3, unrotate)
{
Point3 w = R * P;
Matrix H1,H2;
Point3 actual = R.unrotate(w,H1,H2);
CHECK(assert_equal(P,actual));
Matrix numerical1 = numericalDerivative21(testing::unrotate<Rot3,Point3>, R, w);
CHECK(assert_equal(numerical1,H1,error));
Matrix numerical2 = numericalDerivative22(testing::unrotate<Rot3,Point3>, R, w);
CHECK(assert_equal(numerical2,H2,error));
}
/* ************************************************************************* */
TEST( Rot3, compose )
{
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
Rot3 expected = R1 * R2;
Matrix actualH1, actualH2;
Rot3 actual = R1.compose(R2, actualH1, actualH2);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(testing::compose<Rot3>, R1,
R2, 1e-2);
CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(testing::compose<Rot3>, R1,
R2, 1e-2);
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
TEST( Rot3, inverse )
{
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 I;
Matrix actualH;
CHECK(assert_equal(I,R*R.inverse(actualH)));
CHECK(assert_equal(I,R.inverse()*R));
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R);
CHECK(assert_equal(numericalH,actualH));
}
/* ************************************************************************* */
TEST( Rot3, between )
{
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 origin;
CHECK(assert_equal(R, origin.between(R)));
CHECK(assert_equal(R.inverse(), R.between(origin)));
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
Rot3 expected = R1.inverse() * R2;
Matrix actualH1, actualH2;
Rot3 actual = R1.between(R2, actualH1, actualH2);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(testing::between<Rot3> , R1, R2);
CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(testing::between<Rot3> , R1, R2);
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
Vector w = (Vector(3) << 0.1, 0.27, -0.2);
// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes?
// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0
// => y = log (exp(-w) * exp(w+dw))
Vector testDexpL(const LieVector& dw) {
Vector y = Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw));
return y;
}
TEST( Rot3, dexpL) {
Matrix actualDexpL = Rot3::dexpL(w);
Matrix expectedDexpL = numericalDerivative11(
boost::function<Vector(const LieVector&)>(
boost::bind(testDexpL, _1)), LieVector(zero(3)), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5));
Matrix actualDexpInvL = Rot3::dexpInvL(w);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5));
}
/* ************************************************************************* */
TEST( Rot3, xyz )
{
double t = 0.1, st = sin(t), ct = cos(t);
// Make sure all counterclockwise
// Diagrams below are all from from unchanging axis
// z
// | * Y=(ct,st)
// x----y
Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
CHECK(assert_equal(expected1,Rot3::Rx(t)));
// x
// | * Z=(ct,st)
// y----z
Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
CHECK(assert_equal(expected2,Rot3::Ry(t)));
// y
// | X=* (ct,st)
// z----x
Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
CHECK(assert_equal(expected3,Rot3::Rz(t)));
// Check compound rotation
Rot3 expected = Rot3::Rz(0.3) * Rot3::Ry(0.2) * Rot3::Rx(0.1);
CHECK(assert_equal(expected,Rot3::RzRyRx(0.1,0.2,0.3)));
}
/* ************************************************************************* */
TEST( Rot3, yaw_pitch_roll )
{
double t = 0.1;
// yaw is around z axis
CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t)));
// pitch is around y axis
CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t)));
// roll is around x axis
CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t)));
// Check compound rotation
Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3);
CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3)));
CHECK(assert_equal((Vector)(Vector(3) << 0.1, 0.2, 0.3),expected.ypr()));
}
/* ************************************************************************* */
TEST( Rot3, RQ)
{
// Try RQ on a pure rotation
Matrix actualK;
Vector actual;
boost::tie(actualK, actual) = RQ(R.matrix());
Vector expected = (Vector(3) << 0.14715, 0.385821, 0.231671);
CHECK(assert_equal(I3,actualK));
CHECK(assert_equal(expected,actual,1e-6));
// Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z]
CHECK(assert_equal(expected,R.xyz(),1e-6));
CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz()));
// Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r]
CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr()));
CHECK(assert_equal((Vector)(Vector(3) << 0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy()));
// Try ypr for pure yaw-pitch-roll matrices
CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.0,0.0),Rot3::yaw (0.1).ypr()));
CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.1,0.0),Rot3::pitch(0.1).ypr()));
CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
Matrix A = K * R.matrix();
boost::tie(actualK, actual) = RQ(A);
CHECK(assert_equal(K,actualK));
CHECK(assert_equal(expected,actual,1e-6));
}
/* ************************************************************************* */
TEST( Rot3, expmapStability ) {
Vector w = (Vector(3) << 78e-9, 5e-8, 97e-7);
double theta = w.norm();
double theta2 = theta*theta;
Rot3 actualR = Rot3::Expmap(w);
Matrix W = (Matrix(3, 3) << 0.0, -w(2), w(1),
w(2), 0.0, -w(0),
-w(1), w(0), 0.0 );
Matrix W2 = W*W;
Matrix Rmat = I3 + (1.0-theta2/6.0 + theta2*theta2/120.0
- theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ;
Rot3 expectedR( Rmat );
CHECK(assert_equal(expectedR, actualR, 1e-10));
}
/* ************************************************************************* */
TEST( Rot3, logmapStability ) {
Vector w = (Vector(3) << 1e-8, 0.0, 0.0);
Rot3 R = Rot3::Expmap(w);
// double tr = R.r1().x()+R.r2().y()+R.r3().z();
// std::cout.precision(5000);
// std::cout << "theta: " << w.norm() << std::endl;
// std::cout << "trace: " << tr << std::endl;
// R.print("R = ");
Vector actualw = Rot3::Logmap(R);
CHECK(assert_equal(w, actualw, 1e-15));
}
/* ************************************************************************* */
TEST(Rot3, quaternion) {
// NOTE: This is also verifying the ability to convert Vector to Quaternion
Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
Rot3 R1 = Rot3((Matrix)(Matrix(3, 3) <<
0.271018623057411, 0.278786459830371, 0.921318086098018,
0.578529366719085, 0.717799701969298, -0.387385285854279,
-0.769319620053772, 0.637998195662053, 0.033250932803219));
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) <<
-0.207341903877828, 0.250149415542075, 0.945745528564780,
0.881304914479026, -0.371869043667957, 0.291573424846290,
0.424630407073532, 0.893945571198514, -0.143353873763946));
// Check creating Rot3 from quaternion
EXPECT(assert_equal(R1, Rot3(q1)));
EXPECT(assert_equal(R1, Rot3::quaternion(q1.w(), q1.x(), q1.y(), q1.z())));
EXPECT(assert_equal(R2, Rot3(q2)));
EXPECT(assert_equal(R2, Rot3::quaternion(q2.w(), q2.x(), q2.y(), q2.z())));
// Check converting Rot3 to quaterion
EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs())));
EXPECT(assert_equal(Vector(R2.toQuaternion().coeffs()), Vector(q2.coeffs())));
// Check that quaternion and Rot3 represent the same rotation
Point3 p1(1.0, 2.0, 3.0);
Point3 p2(8.0, 7.0, 9.0);
Point3 expected1 = R1*p1;
Point3 expected2 = R2*p2;
Point3 actual1 = Point3(q1*p1.vector());
Point3 actual2 = Point3(q2*p2.vector());
EXPECT(assert_equal(expected1, actual1));
EXPECT(assert_equal(expected2, actual2));
}
/* ************************************************************************* */
TEST( Rot3, Cayley ) {
Matrix A = skewSymmetric(1,2,-3);
Matrix Q = Cayley(A);
EXPECT(assert_equal(I3, trans(Q)*Q));
EXPECT(assert_equal(A, Cayley(Q)));
}
/* ************************************************************************* */
TEST( Rot3, stream)
{
Rot3 R;
std::ostringstream os;
os << R;
EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n");
}
#endif #endif
/* ************************************************************************* */ /* ************************************************************************* */

474
gtsam/geometry/tests/testRot3Q.cpp
View File

@ -11,478 +11,24 @@
/** /**
* @file testRot3.cpp * @file testRot3.cpp
* @brief Unit tests for Rot3 class * @brief Unit tests for Rot3 class, Quaternion specific
* @author Alireza Fathi * @author Alireza Fathi
*/ */
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h>
#include <boost/math/constants/constants.hpp>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <gtsam/geometry/Point3.h> #include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/lieProxies.h>
#include <boost/math/constants/constants.hpp>
#include <CppUnitLite/TestHarness.h>
#ifdef GTSAM_USE_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
using namespace gtsam; // No quaternion only tests
static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
static Point3 P(0.2, 0.7, -2.0);
static double error = 1e-9, epsilon = 0.001;
/* ************************************************************************* */
TEST( Rot3, constructor)
{
Rot3 expected(eye(3, 3));
Vector r1(3), r2(3), r3(3);
r1(0) = 1;
r1(1) = 0;
r1(2) = 0;
r2(0) = 0;
r2(1) = 1;
r2(2) = 0;
r3(0) = 0;
r3(1) = 0;
r3(2) = 1;
Rot3 actual(r1, r2, r3);
CHECK(assert_equal(actual,expected));
}
/* ************************************************************************* */
TEST( Rot3, constructor2)
{
Matrix R = (Matrix(3, 3) << 11., 12., 13., 21., 22., 23., 31., 32., 33.);
Rot3 actual(R);
Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
CHECK(assert_equal(actual,expected));
}
/* ************************************************************************* */
TEST( Rot3, constructor3)
{
Rot3 expected(1, 2, 3, 4, 5, 6, 7, 8, 9);
Point3 r1(1, 4, 7), r2(2, 5, 8), r3(3, 6, 9);
CHECK(assert_equal(Rot3(r1,r2,r3),expected));
}
/* ************************************************************************* */
TEST( Rot3, equals)
{
CHECK(R.equals(R));
Rot3 zero;
CHECK(!R.equals(zero));
}
/* ************************************************************************* */
// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
Rot3 slow_but_correct_rodriguez(const Vector& w) {
double t = norm_2(w);
Matrix J = skewSymmetric(w / t);
if (t < 1e-5) return Rot3();
Matrix R = eye(3) + sin(t) * J + (1.0 - cos(t)) * (J * J);
return R;
}
/* ************************************************************************* */
TEST( Rot3, rodriguez)
{
Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0);
Vector w = (Vector(3) << epsilon, 0., 0.);
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez2)
{
Vector axis = (Vector(3) << 0.,1.,0.); // rotation around Y
double angle = 3.14 / 4.0;
Rot3 actual = Rot3::rodriguez(axis, angle);
Rot3 expected(0.707388, 0, 0.706825,
0, 1, 0,
-0.706825, 0, 0.707388);
CHECK(assert_equal(expected,actual,1e-5));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez3)
{
Vector w = (Vector(3) << 0.1, 0.2, 0.3);
Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w));
Rot3 R2 = slow_but_correct_rodriguez(w);
CHECK(assert_equal(R2,R1));
}
/* ************************************************************************* */
TEST( Rot3, rodriguez4)
{
Vector axis = (Vector(3) << 0., 0., 1.); // rotation around Z
double angle = M_PI_2;
Rot3 actual = Rot3::rodriguez(axis, angle);
double c=cos(angle),s=sin(angle);
Rot3 expected(c,-s, 0,
s, c, 0,
0, 0, 1);
CHECK(assert_equal(expected,actual,1e-5));
CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual,1e-5));
}
/* ************************************************************************* */
TEST( Rot3, expmap)
{
Vector v = zero(3);
CHECK(assert_equal(R.retract(v), R));
}
/* ************************************************************************* */
TEST(Rot3, log)
{
Vector w1 = (Vector(3) << 0.1, 0.0, 0.0);
Rot3 R1 = Rot3::rodriguez(w1);
CHECK(assert_equal(w1, Rot3::Logmap(R1)));
Vector w2 = (Vector(3) << 0.0, 0.1, 0.0);
Rot3 R2 = Rot3::rodriguez(w2);
CHECK(assert_equal(w2, Rot3::Logmap(R2)));
Vector w3 = (Vector(3) << 0.0, 0.0, 0.1);
Rot3 R3 = Rot3::rodriguez(w3);
CHECK(assert_equal(w3, Rot3::Logmap(R3)));
Vector w = (Vector(3) << 0.1, 0.4, 0.2);
Rot3 R = Rot3::rodriguez(w);
CHECK(assert_equal(w, Rot3::Logmap(R)));
Vector w5 = (Vector(3) << 0.0, 0.0, 0.0);
Rot3 R5 = Rot3::rodriguez(w5);
CHECK(assert_equal(w5, Rot3::Logmap(R5)));
Vector w6 = (Vector(3) << boost::math::constants::pi<double>(), 0.0, 0.0);
Rot3 R6 = Rot3::rodriguez(w6);
CHECK(assert_equal(w6, Rot3::Logmap(R6)));
Vector w7 = (Vector(3) << 0.0, boost::math::constants::pi<double>(), 0.0);
Rot3 R7 = Rot3::rodriguez(w7);
CHECK(assert_equal(w7, Rot3::Logmap(R7)));
Vector w8 = (Vector(3) << 0.0, 0.0, boost::math::constants::pi<double>());
Rot3 R8 = Rot3::rodriguez(w8);
CHECK(assert_equal(w8, Rot3::Logmap(R8)));
}
/* ************************************************************************* */
TEST(Rot3, manifold)
{
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
Rot3 origin;
// log behaves correctly
Vector d12 = gR1.localCoordinates(gR2);
CHECK(assert_equal(gR2, gR1.retract(d12)));
CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
Vector d21 = gR2.localCoordinates(gR1);
CHECK(assert_equal(gR1, gR2.retract(d21)));
CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21)));
// Check that log(t1,t2)=-log(t2,t1)
CHECK(assert_equal(d12,-d21));
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
Vector d = (Vector(3) << 0.1, 0.2, 0.3);
// exp(-d)=inverse(exp(d))
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
Rot3 R2 = Rot3::Expmap (2 * d);
Rot3 R3 = Rot3::Expmap (3 * d);
Rot3 R5 = Rot3::Expmap (5 * d);
CHECK(assert_equal(R5,R2*R3));
CHECK(assert_equal(R5,R3*R2));
}
/* ************************************************************************* */
class AngularVelocity: public Point3 {
public:
AngularVelocity(const Point3& p) :
Point3(p) {
}
AngularVelocity(double wx, double wy, double wz) :
Point3(wx, wy, wz) {
}
};
AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) {
return X.cross(Y);
}
/* ************************************************************************* */
TEST(Rot3, BCH)
{
// Approximate exmap by BCH formula
AngularVelocity w1(0.2, -0.1, 0.1);
AngularVelocity w2(0.01, 0.02, -0.03);
Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector());
Rot3 R3 = R1 * R2;
Vector expected = Rot3::Logmap(R3);
Vector actual = BCH(w1, w2).vector();
CHECK(assert_equal(expected, actual,1e-5));
}
/* ************************************************************************* */
TEST( Rot3, rotate_derivatives)
{
Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
R.rotate(P, actualDrotate1a, actualDrotate2);
R.inverse().rotate(P, actualDrotate1b, boost::none);
Matrix numerical1 = numericalDerivative21(testing::rotate<Rot3,Point3>, R, P);
Matrix numerical2 = numericalDerivative21(testing::rotate<Rot3,Point3>, R.inverse(), P);
Matrix numerical3 = numericalDerivative22(testing::rotate<Rot3,Point3>, R, P);
EXPECT(assert_equal(numerical1,actualDrotate1a,error));
EXPECT(assert_equal(numerical2,actualDrotate1b,error));
EXPECT(assert_equal(numerical3,actualDrotate2, error));
}
/* ************************************************************************* */
TEST( Rot3, unrotate)
{
Point3 w = R * P;
Matrix H1,H2;
Point3 actual = R.unrotate(w,H1,H2);
CHECK(assert_equal(P,actual));
Matrix numerical1 = numericalDerivative21(testing::unrotate<Rot3,Point3>, R, w);
CHECK(assert_equal(numerical1,H1,error));
Matrix numerical2 = numericalDerivative22(testing::unrotate<Rot3,Point3>, R, w);
CHECK(assert_equal(numerical2,H2,error));
}
/* ************************************************************************* */
TEST( Rot3, compose )
{
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
Rot3 expected = R1 * R2;
Matrix actualH1, actualH2;
Rot3 actual = R1.compose(R2, actualH1, actualH2);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(testing::compose<Rot3>, R1,
R2, 1e-2);
CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(testing::compose<Rot3>, R1,
R2, 1e-2);
CHECK(assert_equal(numericalH2,actualH2));
}
/* ************************************************************************* */
TEST( Rot3, inverse )
{
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 I;
Matrix actualH;
CHECK(assert_equal(I,R*R.inverse(actualH)));
CHECK(assert_equal(I,R.inverse()*R));
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R);
CHECK(assert_equal(numericalH,actualH, 1e-4));
}
/* ************************************************************************* */
TEST( Rot3, between )
{
Rot3 r1 = Rot3::Rz(M_PI/3.0);
Rot3 r2 = Rot3::Rz(2.0*M_PI/3.0);
Matrix expectedr1 = (Matrix(3, 3) <<
0.5, -sqrt(3.0)/2.0, 0.0,
sqrt(3.0)/2.0, 0.5, 0.0,
0.0, 0.0, 1.0);
EXPECT(assert_equal(expectedr1, r1.matrix()));
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
Rot3 origin;
CHECK(assert_equal(R, origin.between(R)));
CHECK(assert_equal(R.inverse(), R.between(origin)));
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
Rot3 expected = R1.inverse() * R2;
Matrix actualH1, actualH2;
Rot3 actual = R1.between(R2, actualH1, actualH2);
CHECK(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(testing::between<Rot3> , R1, R2);
CHECK(assert_equal(numericalH1,actualH1, 1e-4));
Matrix numericalH2 = numericalDerivative22(testing::between<Rot3> , R1, R2);
CHECK(assert_equal(numericalH2,actualH2, 1e-4));
}
/* ************************************************************************* */
TEST( Rot3, xyz )
{
double t = 0.1, st = sin(t), ct = cos(t);
// Make sure all counterclockwise
// Diagrams below are all from from unchanging axis
// z
// | * Y=(ct,st)
// x----y
Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
CHECK(assert_equal(expected1,Rot3::Rx(t)));
// x
// | * Z=(ct,st)
// y----z
Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
CHECK(assert_equal(expected2,Rot3::Ry(t)));
// y
// | X=* (ct,st)
// z----x
Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
CHECK(assert_equal(expected3,Rot3::Rz(t)));
// Check compound rotation
Rot3 expected = Rot3::Rz(0.3) * Rot3::Ry(0.2) * Rot3::Rx(0.1);
CHECK(assert_equal(expected,Rot3::RzRyRx(0.1,0.2,0.3)));
}
/* ************************************************************************* */
TEST( Rot3, yaw_pitch_roll )
{
double t = 0.1;
// yaw is around z axis
CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t)));
// pitch is around y axis
CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t)));
// roll is around x axis
CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t)));
// Check compound rotation
Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3);
CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3)));
}
/* ************************************************************************* */
TEST( Rot3, RQ)
{
// Try RQ on a pure rotation
Matrix actualK;
Vector actual;
boost::tie(actualK, actual) = RQ(R.matrix());
Vector expected = (Vector(3) << 0.14715, 0.385821, 0.231671);
CHECK(assert_equal(eye(3),actualK));
CHECK(assert_equal(expected,actual,1e-6));
// Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z]
CHECK(assert_equal(expected,R.xyz(),1e-6));
CHECK(assert_equal((Vector)(Vector(3) <<0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz()));
// Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r]
CHECK(assert_equal((Vector)(Vector(3) <<0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr()));
CHECK(assert_equal((Vector)(Vector(3) <<0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy()));
// Try ypr for pure yaw-pitch-roll matrices
CHECK(assert_equal((Vector)(Vector(3) <<0.1,0.0,0.0),Rot3::yaw (0.1).ypr()));
CHECK(assert_equal((Vector)(Vector(3) <<0.0,0.1,0.0),Rot3::pitch(0.1).ypr()));
CHECK(assert_equal((Vector)(Vector(3) <<0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
Matrix A = K * R.matrix();
boost::tie(actualK, actual) = RQ(A);
CHECK(assert_equal(K,actualK));
CHECK(assert_equal(expected,actual,1e-6));
}
/* ************************************************************************* */
TEST( Rot3, expmapStability ) {
Vector w = (Vector(3) << 78e-9, 5e-8, 97e-7);
double theta = w.norm();
double theta2 = theta*theta;
Rot3 actualR = Rot3::Expmap(w);
Matrix W = (Matrix(3, 3) << 0.0, -w(2), w(1),
w(2), 0.0, -w(0),
-w(1), w(0), 0.0 );
Matrix W2 = W*W;
Matrix Rmat = eye(3) + (1.0-theta2/6.0 + theta2*theta2/120.0
- theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ;
Rot3 expectedR( Rmat );
CHECK(assert_equal(expectedR, actualR, 1e-10));
}
// Does not work with Quaternions
///* ************************************************************************* */
//TEST( Rot3, logmapStability ) {
// Vector w = (Vector(3) << 1e-8, 0.0, 0.0);
// Rot3 R = Rot3::Expmap(w);
//// double tr = R.r1().x()+R.r2().y()+R.r3().z();
//// std::cout.precision(5000);
//// std::cout << "theta: " << w.norm() << std::endl;
//// std::cout << "trace: " << tr << std::endl;
//// R.print("R = ");
// Vector actualw = Rot3::Logmap(R);
// CHECK(assert_equal(w, actualw, 1e-15));
//}
/* ************************************************************************* */
TEST(Rot3, quaternion) {
// NOTE: This is also verifying the ability to convert Vector to Quaternion
Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
Rot3 R1 = Rot3((Matrix)(Matrix(3, 3) <<
0.271018623057411, 0.278786459830371, 0.921318086098018,
0.578529366719085, 0.717799701969298, -0.387385285854279,
-0.769319620053772, 0.637998195662053, 0.033250932803219));
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) <<
-0.207341903877828, 0.250149415542075, 0.945745528564780,
0.881304914479026, -0.371869043667957, 0.291573424846290,
0.424630407073532, 0.893945571198514, -0.143353873763946));
// Check creating Rot3 from quaternion
EXPECT(assert_equal(R1, Rot3(q1)));
EXPECT(assert_equal(R1, Rot3::quaternion(q1.w(), q1.x(), q1.y(), q1.z())));
EXPECT(assert_equal(R2, Rot3(q2)));
EXPECT(assert_equal(R2, Rot3::quaternion(q2.w(), q2.x(), q2.y(), q2.z())));
// Check converting Rot3 to quaterion
EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs())));
EXPECT(assert_equal(Vector(R2.toQuaternion().coeffs()), Vector(q2.coeffs())));
// Check that quaternion and Rot3 represent the same rotation
Point3 p1(1.0, 2.0, 3.0);
Point3 p2(8.0, 7.0, 9.0);
Point3 expected1 = R1*p1;
Point3 expected2 = R2*p2;
Point3 actual1 = Point3(q1*p1.vector());
Point3 actual2 = Point3(q2*p2.vector());
EXPECT(assert_equal(expected1, actual1));
EXPECT(assert_equal(expected2, actual2));
}
/* ************************************************************************* */
TEST( Rot3, stream)
{
Rot3 R;
std::ostringstream os;
os << R;
EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n");
}
#endif #endif

3
gtsam/geometry/tests/testSerializationGeometry.cpp
View File

@ -25,6 +25,7 @@
#include <gtsam/geometry/PinholeCamera.h> #include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3DS2.h> #include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Bundler.h> #include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/Cal3Unified.h>
#include <gtsam/geometry/StereoCamera.h> #include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/StereoPoint2.h> #include <gtsam/geometry/StereoPoint2.h>
@ -46,6 +47,7 @@ static Cal3Bundler cal3(1.0, 2.0, 3.0);
static Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); static Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
static Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4)); static Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4));
static CalibratedCamera cal5(Pose3(rt3, pt3)); static CalibratedCamera cal5(Pose3(rt3, pt3));
static Cal3Unified cal6(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0);
static PinholeCamera<Cal3_S2> cam1(pose3, cal1); static PinholeCamera<Cal3_S2> cam1(pose3, cal1);
static StereoCamera cam2(pose3, cal4ptr); static StereoCamera cam2(pose3, cal4ptr);
@ -66,6 +68,7 @@ TEST (Serialization, text_geometry) {
EXPECT(equalsObj(cal3)); EXPECT(equalsObj(cal3));
EXPECT(equalsObj(cal4)); EXPECT(equalsObj(cal4));
EXPECT(equalsObj(cal5)); EXPECT(equalsObj(cal5));
EXPECT(equalsObj(cal6));
EXPECT(equalsObj(cam1)); EXPECT(equalsObj(cam1));
EXPECT(equalsObj(cam2)); EXPECT(equalsObj(cam2));

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

@ -268,7 +268,7 @@ TEST( triangulation, TriangulationFactor ) {
Key pointKey(1); Key pointKey(1);
SharedNoiseModel model; SharedNoiseModel model;
typedef TriangulationFactor<> Factor; typedef TriangulationFactor<> Factor;
Factor factor(camera1, z1, model, pointKey, sharedCal); Factor factor(camera1, z1, model, pointKey);
// Use the factor to calculate the Jacobians // Use the factor to calculate the Jacobians
Matrix HActual; Matrix HActual;

4
gtsam/geometry/tests/testUnit3.cpp
View File

@ -328,10 +328,6 @@ TEST(Unit3, localCoordinates_retract_expmap) {
//******************************************************************************* //*******************************************************************************
TEST(Unit3, Random) { TEST(Unit3, Random) {
boost::mt19937 rng(42); boost::mt19937 rng(42);
// Check that is deterministic given same random seed
Point3 expected(-0.667578, 0.671447, 0.321713);
Point3 actual = Unit3::Random(rng).point3();
EXPECT(assert_equal(expected,actual,1e-5));
// Check that means are all zero at least // Check that means are all zero at least
Point3 expectedMean, actualMean; Point3 expectedMean, actualMean;
for (size_t i = 0; i < 100; i++) for (size_t i = 0; i < 100; i++)

4
gtsam/geometry/triangulation.h
View File

@ -174,7 +174,7 @@ Point3 triangulateNonlinear(
* @param poses A vector of camera poses * @param poses A vector of camera poses
* @param sharedCal shared pointer to single calibration object * @param sharedCal shared pointer to single calibration object
* @param measurements A vector of camera measurements * @param measurements A vector of camera measurements
* @param rank tolerance, default 1e-9 * @param rank_tol rank tolerance, default 1e-9
* @param optimize Flag to turn on nonlinear refinement of triangulation * @param optimize Flag to turn on nonlinear refinement of triangulation
* @return Returns a Point3 * @return Returns a Point3
*/ */
@ -222,7 +222,7 @@ Point3 triangulatePoint3(const std::vector<Pose3>& poses,
* no other checks to verify the quality of the triangulation. * no other checks to verify the quality of the triangulation.
* @param cameras pinhole cameras * @param cameras pinhole cameras
* @param measurements A vector of camera measurements * @param measurements A vector of camera measurements
* @param rank tolerance, default 1e-9 * @param rank_tol rank tolerance, default 1e-9
* @param optimize Flag to turn on nonlinear refinement of triangulation * @param optimize Flag to turn on nonlinear refinement of triangulation
* @return Returns a Point3 * @return Returns a Point3
*/ */

2
gtsam/linear/Errors.cpp
View File

@ -38,7 +38,7 @@ Errors::Errors(const VectorValues& V) {
/* ************************************************************************* */ /* ************************************************************************* */
void Errors::print(const std::string& s) const { void Errors::print(const std::string& s) const {
odprintf("%s:\n", s.c_str()); cout << s << endl;
BOOST_FOREACH(const Vector& v, *this) BOOST_FOREACH(const Vector& v, *this)
gtsam::print(v); gtsam::print(v);
} }

2
gtsam/linear/GaussianConditional.h
View File

@ -111,7 +111,7 @@ namespace gtsam {
* assumed to have already been solved in and their values are read from \c x. * assumed to have already been solved in and their values are read from \c x.
* This function works for multiple frontal variables. * This function works for multiple frontal variables.
* *
* Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2, * Given the Gaussian conditional with log likelihood \f$ |R x_f - (d - S x_s)|^2 \f$,
* where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator * where \f$ f \f$ are the frontal variables and \f$ s \f$ are the separator
* variables of this conditional, this solve function computes * variables of this conditional, this solve function computes
* \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution. * \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution.

6
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -85,7 +85,7 @@ namespace gtsam {
dims_accumulated.resize(dims.size()+1,0); dims_accumulated.resize(dims.size()+1,0);
dims_accumulated[0]=0; dims_accumulated[0]=0;
for (size_t 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]; dims_accumulated[i] = dims_accumulated[i-1]+dims[i-1];
return dims_accumulated; return dims_accumulated;
} }
@ -361,8 +361,8 @@ VectorValues GaussianFactorGraph::gradientAtZero(
/* ************************************************************************* */ /* ************************************************************************* */
void GaussianFactorGraph::multiplyHessianAdd(double alpha, void GaussianFactorGraph::multiplyHessianAdd(double alpha,
const double* x, double* y) const { const double* x, double* y) const {
vector<size_t> FactorKeys = getkeydim(); vector<size_t> FactorKeys = getkeydim();
BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this)
f->multiplyHessianAdd(alpha, x, y, FactorKeys); f->multiplyHessianAdd(alpha, x, y, FactorKeys);
} }

9
gtsam/linear/HessianFactor.cpp
View File

@ -538,7 +538,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
// copy to yvalues // copy to yvalues
for(DenseIndex i = 0; i < (DenseIndex)size(); ++i) { for(DenseIndex i = 0; i < (DenseIndex)size(); ++i) {
bool didNotExist; bool didNotExist;
VectorValues::iterator it; VectorValues::iterator it;
boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector()); boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector());
if (didNotExist) if (didNotExist)
@ -660,10 +660,11 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
// Do dense elimination // Do dense elimination
GaussianConditional::shared_ptr conditional; GaussianConditional::shared_ptr conditional;
try { try {
VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(keys.size()); size_t numberOfKeysToEliminate = keys.size();
conditional = boost::make_shared<GaussianConditional>(jointFactor->keys(), keys.size(), Ab); VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(numberOfKeysToEliminate);
conditional = boost::make_shared<GaussianConditional>(jointFactor->keys(), numberOfKeysToEliminate, Ab);
// Erase the eliminated keys in the remaining factor // Erase the eliminated keys in the remaining factor
jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + keys.size()); jointFactor->keys_.erase(jointFactor->begin(), jointFactor->begin() + numberOfKeysToEliminate);
} catch(CholeskyFailed&) { } catch(CholeskyFailed&) {
// std::cout << "Problematic Hessian: " << jointFactor->information() << std::endl; // std::cout << "Problematic Hessian: " << jointFactor->information() << std::endl;
throw IndeterminantLinearSystemException(keys.front()); throw IndeterminantLinearSystemException(keys.front());

21
gtsam/linear/tests/testGaussianBayesNet.cpp
View File

@ -15,6 +15,17 @@
* @author Frank Dellaert * @author Frank Dellaert
*/ */
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/numericalDerivative.h>
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
// STL/C++ // STL/C++
#include <iostream> #include <iostream>
#include <sstream> #include <sstream>
@ -22,16 +33,6 @@
#include <boost/tuple/tuple.hpp> #include <boost/tuple/tuple.hpp>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign;
#include <gtsam/base/Testable.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/numericalDerivative.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;

8
gtsam/linear/tests/testGaussianConditional.cpp
View File

@ -25,13 +25,15 @@
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianBayesNet.h> #include <gtsam/linear/GaussianBayesNet.h>
#include <iostream>
#include <sstream>
#include <vector>
#include <boost/assign/std/list.hpp> #include <boost/assign/std/list.hpp>
#include <boost/assign/std/vector.hpp> #include <boost/assign/std/vector.hpp>
#include <boost/assign/list_inserter.hpp> #include <boost/assign/list_inserter.hpp>
#include <boost/make_shared.hpp> #include <boost/make_shared.hpp>
#include <boost/assign/list_of.hpp>
#include <iostream>
#include <sstream>
#include <vector>
using namespace gtsam; using namespace gtsam;
using namespace std; using namespace std;

17
gtsam/linear/tests/testGaussianFactorGraph.cpp
View File

@ -18,20 +18,21 @@
* @author Richard Roberts * @author Richard Roberts
**/ **/
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <boost/assign/list_of.hpp>
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
using namespace boost::assign; using namespace boost::assign;
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/VerticalBlockMatrix.h>
#include <gtsam/inference/VariableSlots.h>
#include <gtsam/inference/VariableIndex.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/GaussianBayesNet.h>
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;

19
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -15,24 +15,25 @@
* @date Dec 15, 2010 * @date Dec 15, 2010
*/ */
#include <vector>
#include <utility>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/map.hpp>
#include <gtsam/base/debug.h>
#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/GaussianConditional.h> #include <gtsam/linear/GaussianConditional.h>
#include <gtsam/linear/VectorValues.h> #include <gtsam/linear/VectorValues.h>
#include <gtsam/base/debug.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
using namespace std; #include <boost/assign/list_of.hpp>
#include <boost/assign/std/vector.hpp>
#include <boost/assign/std/map.hpp>
using namespace boost::assign; using namespace boost::assign;
#include <vector>
#include <utility>
using namespace std;
using namespace gtsam; using namespace gtsam;
const double tol = 1e-5; const double tol = 1e-5;

96
gtsam/navigation/CombinedImuFactor.h
View File

@ -345,12 +345,12 @@ namespace gtsam {
/** Constructor */ /** Constructor */
CombinedImuFactor( CombinedImuFactor(
Key pose_i, ///< previous pose key Key pose_i, ///< previous pose key
Key vel_i, ///< previous velocity key Key vel_i, ///< previous velocity key
Key pose_j, ///< current pose key Key pose_j, ///< current pose key
Key vel_j, ///< current velocity key Key vel_j, ///< current velocity key
Key bias_i, ///< previous bias key Key bias_i, ///< previous bias key
Key bias_j, ///< current bias key Key bias_j, ///< current bias key
const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements
const Vector3& gravity, ///< gravity vector const Vector3& gravity, ///< gravity vector
const Vector3& omegaCoriolis, ///< rotation rate of inertial frame const Vector3& omegaCoriolis, ///< rotation rate of inertial frame
@ -480,33 +480,33 @@ namespace gtsam {
Matrix3 dfPdPi; Matrix3 dfPdPi;
Matrix3 dfVdPi; Matrix3 dfVdPi;
if(use2ndOrderCoriolis_){ if(use2ndOrderCoriolis_){
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
} }
else{ else{
dfPdPi = - Rot_i.matrix(); dfPdPi = - Rot_i.matrix();
dfVdPi = Matrix3::Zero(); dfVdPi = Matrix3::Zero();
} }
(*H1) << (*H1) <<
// dfP/dRi // dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
// dfP/dPi // dfP/dPi
dfPdPi, dfPdPi,
// dfV/dRi // dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
// dfV/dPi // dfV/dPi
dfVdPi, dfVdPi,
// dfR/dRi // dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi // dfR/dPi
Matrix3::Zero(), Matrix3::Zero(),
//dBiasAcc/dPi //dBiasAcc/dPi
Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(), Matrix3::Zero(),
//dBiasOmega/dPi //dBiasOmega/dPi
Matrix3::Zero(), Matrix3::Zero(); Matrix3::Zero(), Matrix3::Zero();
} }
if(H2) { if(H2) {
@ -517,13 +517,13 @@ namespace gtsam {
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper + skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
// dfV/dVi // dfV/dVi
- Matrix3::Identity() - Matrix3::Identity()
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term + 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
// dfR/dVi // dfR/dVi
Matrix3::Zero(), Matrix3::Zero(),
//dBiasAcc/dVi //dBiasAcc/dVi
Matrix3::Zero(), Matrix3::Zero(),
//dBiasOmega/dVi //dBiasOmega/dVi
Matrix3::Zero(); Matrix3::Zero();
} }
if(H3) { if(H3) {
@ -643,21 +643,21 @@ namespace gtsam {
// Predict state at time j // Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+ vel_i * deltaTij + vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij; + 0.5 * gravity * deltaTij*deltaTij;
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij); + gravity * deltaTij);
if(use2ndOrderCoriolis){ if(use2ndOrderCoriolis){
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
} }
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);

74
gtsam/navigation/ImuFactor.h
View File

@ -308,11 +308,11 @@ namespace gtsam {
/** Constructor */ /** Constructor */
ImuFactor( ImuFactor(
Key pose_i, ///< previous pose key Key pose_i, ///< previous pose key
Key vel_i, ///< previous velocity key Key vel_i, ///< previous velocity key
Key pose_j, ///< current pose key Key pose_j, ///< current pose key
Key vel_j, ///< current velocity key Key vel_j, ///< current velocity key
Key bias, ///< previous bias key Key bias, ///< previous bias key
const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements
const Vector3& gravity, ///< gravity vector const Vector3& gravity, ///< gravity vector
const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame
@ -419,29 +419,29 @@ namespace gtsam {
Matrix3 dfPdPi; Matrix3 dfPdPi;
Matrix3 dfVdPi; Matrix3 dfVdPi;
if(use2ndOrderCoriolis_){ if(use2ndOrderCoriolis_){
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij; dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij; dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
} }
else{ else{
dfPdPi = - Rot_i.matrix(); dfPdPi = - Rot_i.matrix();
dfVdPi = Matrix3::Zero(); dfVdPi = Matrix3::Zero();
} }
(*H1) << (*H1) <<
// dfP/dRi // dfP/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr), + preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
// dfP/dPi // dfP/dPi
dfPdPi, dfPdPi,
// dfV/dRi // dfV/dRi
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr), + preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
// dfV/dPi // dfV/dPi
dfVdPi, dfVdPi,
// dfR/dRi // dfR/dRi
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta), Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
// dfR/dPi // dfR/dPi
Matrix3::Zero(); Matrix3::Zero();
} }
if(H2) { if(H2) {
@ -540,22 +540,22 @@ namespace gtsam {
// Predict state at time j // Predict state at time j
/* ---------------------------------------------------------------------------------------------------- */ /* ---------------------------------------------------------------------------------------------------- */
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr + preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
+ vel_i * deltaTij + vel_i * deltaTij
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij; + 0.5 * gravity * deltaTij*deltaTij;
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
+ gravity * deltaTij); + gravity * deltaTij);
if(use2ndOrderCoriolis){ if(use2ndOrderCoriolis){
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
} }
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP); const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);

10
gtsam/navigation/MagFactor.h
View File

@ -37,7 +37,15 @@ class MagFactor: public NoiseModelFactor1<Rot2> {
public: public:
/** Constructor */ /**
* Constructor of factor that estimates nav to body rotation bRn
* @param key of the unknown rotation bRn in the factor graph
* @param measured magnetometer reading, a 3-vector
* @param scale by which a unit vector is scaled to yield a magnetometer reading
* @param direction of the local magnetic field, see e.g. http://www.ngdc.noaa.gov/geomag-web/#igrfwmm
* @param bias of the magnetometer, modeled as purely additive (after scaling)
* @param model of the additive Gaussian noise that is assumed
*/
MagFactor(Key key, const Point3& measured, double scale, MagFactor(Key key, const Point3& measured, double scale,
const Unit3& direction, const Point3& bias, const Unit3& direction, const Point3& bias,
const SharedNoiseModel& model) : const SharedNoiseModel& model) :

2
gtsam/nonlinear/LinearContainerFactor.h
View File

@ -117,7 +117,7 @@ public:
// casting syntactic sugar // casting syntactic sugar
inline bool hasLinearizationPoint() const { return linearizationPoint_; } inline bool hasLinearizationPoint() const { return linearizationPoint_.is_initialized(); }
/** /**
* Simple checks whether this is a Jacobian or Hessian factor * Simple checks whether this is a Jacobian or Hessian factor

64
gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -176,11 +176,11 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
stm << "];\n"; stm << "];\n";
if (firstTimePoses) { if (firstTimePoses) {
lastKey = key; lastKey = key;
firstTimePoses = false; firstTimePoses = false;
} else { } else {
stm << " var" << key << "--" << "var" << lastKey << ";\n"; stm << " var" << key << "--" << "var" << lastKey << ";\n";
lastKey = key; lastKey = key;
} }
} }
stm << "\n"; stm << "\n";
@ -219,37 +219,37 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
// Create factors and variable connections // Create factors and variable connections
for(size_t i = 0; i < this->size(); ++i) { for(size_t i = 0; i < this->size(); ++i) {
if(graphvizFormatting.plotFactorPoints){ if(graphvizFormatting.plotFactorPoints){
// Make each factor a dot // Make each factor a dot
stm << " factor" << i << "[label=\"\", shape=point"; stm << " factor" << i << "[label=\"\", shape=point";
{ {
map<size_t, Point2>::const_iterator pos = graphvizFormatting.factorPositions.find(i); map<size_t, Point2>::const_iterator pos = graphvizFormatting.factorPositions.find(i);
if(pos != graphvizFormatting.factorPositions.end()) if(pos != graphvizFormatting.factorPositions.end())
stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\""; stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\"";
} }
stm << "];\n"; stm << "];\n";
// Make factor-variable connections // Make factor-variable connections
if(graphvizFormatting.connectKeysToFactor && this->at(i)) { if(graphvizFormatting.connectKeysToFactor && this->at(i)) {
BOOST_FOREACH(Key key, *this->at(i)) { BOOST_FOREACH(Key key, *this->at(i)) {
stm << " var" << key << "--" << "factor" << i << ";\n"; stm << " var" << key << "--" << "factor" << i << ";\n";
} }
} }
} }
else { else {
if(this->at(i)) { if(this->at(i)) {
Key k; Key k;
bool firstTime = true; bool firstTime = true;
BOOST_FOREACH(Key key, *this->at(i)) { BOOST_FOREACH(Key key, *this->at(i)) {
if(firstTime){ if(firstTime){
k = key; k = key;
firstTime = false; firstTime = false;
continue; continue;
} }
stm << " var" << key << "--" << "var" << k << ";\n"; stm << " var" << key << "--" << "var" << k << ";\n";
k = key; k = key;
} }
} }
} }
} }
} }

8
gtsam/nonlinear/NonlinearOptimizer.cpp
View File

@ -77,9 +77,11 @@ void NonlinearOptimizer::defaultOptimize() {
params.errorTol, currentError, this->error(), params.verbosity)); params.errorTol, currentError, this->error(), params.verbosity));
// Printing if verbose // Printing if verbose
if (params.verbosity >= NonlinearOptimizerParams::TERMINATION && if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) {
this->iterations() >= params.maxIterations) cout << "iterations: " << this->iterations() << " >? " << params.maxIterations << endl;
cout << "Terminating because reached maximum iterations" << endl; if (this->iterations() >= params.maxIterations)
cout << "Terminating because reached maximum iterations" << endl;
}
} }
/* ************************************************************************* */ /* ************************************************************************* */

10
gtsam/slam/EssentialMatrixFactor.h
View File

@ -29,6 +29,7 @@ public:
/** /**
* Constructor * Constructor
* @param key Essential Matrix variable key
* @param pA point in first camera, in calibrated coordinates * @param pA point in first camera, in calibrated coordinates
* @param pB point in second camera, in calibrated coordinates * @param pB point in second camera, in calibrated coordinates
* @param model noise model is about dot product in ideal, homogeneous coordinates * @param model noise model is about dot product in ideal, homogeneous coordinates
@ -42,6 +43,7 @@ public:
/** /**
* Constructor * Constructor
* @param key Essential Matrix variable key
* @param pA point in first camera, in pixel coordinates * @param pA point in first camera, in pixel coordinates
* @param pB point in second camera, in pixel coordinates * @param pB point in second camera, in pixel coordinates
* @param model noise model is about dot product in ideal, homogeneous coordinates * @param model noise model is about dot product in ideal, homogeneous coordinates
@ -99,6 +101,8 @@ public:
/** /**
* Constructor * Constructor
* @param key1 Essential Matrix variable key
* @param key2 Inverse depth variable key
* @param pA point in first camera, in calibrated coordinates * @param pA point in first camera, in calibrated coordinates
* @param pB point in second camera, in calibrated coordinates * @param pB point in second camera, in calibrated coordinates
* @param model noise model should be in pixels, as well * @param model noise model should be in pixels, as well
@ -113,6 +117,8 @@ public:
/** /**
* Constructor * Constructor
* @param key1 Essential Matrix variable key
* @param key2 Inverse depth variable key
* @param pA point in first camera, in pixel coordinates * @param pA point in first camera, in pixel coordinates
* @param pB point in second camera, in pixel coordinates * @param pB point in second camera, in pixel coordinates
* @param K calibration object, will be used only in constructor * @param K calibration object, will be used only in constructor
@ -216,6 +222,8 @@ public:
/** /**
* Constructor * Constructor
* @param key1 Essential Matrix variable key
* @param key2 Inverse depth variable key
* @param pA point in first camera, in calibrated coordinates * @param pA point in first camera, in calibrated coordinates
* @param pB point in second camera, in calibrated coordinates * @param pB point in second camera, in calibrated coordinates
* @param bRc extra rotation between "body" and "camera" frame * @param bRc extra rotation between "body" and "camera" frame
@ -228,6 +236,8 @@ public:
/** /**
* Constructor * Constructor
* @param key1 Essential Matrix variable key
* @param key2 Inverse depth variable key
* @param pA point in first camera, in pixel coordinates * @param pA point in first camera, in pixel coordinates
* @param pB point in second camera, in pixel coordinates * @param pB point in second camera, in pixel coordinates
* @param K calibration object, will be used only in constructor * @param K calibration object, will be used only in constructor

43
gtsam/slam/ImplicitSchurFactor.h
View File

@ -81,7 +81,7 @@ public:
} }
/// Get matrix P /// Get matrix P
inline const Matrix& getPointCovariance() const { inline const Matrix3& getPointCovariance() const {
return PointCovariance_; return PointCovariance_;
} }
@ -286,26 +286,27 @@ public:
return 0.5 * (result + f); return 0.5 * (result + f);
} }
/// needed to be GaussianFactor - (I - E*P*E')*(F*x - b) // needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
// This is wrong and does not match the definition in Hessian // This is wrong and does not match the definition in Hessian,
// virtual double error(const VectorValues& x) const { // but it matches the definition of the Jacobian factor (JF)
// double errorJF(const VectorValues& x) const {
// // resize does not do malloc if correct size
// e1.resize(size()); // resize does not do malloc if correct size
// e2.resize(size()); e1.resize(size());
// e2.resize(size());
// // e1 = F * x - b = (2m*dm)*dm
// for (size_t k = 0; k < size(); ++k) // e1 = F * x - b = (2m*dm)*dm
// e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2); for (size_t k = 0; k < size(); ++k)
// projectError(e1, e2); e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2);
// projectError(e1, e2);
// double result = 0;
// for (size_t k = 0; k < size(); ++k) double result = 0;
// result += dot(e2[k], e2[k]); for (size_t k = 0; k < size(); ++k)
// result += dot(e2[k], e2[k]);
// std::cout << "implicitFactor::error result " << result << std::endl;
// return 0.5 * result; // std::cout << "implicitFactor::error result " << result << std::endl;
// } return 0.5 * result;
}
/** /**
* @brief Calculate corrected error Q*e = (I - E*P*E')*e * @brief Calculate corrected error Q*e = (I - E*P*E')*e
*/ */

410
gtsam/slam/InitializePose3.cpp Normal file
View File

@ -0,0 +1,410 @@
/* ----------------------------------------------------------------------------
* 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 InitializePose3.h
* @author Luca Carlone
* @author Frank Dellaert
* @date August, 2014
*/
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/base/timing.h>
#include <boost/math/special_functions.hpp>
using namespace std;
namespace gtsam {
namespace InitializePose3 {
//static const Matrix I = eye(1);
static const Matrix I9 = eye(9);
static const Vector zero9 = Vector::Zero(9);
static const Matrix zero33= Matrix::Zero(3,3);
static const Key keyAnchor = symbol('Z', 9999999);
/* ************************************************************************* */
GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g) {
GaussianFactorGraph linearGraph;
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(9);
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g) {
Matrix3 Rij;
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between)
Rij = pose3Between->measured().rotation().matrix();
else
std::cout << "Error in buildLinearOrientationGraph" << std::endl;
// std::cout << "Rij \n" << Rij << std::endl;
const FastVector<Key>& keys = factor->keys();
Key key1 = keys[0], key2 = keys[1];
Matrix M9 = Matrix::Zero(9,9);
M9.block(0,0,3,3) = Rij;
M9.block(3,3,3,3) = Rij;
M9.block(6,6,3,3) = Rij;
linearGraph.add(key1, -I9, key2, M9, zero9, model);
}
// prior on the anchor orientation
linearGraph.add(keyAnchor, I9, (Vector(9) << 1.0, 0.0, 0.0,/* */ 0.0, 1.0, 0.0, /* */ 0.0, 0.0, 1.0), model);
return linearGraph;
}
/* ************************************************************************* */
// Transform VectorValues into valid Rot3
Values normalizeRelaxedRotations(const VectorValues& relaxedRot3) {
gttic(InitializePose3_computeOrientationsChordal);
Matrix ppm = Matrix::Zero(3,3); // plus plus minus
ppm(0,0) = 1; ppm(1,1) = 1; ppm(2,2) = -1;
Values validRot3;
BOOST_FOREACH(const VectorValues::value_type& it, relaxedRot3) {
Key key = it.first;
if (key != keyAnchor) {
const Vector& rotVector = it.second;
Matrix3 rotMat;
rotMat(0,0) = rotVector(0); rotMat(0,1) = rotVector(1); rotMat(0,2) = rotVector(2);
rotMat(1,0) = rotVector(3); rotMat(1,1) = rotVector(4); rotMat(1,2) = rotVector(5);
rotMat(2,0) = rotVector(6); rotMat(2,1) = rotVector(7); rotMat(2,2) = rotVector(8);
Matrix U, V; Vector s;
svd(rotMat, U, s, V);
Matrix3 normalizedRotMat = U * V.transpose();
// std::cout << "rotMat \n" << rotMat << std::endl;
// std::cout << "U V' \n" << U * V.transpose() << std::endl;
// std::cout << "V \n" << V << std::endl;
if(normalizedRotMat.determinant() < 0)
normalizedRotMat = U * ppm * V.transpose();
Rot3 initRot = Rot3(normalizedRotMat);
validRot3.insert(key, initRot);
}
}
return validRot3;
}
/* ************************************************************************* */
// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph) {
gttic(InitializePose3_buildPose3graph);
NonlinearFactorGraph pose3Graph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) {
// recast to a between on Pose3
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between)
pose3Graph.add(pose3Between);
// recast PriorFactor<Pose3> to BetweenFactor<Pose3>
boost::shared_ptr<PriorFactor<Pose3> > pose3Prior =
boost::dynamic_pointer_cast<PriorFactor<Pose3> >(factor);
if (pose3Prior)
pose3Graph.add(
BetweenFactor<Pose3>(keyAnchor, pose3Prior->keys()[0],
pose3Prior->prior(), pose3Prior->get_noiseModel()));
}
return pose3Graph;
}
/* ************************************************************************* */
// Return the orientations of a graph including only BetweenFactors<Pose3>
Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph) {
gttic(InitializePose3_computeOrientationsChordal);
// regularize measurements and plug everything in a factor graph
GaussianFactorGraph relaxedGraph = buildLinearOrientationGraph(pose3Graph);
// Solve the LFG
VectorValues relaxedRot3 = relaxedGraph.optimize();
// normalize and compute Rot3
return normalizeRelaxedRotations(relaxedRot3);
}
/* ************************************************************************* */
// Return the orientations of a graph including only BetweenFactors<Pose3>
Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph, const Values& givenGuess, const size_t maxIter, const bool setRefFrame) {
gttic(InitializePose3_computeOrientationsGradient);
// this works on the inverse rotations, according to Tron&Vidal,2011
Values inverseRot;
inverseRot.insert(keyAnchor, Rot3());
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, givenGuess) {
Key key = key_value.key;
const Pose3& pose = givenGuess.at<Pose3>(key);
inverseRot.insert(key, pose.rotation().inverse());
}
// Create the map of edges incident on each node
KeyVectorMap adjEdgesMap;
KeyRotMap factorId2RotMap;
createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
// calculate max node degree & allocate gradient
size_t maxNodeDeg = 0;
VectorValues grad;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
Key key = key_value.key;
grad.insert(key,Vector3::Zero());
size_t currNodeDeg = (adjEdgesMap.at(key)).size();
if(currNodeDeg > maxNodeDeg)
maxNodeDeg = currNodeDeg;
}
// Create parameters
double b = 1;
double f0 = 1/b - (1/b + M_PI) * exp(-b*M_PI);
double a = (M_PI*M_PI)/(2*f0);
double rho = 2*a*b;
double mu_max = maxNodeDeg * rho;
double stepsize = 2/mu_max; // = 1/(a b dG)
std::cout <<" b " << b <<" f0 " << f0 <<" a " << a <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
double maxGrad;
// gradient iterations
size_t it;
for(it=0; it < maxIter; it++){
//////////////////////////////////////////////////////////////////////////
// compute the gradient at each node
//std::cout << "it " << it <<" b " << b <<" f0 " << f0 <<" a " << a
// <<" rho " << rho <<" stepsize " << stepsize << " maxNodeDeg "<< maxNodeDeg << std::endl;
maxGrad = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
Key key = key_value.key;
//std::cout << "---------------------------key " << DefaultKeyFormatter(key) << std::endl;
Vector gradKey = Vector3::Zero();
// collect the gradient for each edge incident on key
BOOST_FOREACH(const size_t& factorId, adjEdgesMap.at(key)){
Rot3 Rij = factorId2RotMap.at(factorId);
Rot3 Ri = inverseRot.at<Rot3>(key);
if( key == (pose3Graph.at(factorId))->keys()[0] ){
Key key1 = (pose3Graph.at(factorId))->keys()[1];
Rot3 Rj = inverseRot.at<Rot3>(key1);
gradKey = gradKey + gradientTron(Ri, Rij * Rj, a, b);
//std::cout << "key1 " << DefaultKeyFormatter(key1) << " gradientTron(Ri, Rij * Rj, a, b) \n " << gradientTron(Ri, Rij * Rj, a, b) << std::endl;
}else if( key == (pose3Graph.at(factorId))->keys()[1] ){
Key key0 = (pose3Graph.at(factorId))->keys()[0];
Rot3 Rj = inverseRot.at<Rot3>(key0);
gradKey = gradKey + gradientTron(Ri, Rij.between(Rj), a, b);
//std::cout << "key0 " << DefaultKeyFormatter(key0) << " gradientTron(Ri, Rij.inverse() * Rj, a, b) \n " << gradientTron(Ri, Rij.between(Rj), a, b) << std::endl;
}else{
std::cout << "Error in gradient computation" << std::endl;
}
} // end of i-th gradient computation
grad.at(key) = stepsize * gradKey;
double normGradKey = (gradKey).norm();
//std::cout << "key " << DefaultKeyFormatter(key) <<" \n grad \n" << grad.at(key) << std::endl;
if(normGradKey>maxGrad)
maxGrad = normGradKey;
} // end of loop over nodes
//////////////////////////////////////////////////////////////////////////
// update estimates
inverseRot = inverseRot.retract(grad);
//////////////////////////////////////////////////////////////////////////
// check stopping condition
if (it>20 && maxGrad < 5e-3)
break;
} // enf of gradient iterations
std::cout << "nr of gradient iterations " << it << "maxGrad " << maxGrad << std::endl;
// Return correct rotations
const Rot3& Rref = inverseRot.at<Rot3>(keyAnchor); // This will be set to the identity as so far we included no prior
Values estimateRot;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, inverseRot) {
Key key = key_value.key;
if (key != keyAnchor) {
const Rot3& R = inverseRot.at<Rot3>(key);
if(setRefFrame)
estimateRot.insert(key, Rref.compose(R.inverse()));
else
estimateRot.insert(key, R.inverse());
}
}
return estimateRot;
}
/* ************************************************************************* */
void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap, const NonlinearFactorGraph& pose3Graph){
size_t factorId = 0;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose3Graph) {
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between){
Rot3 Rij = pose3Between->measured().rotation();
factorId2RotMap.insert(pair<Key, Rot3 >(factorId,Rij));
Key key1 = pose3Between->key1();
if (adjEdgesMap.find(key1) != adjEdgesMap.end()){ // key is already in
adjEdgesMap.at(key1).push_back(factorId);
}else{
vector<size_t> edge_id;
edge_id.push_back(factorId);
adjEdgesMap.insert(pair<Key, vector<size_t> >(key1, edge_id));
}
Key key2 = pose3Between->key2();
if (adjEdgesMap.find(key2) != adjEdgesMap.end()){ // key is already in
adjEdgesMap.at(key2).push_back(factorId);
}else{
vector<size_t> edge_id;
edge_id.push_back(factorId);
adjEdgesMap.insert(pair<Key, vector<size_t> >(key2, edge_id));
}
}else{
std::cout << "Error in computeOrientationsGradient" << std::endl;
}
factorId++;
}
}
/* ************************************************************************* */
Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b) {
Vector3 logRot = Rot3::Logmap(R1.between(R2));
double th = logRot.norm();
if(th != th){ // the second case means that th = nan (logRot does not work well for +/-pi)
Rot3 R1pert = R1.compose( Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01)) ); // some perturbation
logRot = Rot3::Logmap(R1pert.between(R2));
th = logRot.norm();
}
// exclude small or invalid rotations
if (th > 1e-5 && th == th){ // nonzero valid rotations
logRot = logRot / th;
}else{
logRot = Vector3::Zero();
th = 0.0;
}
double fdot = a*b*th*exp(-b*th);
return fdot*logRot;
}
/* ************************************************************************* */
Values initializeOrientations(const NonlinearFactorGraph& graph) {
// We "extract" the Pose3 subgraph of the original graph: this
// is done to properly model priors and avoiding operating on a larger graph
NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
// Get orientations from relative orientation measurements
return computeOrientationsChordal(pose3Graph);
}
///* ************************************************************************* */
Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot) {
gttic(InitializePose3_computePoses);
// put into Values structure
Values initialPose;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, initialRot){
Key key = key_value.key;
const Rot3& rot = initialRot.at<Rot3>(key);
Pose3 initializedPose = Pose3(rot, Point3());
initialPose.insert(key, initializedPose);
}
// add prior
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
initialPose.insert(keyAnchor, Pose3());
pose3graph.add(PriorFactor<Pose3>(keyAnchor, Pose3(), priorModel));
// Create optimizer
GaussNewtonParams params;
bool singleIter = true;
if(singleIter){
params.maxIterations = 1;
}else{
std::cout << " \n\n\n\n performing more than 1 GN iterations \n\n\n" <<std::endl;
params.setVerbosity("TERMINATION");
}
GaussNewtonOptimizer optimizer(pose3graph, initialPose, params);
Values GNresult = optimizer.optimize();
// put into Values structure
Values estimate;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, GNresult) {
Key key = key_value.key;
if (key != keyAnchor) {
const Pose3& pose = GNresult.at<Pose3>(key);
estimate.insert(key, pose);
}
}
return estimate;
}
/* ************************************************************************* */
Values initialize(const NonlinearFactorGraph& graph) {
gttic(InitializePose3_initialize);
// We "extract" the Pose3 subgraph of the original graph: this
// is done to properly model priors and avoiding operating on a larger graph
NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
// Get orientations from relative orientation measurements
Values valueRot3 = computeOrientationsChordal(pose3Graph);
// Compute the full poses (1 GN iteration on full poses)
return computePoses(pose3Graph, valueRot3);
}
/* ************************************************************************* */
Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient) {
Values initialValues;
// We "extract" the Pose3 subgraph of the original graph: this
// is done to properly model priors and avoiding operating on a larger graph
NonlinearFactorGraph pose3Graph = buildPose3graph(graph);
// Get orientations from relative orientation measurements
Values orientations;
if(useGradient)
orientations = computeOrientationsGradient(pose3Graph, givenGuess);
else
orientations = computeOrientationsChordal(pose3Graph);
// orientations.print("orientations\n");
// Compute the full poses (1 GN iteration on full poses)
return computePoses(pose3Graph, orientations);
// BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, orientations) {
// Key key = key_value.key;
// if (key != keyAnchor) {
// const Point3& pos = givenGuess.at<Pose3>(key).translation();
// const Rot3& rot = orientations.at<Rot3>(key);
// Pose3 initializedPoses = Pose3(rot, pos);
// initialValues.insert(key, initializedPoses);
// }
// }
// return initialValues;
}
} // end of namespace lago
} // end of namespace gtsam

59
gtsam/slam/InitializePose3.h Normal file
View File

@ -0,0 +1,59 @@
/* ----------------------------------------------------------------------------
* 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 InitializePose3.h
* @brief Initialize Pose3 in a factor graph
*
* @author Luca Carlone
* @author Frank Dellaert
* @date August, 2014
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/VectorValues.h>
#include <gtsam/inference/graph.h>
#include <gtsam/geometry/Rot3.h>
namespace gtsam {
typedef std::map<Key, std::vector<size_t> > KeyVectorMap;
typedef std::map<Key, Rot3 > KeyRotMap;
namespace InitializePose3 {
GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const NonlinearFactorGraph& g);
GTSAM_EXPORT Values normalizeRelaxedRotations(const VectorValues& relaxedRot3);
GTSAM_EXPORT Values computeOrientationsChordal(const NonlinearFactorGraph& pose3Graph);
GTSAM_EXPORT Values computeOrientationsGradient(const NonlinearFactorGraph& pose3Graph,
const Values& givenGuess, size_t maxIter = 10000, const bool setRefFrame = true);
GTSAM_EXPORT void createSymbolicGraph(KeyVectorMap& adjEdgesMap, KeyRotMap& factorId2RotMap,
const NonlinearFactorGraph& pose3Graph);
GTSAM_EXPORT Vector3 gradientTron(const Rot3& R1, const Rot3& R2, const double a, const double b);
GTSAM_EXPORT NonlinearFactorGraph buildPose3graph(const NonlinearFactorGraph& graph);
GTSAM_EXPORT Values computePoses(NonlinearFactorGraph& pose3graph, Values& initialRot);
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph);
GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph, const Values& givenGuess, bool useGradient = false);
} // end of namespace lago
} // end of namespace gtsam

2
gtsam/slam/PoseRotationPrior.h
View File

@ -77,7 +77,7 @@ public:
(*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim); (*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim);
} }
return Rotation::Logmap(newR) - Rotation::Logmap(measured_); return measured_.localCoordinates(newR);
} }
private: private:

2
gtsam/slam/SmartFactorBase.h
View File

@ -572,7 +572,7 @@ public:
FastMap<Key,size_t> KeySlotMap; FastMap<Key,size_t> KeySlotMap;
for (size_t slot=0; slot < allKeys.size(); slot++) for (size_t slot=0; slot < allKeys.size(); slot++)
KeySlotMap.insert(std::make_pair<Key,size_t>(allKeys[slot],slot)); KeySlotMap.insert(std::make_pair(allKeys[slot],slot));
// a single point is observed in numKeys cameras // a single point is observed in numKeys cameras
size_t numKeys = this->keys_.size(); // cameras observing current point size_t numKeys = this->keys_.size(); // cameras observing current point

183
gtsam/slam/dataset.cpp
View File

@ -85,7 +85,6 @@ string createRewrittenFileName(const string& name) {
return newpath.string(); return newpath.string();
} }
/* ************************************************************************* */ /* ************************************************************************* */
#endif #endif
/* ************************************************************************* */ /* ************************************************************************* */
@ -103,6 +102,25 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
double v1, v2, v3, v4, v5, v6; double v1, v2, v3, v4, v5, v6;
is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6; is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;
if (noiseFormat == NoiseFormatAUTO)
{
// Try to guess covariance matrix layout
if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0)
{
// NoiseFormatGRAPH
noiseFormat = NoiseFormatGRAPH;
}
else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0)
{
// NoiseFormatCOV
noiseFormat = NoiseFormatCOV;
}
else
{
throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file. Please specify the noise format.");
}
}
// Read matrix and check that diagonal entries are non-zero // Read matrix and check that diagonal entries are non-zero
Matrix M(3, 3); Matrix M(3, 3);
switch (noiseFormat) { switch (noiseFormat) {
@ -164,7 +182,7 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
} }
/* ************************************************************************* */ /* ************************************************************************* */
GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID, GraphAndValues load2D(const string& filename, SharedNoiseModel model, Key maxID,
bool addNoise, bool smart, NoiseFormat noiseFormat, bool addNoise, bool smart, NoiseFormat noiseFormat,
KernelFunctionType kernelFunctionType) { KernelFunctionType kernelFunctionType) {
@ -178,12 +196,12 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID,
string tag; string tag;
// load the poses // load the poses
while (is) { while (!is.eof()) {
if (!(is >> tag)) if (!(is >> tag))
break; break;
if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) { if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
int id; Key id;
double x, y, yaw; double x, y, yaw;
is >> id >> x >> y >> yaw; is >> id >> x >> y >> yaw;
@ -212,9 +230,9 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID,
} }
// Parse the pose constraints // Parse the pose constraints
int id1, id2; Key id1, id2;
bool haveLandmark = false; bool haveLandmark = false;
while (is) { while (!is.eof()) {
if (!(is >> tag)) if (!(is >> tag))
break; break;
@ -250,7 +268,6 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID,
new BetweenFactor<Pose2>(id1, id2, l1Xl2, model)); new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
graph->push_back(factor); graph->push_back(factor);
} }
// Parse measurements // Parse measurements
double bearing, range, bearing_std, range_std; double bearing, range, bearing_std, range_std;
@ -358,12 +375,16 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
} }
/* ************************************************************************* */ /* ************************************************************************* */
GraphAndValues readG2o(const string& g2oFile, GraphAndValues readG2o(const string& g2oFile, const bool is3D,
KernelFunctionType kernelFunctionType) { KernelFunctionType kernelFunctionType) {
// just call load2D // just call load2D
int maxID = 0; int maxID = 0;
bool addNoise = false; bool addNoise = false;
bool smart = true; bool smart = true;
if(is3D)
return load3D(g2oFile);
return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart, return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
NoiseFormatG2O, kernelFunctionType); NoiseFormatG2O, kernelFunctionType);
} }
@ -374,44 +395,97 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
fstream stream(filename.c_str(), fstream::out); fstream stream(filename.c_str(), fstream::out);
// save poses // save 2D & 3D poses
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) {
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " " const Pose2* pose2D = dynamic_cast<const Pose2*>(&key_value.value);
<< pose.y() << " " << pose.theta() << endl; if(pose2D){
stream << "VERTEX_SE2 " << key_value.key << " " << pose2D->x() << " "
<< pose2D->y() << " " << pose2D->theta() << endl;
}
const Pose3* pose3D = dynamic_cast<const Pose3*>(&key_value.value);
if(pose3D){
Point3 p = pose3D->translation();
Rot3 R = pose3D->rotation();
stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " " << p.y() << " " << p.z()
<< " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z()
<< " " << R.toQuaternion().w() << endl;
}
} }
// save edges // save edges (2D or 3D)
BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) { BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
boost::shared_ptr<BetweenFactor<Pose2> > factor = boost::shared_ptr<BetweenFactor<Pose2> > factor =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_); boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
if (!factor) if (factor){
continue; SharedNoiseModel model = factor->get_noiseModel();
boost::shared_ptr<noiseModel::Gaussian> gaussianModel =
boost::dynamic_pointer_cast<noiseModel::Gaussian>(model);
if (!gaussianModel){
model->print("model\n");
throw invalid_argument("writeG2o: invalid noise model!");
}
Matrix Info = gaussianModel->R().transpose() * gaussianModel->R();
Pose2 pose = factor->measured(); //.inverse();
stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
<< pose.x() << " " << pose.y() << " " << pose.theta();
for (int i = 0; i < 3; i++){
for (int j = i; j < 3; j++){
stream << " " << Info(i, j);
}
}
stream << endl;
}
SharedNoiseModel model = factor->get_noiseModel(); boost::shared_ptr< BetweenFactor<Pose3> > factor3D =
boost::shared_ptr<noiseModel::Diagonal> diagonalModel = boost::dynamic_pointer_cast< BetweenFactor<Pose3> >(factor_);
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonalModel)
throw invalid_argument(
"writeG2o: invalid noise model (current version assumes diagonal noise model)!");
Pose2 pose = factor->measured(); //.inverse(); if (factor3D){
stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " " SharedNoiseModel model = factor3D->get_noiseModel();
<< pose.x() << " " << pose.y() << " " << pose.theta() << " "
<< diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " " boost::shared_ptr<noiseModel::Gaussian> gaussianModel =
<< diagonalModel->precision(1) << " " << 0.0 << " " boost::dynamic_pointer_cast<noiseModel::Gaussian>(model);
<< diagonalModel->precision(2) << endl; if (!gaussianModel){
model->print("model\n");
throw invalid_argument("writeG2o: invalid noise model!");
}
Matrix Info = gaussianModel->R().transpose() * gaussianModel->R();
Pose3 pose3D = factor3D->measured();
Point3 p = pose3D.translation();
Rot3 R = pose3D.rotation();
stream << "EDGE_SE3:QUAT " << factor3D->key1() << " " << factor3D->key2() << " "
<< p.x() << " " << p.y() << " " << p.z() << " " << R.toQuaternion().x()
<< " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().w();
Matrix InfoG2o = eye(6);
InfoG2o.block(0,0,3,3) = Info.block(3,3,3,3); // cov translation
InfoG2o.block(3,3,3,3) = Info.block(0,0,3,3); // cov rotation
InfoG2o.block(0,3,3,3) = Info.block(0,3,3,3); // off diagonal
InfoG2o.block(3,0,3,3) = Info.block(3,0,3,3); // off diagonal
for (int i = 0; i < 6; i++){
for (int j = i; j < 6; j++){
stream << " " << InfoG2o(i, j);
}
}
stream << endl;
}
} }
stream.close(); stream.close();
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool load3D(const string& filename) { GraphAndValues load3D(const string& filename) {
ifstream is(filename.c_str()); ifstream is(filename.c_str());
if (!is) if (!is)
return false; throw invalid_argument("load3D: can not find file " + filename);
while (is) { Values::shared_ptr initial(new Values);
NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
while (!is.eof()) {
char buf[LINESIZE]; char buf[LINESIZE];
is.getline(buf, LINESIZE); is.getline(buf, LINESIZE);
istringstream ls(buf); istringstream ls(buf);
@ -419,15 +493,26 @@ bool load3D(const string& filename) {
ls >> tag; ls >> tag;
if (tag == "VERTEX3") { if (tag == "VERTEX3") {
int id; Key id;
double x, y, z, roll, pitch, yaw; double x, y, z, roll, pitch, yaw;
ls >> id >> x >> y >> z >> roll >> pitch >> yaw; ls >> id >> x >> y >> z >> roll >> pitch >> yaw;
Rot3 R = Rot3::ypr(yaw,pitch,roll);
Point3 t = Point3(x, y, z);
initial->insert(id, Pose3(R,t));
}
if (tag == "VERTEX_SE3:QUAT") {
Key id;
double x, y, z, qx, qy, qz, qw;
ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw;
Rot3 R = Rot3::quaternion(qw, qx, qy, qz);
Point3 t = Point3(x, y, z);
initial->insert(id, Pose3(R,t));
} }
} }
is.clear(); /* clears the end-of-file and error flags */ is.clear(); /* clears the end-of-file and error flags */
is.seekg(0, ios::beg); is.seekg(0, ios::beg);
while (is) { while (!is.eof()) {
char buf[LINESIZE]; char buf[LINESIZE];
is.getline(buf, LINESIZE); is.getline(buf, LINESIZE);
istringstream ls(buf); istringstream ls(buf);
@ -435,16 +520,46 @@ bool load3D(const string& filename) {
ls >> tag; ls >> tag;
if (tag == "EDGE3") { if (tag == "EDGE3") {
int id1, id2; Key id1, id2;
double x, y, z, roll, pitch, yaw; double x, y, z, roll, pitch, yaw;
ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw; ls >> id1 >> id2 >> x >> y >> z >> roll >> pitch >> yaw;
Rot3 R = Rot3::ypr(yaw,pitch,roll);
Point3 t = Point3(x, y, z);
Matrix m = eye(6); Matrix m = eye(6);
for (int i = 0; i < 6; i++) for (int i = 0; i < 6; i++)
for (int j = i; j < 6; j++) for (int j = i; j < 6; j++)
ls >> m(i, j); ls >> m(i, j);
SharedNoiseModel model = noiseModel::Gaussian::Information(m);
NonlinearFactor::shared_ptr factor(
new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
graph->push_back(factor);
}
if (tag == "EDGE_SE3:QUAT") {
Matrix m = eye(6);
Key id1, id2;
double x, y, z, qx, qy, qz, qw;
ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw;
Rot3 R = Rot3::quaternion(qw, qx, qy, qz);
Point3 t = Point3(x, y, z);
for (int i = 0; i < 6; i++){
for (int j = i; j < 6; j++){
double mij;
ls >> mij;
m(i, j) = mij;
m(j, i) = mij;
}
}
Matrix mgtsam = eye(6);
mgtsam.block(0,0,3,3) = m.block(3,3,3,3); // cov rotation
mgtsam.block(3,3,3,3) = m.block(0,0,3,3); // cov translation
mgtsam.block(0,3,3,3) = m.block(0,3,3,3); // off diagonal
mgtsam.block(3,0,3,3) = m.block(3,0,3,3); // off diagonal
SharedNoiseModel model = noiseModel::Gaussian::Information(mgtsam);
NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose3>(id1, id2, Pose3(R,t), model));
graph->push_back(factor);
} }
} }
return true; return make_pair(graph, initial);
} }
/* ************************************************************************* */ /* ************************************************************************* */

16
gtsam/slam/dataset.h
View File

@ -57,7 +57,8 @@ enum NoiseFormat {
NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33 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 NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr
NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix ! NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix !
NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33 NoiseFormatCOV, ///< Covariance matrix C11, C12, C13, C22, C23, C33
NoiseFormatAUTO ///< Try to guess covariance matrix layout
}; };
/// Robust kernel type to wrap around quadratic noise model /// Robust kernel type to wrap around quadratic noise model
@ -79,7 +80,7 @@ GTSAM_EXPORT GraphAndValues load2D(
std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0, std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0,
bool addNoise = false, bool addNoise = false,
bool smart = true, // bool smart = true, //
NoiseFormat noiseFormat = NoiseFormatGRAPH, NoiseFormat noiseFormat = NoiseFormatAUTO,
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
/** /**
@ -94,8 +95,8 @@ GTSAM_EXPORT GraphAndValues load2D(
* @return graph and initial values * @return graph and initial values
*/ */
GTSAM_EXPORT GraphAndValues load2D(const std::string& filename, GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
SharedNoiseModel model = SharedNoiseModel(), int maxID = 0, bool addNoise = SharedNoiseModel model = SharedNoiseModel(), Key maxID = 0, bool addNoise =
false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatGRAPH, // false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, //
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
/// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel /// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel
@ -110,11 +111,12 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
/** /**
* @brief This function parses a g2o file and stores the measurements into a * @brief This function parses a g2o file and stores the measurements into a
* NonlinearFactorGraph and the initial guess in a Values structure * NonlinearFactorGraph and the initial guess in a Values structure
* @param filename The name of the g2o file * @param filename The name of the g2o file\
* @param is3D indicates if the file describes a 2D or 3D problem
* @param kernelFunctionType whether to wrap the noise model in a robust kernel * @param kernelFunctionType whether to wrap the noise model in a robust kernel
* @return graph and initial values * @return graph and initial values
*/ */
GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile, const bool is3D = false,
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE); KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
/** /**
@ -130,7 +132,7 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
/** /**
* Load TORO 3D Graph * Load TORO 3D Graph
*/ */
GTSAM_EXPORT bool load3D(const std::string& filename); GTSAM_EXPORT GraphAndValues load3D(const std::string& filename);
/// A measurement with its camera index /// A measurement with its camera index
typedef std::pair<size_t, Point2> SfM_Measurement; typedef std::pair<size_t, Point2> SfM_Measurement;

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

@ -56,6 +56,17 @@ TEST( dataSet, load2D)
EXPECT(assert_equal(expected, *actual)); EXPECT(assert_equal(expected, *actual));
} }
/* ************************************************************************* */
TEST( dataSet, load2DVictoriaPark)
{
const string filename = findExampleDataFile("victoria_park.txt");
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
boost::tie(graph, initial) = load2D(filename);
EXPECT_LONGS_EQUAL(10608,graph->size());
EXPECT_LONGS_EQUAL(7120,initial->size());
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( dataSet, Balbianello) TEST( dataSet, Balbianello)
{ {
@ -116,13 +127,116 @@ TEST( dataSet, readG2o)
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
} }
/* ************************************************************************* */
TEST( dataSet, readG2o3D)
{
const string g2oFile = findExampleDataFile("pose3example");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
bool is3D = true;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
Values expectedValues;
Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
expectedValues.insert(0, Pose3(R0, p0));
Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
expectedValues.insert(1, Pose3(R1, p1));
Rot3 R2 = Rot3::quaternion(0.421446, -0.351729, -0.597838, 0.584174 );
Point3 p2 = Point3(1.993500, 0.023275, 0.003793);
expectedValues.insert(2, Pose3(R2, p2));
Rot3 R3 = Rot3::quaternion(0.067024, 0.331798, -0.200659, 0.919323);
Point3 p3 = Point3(2.004291, 1.024305, 0.018047);
expectedValues.insert(3, Pose3(R3, p3));
Rot3 R4 = Rot3::quaternion(0.765488, -0.035697, -0.462490, 0.445933);
Point3 p4 = Point3(0.999908, 1.055073, 0.020212);
expectedValues.insert(4, Pose3(R4, p4));
EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(6) << 10000.0,10000.0,10000.0,10000.0,10000.0,10000.0));
NonlinearFactorGraph expectedGraph;
Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
expectedGraph.add(BetweenFactor<Pose3>(0, 1, Pose3(R01,p01), model));
Point3 p12 = Point3(0.523923, 0.776654, 0.326659);
Rot3 R12 = Rot3::quaternion(0.105373 , 0.311512, 0.656877, -0.678505 );
expectedGraph.add(BetweenFactor<Pose3>(1, 2, Pose3(R12,p12), model));
Point3 p23 = Point3(0.910927, 0.055169, -0.411761);
Rot3 R23 = Rot3::quaternion(0.568551 , 0.595795, -0.561677, 0.079353 );
expectedGraph.add(BetweenFactor<Pose3>(2, 3, Pose3(R23,p23), model));
Point3 p34 = Point3(0.775288, 0.228798, -0.596923);
Rot3 R34 = Rot3::quaternion(0.542221 , -0.592077, 0.303380, -0.513226 );
expectedGraph.add(BetweenFactor<Pose3>(3, 4, Pose3(R34,p34), model));
Point3 p14 = Point3(-0.577841, 0.628016, -0.543592);
Rot3 R14 = Rot3::quaternion(0.327419 , -0.125250, -0.534379, 0.769122 );
expectedGraph.add(BetweenFactor<Pose3>(1, 4, Pose3(R14,p14), model));
Point3 p30 = Point3(-0.623267, 0.086928, 0.773222);
Rot3 R30 = Rot3::quaternion(0.083672 , 0.104639, 0.627755, 0.766795 );
expectedGraph.add(BetweenFactor<Pose3>(3, 0, Pose3(R30,p30), model));
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
}
/* ************************************************************************* */
TEST( dataSet, readG2o3DNonDiagonalNoise)
{
const string g2oFile = findExampleDataFile("pose3example-offdiagonal.txt");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
bool is3D = true;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D);
Values expectedValues;
Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 );
Point3 p0 = Point3(0.000000, 0.000000, 0.000000);
expectedValues.insert(0, Pose3(R0, p0));
Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
Point3 p1 = Point3(1.001367, 0.015390, 0.004948);
expectedValues.insert(1, Pose3(R1, p1));
EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
Matrix Info = Matrix(6,6);
for (int i = 0; i < 6; i++){
for (int j = i; j < 6; j++){
if(i==j)
Info(i, j) = 10000;
else{
Info(i, j) = i+1; // arbitrary nonzero number
Info(j, i) = i+1;
}
}
}
noiseModel::Gaussian::shared_ptr model = noiseModel::Gaussian::Covariance(Info.inverse());
NonlinearFactorGraph expectedGraph;
Point3 p01 = Point3(1.001367, 0.015390, 0.004948);
Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 );
expectedGraph.add(BetweenFactor<Pose3>(0, 1, Pose3(R01,p01), model));
EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( dataSet, readG2oHuber) TEST( dataSet, readG2oHuber)
{ {
const string g2oFile = findExampleDataFile("pose2example"); const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph::shared_ptr actualGraph; NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues; Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeHUBER); bool is3D = false;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeHUBER);
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); 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); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel);
@ -149,7 +263,8 @@ TEST( dataSet, readG2oTukey)
const string g2oFile = findExampleDataFile("pose2example"); const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph::shared_ptr actualGraph; NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues; Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeTUKEY); bool is3D = false;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D, KernelFunctionTypeTUKEY);
noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699)); 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); SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
@ -188,6 +303,44 @@ TEST( dataSet, writeG2o)
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-5)); EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-5));
} }
/* ************************************************************************* */
TEST( dataSet, writeG2o3D)
{
const string g2oFile = findExampleDataFile("pose3example");
NonlinearFactorGraph::shared_ptr expectedGraph;
Values::shared_ptr expectedValues;
bool is3D = true;
boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile, is3D);
const string filenameToWrite = createRewrittenFileName(g2oFile);
writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite, is3D);
EXPECT(assert_equal(*expectedValues,*actualValues,1e-4));
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4));
}
/* ************************************************************************* */
TEST( dataSet, writeG2o3DNonDiagonalNoise)
{
const string g2oFile = findExampleDataFile("pose3example-offdiagonal");
NonlinearFactorGraph::shared_ptr expectedGraph;
Values::shared_ptr expectedValues;
bool is3D = true;
boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile, is3D);
const string filenameToWrite = createRewrittenFileName(g2oFile);
writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite, is3D);
EXPECT(assert_equal(*expectedValues,*actualValues,1e-4));
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-4));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( dataSet, readBAL_Dubrovnik) TEST( dataSet, readBAL_Dubrovnik)
{ {

259
gtsam/slam/tests/testInitializePose3.cpp Normal file
View File

@ -0,0 +1,259 @@
/* ----------------------------------------------------------------------------
* 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 testInitializePose3.cpp
* @brief Unit tests for 3D SLAM initialization, using rotation relaxation
*
* @author Luca Carlone
* @author Frank Dellaert
* @date August, 2014
*/
#include <gtsam/slam/InitializePose3.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <CppUnitLite/TestHarness.h>
#include <cmath>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3);
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(6, 0.1));
namespace simple {
// We consider a small graph:
// symbolic FG
// x2 0 1
// / | \ 1 2
// / | \ 2 3
// x3 | x1 2 0
// \ | / 0 3
// \ | /
// x0
//
static Point3 p0 = Point3(0,0,0);
static Rot3 R0 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,0.0 ) );
static Point3 p1 = Point3(1,2,0);
static Rot3 R1 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,1.570796 ) );
static Point3 p2 = Point3(0,2,0);
static Rot3 R2 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,3.141593 ) );
static Point3 p3 = Point3(-1,1,0);
static Rot3 R3 = Rot3::Expmap( ( Vector(3) << 0.0,0.0,4.712389 ) );
static Pose3 pose0 = Pose3(R0,p0);
static Pose3 pose1 = Pose3(R1,p1);
static Pose3 pose2 = Pose3(R2,p2);
static Pose3 pose3 = Pose3(R3,p3);
NonlinearFactorGraph graph() {
NonlinearFactorGraph g;
g.add(BetweenFactor<Pose3>(x0, x1, pose0.between(pose1), model));
g.add(BetweenFactor<Pose3>(x1, x2, pose1.between(pose2), model));
g.add(BetweenFactor<Pose3>(x2, x3, pose2.between(pose3), model));
g.add(BetweenFactor<Pose3>(x2, x0, pose2.between(pose0), model));
g.add(BetweenFactor<Pose3>(x0, x3, pose0.between(pose3), model));
g.add(PriorFactor<Pose3>(x0, pose0, model));
return g;
}
}
/* *************************************************************************** */
TEST( InitializePose3, buildPose3graph ) {
NonlinearFactorGraph pose3graph = InitializePose3::buildPose3graph(simple::graph());
// pose3graph.print("");
}
/* *************************************************************************** */
TEST( InitializePose3, orientations ) {
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
Values initial = InitializePose3::computeOrientationsChordal(pose3Graph);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal(simple::R0, initial.at<Rot3>(x0), 1e-6));
EXPECT(assert_equal(simple::R1, initial.at<Rot3>(x1), 1e-6));
EXPECT(assert_equal(simple::R2, initial.at<Rot3>(x2), 1e-6));
EXPECT(assert_equal(simple::R3, initial.at<Rot3>(x3), 1e-6));
}
/* *************************************************************************** */
TEST( InitializePose3, orientationsGradientSymbolicGraph ) {
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
KeyVectorMap adjEdgesMap;
KeyRotMap factorId2RotMap;
InitializePose3::createSymbolicGraph(adjEdgesMap, factorId2RotMap, pose3Graph);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[0], 0, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[1], 3, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[2], 4, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0)[3], 5, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x0).size(), 4, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x1)[0], 0, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x1)[1], 1, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x1).size(), 2, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2)[0], 1, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2)[1], 2, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2)[2], 3, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x2).size(), 3, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x3)[0], 2, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x3)[1], 4, 1e-9);
EXPECT_DOUBLES_EQUAL(adjEdgesMap.at(x3).size(), 2, 1e-9);
// This includes the anchor
EXPECT_DOUBLES_EQUAL(adjEdgesMap.size(), 5, 1e-9);
}
/* *************************************************************************** */
TEST( InitializePose3, singleGradient ) {
Rot3 R1 = Rot3();
Matrix M = Matrix3::Zero();
M(0,1) = -1; M(1,0) = 1; M(2,2) = 1;
Rot3 R2 = Rot3(M);
double a = 6.010534238540223;
double b = 1.0;
Vector actual = InitializePose3::gradientTron(R1, R2, a, b);
Vector expected = Vector3::Zero();
expected(2) = 1.962658662803917;
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal(expected, actual, 1e-6));
}
/* *************************************************************************** */
TEST( InitializePose3, iterationGradient ) {
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
// Wrong initial guess - initialization should fix the rotations
Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01));
Values givenPoses;
givenPoses.insert(x0,simple::pose0);
givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) ));
givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) ));
givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) ));
size_t maxIter = 1; // test gradient at the first iteration
bool setRefFrame = false;
Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, maxIter, setRefFrame);
Matrix M0 = (Matrix(3,3) << 0.999435813876064, -0.033571481675497, 0.001004768630281,
0.033572116359134, 0.999436104312325, -0.000621610948719,
-0.000983333645009, 0.000654992453817, 0.999999302019670);
Rot3 R0Expected = Rot3(M0);
EXPECT(assert_equal(R0Expected, orientations.at<Rot3>(x0), 1e-5));
Matrix M1 = (Matrix(3,3) << 0.999905367545392, -0.010866391403031, 0.008436675399114,
0.010943459008004, 0.999898317528125, -0.009143047050380,
-0.008336465609239, 0.009234508232789, 0.999922610604863);
Rot3 R1Expected = Rot3(M1);
EXPECT(assert_equal(R1Expected, orientations.at<Rot3>(x1), 1e-5));
Matrix M2 = (Matrix(3,3) << 0.998936644682875, 0.045376417678595, -0.008158469732553,
-0.045306446926148, 0.998936408933058, 0.008566024448664,
0.008538487960253, -0.008187284445083, 0.999930028850403);
Rot3 R2Expected = Rot3(M2);
EXPECT(assert_equal(R2Expected, orientations.at<Rot3>(x2), 1e-5));
Matrix M3 = (Matrix(3,3) << 0.999898767273093, -0.010834701971459, 0.009223038487275,
0.010911315499947, 0.999906044037258, -0.008297366559388,
-0.009132272433995, 0.008397162077148, 0.999923041673329);
Rot3 R3Expected = Rot3(M3);
EXPECT(assert_equal(R3Expected, orientations.at<Rot3>(x3), 1e-5));
}
/* *************************************************************************** */
TEST( InitializePose3, orientationsGradient ) {
NonlinearFactorGraph pose3Graph = InitializePose3::buildPose3graph(simple::graph());
// Wrong initial guess - initialization should fix the rotations
Rot3 Rpert = Rot3::Expmap((Vector(3)<< 0.01, 0.01, 0.01));
Values givenPoses;
givenPoses.insert(x0,simple::pose0);
givenPoses.insert(x1,(simple::pose0).compose( Pose3(Rpert,Point3()) ));
givenPoses.insert(x2, (simple::pose0).compose( Pose3(Rpert.inverse(),Point3()) ));
givenPoses.insert(x3, (simple::pose0).compose( Pose3(Rpert,Point3()) ));
// do 10 gradient iterations
bool setRefFrame = false;
Values orientations = InitializePose3::computeOrientationsGradient(pose3Graph, givenPoses, 10, setRefFrame);
// const Key keyAnchor = symbol('Z', 9999999);
// givenPoses.insert(keyAnchor,simple::pose0);
// string g2oFile = "/home/aspn/Desktop/toyExample.g2o";
// writeG2o(pose3Graph, givenPoses, g2oFile);
const string matlabResultsfile = findExampleDataFile("simpleGraph10gradIter");
NonlinearFactorGraph::shared_ptr matlabGraph;
Values::shared_ptr matlabValues;
bool is3D = true;
boost::tie(matlabGraph, matlabValues) = readG2o(matlabResultsfile, is3D);
Rot3 R0Expected = matlabValues->at<Pose3>(1).rotation();
EXPECT(assert_equal(R0Expected, orientations.at<Rot3>(x0), 1e-4));
Rot3 R1Expected = matlabValues->at<Pose3>(2).rotation();
EXPECT(assert_equal(R1Expected, orientations.at<Rot3>(x1), 1e-4));
Rot3 R2Expected = matlabValues->at<Pose3>(3).rotation();
EXPECT(assert_equal(R2Expected, orientations.at<Rot3>(x2), 1e-3));
Rot3 R3Expected = matlabValues->at<Pose3>(4).rotation();
EXPECT(assert_equal(R3Expected, orientations.at<Rot3>(x3), 1e-4));
}
/* *************************************************************************** */
TEST( InitializePose3, posesWithGivenGuess ) {
Values givenPoses;
givenPoses.insert(x0,simple::pose0);
givenPoses.insert(x1,simple::pose1);
givenPoses.insert(x2,simple::pose2);
givenPoses.insert(x3,simple::pose3);
Values initial = InitializePose3::initialize(simple::graph(), givenPoses);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal(givenPoses, initial, 1e-6));
}
/* ************************************************************************* */
TEST( InitializePose3, initializePoses )
{
const string g2oFile = findExampleDataFile("pose3example-grid");
NonlinearFactorGraph::shared_ptr inputGraph;
Values::shared_ptr expectedValues;
bool is3D = true;
boost::tie(inputGraph, expectedValues) = readG2o(g2oFile, is3D);
noiseModel::Unit::shared_ptr priorModel = noiseModel::Unit::Create(6);
inputGraph->add(PriorFactor<Pose3>(0, Pose3(), priorModel));
Values initial = InitializePose3::initialize(*inputGraph);
EXPECT(assert_equal(*expectedValues,initial,1e-4));
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

56
gtsam/slam/tests/testLago.cpp
View File

@ -36,7 +36,7 @@ using namespace boost::assign;
static 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)); static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
namespace simple { namespace simpleLago {
// We consider a small graph: // We consider a small graph:
// symbolic FG // symbolic FG
// x2 0 1 // x2 0 1
@ -67,7 +67,7 @@ NonlinearFactorGraph graph() {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, checkSTandChords ) { TEST( Lago, checkSTandChords ) {
NonlinearFactorGraph g = simple::graph(); NonlinearFactorGraph g = simpleLago::graph();
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g); BetweenFactor<Pose2> >(g);
@ -84,7 +84,7 @@ TEST( Lago, checkSTandChords ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, orientationsOverSpanningTree ) { TEST( Lago, orientationsOverSpanningTree ) {
NonlinearFactorGraph g = simple::graph(); NonlinearFactorGraph g = simpleLago::graph();
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g); BetweenFactor<Pose2> >(g);
@ -115,7 +115,7 @@ TEST( Lago, orientationsOverSpanningTree ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, regularizedMeasurements ) { TEST( Lago, regularizedMeasurements ) {
NonlinearFactorGraph g = simple::graph(); NonlinearFactorGraph g = simpleLago::graph();
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g); BetweenFactor<Pose2> >(g);
@ -141,7 +141,7 @@ TEST( Lago, regularizedMeasurements ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, smallGraphVectorValues ) { TEST( Lago, smallGraphVectorValues ) {
bool useOdometricPath = false; bool useOdometricPath = false;
VectorValues initial = lago::initializeOrientations(simple::graph(), useOdometricPath); VectorValues initial = lago::initializeOrientations(simpleLago::graph(), useOdometricPath);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
@ -153,7 +153,7 @@ TEST( Lago, smallGraphVectorValues ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, smallGraphVectorValuesSP ) { TEST( Lago, smallGraphVectorValuesSP ) {
VectorValues initial = lago::initializeOrientations(simple::graph()); VectorValues initial = lago::initializeOrientations(simpleLago::graph());
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6)); EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
@ -165,8 +165,8 @@ TEST( Lago, smallGraphVectorValuesSP ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, multiplePosePriors ) { TEST( Lago, multiplePosePriors ) {
bool useOdometricPath = false; bool useOdometricPath = false;
NonlinearFactorGraph g = simple::graph(); NonlinearFactorGraph g = simpleLago::graph();
g.add(PriorFactor<Pose2>(x1, simple::pose1, model)); g.add(PriorFactor<Pose2>(x1, simpleLago::pose1, model));
VectorValues initial = lago::initializeOrientations(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 // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@ -178,8 +178,8 @@ TEST( Lago, multiplePosePriors ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, multiplePosePriorsSP ) { TEST( Lago, multiplePosePriorsSP ) {
NonlinearFactorGraph g = simple::graph(); NonlinearFactorGraph g = simpleLago::graph();
g.add(PriorFactor<Pose2>(x1, simple::pose1, model)); g.add(PriorFactor<Pose2>(x1, simpleLago::pose1, model));
VectorValues initial = lago::initializeOrientations(g); VectorValues initial = lago::initializeOrientations(g);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@ -192,8 +192,8 @@ TEST( Lago, multiplePosePriorsSP ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, multiplePoseAndRotPriors ) { TEST( Lago, multiplePoseAndRotPriors ) {
bool useOdometricPath = false; bool useOdometricPath = false;
NonlinearFactorGraph g = simple::graph(); NonlinearFactorGraph g = simpleLago::graph();
g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model)); g.add(PriorFactor<Rot2>(x1, simpleLago::pose1.theta(), model));
VectorValues initial = lago::initializeOrientations(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 // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@ -205,8 +205,8 @@ TEST( Lago, multiplePoseAndRotPriors ) {
/* *************************************************************************** */ /* *************************************************************************** */
TEST( Lago, multiplePoseAndRotPriorsSP ) { TEST( Lago, multiplePoseAndRotPriorsSP ) {
NonlinearFactorGraph g = simple::graph(); NonlinearFactorGraph g = simpleLago::graph();
g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model)); g.add(PriorFactor<Rot2>(x1, simpleLago::pose1.theta(), model));
VectorValues initial = lago::initializeOrientations(g); VectorValues initial = lago::initializeOrientations(g);
// comparison is up to M_PI, that's why we add some multiples of 2*M_PI // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
@ -221,20 +221,20 @@ TEST( Lago, smallGraphValues ) {
// we set the orientations in the initial guess to zero // we set the orientations in the initial guess to zero
Values initialGuess; Values initialGuess;
initialGuess.insert(x0,Pose2(simple::pose0.x(),simple::pose0.y(),0.0)); initialGuess.insert(x0,Pose2(simpleLago::pose0.x(),simpleLago::pose0.y(),0.0));
initialGuess.insert(x1,Pose2(simple::pose1.x(),simple::pose1.y(),0.0)); initialGuess.insert(x1,Pose2(simpleLago::pose1.x(),simpleLago::pose1.y(),0.0));
initialGuess.insert(x2,Pose2(simple::pose2.x(),simple::pose2.y(),0.0)); initialGuess.insert(x2,Pose2(simpleLago::pose2.x(),simpleLago::pose2.y(),0.0));
initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.y(),0.0)); initialGuess.insert(x3,Pose2(simpleLago::pose3.x(),simpleLago::pose3.y(),0.0));
// lago does not touch the Cartesian part and only fixed the orientations // lago does not touch the Cartesian part and only fixed the orientations
Values actual = lago::initialize(simple::graph(), initialGuess); Values actual = lago::initialize(simpleLago::graph(), initialGuess);
// we are in a noiseless case // we are in a noiseless case
Values expected; Values expected;
expected.insert(x0,simple::pose0); expected.insert(x0,simpleLago::pose0);
expected.insert(x1,simple::pose1); expected.insert(x1,simpleLago::pose1);
expected.insert(x2,simple::pose2); expected.insert(x2,simpleLago::pose2);
expected.insert(x3,simple::pose3); expected.insert(x3,simpleLago::pose3);
EXPECT(assert_equal(expected, actual, 1e-6)); EXPECT(assert_equal(expected, actual, 1e-6));
} }
@ -243,14 +243,14 @@ TEST( Lago, smallGraphValues ) {
TEST( Lago, smallGraph2 ) { TEST( Lago, smallGraph2 ) {
// lago does not touch the Cartesian part and only fixed the orientations // lago does not touch the Cartesian part and only fixed the orientations
Values actual = lago::initialize(simple::graph()); Values actual = lago::initialize(simpleLago::graph());
// we are in a noiseless case // we are in a noiseless case
Values expected; Values expected;
expected.insert(x0,simple::pose0); expected.insert(x0,simpleLago::pose0);
expected.insert(x1,simple::pose1); expected.insert(x1,simpleLago::pose1);
expected.insert(x2,simple::pose2); expected.insert(x2,simpleLago::pose2);
expected.insert(x3,simple::pose3); expected.insert(x3,simpleLago::pose3);
EXPECT(assert_equal(expected, actual, 1e-6)); EXPECT(assert_equal(expected, actual, 1e-6));
} }

26
gtsam/slam/tests/testPoseRotationPrior.cpp
View File

@ -35,6 +35,7 @@ const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap((Vector(3)
// Pose2 examples // Pose2 examples
const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0); const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);
const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2); const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2);
const Rot2 rot2C = Rot2::fromAngle(M_PI-0.01), rot2D = Rot2::fromAngle(M_PI+0.01);
/* ************************************************************************* */ /* ************************************************************************* */
LieVector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) { LieVector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) {
@ -62,10 +63,18 @@ TEST( testPoseRotationFactor, level3_error ) {
Pose3 pose1(rot3A, point3A); Pose3 pose1(rot3A, point3A);
Pose3RotationPrior factor(poseKey, rot3C, model3); Pose3RotationPrior factor(poseKey, rot3C, model3);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal((Vector(3) << -0.1,-0.2,-0.3), factor.evaluateError(pose1, actH1))); #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
EXPECT(assert_equal((Vector(3) << -0.1, -0.2,-0.3), factor.evaluateError(pose1, actH1)));
#else
EXPECT(assert_equal((Vector(3) << -0.1, -0.2, -0.3), factor.evaluateError(pose1, actH1),1e-2));
#endif
// the derivative is more complex, but is close to the identity for Rot3 around the origin
/*
Matrix expH1 = numericalDerivative11<LieVector,Pose3>( Matrix expH1 = numericalDerivative11<LieVector,Pose3>(
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5); boost::bind(evalFactorError3, factor, _1), pose1, 1e-2);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));*/
// If not using true expmap will be close, but not exact around the origin
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -90,6 +99,17 @@ TEST( testPoseRotationFactor, level2_error ) {
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
/* ************************************************************************* */
TEST( testPoseRotationFactor, level2_error_wrap ) {
Pose2 pose1(rot2C, point2A);
Pose2RotationPrior factor(poseKey, rot2D, model1);
Matrix actH1;
EXPECT(assert_equal((Vector(1) << -0.02), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>(
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */ /* ************************************************************************* */

2
gtsam/symbolic/tests/testSymbolicBayesTree.cpp
View File

@ -251,7 +251,7 @@ TEST( BayesTree, shortcutCheck )
// Check if all the cached shortcuts are cleared // Check if all the cached shortcuts are cleared
rootClique->deleteCachedShortcuts(); rootClique->deleteCachedShortcuts();
BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) { BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) {
bool notCleared = clique->cachedSeparatorMarginal(); bool notCleared = clique->cachedSeparatorMarginal().is_initialized();
CHECK( notCleared == false); CHECK( notCleared == false);
} }
EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals()); EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals());

138
gtsam_unstable/base/DSFMap.h
View File

@ -34,89 +34,89 @@ class DSFMap {
protected: protected:
/// We store the forest in an STL map, but parents are done with pointers /// We store the forest in an STL map, but parents are done with pointers
struct Entry { struct Entry {
typename std::map<KEY, Entry>::iterator parent_; typename std::map<KEY, Entry>::iterator parent_;
size_t rank_; size_t rank_;
Entry() {} Entry() {}
}; };
typedef typename std::map<KEY, Entry> Map; typedef typename std::map<KEY, Entry> Map;
typedef typename Map::iterator iterator; typedef typename Map::iterator iterator;
mutable Map entries_; mutable Map entries_;
/// Given key, find iterator to initial entry /// Given key, find iterator to initial entry
iterator find__(const KEY& key) const { iterator find__(const KEY& key) const {
static const Entry empty; static const Entry empty;
iterator it = entries_.find(key); iterator it = entries_.find(key);
// if key does not exist, create and return itself // if key does not exist, create and return itself
if (it == entries_.end()) { if (it == entries_.end()) {
it = entries_.insert(std::make_pair(key, empty)).first; it = entries_.insert(std::make_pair(key, empty)).first;
it->second.parent_ = it; it->second.parent_ = it;
it->second.rank_ = 0; it->second.rank_ = 0;
} }
return it; return it;
} }
/// Given iterator to initial entry, find the root Entry /// Given iterator to initial entry, find the root Entry
iterator find_(const iterator& it) const { iterator find_(const iterator& it) const {
// follow parent pointers until we reach set representative // follow parent pointers until we reach set representative
iterator& parent = it->second.parent_; iterator& parent = it->second.parent_;
if (parent != it) if (parent != it)
parent = find_(parent); // not yet, recurse! parent = find_(parent); // not yet, recurse!
return parent; return parent;
} }
/// Given key, find the root Entry /// Given key, find the root Entry
inline iterator find_(const KEY& key) const { inline iterator find_(const KEY& key) const {
iterator initial = find__(key); iterator initial = find__(key);
return find_(initial); return find_(initial);
} }
public: public:
typedef std::set<KEY> Set; typedef std::set<KEY> Set;
/// constructor /// constructor
DSFMap() { DSFMap() {
} }
/// Given key, find the representative key for the set in which it lives /// Given key, find the representative key for the set in which it lives
inline KEY find(const KEY& key) const { inline KEY find(const KEY& key) const {
iterator root = find_(key); iterator root = find_(key);
return root->first; return root->first;
} }
/// Merge two sets /// Merge two sets
void merge(const KEY& x, const KEY& y) { void merge(const KEY& x, const KEY& y) {
// straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure // straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure
iterator xRoot = find_(x); iterator xRoot = find_(x);
iterator yRoot = find_(y); iterator yRoot = find_(y);
if (xRoot == yRoot) if (xRoot == yRoot)
return; return;
// Merge sets // Merge sets
if (xRoot->second.rank_ < yRoot->second.rank_) if (xRoot->second.rank_ < yRoot->second.rank_)
xRoot->second.parent_ = yRoot; xRoot->second.parent_ = yRoot;
else if (xRoot->second.rank_ > yRoot->second.rank_) else if (xRoot->second.rank_ > yRoot->second.rank_)
yRoot->second.parent_ = xRoot; yRoot->second.parent_ = xRoot;
else { else {
yRoot->second.parent_ = xRoot; yRoot->second.parent_ = xRoot;
xRoot->second.rank_ = xRoot->second.rank_ + 1; xRoot->second.rank_ = xRoot->second.rank_ + 1;
} }
} }
/// return all sets, i.e. a partition of all elements /// return all sets, i.e. a partition of all elements
std::map<KEY, Set> sets() const { std::map<KEY, Set> sets() const {
std::map<KEY, Set> sets; std::map<KEY, Set> sets;
iterator it = entries_.begin(); iterator it = entries_.begin();
for (; it != entries_.end(); it++) { for (; it != entries_.end(); it++) {
iterator root = find_(it); iterator root = find_(it);
sets[root->first].insert(it->first); sets[root->first].insert(it->first);
} }
return sets; return sets;
} }
}; };

4
gtsam_unstable/discrete/tests/testLoopyBelief.cpp
View File

@ -1,8 +1,8 @@
/** /**
* @file testLoopyBelief.cpp * @file testLoopyBelief.cpp
* @brief * @brief
* @author Duy-Nguyen Ta * @author Duy-Nguyen Ta
* @date Oct 11, 2013 * @date Oct 11, 2013
*/ */
#include <gtsam/inference/VariableIndex.h> #include <gtsam/inference/VariableIndex.h>

155
gtsam_unstable/examples/ConcurrentCalibration.cpp Normal file
View File

@ -0,0 +1,155 @@
/* ----------------------------------------------------------------------------
* 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 ConcurrentCalibration.cpp
* @brief First step towards estimating monocular calibration in concurrent
* filter/smoother framework. To start with, just batch LM.
* @date June 11, 2014
* @author Chris Beall
*/
#include <gtsam/geometry/Pose3.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/ProjectionFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/dataset.h>
#include <string>
#include <fstream>
#include <iostream>
#include <boost/lexical_cast.hpp>
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(2,1);
string calibration_loc = findExampleDataFile("VO_calibration00s.txt");
string pose_loc = findExampleDataFile("VO_camera_poses00s.txt");
string factor_loc = findExampleDataFile("VO_stereo_factors00s.txt");
//read camera calibration info from file
// 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 >> u0 >> v0 >> b;
//create stereo camera calibration object
const Cal3_S2::shared_ptr K(new Cal3_S2(fx,fy,s,u0,v0));
const Cal3_S2::shared_ptr noisy_K(new Cal3_S2(fx*1.2,fy*1.2,s,u0-10,v0+10));
initial_estimate.insert(Symbol('K', 0), *noisy_K);
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 1e-5, 100, 100));
graph.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), *noisy_K, calNoise));
ifstream pose_file(pose_loc.c_str());
cout << "Reading camera poses" << endl;
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));
}
noiseModel::Isotropic::shared_ptr poseNoise = noiseModel::Isotropic::Sigma(6, 0.01);
graph.push_back(PriorFactor<Pose3>(Symbol('x', pose_id), Pose3(m), poseNoise));
// 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));
graph.push_back(GeneralSFMFactor2<Cal3_S2>(Point2(uL,v), model, Symbol('x', x), Symbol('l', l), Symbol('K', 0)));
//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));
//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;
LevenbergMarquardtParams params;
params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA;
params.verbosity = NonlinearOptimizerParams::ERROR;
//create Levenberg-Marquardt optimizer to optimize the factor graph
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate,params);
// Values result = optimizer.optimize();
string K_values_file = "K_values.txt";
ofstream stream_K(K_values_file.c_str());
double currentError;
stream_K << optimizer.iterations() << " " << optimizer.values().at<Cal3_S2>(Symbol('K',0)).vector().transpose() << endl;
// Iterative loop
do {
// Do next iteration
currentError = optimizer.error();
optimizer.iterate();
stream_K << optimizer.iterations() << " " << optimizer.values().at<Cal3_S2>(Symbol('K',0)).vector().transpose() << endl;
if(params.verbosity >= NonlinearOptimizerParams::ERROR) cout << "newError: " << optimizer.error() << endl;
} while(optimizer.iterations() < params.maxIterations &&
!checkConvergence(params.relativeErrorTol, params.absoluteErrorTol,
params.errorTol, currentError, optimizer.error(), params.verbosity));
Values result = optimizer.values();
cout << "Final result sample:" << endl;
Values pose_values = result.filter<Pose3>();
pose_values.print("Final camera poses:\n");
Values(result.filter<Cal3_S2>()).print("Final K\n");
noisy_K->print("Initial noisy K\n");
K->print("Initial correct K\n");
return 0;
}

47
gtsam_unstable/gtsam_unstable.h
View File

@ -24,6 +24,7 @@ virtual class gtsam::GaussianFactor;
virtual class gtsam::HessianFactor; virtual class gtsam::HessianFactor;
virtual class gtsam::JacobianFactor; virtual class gtsam::JacobianFactor;
virtual class gtsam::Cal3_S2; virtual class gtsam::Cal3_S2;
virtual class gtsam::Cal3DS2;
class gtsam::GaussianFactorGraph; class gtsam::GaussianFactorGraph;
class gtsam::NonlinearFactorGraph; class gtsam::NonlinearFactorGraph;
class gtsam::Ordering; class gtsam::Ordering;
@ -358,6 +359,11 @@ virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor {
Vector calcIndicatorProb(const gtsam::Values& x); Vector calcIndicatorProb(const gtsam::Values& x);
void setValAValB(const gtsam::Values valA, const gtsam::Values valB); void setValAValB(const gtsam::Values valA, const gtsam::Values valB);
void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph);
void updateNoiseModels_givenCovs(const gtsam::Values& values, Matrix cov1, Matrix cov2, Matrix cov12);
Matrix get_model_inlier_cov();
Matrix get_model_outlier_cov();
void serializable() const; // enabling serialization functionality void serializable() const; // enabling serialization functionality
}; };
@ -745,4 +751,45 @@ virtual class OdometryFactorBase : gtsam::NoiseModelFactor {
void print(string s) const; void print(string s) const;
}; };
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam_unstable/slam/ProjectionFactorPPP.h>
template<POSE, LANDMARK, CALIBRATION>
virtual class ProjectionFactorPPP : gtsam::NoiseModelFactor {
ProjectionFactorPPP(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
size_t poseKey, size_t transformKey, size_t pointKey, const CALIBRATION* k);
ProjectionFactorPPP(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
size_t poseKey, size_t transformKey, size_t pointKey, const CALIBRATION* k, bool throwCheirality, bool verboseCheirality);
gtsam::Point2 measured() const;
CALIBRATION* calibration() const;
bool verboseCheirality() const;
bool throwCheirality() const;
// enabling serialization functionality
void serialize() const;
};
typedef gtsam::ProjectionFactorPPP<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> ProjectionFactorPPPCal3_S2;
typedef gtsam::ProjectionFactorPPP<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> ProjectionFactorPPPCal3DS2;
#include <gtsam_unstable/slam/ProjectionFactorPPPC.h>
template<POSE, LANDMARK, CALIBRATION>
virtual class ProjectionFactorPPPC : gtsam::NoiseModelFactor {
ProjectionFactorPPPC(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
size_t poseKey, size_t transformKey, size_t pointKey, size_t calibKey);
ProjectionFactorPPPC(const gtsam::Point2& measured, const gtsam::noiseModel::Base* noiseModel,
size_t poseKey, size_t transformKey, size_t pointKey, size_t calibKey, bool throwCheirality, bool verboseCheirality);
gtsam::Point2 measured() const;
bool verboseCheirality() const;
bool throwCheirality() const;
// enabling serialization functionality
void serialize() const;
};
typedef gtsam::ProjectionFactorPPPC<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> ProjectionFactorPPPCCal3_S2;
typedef gtsam::ProjectionFactorPPPC<gtsam::Pose3, gtsam::Point3, gtsam::Cal3DS2> ProjectionFactorPPPCCal3DS2;
} //\namespace gtsam } //\namespace gtsam

792
gtsam_unstable/partition/GenericGraph.cpp
View File

@ -19,459 +19,459 @@ using namespace std;
namespace gtsam { namespace partition { namespace gtsam { namespace partition {
/** /**
* Note: Need to be able to handle a graph with factors that involve variables not in the given {keys} * Note: Need to be able to handle a graph with factors that involve variables not in the given {keys}
*/ */
list<vector<size_t> > findIslands(const GenericGraph2D& graph, const vector<size_t>& keys, WorkSpace& workspace, list<vector<size_t> > findIslands(const GenericGraph2D& graph, const vector<size_t>& keys, WorkSpace& workspace,
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
{ {
typedef pair<int, int> IntPair; typedef pair<int, int> IntPair;
typedef list<sharedGenericFactor2D> FactorList; typedef list<sharedGenericFactor2D> FactorList;
typedef map<IntPair, FactorList::iterator> Connections; typedef map<IntPair, FactorList::iterator> Connections;
// create disjoin set forest // create disjoin set forest
DSFVector dsf(workspace.dsf, keys); DSFVector dsf(workspace.dsf, keys);
FactorList factors(graph.begin(), graph.end()); FactorList factors(graph.begin(), graph.end());
size_t nrFactors = factors.size(); size_t nrFactors = factors.size();
FactorList::iterator itEnd; FactorList::iterator itEnd;
workspace.prepareDictionary(keys); workspace.prepareDictionary(keys);
while (nrFactors) { while (nrFactors) {
Connections connections; Connections connections;
bool succeed = false; bool succeed = false;
itEnd = factors.end(); itEnd = factors.end();
list<FactorList::iterator> toErase; list<FactorList::iterator> toErase;
for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) { for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {
// remove invalid factors // remove invalid factors
GenericNode2D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2; GenericNode2D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) { if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
toErase.push_back(itFactor); nrFactors--; continue; toErase.push_back(itFactor); nrFactors--; continue;
} }
size_t label1 = dsf.findSet(key1.index); size_t label1 = dsf.findSet(key1.index);
size_t label2 = dsf.findSet(key2.index); size_t label2 = dsf.findSet(key2.index);
if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; } if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; }
// merge two trees if the connection is strong enough, otherwise cache it // merge two trees if the connection is strong enough, otherwise cache it
// an odometry factor always merges two islands // an odometry factor always merges two islands
if (key1.type == NODE_POSE_2D && key2.type == NODE_POSE_2D) { if (key1.type == NODE_POSE_2D && key2.type == NODE_POSE_2D) {
toErase.push_back(itFactor); nrFactors--; toErase.push_back(itFactor); nrFactors--;
dsf.makeUnionInPlace(label1, label2); dsf.makeUnionInPlace(label1, label2);
succeed = true; succeed = true;
break; break;
} }
// single landmark island only need one measurement // single landmark island only need one measurement
if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) || if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) ||
(dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) { (dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) {
toErase.push_back(itFactor); nrFactors--; toErase.push_back(itFactor); nrFactors--;
dsf.makeUnionInPlace(label1, label2); dsf.makeUnionInPlace(label1, label2);
succeed = true; succeed = true;
break; break;
} }
// stack the current factor with the cached constraint // stack the current factor with the cached constraint
IntPair labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1); IntPair labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1);
Connections::iterator itCached = connections.find(labels); Connections::iterator itCached = connections.find(labels);
if (itCached == connections.end()) { if (itCached == connections.end()) {
connections.insert(make_pair(labels, itFactor)); connections.insert(make_pair(labels, itFactor));
continue; continue;
} else { } else {
GenericNode2D key21 = (*itCached->second)->key1, key22 = (*itCached->second)->key2; GenericNode2D key21 = (*itCached->second)->key1, key22 = (*itCached->second)->key2;
// if observe the same landmark, we can not merge, abandon the current factor // if observe the same landmark, we can not merge, abandon the current factor
if ((key1.index == key21.index && key1.type == NODE_LANDMARK_2D) || if ((key1.index == key21.index && key1.type == NODE_LANDMARK_2D) ||
(key1.index == key22.index && key1.type == NODE_LANDMARK_2D) || (key1.index == key22.index && key1.type == NODE_LANDMARK_2D) ||
(key2.index == key21.index && key2.type == NODE_LANDMARK_2D) || (key2.index == key21.index && key2.type == NODE_LANDMARK_2D) ||
(key2.index == key22.index && key2.type == NODE_LANDMARK_2D)) { (key2.index == key22.index && key2.type == NODE_LANDMARK_2D)) {
toErase.push_back(itFactor); nrFactors--; toErase.push_back(itFactor); nrFactors--;
continue; continue;
} else { } else {
toErase.push_back(itFactor); nrFactors--; toErase.push_back(itFactor); nrFactors--;
toErase.push_back(itCached->second); nrFactors--; toErase.push_back(itCached->second); nrFactors--;
dsf.makeUnionInPlace(label1, label2); dsf.makeUnionInPlace(label1, label2);
connections.erase(itCached); connections.erase(itCached);
succeed = true; succeed = true;
break; break;
} }
} }
} }
// erase unused factors // erase unused factors
BOOST_FOREACH(const FactorList::iterator& it, toErase) BOOST_FOREACH(const FactorList::iterator& it, toErase)
factors.erase(it); factors.erase(it);
if (!succeed) break; if (!succeed) break;
} }
list<vector<size_t> > islands; list<vector<size_t> > islands;
map<size_t, vector<size_t> > arrays = dsf.arrays(); map<size_t, vector<size_t> > arrays = dsf.arrays();
size_t key; vector<size_t> array; size_t key; vector<size_t> array;
BOOST_FOREACH(boost::tie(key, array), arrays) BOOST_FOREACH(boost::tie(key, array), arrays)
islands.push_back(array); islands.push_back(array);
return islands; return islands;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void print(const GenericGraph2D& graph, const std::string name) { void print(const GenericGraph2D& graph, const std::string name) {
cout << name << endl; cout << name << endl;
BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph) BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph)
cout << factor_->key1.index << " " << factor_->key2.index << endl; cout << factor_->key1.index << " " << factor_->key2.index << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void print(const GenericGraph3D& graph, const std::string name) { void print(const GenericGraph3D& graph, const std::string name) {
cout << name << endl; cout << name << endl;
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph)
cout << factor_->key1.index << " " << factor_->key2.index << " (" << cout << factor_->key1.index << " " << factor_->key2.index << " (" <<
factor_->key1.type << ", " << factor_->key2.type <<")" << endl; factor_->key1.type << ", " << factor_->key2.type <<")" << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// create disjoin set forest // create disjoin set forest
DSFVector createDSF(const GenericGraph3D& graph, const vector<size_t>& keys, const WorkSpace& workspace) { DSFVector createDSF(const GenericGraph3D& graph, const vector<size_t>& keys, const WorkSpace& workspace) {
DSFVector dsf(workspace.dsf, keys); DSFVector dsf(workspace.dsf, keys);
typedef list<sharedGenericFactor3D> FactorList; typedef list<sharedGenericFactor3D> FactorList;
FactorList factors(graph.begin(), graph.end()); FactorList factors(graph.begin(), graph.end());
size_t nrFactors = factors.size(); size_t nrFactors = factors.size();
FactorList::iterator itEnd; FactorList::iterator itEnd;
while (nrFactors) { while (nrFactors) {
bool succeed = false; bool succeed = false;
itEnd = factors.end(); itEnd = factors.end();
list<FactorList::iterator> toErase; list<FactorList::iterator> toErase;
for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) { for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {
// remove invalid factors // remove invalid factors
if (graph.size() == 178765) cout << "kai21" << endl; if (graph.size() == 178765) cout << "kai21" << endl;
GenericNode3D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2; GenericNode3D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
if (graph.size() == 178765) cout << "kai21: " << key1.index << " " << key2.index << endl; if (graph.size() == 178765) cout << "kai21: " << key1.index << " " << key2.index << endl;
if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) { if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
toErase.push_back(itFactor); nrFactors--; continue; toErase.push_back(itFactor); nrFactors--; continue;
} }
if (graph.size() == 178765) cout << "kai22" << endl; if (graph.size() == 178765) cout << "kai22" << endl;
size_t label1 = dsf.findSet(key1.index); size_t label1 = dsf.findSet(key1.index);
size_t label2 = dsf.findSet(key2.index); size_t label2 = dsf.findSet(key2.index);
if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; } if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; }
if (graph.size() == 178765) cout << "kai23" << endl; if (graph.size() == 178765) cout << "kai23" << endl;
// merge two trees if the connection is strong enough, otherwise cache it // merge two trees if the connection is strong enough, otherwise cache it
// an odometry factor always merges two islands // an odometry factor always merges two islands
if ((key1.type == NODE_POSE_3D && key2.type == NODE_LANDMARK_3D) || if ((key1.type == NODE_POSE_3D && key2.type == NODE_LANDMARK_3D) ||
(key1.type == NODE_POSE_3D && key2.type == NODE_POSE_3D)) { (key1.type == NODE_POSE_3D && key2.type == NODE_POSE_3D)) {
toErase.push_back(itFactor); nrFactors--; toErase.push_back(itFactor); nrFactors--;
dsf.makeUnionInPlace(label1, label2); dsf.makeUnionInPlace(label1, label2);
succeed = true; succeed = true;
break; break;
} }
if (graph.size() == 178765) cout << "kai24" << endl; if (graph.size() == 178765) cout << "kai24" << endl;
} }
// erase unused factors // erase unused factors
BOOST_FOREACH(const FactorList::iterator& it, toErase) BOOST_FOREACH(const FactorList::iterator& it, toErase)
factors.erase(it); factors.erase(it);
if (!succeed) break; if (!succeed) break;
} }
return dsf; return dsf;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// first check the type of the key (pose or landmark), and then check whether it is singular // first check the type of the key (pose or landmark), and then check whether it is singular
inline bool isSingular(const set<size_t>& singularCameras, const set<size_t>& singularLandmarks, const GenericNode3D& node) { inline bool isSingular(const set<size_t>& singularCameras, const set<size_t>& singularLandmarks, const GenericNode3D& node) {
switch(node.type) { switch(node.type) {
case NODE_POSE_3D: case NODE_POSE_3D:
return singularCameras.find(node.index) != singularCameras.end(); break; return singularCameras.find(node.index) != singularCameras.end(); break;
case NODE_LANDMARK_3D: case NODE_LANDMARK_3D:
return singularLandmarks.find(node.index) != singularLandmarks.end(); break; return singularLandmarks.find(node.index) != singularLandmarks.end(); break;
default: default:
throw runtime_error("unrecognized key type!"); throw runtime_error("unrecognized key type!");
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
void findSingularCamerasLandmarks(const GenericGraph3D& graph, const WorkSpace& workspace, void findSingularCamerasLandmarks(const GenericGraph3D& graph, const WorkSpace& workspace,
const vector<bool>& isCamera, const vector<bool>& isLandmark, const vector<bool>& isCamera, const vector<bool>& isLandmark,
set<size_t>& singularCameras, set<size_t>& singularLandmarks, vector<int>& nrConstraints, set<size_t>& singularCameras, set<size_t>& singularLandmarks, vector<int>& nrConstraints,
bool& foundSingularCamera, bool& foundSingularLandmark, bool& foundSingularCamera, bool& foundSingularLandmark,
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {
// compute the constraint number per camera // compute the constraint number per camera
std::fill(nrConstraints.begin(), nrConstraints.end(), 0); std::fill(nrConstraints.begin(), nrConstraints.end(), 0);
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
const int& key1 = factor_->key1.index; const int& key1 = factor_->key1.index;
const int& key2 = factor_->key2.index; const int& key2 = factor_->key2.index;
if (workspace.dictionary[key1] != -1 && workspace.dictionary[key2] != -1 && if (workspace.dictionary[key1] != -1 && workspace.dictionary[key2] != -1 &&
!isSingular(singularCameras, singularLandmarks, factor_->key1) && !isSingular(singularCameras, singularLandmarks, factor_->key1) &&
!isSingular(singularCameras, singularLandmarks, factor_->key2)) { !isSingular(singularCameras, singularLandmarks, factor_->key2)) {
nrConstraints[key1]++; nrConstraints[key1]++;
nrConstraints[key2]++; nrConstraints[key2]++;
// a single pose constraint is sufficient for stereo, so we add 2 to the counter // a single pose constraint is sufficient for stereo, so we add 2 to the counter
// for a total of 3, i.e. the same as 3 landmarks fully constraining the camera // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera
if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){ if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
nrConstraints[key1]+=2; nrConstraints[key1]+=2;
nrConstraints[key2]+=2; nrConstraints[key2]+=2;
} }
} }
} }
// find singular cameras and landmarks // find singular cameras and landmarks
foundSingularCamera = false; foundSingularCamera = false;
foundSingularLandmark = false; foundSingularLandmark = false;
for (size_t i=0; i<nrConstraints.size(); i++) { for (size_t i=0; i<nrConstraints.size(); i++) {
if (isCamera[i] && nrConstraints[i] < minNrConstraintsPerCamera && if (isCamera[i] && nrConstraints[i] < minNrConstraintsPerCamera &&
singularCameras.find(i) == singularCameras.end()) { singularCameras.find(i) == singularCameras.end()) {
singularCameras.insert(i); singularCameras.insert(i);
foundSingularCamera = true; foundSingularCamera = true;
} }
if (isLandmark[i] && nrConstraints[i] < minNrConstraintsPerLandmark && if (isLandmark[i] && nrConstraints[i] < minNrConstraintsPerLandmark &&
singularLandmarks.find(i) == singularLandmarks.end()) { singularLandmarks.find(i) == singularLandmarks.end()) {
singularLandmarks.insert(i); singularLandmarks.insert(i);
foundSingularLandmark = true; foundSingularLandmark = true;
} }
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
list<vector<size_t> > findIslands(const GenericGraph3D& graph, const vector<size_t>& keys, WorkSpace& workspace, list<vector<size_t> > findIslands(const GenericGraph3D& graph, const vector<size_t>& keys, WorkSpace& workspace,
const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) { const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
// create disjoint set forest // create disjoint set forest
workspace.prepareDictionary(keys); workspace.prepareDictionary(keys);
DSFVector dsf = createDSF(graph, keys, workspace); DSFVector dsf = createDSF(graph, keys, workspace);
const bool verbose = false; const bool verbose = false;
bool foundSingularCamera = true; bool foundSingularCamera = true;
bool foundSingularLandmark = true; bool foundSingularLandmark = true;
list<vector<size_t> > islands; list<vector<size_t> > islands;
set<size_t> singularCameras, singularLandmarks; set<size_t> singularCameras, singularLandmarks;
vector<bool> isCamera(workspace.dictionary.size(), false); vector<bool> isCamera(workspace.dictionary.size(), false);
vector<bool> isLandmark(workspace.dictionary.size(), false); vector<bool> isLandmark(workspace.dictionary.size(), false);
// check the constraint number of every variable // check the constraint number of every variable
// find the camera and landmark keys // find the camera and landmark keys
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
//assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM //assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM
if (workspace.dictionary[factor_->key1.index] != -1) { if (workspace.dictionary[factor_->key1.index] != -1) {
if (factor_->key1.type == NODE_POSE_3D) if (factor_->key1.type == NODE_POSE_3D)
isCamera[factor_->key1.index] = true; isCamera[factor_->key1.index] = true;
else else
isLandmark[factor_->key1.index] = true; isLandmark[factor_->key1.index] = true;
} }
if (workspace.dictionary[factor_->key2.index] != -1) { if (workspace.dictionary[factor_->key2.index] != -1) {
if (factor_->key2.type == NODE_POSE_3D) if (factor_->key2.type == NODE_POSE_3D)
isCamera[factor_->key2.index] = true; isCamera[factor_->key2.index] = true;
else else
isLandmark[factor_->key2.index] = true; isLandmark[factor_->key2.index] = true;
} }
} }
vector<int> nrConstraints(workspace.dictionary.size(), 0); vector<int> nrConstraints(workspace.dictionary.size(), 0);
// iterate until all singular variables have been removed. Removing a singular variable // iterate until all singular variables have been removed. Removing a singular variable
// can cause another to become singular, so this will probably run several times // can cause another to become singular, so this will probably run several times
while (foundSingularCamera || foundSingularLandmark) { while (foundSingularCamera || foundSingularLandmark) {
findSingularCamerasLandmarks(graph, workspace, isCamera, isLandmark, // input findSingularCamerasLandmarks(graph, workspace, isCamera, isLandmark, // input
singularCameras, singularLandmarks, nrConstraints, // output singularCameras, singularLandmarks, nrConstraints, // output
foundSingularCamera, foundSingularLandmark, // output foundSingularCamera, foundSingularLandmark, // output
minNrConstraintsPerCamera, minNrConstraintsPerLandmark); // input minNrConstraintsPerCamera, minNrConstraintsPerLandmark); // input
} }
// add singular variables directly as islands // add singular variables directly as islands
if (!singularCameras.empty()) { if (!singularCameras.empty()) {
if (verbose) cout << "singular cameras:"; if (verbose) cout << "singular cameras:";
BOOST_FOREACH(const size_t i, singularCameras) { BOOST_FOREACH(const size_t i, singularCameras) {
islands.push_back(vector<size_t>(1, i)); // <--------------------------- islands.push_back(vector<size_t>(1, i)); // <---------------------------
if (verbose) cout << i << " "; if (verbose) cout << i << " ";
} }
if (verbose) cout << endl; if (verbose) cout << endl;
} }
if (!singularLandmarks.empty()) { if (!singularLandmarks.empty()) {
if (verbose) cout << "singular landmarks:"; if (verbose) cout << "singular landmarks:";
BOOST_FOREACH(const size_t i, singularLandmarks) { BOOST_FOREACH(const size_t i, singularLandmarks) {
islands.push_back(vector<size_t>(1, i)); // <--------------------------- islands.push_back(vector<size_t>(1, i)); // <---------------------------
if (verbose) cout << i << " "; if (verbose) cout << i << " ";
} }
if (verbose) cout << endl; if (verbose) cout << endl;
} }
// regenerating islands // regenerating islands
map<size_t, vector<size_t> > labelIslands = dsf.arrays(); map<size_t, vector<size_t> > labelIslands = dsf.arrays();
size_t label; vector<size_t> island; size_t label; vector<size_t> island;
BOOST_FOREACH(boost::tie(label, island), labelIslands) { BOOST_FOREACH(boost::tie(label, island), labelIslands) {
vector<size_t> filteredIsland; // remove singular cameras from array vector<size_t> filteredIsland; // remove singular cameras from array
filteredIsland.reserve(island.size()); filteredIsland.reserve(island.size());
BOOST_FOREACH(const size_t key, island) { BOOST_FOREACH(const size_t key, island) {
if ((isCamera[key] && singularCameras.find(key) == singularCameras.end()) || // not singular if ((isCamera[key] && singularCameras.find(key) == singularCameras.end()) || // not singular
(isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) || // not singular (isLandmark[key] && singularLandmarks.find(key) == singularLandmarks.end()) || // not singular
(!isCamera[key] && !isLandmark[key])) { // the key is not involved in any factor, so the type is undertermined (!isCamera[key] && !isLandmark[key])) { // the key is not involved in any factor, so the type is undertermined
filteredIsland.push_back(key); filteredIsland.push_back(key);
} }
} }
islands.push_back(filteredIsland); islands.push_back(filteredIsland);
} }
// sanity check // sanity check
size_t nrKeys = 0; size_t nrKeys = 0;
BOOST_FOREACH(const vector<size_t>& island, islands) BOOST_FOREACH(const vector<size_t>& island, islands)
nrKeys += island.size(); nrKeys += island.size();
if (nrKeys != keys.size()) { if (nrKeys != keys.size()) {
cout << nrKeys << " vs " << keys.size() << endl; cout << nrKeys << " vs " << keys.size() << endl;
throw runtime_error("findIslands: the number of keys is inconsistent!"); throw runtime_error("findIslands: the number of keys is inconsistent!");
} }
if (verbose) cout << "found " << islands.size() << " islands!" << endl; if (verbose) cout << "found " << islands.size() << " islands!" << endl;
return islands; return islands;
} }
/* ************************************************************************* */ /* ************************************************************************* */
// return the number of intersection between two **sorted** landmark vectors // return the number of intersection between two **sorted** landmark vectors
inline int getNrCommonLandmarks(const vector<size_t>& landmarks1, const vector<size_t>& landmarks2){ inline int getNrCommonLandmarks(const vector<size_t>& landmarks1, const vector<size_t>& landmarks2){
size_t i1 = 0, i2 = 0; size_t i1 = 0, i2 = 0;
int nrCommonLandmarks = 0; int nrCommonLandmarks = 0;
while (i1 < landmarks1.size() && i2 < landmarks2.size()) { while (i1 < landmarks1.size() && i2 < landmarks2.size()) {
if (landmarks1[i1] < landmarks2[i2]) if (landmarks1[i1] < landmarks2[i2])
i1 ++; i1 ++;
else if (landmarks1[i1] > landmarks2[i2]) else if (landmarks1[i1] > landmarks2[i2])
i2 ++; i2 ++;
else { else {
i1++; i2++; i1++; i2++;
nrCommonLandmarks ++; nrCommonLandmarks ++;
} }
} }
return nrCommonLandmarks; return nrCommonLandmarks;
} }
/* ************************************************************************* */ /* ************************************************************************* */
void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys, void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
const std::vector<int>& dictionary, GenericGraph3D& reducedGraph) { const std::vector<int>& dictionary, GenericGraph3D& reducedGraph) {
typedef size_t CameraKey; typedef size_t CameraKey;
typedef pair<CameraKey, CameraKey> CameraPair; typedef pair<CameraKey, CameraKey> CameraPair;
typedef size_t LandmarkKey; typedef size_t LandmarkKey;
// get a mapping from each landmark to its connected cameras // get a mapping from each landmark to its connected cameras
vector<vector<LandmarkKey> > cameraToLandmarks(dictionary.size()); vector<vector<LandmarkKey> > cameraToLandmarks(dictionary.size());
// for odometry xi-xj where i<j, we always store cameraToCamera[i] = j, otherwise equal to -1 if no odometry // for odometry xi-xj where i<j, we always store cameraToCamera[i] = j, otherwise equal to -1 if no odometry
vector<int> cameraToCamera(dictionary.size(), -1); vector<int> cameraToCamera(dictionary.size(), -1);
size_t key_i, key_j; size_t key_i, key_j;
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
if (factor_->key1.type == NODE_POSE_3D) { if (factor_->key1.type == NODE_POSE_3D) {
if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor
cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index); cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index);
} }
else { // odometry factor else { // odometry factor
if (factor_->key1.index < factor_->key2.index) { if (factor_->key1.index < factor_->key2.index) {
key_i = factor_->key1.index; key_i = factor_->key1.index;
key_j = factor_->key2.index; key_j = factor_->key2.index;
} else { } else {
key_i = factor_->key2.index; key_i = factor_->key2.index;
key_j = factor_->key1.index; key_j = factor_->key1.index;
} }
cameraToCamera[key_i] = key_j; cameraToCamera[key_i] = key_j;
} }
} }
} }
// sort the landmark keys for the late getNrCommonLandmarks call // sort the landmark keys for the late getNrCommonLandmarks call
BOOST_FOREACH(vector<LandmarkKey> &landmarks, cameraToLandmarks){ BOOST_FOREACH(vector<LandmarkKey> &landmarks, cameraToLandmarks){
if (!landmarks.empty()) if (!landmarks.empty())
std::sort(landmarks.begin(), landmarks.end()); std::sort(landmarks.begin(), landmarks.end());
} }
// generate the reduced graph // generate the reduced graph
reducedGraph.clear(); reducedGraph.clear();
int factorIndex = 0; int factorIndex = 0;
int camera1, camera2, nrTotalConstraints; int camera1, camera2, nrTotalConstraints;
bool hasOdometry; bool hasOdometry;
for (size_t i1=0; i1<cameraKeys.size()-1; ++i1) { for (size_t i1=0; i1<cameraKeys.size()-1; ++i1) {
for (size_t i2=i1+1; i2<cameraKeys.size(); ++i2) { for (size_t i2=i1+1; i2<cameraKeys.size(); ++i2) {
camera1 = cameraKeys[i1]; camera1 = cameraKeys[i1];
camera2 = cameraKeys[i2]; camera2 = cameraKeys[i2];
int nrCommonLandmarks = getNrCommonLandmarks(cameraToLandmarks[camera1], cameraToLandmarks[camera2]); int nrCommonLandmarks = getNrCommonLandmarks(cameraToLandmarks[camera1], cameraToLandmarks[camera2]);
hasOdometry = cameraToCamera[camera1] == camera2; hasOdometry = cameraToCamera[camera1] == camera2;
if (nrCommonLandmarks > 0 || hasOdometry) { if (nrCommonLandmarks > 0 || hasOdometry) {
nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0); nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0);
reducedGraph.push_back(boost::make_shared<GenericFactor3D>(camera1, camera2, reducedGraph.push_back(boost::make_shared<GenericFactor3D>(camera1, camera2,
factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints)); factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints));
} }
} }
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals, void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) { WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
workspace.prepareDictionary(frontals); workspace.prepareDictionary(frontals);
vector<size_t> nrConstraints(workspace.dictionary.size(), 0); vector<size_t> nrConstraints(workspace.dictionary.size(), 0);
// summarize the constraint number // summarize the constraint number
const vector<int>& dictionary = workspace.dictionary; const vector<int>& dictionary = workspace.dictionary;
vector<bool> isValidCamera(workspace.dictionary.size(), false); vector<bool> isValidCamera(workspace.dictionary.size(), false);
vector<bool> isValidLandmark(workspace.dictionary.size(), false); vector<bool> isValidLandmark(workspace.dictionary.size(), false);
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) { BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
assert(factor_->key1.type == NODE_POSE_3D); assert(factor_->key1.type == NODE_POSE_3D);
//assert(factor_->key2.type == NODE_LANDMARK_3D); //assert(factor_->key2.type == NODE_LANDMARK_3D);
const size_t& key1 = factor_->key1.index; const size_t& key1 = factor_->key1.index;
const size_t& key2 = factor_->key2.index; const size_t& key2 = factor_->key2.index;
if (dictionary[key1] == -1 || dictionary[key2] == -1) if (dictionary[key1] == -1 || dictionary[key2] == -1)
continue; continue;
isValidCamera[key1] = true; isValidCamera[key1] = true;
if(factor_->key2.type == NODE_LANDMARK_3D) if(factor_->key2.type == NODE_LANDMARK_3D)
isValidLandmark[key2] = true; isValidLandmark[key2] = true;
else else
isValidCamera[key2] = true; isValidCamera[key2] = true;
nrConstraints[key1]++; nrConstraints[key1]++;
nrConstraints[key2]++; nrConstraints[key2]++;
// a single pose constraint is sufficient for stereo, so we add 2 to the counter // a single pose constraint is sufficient for stereo, so we add 2 to the counter
// for a total of 3, i.e. the same as 3 landmarks fully constraining the camera // for a total of 3, i.e. the same as 3 landmarks fully constraining the camera
if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){ if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
nrConstraints[key1]+=2; nrConstraints[key1]+=2;
nrConstraints[key2]+=2; nrConstraints[key2]+=2;
} }
} }
// find the minimum constraint for cameras and landmarks // find the minimum constraint for cameras and landmarks
size_t minFoundConstraintsPerCamera = 10000; size_t minFoundConstraintsPerCamera = 10000;
size_t minFoundConstraintsPerLandmark = 10000; size_t minFoundConstraintsPerLandmark = 10000;
for (size_t i=0; i<isValidCamera.size(); i++) { for (size_t i=0; i<isValidCamera.size(); i++) {
if (isValidCamera[i]) { if (isValidCamera[i]) {
minFoundConstraintsPerCamera = std::min(nrConstraints[i], minFoundConstraintsPerCamera); minFoundConstraintsPerCamera = std::min(nrConstraints[i], minFoundConstraintsPerCamera);
if (nrConstraints[i] < minNrConstraintsPerCamera) if (nrConstraints[i] < minNrConstraintsPerCamera)
cout << "!!!!!!!!!!!!!!!!!!! camera with " << nrConstraints[i] << " constraint: " << i << endl; cout << "!!!!!!!!!!!!!!!!!!! camera with " << nrConstraints[i] << " constraint: " << i << endl;
} }
} }
for (size_t j=0; j<isValidLandmark.size(); j++) { for (size_t j=0; j<isValidLandmark.size(); j++) {
if (isValidLandmark[j]) { if (isValidLandmark[j]) {
minFoundConstraintsPerLandmark = std::min(nrConstraints[j], minFoundConstraintsPerLandmark); minFoundConstraintsPerLandmark = std::min(nrConstraints[j], minFoundConstraintsPerLandmark);
if (nrConstraints[j] < minNrConstraintsPerLandmark) if (nrConstraints[j] < minNrConstraintsPerLandmark)
cout << "!!!!!!!!!!!!!!!!!!! landmark with " << nrConstraints[j] << " constraint: " << j << endl; cout << "!!!!!!!!!!!!!!!!!!! landmark with " << nrConstraints[j] << " constraint: " << j << endl;
} }
} }
// debug info // debug info
BOOST_FOREACH(const size_t key, frontals) { BOOST_FOREACH(const size_t key, frontals) {
if (isValidCamera[key] && nrConstraints[key] < minNrConstraintsPerCamera) if (isValidCamera[key] && nrConstraints[key] < minNrConstraintsPerCamera)
cout << "singular camera:" << key << " with " << nrConstraints[key] << " constraints" << endl; cout << "singular camera:" << key << " with " << nrConstraints[key] << " constraints" << endl;
} }
if (minFoundConstraintsPerCamera < minNrConstraintsPerCamera) if (minFoundConstraintsPerCamera < minNrConstraintsPerCamera)
throw runtime_error("checkSingularity:minConstraintsPerCamera < " + boost::lexical_cast<string>(minFoundConstraintsPerCamera)); throw runtime_error("checkSingularity:minConstraintsPerCamera < " + boost::lexical_cast<string>(minFoundConstraintsPerCamera));
if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark) if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark)
throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + boost::lexical_cast<string>(minFoundConstraintsPerLandmark)); throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + boost::lexical_cast<string>(minFoundConstraintsPerLandmark));
} }
}} // namespace }} // namespace

216
gtsam_unstable/partition/GenericGraph.h
View File

@ -17,133 +17,133 @@
namespace gtsam { namespace partition { namespace gtsam { namespace partition {
/*************************************************** /***************************************************
* 2D generic factors and their factor graph * 2D generic factors and their factor graph
***************************************************/ ***************************************************/
enum GenericNode2DType { NODE_POSE_2D, NODE_LANDMARK_2D }; enum GenericNode2DType { NODE_POSE_2D, NODE_LANDMARK_2D };
/** the index of the node and the type of the node */ /** the index of the node and the type of the node */
struct GenericNode2D { struct GenericNode2D {
std::size_t index; std::size_t index;
GenericNode2DType type; GenericNode2DType type;
GenericNode2D (const std::size_t& index_in, const GenericNode2DType& type_in) : index(index_in), type(type_in) {} GenericNode2D (const std::size_t& index_in, const GenericNode2DType& type_in) : index(index_in), type(type_in) {}
}; };
/** a factor always involves two nodes/variables for now */ /** a factor always involves two nodes/variables for now */
struct GenericFactor2D { struct GenericFactor2D {
GenericNode2D key1; GenericNode2D key1;
GenericNode2D key2; GenericNode2D key2;
int index; // the factor index in the original nonlinear factor graph int index; // the factor index in the original nonlinear factor graph
int weight; // the weight of the edge int weight; // the weight of the edge
GenericFactor2D(const size_t index1, const GenericNode2DType type1, const size_t index2, const GenericNode2DType type2, const int index_ = -1, const int weight_ = 1) GenericFactor2D(const size_t index1, const GenericNode2DType type1, const size_t index2, const GenericNode2DType type2, const int index_ = -1, const int weight_ = 1)
: key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {} : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {}
GenericFactor2D(const size_t index1, const char type1, const size_t index2, const char type2, const int index_ = -1, const int weight_ = 1) GenericFactor2D(const size_t index1, const char type1, const size_t index2, const char type2, const int index_ = -1, const int weight_ = 1)
: key1(index1, type1 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), : key1(index1, type1 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D),
key2(index2, type2 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_), weight(weight_) {} key2(index2, type2 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_), weight(weight_) {}
}; };
/** graph is a collection of factors */ /** graph is a collection of factors */
typedef boost::shared_ptr<GenericFactor2D> sharedGenericFactor2D; typedef boost::shared_ptr<GenericFactor2D> sharedGenericFactor2D;
typedef std::vector<sharedGenericFactor2D> GenericGraph2D; typedef std::vector<sharedGenericFactor2D> GenericGraph2D;
/** merge nodes in DSF using constraints captured by the given graph */ /** merge nodes in DSF using constraints captured by the given graph */
std::list<std::vector<size_t> > findIslands(const GenericGraph2D& graph, const std::vector<size_t>& keys, WorkSpace& workspace, std::list<std::vector<size_t> > findIslands(const GenericGraph2D& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark); const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark);
/** eliminate the sensors from generic graph */ /** eliminate the sensors from generic graph */
inline void reduceGenericGraph(const GenericGraph2D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys, inline void reduceGenericGraph(const GenericGraph2D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
const std::vector<int>& dictionary, GenericGraph2D& reducedGraph) { const std::vector<int>& dictionary, GenericGraph2D& reducedGraph) {
throw std::runtime_error("reduceGenericGraph 2d not implemented"); throw std::runtime_error("reduceGenericGraph 2d not implemented");
} }
/** check whether the 2D graph is singular (under constrained) , Dummy function for 2D */ /** check whether the 2D graph is singular (under constrained) , Dummy function for 2D */
inline void checkSingularity(const GenericGraph2D& graph, const std::vector<size_t>& frontals, inline void checkSingularity(const GenericGraph2D& graph, const std::vector<size_t>& frontals,
WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { return; } WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { return; }
/** print the graph **/ /** print the graph **/
void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D"); void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D");
/*************************************************** /***************************************************
* 3D generic factors and their factor graph * 3D generic factors and their factor graph
***************************************************/ ***************************************************/
enum GenericNode3DType { NODE_POSE_3D, NODE_LANDMARK_3D }; enum GenericNode3DType { NODE_POSE_3D, NODE_LANDMARK_3D };
// const int minNrConstraintsPerCamera = 7; // const int minNrConstraintsPerCamera = 7;
// const int minNrConstraintsPerLandmark = 2; // const int minNrConstraintsPerLandmark = 2;
/** the index of the node and the type of the node */ /** the index of the node and the type of the node */
struct GenericNode3D { struct GenericNode3D {
std::size_t index; std::size_t index;
GenericNode3DType type; GenericNode3DType type;
GenericNode3D (const std::size_t& index_in, const GenericNode3DType& type_in) : index(index_in), type(type_in) {} GenericNode3D (const std::size_t& index_in, const GenericNode3DType& type_in) : index(index_in), type(type_in) {}
}; };
/** a factor always involves two nodes/variables for now */ /** a factor always involves two nodes/variables for now */
struct GenericFactor3D { struct GenericFactor3D {
GenericNode3D key1; GenericNode3D key1;
GenericNode3D key2; GenericNode3D key2;
int index; // the index in the entire graph, 0-based int index; // the index in the entire graph, 0-based
int weight; // the weight of the edge int weight; // the weight of the edge
GenericFactor3D() :key1(-1, NODE_POSE_3D), key2(-1, NODE_LANDMARK_3D), index(-1), weight(1) {} GenericFactor3D() :key1(-1, NODE_POSE_3D), key2(-1, NODE_LANDMARK_3D), index(-1), weight(1) {}
GenericFactor3D(const size_t index1, const size_t index2, const int index_ = -1, GenericFactor3D(const size_t index1, const size_t index2, const int index_ = -1,
const GenericNode3DType type1 = NODE_POSE_3D, const GenericNode3DType type2 = NODE_LANDMARK_3D, const int weight_ = 1) const GenericNode3DType type1 = NODE_POSE_3D, const GenericNode3DType type2 = NODE_LANDMARK_3D, const int weight_ = 1)
: key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {} : key1(index1, type1), key2(index2, type2), index(index_), weight(weight_) {}
}; };
/** graph is a collection of factors */ /** graph is a collection of factors */
typedef boost::shared_ptr<GenericFactor3D> sharedGenericFactor3D; typedef boost::shared_ptr<GenericFactor3D> sharedGenericFactor3D;
typedef std::vector<sharedGenericFactor3D> GenericGraph3D; typedef std::vector<sharedGenericFactor3D> GenericGraph3D;
/** merge nodes in DSF using constraints captured by the given graph */ /** merge nodes in DSF using constraints captured by the given graph */
std::list<std::vector<size_t> > findIslands(const GenericGraph3D& graph, const std::vector<size_t>& keys, WorkSpace& workspace, std::list<std::vector<size_t> > findIslands(const GenericGraph3D& graph, const std::vector<size_t>& keys, WorkSpace& workspace,
const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark); const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
/** eliminate the sensors from generic graph */ /** eliminate the sensors from generic graph */
void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys, void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
const std::vector<int>& dictionary, GenericGraph3D& reducedGraph); const std::vector<int>& dictionary, GenericGraph3D& reducedGraph);
/** check whether the 3D graph is singular (under constrained) */ /** check whether the 3D graph is singular (under constrained) */
void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals, void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark); WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
/** print the graph **/ /** print the graph **/
void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D"); void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D");
/*************************************************** /***************************************************
* unary generic factors and their factor graph * unary generic factors and their factor graph
***************************************************/ ***************************************************/
/** a factor involves a single variable */ /** a factor involves a single variable */
struct GenericUnaryFactor { struct GenericUnaryFactor {
GenericNode2D key; GenericNode2D key;
int index; // the factor index in the original nonlinear factor graph int index; // the factor index in the original nonlinear factor graph
GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_ = -1) GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_ = -1)
: key(key_, type_), index(index_) {} : key(key_, type_), index(index_) {}
GenericUnaryFactor(const size_t key_, const char type_, const int index_ = -1) GenericUnaryFactor(const size_t key_, const char type_, const int index_ = -1)
: key(key_, type_ == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_) {} : key(key_, type_ == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_) {}
}; };
/** graph is a collection of factors */ /** graph is a collection of factors */
typedef boost::shared_ptr<GenericUnaryFactor> sharedGenericUnaryFactor; typedef boost::shared_ptr<GenericUnaryFactor> sharedGenericUnaryFactor;
typedef std::vector<sharedGenericUnaryFactor> GenericUnaryGraph; typedef std::vector<sharedGenericUnaryFactor> GenericUnaryGraph;
/*************************************************** /***************************************************
* utility functions * utility functions
***************************************************/ ***************************************************/
inline bool hasCommonCamera(const std::set<size_t>& cameras1, const std::set<size_t>& cameras2) { inline bool hasCommonCamera(const std::set<size_t>& cameras1, const std::set<size_t>& cameras2) {
if (cameras1.empty() || cameras2.empty()) if (cameras1.empty() || cameras2.empty())
throw std::invalid_argument("hasCommonCamera: the input camera set is empty!"); throw std::invalid_argument("hasCommonCamera: the input camera set is empty!");
std::set<size_t>::const_iterator it1 = cameras1.begin(); std::set<size_t>::const_iterator it1 = cameras1.begin();
std::set<size_t>::const_iterator it2 = cameras2.begin(); std::set<size_t>::const_iterator it2 = cameras2.begin();
while (it1 != cameras1.end() && it2 != cameras2.end()) { while (it1 != cameras1.end() && it2 != cameras2.end()) {
if (*it1 == *it2) if (*it1 == *it2)
return true; return true;
else if (*it1 < *it2) else if (*it1 < *it2)
it1++; it1++;
else else
it2++; it2++;
} }
return false; return false;
} }
}} // namespace }} // namespace

404
gtsam_unstable/partition/NestedDissection-inl.h
View File

@ -16,236 +16,236 @@
namespace gtsam { namespace partition { namespace gtsam { namespace partition {
/* ************************************************************************* */ /* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph> template <class NLG, class SubNLG, class GenericGraph>
NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection( NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose) : const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose) :
fg_(fg), ordering_(ordering){ fg_(fg), ordering_(ordering){
GenericUnaryGraph unaryFactors; GenericUnaryGraph unaryFactors;
GenericGraph gfg; GenericGraph gfg;
boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering); boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
// build reverse mapping from integer to symbol // build reverse mapping from integer to symbol
int numNodes = ordering.size(); int numNodes = ordering.size();
int2symbol_.resize(numNodes); int2symbol_.resize(numNodes);
Ordering::const_iterator it = ordering.begin(), itLast = ordering.end(); Ordering::const_iterator it = ordering.begin(), itLast = ordering.end();
while(it != itLast) while(it != itLast)
int2symbol_[it->second] = (it++)->first; int2symbol_[it->second] = (it++)->first;
vector<size_t> keys; vector<size_t> keys;
keys.reserve(numNodes); keys.reserve(numNodes);
for(int i=0; i<ordering.size(); ++i) for(int i=0; i<ordering.size(); ++i)
keys.push_back(i); keys.push_back(i);
WorkSpace workspace(numNodes); WorkSpace workspace(numNodes);
root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), numNodeStopPartition, minNodesPerMap, boost::shared_ptr<SubNLG>(), workspace, verbose); root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), numNodeStopPartition, minNodesPerMap, boost::shared_ptr<SubNLG>(), workspace, verbose);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph> template <class NLG, class SubNLG, class GenericGraph>
NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection( NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& cuts, const bool verbose) : fg_(fg), ordering_(ordering){ const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& cuts, const bool verbose) : fg_(fg), ordering_(ordering){
GenericUnaryGraph unaryFactors; GenericUnaryGraph unaryFactors;
GenericGraph gfg; GenericGraph gfg;
boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering); boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
// build reverse mapping from integer to symbol // build reverse mapping from integer to symbol
int numNodes = ordering.size(); int numNodes = ordering.size();
int2symbol_.resize(numNodes); int2symbol_.resize(numNodes);
Ordering::const_iterator it = ordering.begin(), itLast = ordering.end(); Ordering::const_iterator it = ordering.begin(), itLast = ordering.end();
while(it != itLast) while(it != itLast)
int2symbol_[it->second] = (it++)->first; int2symbol_[it->second] = (it++)->first;
vector<size_t> keys; vector<size_t> keys;
keys.reserve(numNodes); keys.reserve(numNodes);
for(int i=0; i<ordering.size(); ++i) for(int i=0; i<ordering.size(); ++i)
keys.push_back(i); keys.push_back(i);
WorkSpace workspace(numNodes); WorkSpace workspace(numNodes);
root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), cuts, boost::shared_ptr<SubNLG>(), workspace, verbose); root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), cuts, boost::shared_ptr<SubNLG>(), workspace, verbose);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph> template <class NLG, class SubNLG, class GenericGraph>
boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::makeSubNLG( boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::makeSubNLG(
const NLG& fg, const vector<size_t>& frontals, const vector<size_t>& sep, const boost::shared_ptr<SubNLG>& parent) const { const NLG& fg, const vector<size_t>& frontals, const vector<size_t>& sep, const boost::shared_ptr<SubNLG>& parent) const {
OrderedSymbols frontalKeys; OrderedSymbols frontalKeys;
BOOST_FOREACH(const size_t index, frontals) BOOST_FOREACH(const size_t index, frontals)
frontalKeys.push_back(int2symbol_[index]); frontalKeys.push_back(int2symbol_[index]);
UnorderedSymbols sepKeys; UnorderedSymbols sepKeys;
BOOST_FOREACH(const size_t index, sep) BOOST_FOREACH(const size_t index, sep)
sepKeys.insert(int2symbol_[index]); sepKeys.insert(int2symbol_[index]);
return boost::make_shared<SubNLG>(fg, frontalKeys, sepKeys, parent); return boost::make_shared<SubNLG>(fg, frontalKeys, sepKeys, parent);
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph> template <class NLG, class SubNLG, class GenericGraph>
void NestedDissection<NLG, SubNLG, GenericGraph>::processFactor( void NestedDissection<NLG, SubNLG, GenericGraph>::processFactor(
const typename GenericGraph::value_type& factor, const std::vector<int>& partitionTable, // input const typename GenericGraph::value_type& factor, const std::vector<int>& partitionTable, // input
vector<GenericGraph>& frontalFactors, NLG& sepFactors, vector<set<size_t> >& childSeps, // output factor graphs vector<GenericGraph>& frontalFactors, NLG& sepFactors, vector<set<size_t> >& childSeps, // output factor graphs
typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques
list<size_t> sep_; // the separator variables involved in the current factor list<size_t> sep_; // the separator variables involved in the current factor
int partition1 = partitionTable[factor->key1.index]; int partition1 = partitionTable[factor->key1.index];
int partition2 = partitionTable[factor->key2.index]; int partition2 = partitionTable[factor->key2.index];
if (partition1 <= 0 && partition2 <= 0) { // is a factor in the current clique if (partition1 <= 0 && partition2 <= 0) { // is a factor in the current clique
sepFactors.push_back(fg_[factor->index]); sepFactors.push_back(fg_[factor->index]);
} }
else if (partition1 > 0 && partition2 > 0 && partition1 != partition2) { // is a weeklink (factor between two child cliques) else if (partition1 > 0 && partition2 > 0 && partition1 != partition2) { // is a weeklink (factor between two child cliques)
weeklinks.push_back(fg_[factor->index]); weeklinks.push_back(fg_[factor->index]);
} }
else if (partition1 > 0 && partition2 > 0 && partition1 == partition2) { // is a local factor in one of the child cliques else if (partition1 > 0 && partition2 > 0 && partition1 == partition2) { // is a local factor in one of the child cliques
frontalFactors[partition1 - 1].push_back(factor); frontalFactors[partition1 - 1].push_back(factor);
} }
else { // is a joint factor in the child clique (involving varaibles in the current clique) else { // is a joint factor in the child clique (involving varaibles in the current clique)
if (partition1 > 0 && partition2 <= 0) { if (partition1 > 0 && partition2 <= 0) {
frontalFactors[partition1 - 1].push_back(factor); frontalFactors[partition1 - 1].push_back(factor);
childSeps[partition1 - 1].insert(factor->key2.index); childSeps[partition1 - 1].insert(factor->key2.index);
} else if (partition1 <= 0 && partition2 > 0) { } else if (partition1 <= 0 && partition2 > 0) {
frontalFactors[partition2 - 1].push_back(factor); frontalFactors[partition2 - 1].push_back(factor);
childSeps[partition2 - 1].insert(factor->key1.index); childSeps[partition2 - 1].insert(factor->key1.index);
} else } else
throw runtime_error("processFactor: unexpected entries in the partition table!"); throw runtime_error("processFactor: unexpected entries in the partition table!");
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* given a factor graph and its partition {nodeMap}, split the factors between the child cliques ({frontalFactors}) * given a factor graph and its partition {nodeMap}, split the factors between the child cliques ({frontalFactors})
* and the current clique ({sepFactors}). Also split the variables between the child cliques ({childFrontals}) * and the current clique ({sepFactors}). Also split the variables between the child cliques ({childFrontals})
* and the current clique ({localFrontals}). Those separator variables involved in {frontalFactors} are put into * and the current clique ({localFrontals}). Those separator variables involved in {frontalFactors} are put into
* the correspoding ordering in {childSeps}. * the correspoding ordering in {childSeps}.
*/ */
// TODO: frontalFactors and localFrontals should be generated in findSeparator // TODO: frontalFactors and localFrontals should be generated in findSeparator
template <class NLG, class SubNLG, class GenericGraph> template <class NLG, class SubNLG, class GenericGraph>
void NestedDissection<NLG, SubNLG, GenericGraph>::partitionFactorsAndVariables( void NestedDissection<NLG, SubNLG, GenericGraph>::partitionFactorsAndVariables(
const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& keys, //input const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& keys, //input
const std::vector<int>& partitionTable, const int numSubmaps, // input const std::vector<int>& partitionTable, const int numSubmaps, // input
vector<GenericGraph>& frontalFactors, vector<GenericUnaryGraph>& frontalUnaryFactors, NLG& sepFactors, // output factor graphs vector<GenericGraph>& frontalFactors, vector<GenericUnaryGraph>& frontalUnaryFactors, NLG& sepFactors, // output factor graphs
vector<vector<size_t> >& childFrontals, vector<vector<size_t> >& childSeps, vector<size_t>& localFrontals, // output sub-orderings vector<vector<size_t> >& childFrontals, vector<vector<size_t> >& childSeps, vector<size_t>& localFrontals, // output sub-orderings
typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques
// make three lists of variables A, B, and C // make three lists of variables A, B, and C
int partition; int partition;
childFrontals.resize(numSubmaps); childFrontals.resize(numSubmaps);
BOOST_FOREACH(const size_t key, keys){ BOOST_FOREACH(const size_t key, keys){
partition = partitionTable[key]; partition = partitionTable[key];
switch (partition) { switch (partition) {
case -1: break; // the separator of the separator variables case -1: break; // the separator of the separator variables
case 0: localFrontals.push_back(key); break; // the separator variables case 0: localFrontals.push_back(key); break; // the separator variables
default: childFrontals[partition-1].push_back(key); // the frontal variables default: childFrontals[partition-1].push_back(key); // the frontal variables
} }
} }
// group the factors to {frontalFactors} and {sepFactors},and find the joint variables // group the factors to {frontalFactors} and {sepFactors},and find the joint variables
vector<set<size_t> > childSeps_; vector<set<size_t> > childSeps_;
childSeps_.resize(numSubmaps); childSeps_.resize(numSubmaps);
childSeps.reserve(numSubmaps); childSeps.reserve(numSubmaps);
frontalFactors.resize(numSubmaps); frontalFactors.resize(numSubmaps);
frontalUnaryFactors.resize(numSubmaps); frontalUnaryFactors.resize(numSubmaps);
BOOST_FOREACH(typename GenericGraph::value_type factor, fg) BOOST_FOREACH(typename GenericGraph::value_type factor, fg)
processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks); processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks);
BOOST_FOREACH(const set<size_t>& childSep, childSeps_) BOOST_FOREACH(const set<size_t>& childSep, childSeps_)
childSeps.push_back(vector<size_t>(childSep.begin(), childSep.end())); childSeps.push_back(vector<size_t>(childSep.begin(), childSep.end()));
// add unary factor to the current cluster or pass it to one of the child clusters // add unary factor to the current cluster or pass it to one of the child clusters
BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) { BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) {
partition = partitionTable[unaryFactor_->key.index]; partition = partitionTable[unaryFactor_->key.index];
if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]); if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]);
else frontalUnaryFactors[partition-1].push_back(unaryFactor_); else frontalUnaryFactors[partition-1].push_back(unaryFactor_);
} }
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph> template <class NLG, class SubNLG, class GenericGraph>
NLG NestedDissection<NLG, SubNLG, GenericGraph>::collectOriginalFactors( NLG NestedDissection<NLG, SubNLG, GenericGraph>::collectOriginalFactors(
const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const { const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const {
NLG sepFactors; NLG sepFactors;
typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end(); typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end();
while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]); while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]);
BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors)
sepFactors.push_back(fg_[unaryFactor_->index]); sepFactors.push_back(fg_[unaryFactor_->index]);
return sepFactors; return sepFactors;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph> template <class NLG, class SubNLG, class GenericGraph>
boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::recursivePartition( boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::recursivePartition(
const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector<size_t>& frontals, const vector<size_t>& sep, const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector<size_t>& frontals, const vector<size_t>& sep,
const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const { const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const {
// if no split needed // if no split needed
NLG sepFactors; // factors that should remain in the current cluster NLG sepFactors; // factors that should remain in the current cluster
if (frontals.size() <= numNodeStopPartition || gfg.size() <= numNodeStopPartition) { if (frontals.size() <= numNodeStopPartition || gfg.size() <= numNodeStopPartition) {
sepFactors = collectOriginalFactors(gfg, unaryFactors); sepFactors = collectOriginalFactors(gfg, unaryFactors);
return makeSubNLG(sepFactors, frontals, sep, parent); return makeSubNLG(sepFactors, frontals, sep, parent);
} }
// find the nested dissection separator // find the nested dissection separator
int numSubmaps = findSeparator(gfg, frontals, minNodesPerMap, workspace, verbose, int2symbol_, NLG::reduceGraph(), int numSubmaps = findSeparator(gfg, frontals, minNodesPerMap, workspace, verbose, int2symbol_, NLG::reduceGraph(),
NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark()); NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
partition::PartitionTable& partitionTable = workspace.partitionTable; partition::PartitionTable& partitionTable = workspace.partitionTable;
if (numSubmaps == 0) throw runtime_error("recursivePartition: get zero submap after ND!"); if (numSubmaps == 0) throw runtime_error("recursivePartition: get zero submap after ND!");
// split the factors between child cliques and the current clique // split the factors between child cliques and the current clique
vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks; vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps; vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps, partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks); frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
// make a new cluster // make a new cluster
boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent); boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
current->setWeeklinks(weeklinks); current->setWeeklinks(weeklinks);
// check whether all the submaps are fully constrained // check whether all the submaps are fully constrained
for (int i=0; i<numSubmaps; i++) { for (int i=0; i<numSubmaps; i++) {
checkSingularity(frontalFactors[i], childFrontals[i], workspace, NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark()); checkSingularity(frontalFactors[i], childFrontals[i], workspace, NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
} }
// create child clusters // create child clusters
for (int i=0; i<numSubmaps; i++) { for (int i=0; i<numSubmaps; i++) {
boost::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], boost::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i],
numNodeStopPartition, minNodesPerMap, current, workspace, verbose); numNodeStopPartition, minNodesPerMap, current, workspace, verbose);
current->addChild(child); current->addChild(child);
} }
return current; return current;
} }
/* ************************************************************************* */ /* ************************************************************************* */
template <class NLG, class SubNLG, class GenericGraph> template <class NLG, class SubNLG, class GenericGraph>
boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::recursivePartition( boost::shared_ptr<SubNLG> NestedDissection<NLG, SubNLG, GenericGraph>::recursivePartition(
const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector<size_t>& frontals, const vector<size_t>& sep, const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const vector<size_t>& frontals, const vector<size_t>& sep,
const boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const { const boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const {
// if there is no need to cut any more // if there is no need to cut any more
NLG sepFactors; // factors that should remain in the current cluster NLG sepFactors; // factors that should remain in the current cluster
if (!cuts.get()) { if (!cuts.get()) {
sepFactors = collectOriginalFactors(gfg, unaryFactors); sepFactors = collectOriginalFactors(gfg, unaryFactors);
return makeSubNLG(sepFactors, frontals, sep, parent); return makeSubNLG(sepFactors, frontals, sep, parent);
} }
// retrieve the current partitioning info // retrieve the current partitioning info
int numSubmaps = 2; int numSubmaps = 2;
partition::PartitionTable& partitionTable = cuts->partitionTable; partition::PartitionTable& partitionTable = cuts->partitionTable;
// split the factors between child cliques and the current clique // split the factors between child cliques and the current clique
vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks; vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps; vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps, partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks); frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
// make a new cluster // make a new cluster
boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent); boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
current->setWeeklinks(weeklinks); current->setWeeklinks(weeklinks);
// create child clusters // create child clusters
for (int i=0; i<2; i++) { for (int i=0; i<2; i++) {
boost::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i], boost::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i],
cuts->children.empty() ? boost::shared_ptr<Cuts>() : cuts->children[i], current, workspace, verbose); cuts->children.empty() ? boost::shared_ptr<Cuts>() : cuts->children[i], current, workspace, verbose);
current->addChild(child); current->addChild(child);
} }
return current; return current;
} }
}} //namespace }} //namespace

80
gtsam_unstable/partition/NestedDissection.h
View File

@ -14,56 +14,56 @@
namespace gtsam { namespace partition { namespace gtsam { namespace partition {
/** /**
* Apply nested dissection algorithm to nonlinear factor graphs * Apply nested dissection algorithm to nonlinear factor graphs
*/ */
template <class NLG, class SubNLG, class GenericGraph> template <class NLG, class SubNLG, class GenericGraph>
class NestedDissection { class NestedDissection {
public: public:
typedef boost::shared_ptr<SubNLG> sharedSubNLG; typedef boost::shared_ptr<SubNLG> sharedSubNLG;
private: private:
NLG fg_; // the original nonlinear factor graph NLG fg_; // the original nonlinear factor graph
Ordering ordering_; // the variable ordering in the nonlinear factor graph Ordering ordering_; // the variable ordering in the nonlinear factor graph
std::vector<Symbol> int2symbol_; // the mapping from integer key to symbol std::vector<Symbol> int2symbol_; // the mapping from integer key to symbol
sharedSubNLG root_; // the root of generated cluster tree sharedSubNLG root_; // the root of generated cluster tree
public: public:
sharedSubNLG root() const { return root_; } sharedSubNLG root() const { return root_; }
public: public:
/* constructor with post-determined partitoning*/ /* constructor with post-determined partitoning*/
NestedDissection(const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose = false); NestedDissection(const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose = false);
/* constructor with pre-determined cuts*/ /* constructor with pre-determined cuts*/
NestedDissection(const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& cuts, const bool verbose = false); NestedDissection(const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& cuts, const bool verbose = false);
private: private:
/* convert generic subgraph to nonlinear subgraph */ /* convert generic subgraph to nonlinear subgraph */
sharedSubNLG makeSubNLG(const NLG& fg, const std::vector<size_t>& frontals, const std::vector<size_t>& sep, const boost::shared_ptr<SubNLG>& parent) const; sharedSubNLG makeSubNLG(const NLG& fg, const std::vector<size_t>& frontals, const std::vector<size_t>& sep, const boost::shared_ptr<SubNLG>& parent) const;
void processFactor(const typename GenericGraph::value_type& factor, const std::vector<int>& partitionTable, // input void processFactor(const typename GenericGraph::value_type& factor, const std::vector<int>& partitionTable, // input
std::vector<GenericGraph>& frontalFactors, NLG& sepFactors, std::vector<std::set<size_t> >& childSeps, // output factor graphs std::vector<GenericGraph>& frontalFactors, NLG& sepFactors, std::vector<std::set<size_t> >& childSeps, // output factor graphs
typename SubNLG::Weeklinks& weeklinks) const; typename SubNLG::Weeklinks& weeklinks) const;
/* recursively partition the generic graph */ /* recursively partition the generic graph */
void partitionFactorsAndVariables( void partitionFactorsAndVariables(
const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const GenericGraph& fg, const GenericUnaryGraph& unaryFactors,
const std::vector<size_t>& keys, const std::vector<int>& partitionTable, const int numSubmaps, // input const std::vector<size_t>& keys, const std::vector<int>& partitionTable, const int numSubmaps, // input
std::vector<GenericGraph>& frontalFactors, vector<GenericUnaryGraph>& frontalUnaryFactors, NLG& sepFactors, // output factor graphs std::vector<GenericGraph>& frontalFactors, vector<GenericUnaryGraph>& frontalUnaryFactors, NLG& sepFactors, // output factor graphs
std::vector<std::vector<size_t> >& childFrontals, std::vector<std::vector<size_t> >& childSeps, std::vector<size_t>& localFrontals, // output sub-orderings std::vector<std::vector<size_t> >& childFrontals, std::vector<std::vector<size_t> >& childSeps, std::vector<size_t>& localFrontals, // output sub-orderings
typename SubNLG::Weeklinks& weeklinks) const; typename SubNLG::Weeklinks& weeklinks) const;
NLG collectOriginalFactors(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const; NLG collectOriginalFactors(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const;
/* recursively partition the generic graph */ /* recursively partition the generic graph */
sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& frontals, const std::vector<size_t>& sep, sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& frontals, const std::vector<size_t>& sep,
const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const; const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const;
/* recursively partition the generic graph */ /* recursively partition the generic graph */
sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& frontals, const std::vector<size_t>& sep, sharedSubNLG recursivePartition(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& frontals, const std::vector<size_t>& sep,
const boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const; const boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const;
}; };
}} //namespace }} //namespace

44
gtsam_unstable/partition/PartitionWorkSpace.h
View File

@ -13,32 +13,32 @@
namespace gtsam { namespace partition { namespace gtsam { namespace partition {
typedef std::vector<int> PartitionTable; typedef std::vector<int> PartitionTable;
// the work space, preallocated memory // the work space, preallocated memory
struct WorkSpace { struct WorkSpace {
std::vector<int> dictionary; // a mapping from the integer key in the original graph to 0-based index in the subgraph, useful when handling a subset of keys and graphs std::vector<int> dictionary; // a mapping from the integer key in the original graph to 0-based index in the subgraph, useful when handling a subset of keys and graphs
boost::shared_ptr<std::vector<size_t> > dsf; // a block memory pre-allocated for DSFVector boost::shared_ptr<std::vector<size_t> > dsf; // a block memory pre-allocated for DSFVector
PartitionTable partitionTable; // a mapping from a key to the submap index, 0 means the separator, i means the ith submap PartitionTable partitionTable; // a mapping from a key to the submap index, 0 means the separator, i means the ith submap
// constructor // constructor
WorkSpace(const size_t numNodes) : dictionary(numNodes,0), WorkSpace(const size_t numNodes) : dictionary(numNodes,0),
dsf(new std::vector<size_t>(numNodes, 0)), partitionTable(numNodes, -1) { } dsf(new std::vector<size_t>(numNodes, 0)), partitionTable(numNodes, -1) { }
// set up dictionary: -1: no such key, none-zero: the corresponding 0-based index // set up dictionary: -1: no such key, none-zero: the corresponding 0-based index
inline void prepareDictionary(const std::vector<size_t>& keys) { inline void prepareDictionary(const std::vector<size_t>& keys) {
int index = 0; int index = 0;
std::fill(dictionary.begin(), dictionary.end(), -1); std::fill(dictionary.begin(), dictionary.end(), -1);
std::vector<size_t>::const_iterator it=keys.begin(), itLast=keys.end(); std::vector<size_t>::const_iterator it=keys.begin(), itLast=keys.end();
while(it!=itLast) dictionary[*(it++)] = index++; while(it!=itLast) dictionary[*(it++)] = index++;
} }
}; };
// manually defined cuts // manually defined cuts
struct Cuts { struct Cuts {
PartitionTable partitionTable; PartitionTable partitionTable;
std::vector<boost::shared_ptr<Cuts> > children; std::vector<boost::shared_ptr<Cuts> > children;
}; };
}} // namespace }} // namespace

270
gtsam_unstable/partition/tests/testGenericGraph.cpp
View File

@ -29,29 +29,29 @@ using namespace gtsam::partition;
*/ */
TEST ( GenerciGraph, findIslands ) TEST ( GenerciGraph, findIslands )
{ {
GenericGraph2D graph; GenericGraph2D graph;
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9; std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8, 9;
WorkSpace workspace(10); // from 0 to 9 WorkSpace workspace(10); // from 0 to 9
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2); list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(2, islands.size()); LONGS_EQUAL(2, islands.size());
vector<size_t> island1; island1 += 1, 2, 3, 7, 8; vector<size_t> island1; island1 += 1, 2, 3, 7, 8;
vector<size_t> island2; island2 += 4, 5, 6, 9; vector<size_t> island2; island2 += 4, 5, 6, 9;
CHECK(island1 == islands.front()); CHECK(island1 == islands.front());
CHECK(island2 == islands.back()); CHECK(island2 == islands.back());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -62,27 +62,27 @@ TEST ( GenerciGraph, findIslands )
*/ */
TEST( GenerciGraph, findIslands2 ) TEST( GenerciGraph, findIslands2 )
{ {
GenericGraph2D graph; GenericGraph2D graph;
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 2, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(5, NODE_POSE_2D, 6, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8; std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6, 7, 8;
WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2); list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(1, islands.size()); LONGS_EQUAL(1, islands.size());
vector<size_t> island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8; vector<size_t> island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8;
CHECK(island1 == islands.front()); CHECK(island1 == islands.front());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -92,21 +92,21 @@ TEST( GenerciGraph, findIslands2 )
*/ */
TEST ( GenerciGraph, findIslands3 ) TEST ( GenerciGraph, findIslands3 )
{ {
GenericGraph2D graph; GenericGraph2D graph;
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 6, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 3, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_POSE_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6; std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6;
WorkSpace workspace(7); // from 0 to 9 WorkSpace workspace(7); // from 0 to 9
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2); list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(2, islands.size()); LONGS_EQUAL(2, islands.size());
vector<size_t> island1; island1 += 1, 5; vector<size_t> island1; island1 += 1, 5;
vector<size_t> island2; island2 += 2, 3, 4, 6; vector<size_t> island2; island2 += 2, 3, 4, 6;
CHECK(island1 == islands.front()); CHECK(island1 == islands.front());
CHECK(island2 == islands.back()); CHECK(island2 == islands.back());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -115,18 +115,18 @@ TEST ( GenerciGraph, findIslands3 )
*/ */
TEST ( GenerciGraph, findIslands4 ) TEST ( GenerciGraph, findIslands4 )
{ {
GenericGraph2D graph; GenericGraph2D graph;
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 4, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(7, NODE_POSE_2D, 7, NODE_LANDMARK_2D));
std::vector<size_t> keys; keys += 3, 4, 7; std::vector<size_t> keys; keys += 3, 4, 7;
WorkSpace workspace(8); // from 0 to 7 WorkSpace workspace(8); // from 0 to 7
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2); list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(2, islands.size()); LONGS_EQUAL(2, islands.size());
vector<size_t> island1; island1 += 3, 4; vector<size_t> island1; island1 += 3, 4;
vector<size_t> island2; island2 += 7; vector<size_t> island2; island2 += 7;
CHECK(island1 == islands.front()); CHECK(island1 == islands.front());
CHECK(island2 == islands.back()); CHECK(island2 == islands.back());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -137,24 +137,24 @@ TEST ( GenerciGraph, findIslands4 )
*/ */
TEST ( GenerciGraph, findIslands5 ) TEST ( GenerciGraph, findIslands5 )
{ {
GenericGraph2D graph; GenericGraph2D graph;
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 3, NODE_POSE_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(1, NODE_POSE_2D, 3, NODE_POSE_2D));
graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_POSE_2D)); graph.push_back(boost::make_shared<GenericFactor2D>(2, NODE_POSE_2D, 4, NODE_POSE_2D));
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5; std::vector<size_t> keys; keys += 1, 2, 3, 4, 5;
WorkSpace workspace(6); // from 0 to 5 WorkSpace workspace(6); // from 0 to 5
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2); list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
LONGS_EQUAL(2, islands.size()); LONGS_EQUAL(2, islands.size());
vector<size_t> island1; island1 += 1, 3, 5; vector<size_t> island1; island1 += 1, 3, 5;
vector<size_t> island2; island2 += 2, 4; vector<size_t> island2; island2 += 2, 4;
CHECK(island1 == islands.front()); CHECK(island1 == islands.front());
CHECK(island2 == islands.back()); CHECK(island2 == islands.back());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -165,31 +165,31 @@ TEST ( GenerciGraph, findIslands5 )
*/ */
TEST ( GenerciGraph, reduceGenericGraph ) TEST ( GenerciGraph, reduceGenericGraph )
{ {
GenericGraph3D graph; GenericGraph3D graph;
graph.push_back(boost::make_shared<GenericFactor3D>(1, 3)); graph.push_back(boost::make_shared<GenericFactor3D>(1, 3));
graph.push_back(boost::make_shared<GenericFactor3D>(1, 4)); graph.push_back(boost::make_shared<GenericFactor3D>(1, 4));
graph.push_back(boost::make_shared<GenericFactor3D>(1, 5)); graph.push_back(boost::make_shared<GenericFactor3D>(1, 5));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 5)); graph.push_back(boost::make_shared<GenericFactor3D>(2, 5));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 6)); graph.push_back(boost::make_shared<GenericFactor3D>(2, 6));
std::vector<size_t> cameraKeys, landmarkKeys; std::vector<size_t> cameraKeys, landmarkKeys;
cameraKeys.push_back(1); cameraKeys.push_back(1);
cameraKeys.push_back(2); cameraKeys.push_back(2);
landmarkKeys.push_back(3); landmarkKeys.push_back(3);
landmarkKeys.push_back(4); landmarkKeys.push_back(4);
landmarkKeys.push_back(5); landmarkKeys.push_back(5);
landmarkKeys.push_back(6); landmarkKeys.push_back(6);
std::vector<int> dictionary; std::vector<int> dictionary;
dictionary.resize(7, -1); // from 0 to 6 dictionary.resize(7, -1); // from 0 to 6
dictionary[1] = 0; dictionary[1] = 0;
dictionary[2] = 1; dictionary[2] = 1;
GenericGraph3D reduced; GenericGraph3D reduced;
std::map<size_t, vector<size_t> > cameraToLandmarks; std::map<size_t, vector<size_t> > cameraToLandmarks;
reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced); reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
LONGS_EQUAL(1, reduced.size()); LONGS_EQUAL(1, reduced.size());
LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index); LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -200,53 +200,53 @@ TEST ( GenerciGraph, reduceGenericGraph )
*/ */
TEST ( GenericGraph, reduceGenericGraph2 ) TEST ( GenericGraph, reduceGenericGraph2 )
{ {
GenericGraph3D graph; GenericGraph3D graph;
graph.push_back(boost::make_shared<GenericFactor3D>(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D)); graph.push_back(boost::make_shared<GenericFactor3D>(1, 3, 0, NODE_POSE_3D, NODE_LANDMARK_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D)); graph.push_back(boost::make_shared<GenericFactor3D>(1, 4, 1, NODE_POSE_3D, NODE_LANDMARK_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D)); graph.push_back(boost::make_shared<GenericFactor3D>(1, 5, 2, NODE_POSE_3D, NODE_LANDMARK_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D)); graph.push_back(boost::make_shared<GenericFactor3D>(2, 5, 3, NODE_POSE_3D, NODE_LANDMARK_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D)); graph.push_back(boost::make_shared<GenericFactor3D>(2, 6, 4, NODE_POSE_3D, NODE_LANDMARK_3D));
graph.push_back(boost::make_shared<GenericFactor3D>(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D)); graph.push_back(boost::make_shared<GenericFactor3D>(2, 7, 5, NODE_POSE_3D, NODE_POSE_3D));
std::vector<size_t> cameraKeys, landmarkKeys; std::vector<size_t> cameraKeys, landmarkKeys;
cameraKeys.push_back(1); cameraKeys.push_back(1);
cameraKeys.push_back(2); cameraKeys.push_back(2);
cameraKeys.push_back(7); cameraKeys.push_back(7);
landmarkKeys.push_back(3); landmarkKeys.push_back(3);
landmarkKeys.push_back(4); landmarkKeys.push_back(4);
landmarkKeys.push_back(5); landmarkKeys.push_back(5);
landmarkKeys.push_back(6); landmarkKeys.push_back(6);
std::vector<int> dictionary; std::vector<int> dictionary;
dictionary.resize(8, -1); // from 0 to 7 dictionary.resize(8, -1); // from 0 to 7
dictionary[1] = 0; dictionary[1] = 0;
dictionary[2] = 1; dictionary[2] = 1;
dictionary[7] = 6; dictionary[7] = 6;
GenericGraph3D reduced; GenericGraph3D reduced;
std::map<size_t, vector<size_t> > cameraToLandmarks; std::map<size_t, vector<size_t> > cameraToLandmarks;
reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced); reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
LONGS_EQUAL(2, reduced.size()); LONGS_EQUAL(2, reduced.size());
LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index); LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
LONGS_EQUAL(2, reduced[1]->key1.index); LONGS_EQUAL(7, reduced[1]->key2.index); LONGS_EQUAL(2, reduced[1]->key1.index); LONGS_EQUAL(7, reduced[1]->key2.index);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST ( GenerciGraph, hasCommonCamera ) TEST ( GenerciGraph, hasCommonCamera )
{ {
std::set<size_t> cameras1; cameras1 += 1, 2, 3, 4, 5; std::set<size_t> cameras1; cameras1 += 1, 2, 3, 4, 5;
std::set<size_t> cameras2; cameras2 += 8, 7, 6, 5; std::set<size_t> cameras2; cameras2 += 8, 7, 6, 5;
bool actual = hasCommonCamera(cameras1, cameras2); bool actual = hasCommonCamera(cameras1, cameras2);
CHECK(actual); CHECK(actual);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST ( GenerciGraph, hasCommonCamera2 ) TEST ( GenerciGraph, hasCommonCamera2 )
{ {
std::set<size_t> cameras1; cameras1 += 1, 3, 5, 7; std::set<size_t> cameras1; cameras1 += 1, 3, 5, 7;
std::set<size_t> cameras2; cameras2 += 2, 4, 6, 8, 10; std::set<size_t> cameras2; cameras2 += 2, 4, 6, 8, 10;
bool actual = hasCommonCamera(cameras1, cameras2); bool actual = hasCommonCamera(cameras1, cameras2);
CHECK(!actual); CHECK(!actual);
} }
/* ************************************************************************* */ /* ************************************************************************* */

434
gtsam_unstable/partition/tests/testNestedDissection.cpp
View File

@ -32,22 +32,22 @@ using namespace gtsam::partition;
// l1 // l1
TEST ( NestedDissection, oneIsland ) TEST ( NestedDissection, oneIsland )
{ {
using namespace submapPlanarSLAM; using namespace submapPlanarSLAM;
typedef TSAM2D::SubNLG SubNLG; typedef TSAM2D::SubNLG SubNLG;
Graph fg; Graph fg;
fg.addOdometry(1, 2, Pose2(), odoNoise); fg.addOdometry(1, 2, Pose2(), odoNoise);
fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise); fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise);
fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise); fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise);
fg.addPoseConstraint(1, Pose2()); fg.addPoseConstraint(1, Pose2());
Ordering ordering; ordering += x1, x2, l1; Ordering ordering; ordering += x1, x2, l1;
int numNodeStopPartition = 1e3; int numNodeStopPartition = 1e3;
int minNodesPerMap = 1e3; int minNodesPerMap = 1e3;
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap); NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
LONGS_EQUAL(4, nd.root()->size()); LONGS_EQUAL(4, nd.root()->size());
LONGS_EQUAL(3, nd.root()->frontal().size()); LONGS_EQUAL(3, nd.root()->frontal().size());
LONGS_EQUAL(0, nd.root()->children().size()); LONGS_EQUAL(0, nd.root()->children().size());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -56,35 +56,35 @@ TEST ( NestedDissection, oneIsland )
// x2/ \x5 // x2/ \x5
TEST ( NestedDissection, TwoIslands ) TEST ( NestedDissection, TwoIslands )
{ {
using namespace submapPlanarSLAM; using namespace submapPlanarSLAM;
typedef TSAM2D::SubNLG SubNLG; typedef TSAM2D::SubNLG SubNLG;
Graph fg; Graph fg;
fg.addOdometry(1, 2, Pose2(), odoNoise); fg.addOdometry(1, 2, Pose2(), odoNoise);
fg.addOdometry(1, 3, Pose2(), odoNoise); fg.addOdometry(1, 3, Pose2(), odoNoise);
fg.addOdometry(2, 3, Pose2(), odoNoise); fg.addOdometry(2, 3, Pose2(), odoNoise);
fg.addOdometry(3, 4, Pose2(), odoNoise); fg.addOdometry(3, 4, Pose2(), odoNoise);
fg.addOdometry(4, 5, Pose2(), odoNoise); fg.addOdometry(4, 5, Pose2(), odoNoise);
fg.addOdometry(3, 5, Pose2(), odoNoise); fg.addOdometry(3, 5, Pose2(), odoNoise);
fg.addPoseConstraint(1, Pose2()); fg.addPoseConstraint(1, Pose2());
fg.addPoseConstraint(4, Pose2()); fg.addPoseConstraint(4, Pose2());
Ordering ordering; ordering += x1, x2, x3, x4, x5; Ordering ordering; ordering += x1, x2, x3, x4, x5;
int numNodeStopPartition = 2; int numNodeStopPartition = 2;
int minNodesPerMap = 1; int minNodesPerMap = 1;
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap); NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
// root submap // root submap
LONGS_EQUAL(0, nd.root()->size()); LONGS_EQUAL(0, nd.root()->size());
LONGS_EQUAL(1, nd.root()->frontal().size()); LONGS_EQUAL(1, nd.root()->frontal().size());
LONGS_EQUAL(0, nd.root()->separator().size()); LONGS_EQUAL(0, nd.root()->separator().size());
LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
// the 1st submap // the 1st submap
LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size());
LONGS_EQUAL(4, nd.root()->children()[0]->size()); LONGS_EQUAL(4, nd.root()->children()[0]->size());
// the 2nd submap // the 2nd submap
LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size());
LONGS_EQUAL(4, nd.root()->children()[1]->size()); LONGS_EQUAL(4, nd.root()->children()[1]->size());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -93,40 +93,40 @@ TEST ( NestedDissection, TwoIslands )
// x2/ \x5 // x2/ \x5
TEST ( NestedDissection, FourIslands ) TEST ( NestedDissection, FourIslands )
{ {
using namespace submapPlanarSLAM; using namespace submapPlanarSLAM;
typedef TSAM2D::SubNLG SubNLG; typedef TSAM2D::SubNLG SubNLG;
Graph fg; Graph fg;
fg.addOdometry(1, 3, Pose2(), odoNoise); fg.addOdometry(1, 3, Pose2(), odoNoise);
fg.addOdometry(2, 3, Pose2(), odoNoise); fg.addOdometry(2, 3, Pose2(), odoNoise);
fg.addOdometry(3, 4, Pose2(), odoNoise); fg.addOdometry(3, 4, Pose2(), odoNoise);
fg.addOdometry(3, 5, Pose2(), odoNoise); fg.addOdometry(3, 5, Pose2(), odoNoise);
fg.addPoseConstraint(1, Pose2()); fg.addPoseConstraint(1, Pose2());
fg.addPoseConstraint(4, Pose2()); fg.addPoseConstraint(4, Pose2());
Ordering ordering; ordering += x1, x2, x3, x4, x5; Ordering ordering; ordering += x1, x2, x3, x4, x5;
int numNodeStopPartition = 2; int numNodeStopPartition = 2;
int minNodesPerMap = 1; int minNodesPerMap = 1;
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap); NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
LONGS_EQUAL(0, nd.root()->size()); LONGS_EQUAL(0, nd.root()->size());
LONGS_EQUAL(1, nd.root()->frontal().size()); LONGS_EQUAL(1, nd.root()->frontal().size());
LONGS_EQUAL(0, nd.root()->separator().size()); LONGS_EQUAL(0, nd.root()->separator().size());
LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps
// the 1st submap // the 1st submap
LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size());
LONGS_EQUAL(2, nd.root()->children()[0]->size()); LONGS_EQUAL(2, nd.root()->children()[0]->size());
// the 2nd submap // the 2nd submap
LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size());
LONGS_EQUAL(2, nd.root()->children()[1]->size()); LONGS_EQUAL(2, nd.root()->children()[1]->size());
// the 3rd submap // the 3rd submap
LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size());
LONGS_EQUAL(1, nd.root()->children()[2]->size()); LONGS_EQUAL(1, nd.root()->children()[2]->size());
// the 4th submap // the 4th submap
LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size()); LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size());
LONGS_EQUAL(1, nd.root()->children()[3]->size()); LONGS_EQUAL(1, nd.root()->children()[3]->size());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -137,41 +137,41 @@ TEST ( NestedDissection, FourIslands )
// x5 // x5
TEST ( NestedDissection, weekLinks ) TEST ( NestedDissection, weekLinks )
{ {
using namespace submapPlanarSLAM; using namespace submapPlanarSLAM;
typedef TSAM2D::SubNLG SubNLG; typedef TSAM2D::SubNLG SubNLG;
Graph fg; Graph fg;
fg.addOdometry(1, 2, Pose2(), odoNoise); fg.addOdometry(1, 2, Pose2(), odoNoise);
fg.addOdometry(2, 3, Pose2(), odoNoise); fg.addOdometry(2, 3, Pose2(), odoNoise);
fg.addOdometry(2, 4, Pose2(), odoNoise); fg.addOdometry(2, 4, Pose2(), odoNoise);
fg.addOdometry(3, 4, Pose2(), odoNoise); fg.addOdometry(3, 4, Pose2(), odoNoise);
fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise); fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise);
fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise); fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise);
fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise); fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise);
fg.addPoseConstraint(1, Pose2()); fg.addPoseConstraint(1, Pose2());
fg.addPoseConstraint(4, Pose2()); fg.addPoseConstraint(4, Pose2());
fg.addPoseConstraint(5, Pose2()); fg.addPoseConstraint(5, Pose2());
Ordering ordering; ordering += x1, x2, x3, x4, x5, l6; Ordering ordering; ordering += x1, x2, x3, x4, x5, l6;
int numNodeStopPartition = 2; int numNodeStopPartition = 2;
int minNodesPerMap = 1; int minNodesPerMap = 1;
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap); NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
LONGS_EQUAL(0, nd.root()->size()); // one weeklink LONGS_EQUAL(0, nd.root()->size()); // one weeklink
LONGS_EQUAL(1, nd.root()->frontal().size()); LONGS_EQUAL(1, nd.root()->frontal().size());
LONGS_EQUAL(0, nd.root()->separator().size()); LONGS_EQUAL(0, nd.root()->separator().size());
LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps
LONGS_EQUAL(1, nd.root()->weeklinks().size()); LONGS_EQUAL(1, nd.root()->weeklinks().size());
// the 1st submap // the 1st submap
LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4 LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4
LONGS_EQUAL(4, nd.root()->children()[0]->size()); LONGS_EQUAL(4, nd.root()->children()[0]->size());
// the 2nd submap // the 2nd submap
LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6 LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6
LONGS_EQUAL(4, nd.root()->children()[1]->size()); LONGS_EQUAL(4, nd.root()->children()[1]->size());
// //
// the 3rd submap // the 3rd submap
LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5 LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5
LONGS_EQUAL(1, nd.root()->children()[2]->size()); LONGS_EQUAL(1, nd.root()->children()[2]->size());
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -184,86 +184,86 @@ TEST ( NestedDissection, weekLinks )
*/ */
TEST ( NestedDissection, manual_cuts ) TEST ( NestedDissection, manual_cuts )
{ {
using namespace submapPlanarSLAM; using namespace submapPlanarSLAM;
typedef partition::Cuts Cuts; typedef partition::Cuts Cuts;
typedef TSAM2D::SubNLG SubNLG; typedef TSAM2D::SubNLG SubNLG;
typedef partition::PartitionTable PartitionTable; typedef partition::PartitionTable PartitionTable;
Graph fg; Graph fg;
fg.addOdometry(x0, x1, Pose2(1.0, 0, 0), odoNoise); fg.addOdometry(x0, x1, Pose2(1.0, 0, 0), odoNoise);
fg.addOdometry(x1, x2, Pose2(1.0, 0, 0), odoNoise); fg.addOdometry(x1, x2, Pose2(1.0, 0, 0), odoNoise);
fg.addBearingRange(x0, l1, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); fg.addBearingRange(x0, l1, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
fg.addBearingRange(x0, l4, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise); fg.addBearingRange(x0, l4, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise);
fg.addBearingRange(x0, l2, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise); fg.addBearingRange(x0, l2, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x0, l5, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise); fg.addBearingRange(x0, l5, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x1, l1, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise); fg.addBearingRange(x1, l1, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x1, l2, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); fg.addBearingRange(x1, l2, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
fg.addBearingRange(x1, l3, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise); fg.addBearingRange(x1, l3, Rot2::fromAngle( M_PI_4), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x1, l4, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise); fg.addBearingRange(x1, l4, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x1, l5, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); fg.addBearingRange(x1, l5, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
fg.addBearingRange(x1, l6, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise); fg.addBearingRange(x1, l6, Rot2::fromAngle(-M_PI_4), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x2, l2, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise); fg.addBearingRange(x2, l2, Rot2::fromAngle( M_PI_4 * 3), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x2, l5, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise); fg.addBearingRange(x2, l5, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), bearingRangeNoise);
fg.addBearingRange(x2, l3, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise); fg.addBearingRange(x2, l3, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
fg.addBearingRange(x2, l6, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise); fg.addBearingRange(x2, l6, Rot2::fromAngle(-M_PI_2), 1, bearingRangeNoise);
fg.addPrior(x0, Pose2(0.1, 0, 0), priorNoise); fg.addPrior(x0, Pose2(0.1, 0, 0), priorNoise);
// generate ordering // generate ordering
Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6; Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6;
// define cuts // define cuts
boost::shared_ptr<Cuts> cuts(new Cuts()); boost::shared_ptr<Cuts> cuts(new Cuts());
cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable; cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable;
//x0 x1 x2 l1 l2 l3 l4 l5 l6 //x0 x1 x2 l1 l2 l3 l4 l5 l6
(*p)[0]=1; (*p)[1]=0; (*p)[2]=2; (*p)[3]=1; (*p)[4]=0; (*p)[5]=2; (*p)[6]=1; (*p)[7]=0; (*p)[8]=2; (*p)[0]=1; (*p)[1]=0; (*p)[2]=2; (*p)[3]=1; (*p)[4]=0; (*p)[5]=2; (*p)[6]=1; (*p)[7]=0; (*p)[8]=2;
cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts())); cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable; cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable;
//x0 x1 x2 l1 l2 l3 l4 l5 l6 //x0 x1 x2 l1 l2 l3 l4 l5 l6
(*p)[0]=0; (*p)[1]=-1; (*p)[2]=-1; (*p)[3]=1; (*p)[4]=-1; (*p)[5]=-1; (*p)[6]=2; (*p)[7]=-1; (*p)[8]=-1; (*p)[0]=0; (*p)[1]=-1; (*p)[2]=-1; (*p)[3]=1; (*p)[4]=-1; (*p)[5]=-1; (*p)[6]=2; (*p)[7]=-1; (*p)[8]=-1;
cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts())); cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable; cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable;
//x0 x1 x2 l1 l2 l3 l4 l5 l6 //x0 x1 x2 l1 l2 l3 l4 l5 l6
(*p)[0]=-1; (*p)[1]=-1; (*p)[2]=0; (*p)[3]=-1; (*p)[4]=-1; (*p)[5]=1; (*p)[6]=-1; (*p)[7]=-1; (*p)[8]=2; (*p)[0]=-1; (*p)[1]=-1; (*p)[2]=0; (*p)[3]=-1; (*p)[4]=-1; (*p)[5]=1; (*p)[6]=-1; (*p)[7]=-1; (*p)[8]=2;
// nested dissection // nested dissection
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, cuts); NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, cuts);
LONGS_EQUAL(2, nd.root()->size()); LONGS_EQUAL(2, nd.root()->size());
LONGS_EQUAL(3, nd.root()->frontal().size()); LONGS_EQUAL(3, nd.root()->frontal().size());
LONGS_EQUAL(0, nd.root()->separator().size()); LONGS_EQUAL(0, nd.root()->separator().size());
LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
LONGS_EQUAL(0, nd.root()->weeklinks().size()); LONGS_EQUAL(0, nd.root()->weeklinks().size());
// the 1st submap // the 1st submap
LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0 LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0
LONGS_EQUAL(4, nd.root()->children()[0]->size()); LONGS_EQUAL(4, nd.root()->children()[0]->size());
LONGS_EQUAL(2, nd.root()->children()[0]->children().size()); LONGS_EQUAL(2, nd.root()->children()[0]->children().size());
// the 1-1st submap // the 1-1st submap
LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1 LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1
LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size()); LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size());
// the 1-2nd submap // the 1-2nd submap
LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4 LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4
LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size()); LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size());
// the 2nd submap // the 2nd submap
LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2 LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2
LONGS_EQUAL(3, nd.root()->children()[1]->size()); LONGS_EQUAL(3, nd.root()->children()[1]->size());
LONGS_EQUAL(2, nd.root()->children()[1]->children().size()); LONGS_EQUAL(2, nd.root()->children()[1]->children().size());
// the 2-1st submap // the 2-1st submap
LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3 LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3
LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size()); LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size());
// the 2-2nd submap // the 2-2nd submap
LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6 LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6
LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size()); LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size());
} }
@ -272,65 +272,65 @@ TEST ( NestedDissection, manual_cuts )
// / | / \ | \ // / | / \ | \
// x0 x1 x2 x3 // x0 x1 x2 x3
TEST( NestedDissection, Graph3D) { TEST( NestedDissection, Graph3D) {
using namespace gtsam::submapVisualSLAM; using namespace gtsam::submapVisualSLAM;
typedef TSAM3D::SubNLG SubNLG; typedef TSAM3D::SubNLG SubNLG;
typedef partition::PartitionTable PartitionTable; typedef partition::PartitionTable PartitionTable;
vector<GeneralCamera> cameras; vector<GeneralCamera> cameras;
cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-2., 0., 0.)))); cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-2., 0., 0.))));
cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-1., 0., 0.)))); cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3(-1., 0., 0.))));
cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 1., 0., 0.)))); cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 1., 0., 0.))));
cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 2., 0., 0.)))); cameras.push_back(GeneralCamera(Pose3(Rot3(), Point3( 2., 0., 0.))));
vector<Point3> points; vector<Point3> points;
for (int cube_index = 0; cube_index <= 3; cube_index++) { for (int cube_index = 0; cube_index <= 3; cube_index++) {
Point3 center((cube_index-1) * 3, 0.5, 10.); Point3 center((cube_index-1) * 3, 0.5, 10.);
points.push_back(center + Point3(-0.5, -0.5, -0.5)); points.push_back(center + Point3(-0.5, -0.5, -0.5));
points.push_back(center + Point3(-0.5, 0.5, -0.5)); points.push_back(center + Point3(-0.5, 0.5, -0.5));
points.push_back(center + Point3( 0.5, 0.5, -0.5)); points.push_back(center + Point3( 0.5, 0.5, -0.5));
points.push_back(center + Point3( 0.5, -0.5, -0.5)); points.push_back(center + Point3( 0.5, -0.5, -0.5));
points.push_back(center + Point3(-0.5, -0.5, 0.5)); points.push_back(center + Point3(-0.5, -0.5, 0.5));
points.push_back(center + Point3(-0.5, 0.5, 0.5)); points.push_back(center + Point3(-0.5, 0.5, 0.5));
points.push_back(center + Point3( 0.5, 0.5, 0.5)); points.push_back(center + Point3( 0.5, 0.5, 0.5));
points.push_back(center + Point3( 0.5, 0.5, 0.5)); points.push_back(center + Point3( 0.5, 0.5, 0.5));
} }
Graph graph; Graph graph;
SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.)); SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.));
SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.)); SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.));
for (int j=1; j<=8; j++) for (int j=1; j<=8; j++)
graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
for (int j=1; j<=16; j++) for (int j=1; j<=16; j++)
graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
for (int j=9; j<=24; j++) for (int j=9; j<=24; j++)
graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
for (int j=17; j<=24; j++) for (int j=17; j<=24; j++)
graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise); graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
// make an easy ordering // make an easy ordering
Ordering ordering; ordering += x0, x1, x2, x3; Ordering ordering; ordering += x0, x1, x2, x3;
for (int j=1; j<=24; j++) for (int j=1; j<=24; j++)
ordering += Symbol('l', j); ordering += Symbol('l', j);
// nested dissection // nested dissection
const int numNodeStopPartition = 10; const int numNodeStopPartition = 10;
const int minNodesPerMap = 5; const int minNodesPerMap = 5;
NestedDissection<Graph, SubNLG, GenericGraph3D> nd(graph, ordering, numNodeStopPartition, minNodesPerMap); NestedDissection<Graph, SubNLG, GenericGraph3D> nd(graph, ordering, numNodeStopPartition, minNodesPerMap);
LONGS_EQUAL(0, nd.root()->size()); LONGS_EQUAL(0, nd.root()->size());
LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16 LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16
LONGS_EQUAL(0, nd.root()->separator().size()); LONGS_EQUAL(0, nd.root()->separator().size());
LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
LONGS_EQUAL(0, nd.root()->weeklinks().size()); LONGS_EQUAL(0, nd.root()->weeklinks().size());
// the 1st submap // the 1st submap
LONGS_EQUAL(10, nd.root()->children()[0]->frontal().size()); // x0, x1, l1-l8 LONGS_EQUAL(10, nd.root()->children()[0]->frontal().size()); // x0, x1, l1-l8
LONGS_EQUAL(24, nd.root()->children()[0]->size()); // 8 + 16 LONGS_EQUAL(24, nd.root()->children()[0]->size()); // 8 + 16
LONGS_EQUAL(0, nd.root()->children()[0]->children().size()); LONGS_EQUAL(0, nd.root()->children()[0]->children().size());
// the 2nd submap // the 2nd submap
LONGS_EQUAL(10, nd.root()->children()[1]->frontal().size()); // x2, x3, l1-l8 LONGS_EQUAL(10, nd.root()->children()[1]->frontal().size()); // x2, x3, l1-l8
LONGS_EQUAL(24, nd.root()->children()[1]->size()); // 16 + 8 LONGS_EQUAL(24, nd.root()->children()[1]->size()); // 16 + 8
LONGS_EQUAL(0, nd.root()->children()[1]->children().size()); LONGS_EQUAL(0, nd.root()->children()[1]->children().size());
} }

100
gtsam_unstable/slam/BetweenFactorEM.h
View File

@ -295,87 +295,87 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
SharedGaussian get_model_inlier() const { SharedGaussian get_model_inlier() const {
return model_inlier_; return model_inlier_;
} }
/* ************************************************************************* */ /* ************************************************************************* */
SharedGaussian get_model_outlier() const { SharedGaussian get_model_outlier() const {
return model_outlier_; return model_outlier_;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix get_model_inlier_cov() const { Matrix get_model_inlier_cov() const {
return (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); return (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix get_model_outlier_cov() const { Matrix get_model_outlier_cov() const {
return (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); return (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
} }
/* ************************************************************************* */ /* ************************************************************************* */
void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){ void updateNoiseModels(const gtsam::Values& values, const gtsam::NonlinearFactorGraph& graph){
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
* (note these are given in the E step, where indicator probabilities are calculated). * (note these are given in the E step, where indicator probabilities are calculated).
* *
* Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
* unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
* *
* TODO: improve efficiency (info form) * TODO: improve efficiency (info form)
*/ */
// get joint covariance of the involved states // get joint covariance of the involved states
std::vector<gtsam::Key> Keys; std::vector<gtsam::Key> Keys;
Keys.push_back(key1_); Keys.push_back(key1_);
Keys.push_back(key2_); Keys.push_back(key2_);
Marginals marginals( graph, values, Marginals::QR ); Marginals marginals( graph, values, Marginals::QR );
JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys); JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
Matrix cov1 = joint_marginal12(key1_, key1_); Matrix cov1 = joint_marginal12(key1_, key1_);
Matrix cov2 = joint_marginal12(key2_, key2_); Matrix cov2 = joint_marginal12(key2_, key2_);
Matrix cov12 = joint_marginal12(key1_, key2_); Matrix cov12 = joint_marginal12(key1_, key2_);
updateNoiseModels_givenCovs(values, cov1, cov2, cov12); updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
} }
/* ************************************************************************* */ /* ************************************************************************* */
void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){ void updateNoiseModels_givenCovs(const gtsam::Values& values, const Matrix& cov1, const Matrix& cov2, const Matrix& cov12){
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories /* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
* (note these are given in the E step, where indicator probabilities are calculated). * (note these are given in the E step, where indicator probabilities are calculated).
* *
* Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the * Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
* unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes). * unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
* *
* TODO: improve efficiency (info form) * TODO: improve efficiency (info form)
*/ */
const T& p1 = values.at<T>(key1_); const T& p1 = values.at<T>(key1_);
const T& p2 = values.at<T>(key2_); const T& p2 = values.at<T>(key2_);
Matrix H1, H2; Matrix H1, H2;
T hx = p1.between(p2, H1, H2); // h(x) T hx = p1.between(p2, H1, H2); // h(x)
Matrix H; Matrix H;
H.resize(H1.rows(), H1.rows()+H2.rows()); H.resize(H1.rows(), H1.rows()+H2.rows());
H << H1, H2; // H = [H1 H2] H << H1, H2; // H = [H1 H2]
Matrix joint_cov; Matrix joint_cov;
joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols()); joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols());
joint_cov << cov1, cov12, joint_cov << cov1, cov12,
cov12.transpose(), cov2; cov12.transpose(), cov2;
Matrix cov_state = H*joint_cov*H.transpose(); Matrix cov_state = H*joint_cov*H.transpose();
// model_inlier_->print("before:"); // model_inlier_->print("before:");
// update inlier and outlier noise models // update inlier and outlier noise models
Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse(); Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state); model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse(); Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state); model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
// model_inlier_->print("after:"); // model_inlier_->print("after:");
// std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl; // std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */

105
gtsam_unstable/slam/BiasedGPSFactor.h Normal file
View File

@ -0,0 +1,105 @@
/* ----------------------------------------------------------------------------
* 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 BiasedGPSFactor.h
* @author Luca Carlone
**/
#pragma once
#include <ostream>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam {
/**
* A class to model GPS measurements, including a bias term which models
* common-mode errors and that can be partially corrected if other sensors are used
* @addtogroup SLAM
*/
class BiasedGPSFactor: public NoiseModelFactor2<Pose3, Point3> {
private:
typedef BiasedGPSFactor This;
typedef NoiseModelFactor2<Pose3, Point3> Base;
Point3 measured_; /** The measurement */
public:
// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<BiasedGPSFactor> shared_ptr;
/** default constructor - only use for serialization */
BiasedGPSFactor() {}
/** Constructor */
BiasedGPSFactor(Key posekey, Key biaskey, const Point3 measured,
const SharedNoiseModel& model) :
Base(model, posekey, biaskey), measured_(measured) {
}
virtual ~BiasedGPSFactor() {}
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "BiasedGPSFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
measured_.print(" measured: ");
this->noiseModel_->print(" noise model: ");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol) && this->measured_.equals(e->measured_, tol);
}
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const Pose3& pose, const Point3& bias,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H2 =
boost::none) const {
if (H1 || H2){
H1->resize(3,6); // jacobian wrt pose
(*H1) << Matrix3::Zero(), pose.rotation().matrix();
H2->resize(3,3); // jacobian wrt bias
(*H2) << Matrix3::Identity();
}
return pose.translation().vector() + bias.vector() - measured_.vector();
}
/** return the measured */
const Point3 measured() const {
return measured_;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(measured_);
}
}; // \class BiasedGPSFactor
} /// namespace gtsam

135
gtsam_unstable/slam/GaussMarkov1stOrderFactor.h Normal file
View File

@ -0,0 +1,135 @@
/* ----------------------------------------------------------------------------
* 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 GaussMarkov1stOrderFactor.h
* @author Vadim Indelman, Stephen Williams, Luca Carlone
* @date Jan 17, 2012
**/
#pragma once
#include <ostream>
#include <gtsam/base/Testable.h>
#include <gtsam/base/Lie.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/linear/NoiseModel.h>
namespace gtsam {
/*
* - The 1st order GaussMarkov factor relates two keys of the same type. This relation is given via
* key_2 = exp(-1/tau*delta_t) * key1 + w_d
* where tau is the time constant and delta_t is the time difference between the two keys.
* w_d is the equivalent discrete noise, whose covariance is calculated from the continuous noise model and delta_t.
* - w_d is approximated as a Gaussian noise.
* - In the multi-dimensional case, tau is a vector, and the above equation is applied on each element
* in the state (represented by keys), using the appropriate time constant in the vector tau.
*/
/*
* A class for a measurement predicted by "GaussMarkov1stOrderFactor(config[key1],config[key2])"
* KEY1::Value is the Lie Group type
* T is the measurement type, by default the same
*/
template<class VALUE>
class GaussMarkov1stOrderFactor: public NoiseModelFactor2<VALUE, VALUE> {
private:
typedef GaussMarkov1stOrderFactor<VALUE> This;
typedef NoiseModelFactor2<VALUE, VALUE> Base;
double dt_;
Vector tau_;
public:
// shorthand for a smart pointer to a factor
typedef typename boost::shared_ptr<GaussMarkov1stOrderFactor> shared_ptr;
/** default constructor - only use for serialization */
GaussMarkov1stOrderFactor() {}
/** Constructor */
GaussMarkov1stOrderFactor(const Key& key1, const Key& key2, double delta_t, Vector tau,
const SharedGaussian& model) :
Base(calcDiscreteNoiseModel(model, delta_t), key1, key2), dt_(delta_t), tau_(tau) {
}
virtual ~GaussMarkov1stOrderFactor() {}
/** implement functions needed for Testable */
/** print */
virtual void print(const std::string& s, const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "GaussMarkov1stOrderFactor("
<< keyFormatter(this->key1()) << ","
<< keyFormatter(this->key2()) << ")\n";
this->noiseModel_->print(" noise model");
}
/** equals */
virtual bool equals(const NonlinearFactor& expected, double tol=1e-9) const {
const This *e = dynamic_cast<const This*> (&expected);
return e != NULL && Base::equals(*e, tol);
}
/** implement functions needed to derive from Factor */
/** vector of errors */
Vector evaluateError(const VALUE& p1, const VALUE& p2,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const {
Vector v1( VALUE::Logmap(p1) );
Vector v2( VALUE::Logmap(p2) );
Vector alpha(tau_.size());
Vector alpha_v1(tau_.size());
for(int i=0; i<tau_.size(); i++){
alpha(i) = exp(- 1/tau_(i)*dt_ );
alpha_v1(i) = alpha(i) * v1(i);
}
Vector hx(v2 - alpha_v1);
if(H1) *H1 = - diag(alpha);
if(H2) *H2 = eye(v2.size());
return hx;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(dt_);
ar & BOOST_SERIALIZATION_NVP(tau_);
}
SharedGaussian calcDiscreteNoiseModel(const SharedGaussian& model, double delta_t){
/* Q_d (approx)= Q * delta_t */
/* In practice, square root of the information matrix is represented, so that:
* R_d (approx)= R / sqrt(delta_t)
* */
noiseModel::Gaussian::shared_ptr gaussian_model = boost::dynamic_pointer_cast<noiseModel::Gaussian>(model);
SharedGaussian model_d(noiseModel::Gaussian::SqrtInformation(gaussian_model->R()/sqrt(delta_t)));
return model_d;
}
}; // \class GaussMarkov1stOrderFactor
} /// namespace gtsam

181
gtsam_unstable/slam/ProjectionFactorPPP.h Normal file
View File

@ -0,0 +1,181 @@
/* ----------------------------------------------------------------------------
* 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 ProjectionFactorPPP.h
* @brief Derived from ProjectionFactor, but estimates body-camera transform
* in addition to body pose and 3D landmark
* @author Chris Beall
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <boost/optional.hpp>
namespace gtsam {
/**
* Non-linear factor for a constraint derived from a 2D measurement. The calibration is known here.
* i.e. the main building block for visual SLAM.
* @addtogroup SLAM
*/
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class ProjectionFactorPPP: public NoiseModelFactor3<POSE, POSE, LANDMARK> {
protected:
// Keep a copy of measurement and calibration for I/O
Point2 measured_; ///< 2D measurement
boost::shared_ptr<CALIBRATION> K_; ///< shared pointer to calibration object
// verbosity handling for Cheirality Exceptions
bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
public:
/// shorthand for base class type
typedef NoiseModelFactor3<POSE, POSE, LANDMARK> Base;
/// shorthand for this class
typedef ProjectionFactorPPP<POSE, LANDMARK, CALIBRATION> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
ProjectionFactorPPP() : throwCheirality_(false), verboseCheirality_(false) {}
/**
* Constructor
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation
* @param poseKey is the index of the camera
* @param transformKey is the index of the body-camera transform
* @param pointKey is the index of the landmark
* @param K shared pointer to the constant calibration
*/
ProjectionFactorPPP(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key transformKey, Key pointKey,
const boost::shared_ptr<CALIBRATION>& K) :
Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K),
throwCheirality_(false), verboseCheirality_(false) {}
/**
* Constructor with exception-handling flags
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation
* @param poseKey is the index of the camera
* @param pointKey is the index of the landmark
* @param K shared pointer to the constant calibration
* @param throwCheirality determines whether Cheirality exceptions are rethrown
* @param verboseCheirality determines whether exceptions are printed for Cheirality
*/
ProjectionFactorPPP(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key transformKey, Key pointKey,
const boost::shared_ptr<CALIBRATION>& K,
bool throwCheirality, bool verboseCheirality) :
Base(model, poseKey, transformKey, pointKey), measured_(measured), K_(K),
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
/** Virtual destructor */
virtual ~ProjectionFactorPPP() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "ProjectionFactorPPP, z = ";
measured_.print();
Base::print("", keyFormatter);
}
/// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol)
&& this->K_->equals(*e->K_, tol);
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const {
try {
if(H1 || H2 || H3) {
gtsam::Matrix H0, H02;
PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), *K_);
Point2 reprojectionError(camera.project(point, H1, H3) - measured_);
*H2 = *H1 * H02;
*H1 = *H1 * H0;
return reprojectionError.vector();
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(transform), *K_);
Point2 reprojectionError(camera.project(point, H1, H3) - measured_);
return reprojectionError.vector();
}
} catch( CheiralityException& e) {
if (H1) *H1 = zeros(2,6);
if (H2) *H2 = zeros(2,6);
if (H3) *H3 = zeros(2,3);
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
if (throwCheirality_)
throw e;
}
return ones(2) * 2.0 * K_->fx();
}
/** return the measurement */
const Point2& measured() const {
return measured_;
}
/** return the calibration object */
inline const boost::shared_ptr<CALIBRATION> calibration() const {
return K_;
}
/** return verbosity */
inline bool verboseCheirality() const { return verboseCheirality_; }
/** return flag for throwing cheirality exceptions */
inline bool throwCheirality() const { return throwCheirality_; }
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(K_);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
}
};
} // \ namespace gtsam

171
gtsam_unstable/slam/ProjectionFactorPPPC.h Normal file
View File

@ -0,0 +1,171 @@
/* ----------------------------------------------------------------------------
* 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 ProjectionFactorPPPC.h
* @brief Derived from ProjectionFactor, but estimates body-camera transform
* and calibration in addition to body pose and 3D landmark
* @author Chris Beall
*/
#pragma once
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <boost/optional.hpp>
namespace gtsam {
/**
* Non-linear factor for a constraint derived from a 2D measurement. This factor
* estimates the body pose, body-camera transform, 3D landmark, and calibration.
* @addtogroup SLAM
*/
template<class POSE, class LANDMARK, class CALIBRATION = Cal3_S2>
class ProjectionFactorPPPC: public NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> {
protected:
Point2 measured_; ///< 2D measurement
// verbosity handling for Cheirality Exceptions
bool throwCheirality_; ///< If true, rethrows Cheirality exceptions (default: false)
bool verboseCheirality_; ///< If true, prints text for Cheirality exceptions (default: false)
public:
/// shorthand for base class type
typedef NoiseModelFactor4<POSE, POSE, LANDMARK, CALIBRATION> Base;
/// shorthand for this class
typedef ProjectionFactorPPPC<POSE, LANDMARK, CALIBRATION> This;
/// shorthand for a smart pointer to a factor
typedef boost::shared_ptr<This> shared_ptr;
/// Default constructor
ProjectionFactorPPPC() : throwCheirality_(false), verboseCheirality_(false) {}
/**
* Constructor
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation
* @param poseKey is the index of the camera
* @param pointKey is the index of the landmark
* @param K shared pointer to the constant calibration
*/
ProjectionFactorPPPC(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key transformKey, Key pointKey, Key calibKey) :
Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured),
throwCheirality_(false), verboseCheirality_(false) {}
/**
* Constructor with exception-handling flags
* TODO: Mark argument order standard (keys, measurement, parameters)
* @param measured is the 2 dimensional location of point in image (the measurement)
* @param model is the standard deviation
* @param poseKey is the index of the camera
* @param pointKey is the index of the landmark
* @param K shared pointer to the constant calibration
* @param throwCheirality determines whether Cheirality exceptions are rethrown
* @param verboseCheirality determines whether exceptions are printed for Cheirality
*/
ProjectionFactorPPPC(const Point2& measured, const SharedNoiseModel& model,
Key poseKey, Key transformKey, Key pointKey, Key calibKey,
bool throwCheirality, bool verboseCheirality) :
Base(model, poseKey, transformKey, pointKey, calibKey), measured_(measured),
throwCheirality_(throwCheirality), verboseCheirality_(verboseCheirality) {}
/** Virtual destructor */
virtual ~ProjectionFactorPPPC() {}
/// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const {
return boost::static_pointer_cast<gtsam::NonlinearFactor>(
gtsam::NonlinearFactor::shared_ptr(new This(*this))); }
/**
* print
* @param s optional string naming the factor
* @param keyFormatter optional formatter useful for printing Symbols
*/
void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "ProjectionFactorPPPC, z = ";
measured_.print();
Base::print("", keyFormatter);
}
/// equals
virtual bool equals(const NonlinearFactor& p, double tol = 1e-9) const {
const This *e = dynamic_cast<const This*>(&p);
return e
&& Base::equals(p, tol)
&& this->measured_.equals(e->measured_, tol);
}
/// Evaluate error h(x)-z and optionally derivatives
Vector evaluateError(const Pose3& pose, const Pose3& transform, const Point3& point, const CALIBRATION& K,
boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none,
boost::optional<Matrix&> H4 = boost::none) const {
try {
if(H1 || H2 || H3 || H4) {
gtsam::Matrix H0, H02;
PinholeCamera<CALIBRATION> camera(pose.compose(transform, H0, H02), K);
Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
*H2 = *H1 * H02;
*H1 = *H1 * H0;
return reprojectionError.vector();
} else {
PinholeCamera<CALIBRATION> camera(pose.compose(transform), K);
Point2 reprojectionError(camera.project(point, H1, H3, H4) - measured_);
return reprojectionError.vector();
}
} catch( CheiralityException& e) {
if (H1) *H1 = zeros(2,6);
if (H2) *H2 = zeros(2,6);
if (H3) *H3 = zeros(2,3);
if (H4) *H4 = zeros(2,CALIBRATION::Dim());
if (verboseCheirality_)
std::cout << e.what() << ": Landmark "<< DefaultKeyFormatter(this->key2()) <<
" moved behind camera " << DefaultKeyFormatter(this->key1()) << std::endl;
if (throwCheirality_)
throw e;
}
return ones(2) * 2.0 * K.fx();
}
/** return the measurement */
const Point2& measured() const {
return measured_;
}
/** return verbosity */
inline bool verboseCheirality() const { return verboseCheirality_; }
/** return flag for throwing cheirality exceptions */
inline bool throwCheirality() const { return throwCheirality_; }
private:
/// Serialization function
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
ar & BOOST_SERIALIZATION_NVP(measured_);
ar & BOOST_SERIALIZATION_NVP(throwCheirality_);
ar & BOOST_SERIALIZATION_NVP(verboseCheirality_);
}
};
} // \ namespace gtsam

Some files were not shown because too many files have changed in this diff Show More