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
commit
a9e3545a29
202
.cproject
202
.cproject
|
|
@ -1,19 +1,17 @@
|
|||
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
|
||||
<?fileVersion 4.0.0?>
|
||||
|
||||
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
|
||||
<storageModule moduleId="org.eclipse.cdt.core.settings">
|
||||
<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">
|
||||
<externalSettings/>
|
||||
<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.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.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.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
</extensions>
|
||||
</storageModule>
|
||||
<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">
|
||||
<externalSettings/>
|
||||
<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.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.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.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
</extensions>
|
||||
</storageModule>
|
||||
<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">
|
||||
<externalSettings/>
|
||||
<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.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.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.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
|
||||
</extensions>
|
||||
</storageModule>
|
||||
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
|
||||
|
|
@ -790,18 +788,18 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>tests/testPose3.run</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testGaussianFactorGraphUnordered.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>all</buildTarget>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testGaussianBayesNetUnordered.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -1008,8 +1006,8 @@
|
|||
</target>
|
||||
<target name="schedulingQuals13.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>schedulingQuals13.run</buildTarget>
|
||||
<buildArguments/>
|
||||
<buildTarget>testErrors.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -1238,7 +1236,47 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testGaussianISAM2.run</buildTarget>
|
||||
|
|
@ -1318,7 +1356,14 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>timing.tests</buildTarget>
|
||||
|
|
@ -1352,16 +1397,14 @@
|
|||
</target>
|
||||
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments/>
|
||||
<buildTarget>testGraph.run</buildTarget>
|
||||
<buildTarget>testSimulated2D.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>
|
||||
<buildTarget>testSimulated3D.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -1374,47 +1417,47 @@
|
|||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testGaussianISAM.run</buildTarget>
|
||||
<buildTarget>testEliminationTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testDoglegOptimizer.run</buildTarget>
|
||||
<buildTarget>testInference.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testNonlinearFactorGraph.run</buildTarget>
|
||||
<buildTarget>testKey.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testIterative.run</buildTarget>
|
||||
<buildArguments>-j1</buildArguments>
|
||||
<buildTarget>testSymbolicBayesTree.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testSubgraphSolver.run</buildTarget>
|
||||
<buildArguments>-j1</buildArguments>
|
||||
<buildTarget>testSymbolicSequentialSolver.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<useDefaultCommand>false</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>testGaussianFactorGraphB.run</buildTarget>
|
||||
|
|
@ -1624,8 +1667,40 @@
|
|||
</target>
|
||||
<target name="testDSFVector.run" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j2</buildArguments>
|
||||
<buildTarget>testDSFVector.run</buildTarget>
|
||||
<buildArguments>VERBOSE=1</buildArguments>
|
||||
<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>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -2299,18 +2374,26 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>SelfCalibrationExample.run</buildTarget>
|
||||
<buildArguments/>
|
||||
<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>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</target>
|
||||
<target name="SFMExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j5</buildArguments>
|
||||
<buildTarget>SFMExample.run</buildTarget>
|
||||
<buildArguments/>
|
||||
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
@ -2497,7 +2580,31 @@
|
|||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
</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>
|
||||
<buildArguments>-j2 VERBOSE=1</buildArguments>
|
||||
<buildTarget>check.geometry</buildTarget>
|
||||
|
|
@ -2779,8 +2886,7 @@
|
|||
</target>
|
||||
<target name="check.nonlinear_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
|
||||
<buildCommand>make</buildCommand>
|
||||
<buildArguments>-j6 -j8</buildArguments>
|
||||
<buildTarget>check.nonlinear_unstable</buildTarget>
|
||||
<buildTarget>tests/testGaussianISAM2</buildTarget>
|
||||
<stopOnError>true</stopOnError>
|
||||
<useDefaultCommand>true</useDefaultCommand>
|
||||
<runAllBuilders>true</runAllBuilders>
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/build*
|
||||
*.pyc
|
||||
*.DS_Store
|
||||
/debug/
|
||||
*.txt.user
|
||||
/release/
|
||||
/examples/Data/dubrovnik-3-7-pre-rewritten.txt
|
||||
/examples/Data/pose2example-rewritten.txt
|
||||
/examples/Data/pose3example-rewritten.txt
|
||||
|
|
|
|||
|
|
@ -2,9 +2,15 @@
|
|||
project(GTSAM CXX C)
|
||||
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 (GTSAM_VERSION_MAJOR 3)
|
||||
set (GTSAM_VERSION_MINOR 1)
|
||||
set (GTSAM_VERSION_MINOR 2)
|
||||
set (GTSAM_VERSION_PATCH 0)
|
||||
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
|
||||
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
|
||||
|
|
@ -123,6 +129,11 @@ else()
|
|||
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_package(TBB)
|
||||
|
|
@ -169,9 +180,9 @@ endif()
|
|||
|
||||
###############################################################################
|
||||
# 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)
|
||||
find_package(OpenMP) # do this here to generate correct message if disabled
|
||||
|
||||
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)
|
||||
set(GTSAM_USE_EIGEN_MKL_OPENMP 1) # This will go into config.h
|
||||
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
|
||||
|
|
|
|||
|
|
@ -1,5 +1,6 @@
|
|||
README - Georgia Tech Smoothing and Mapping library
|
||||
===================================================
|
||||
Version: Pre-Release 3.2.0
|
||||
|
||||
What is GTSAM?
|
||||
--------------
|
||||
|
|
|
|||
|
|
@ -58,6 +58,7 @@ FIND_PATH(MKL_ROOT_DIR
|
|||
/opt/intel/mkl/*/
|
||||
/opt/intel/cmkl/
|
||||
/opt/intel/cmkl/*/
|
||||
/opt/intel/*/mkl/
|
||||
/Library/Frameworks/Intel_MKL.framework/Versions/Current/lib/universal
|
||||
"C:/Program Files (x86)/Intel/ComposerXE-2011/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/
|
||||
)
|
||||
|
||||
FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY
|
||||
mkl_gnu_thread
|
||||
PATHS
|
||||
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
|
||||
${MKL_ROOT_DIR}/lib/
|
||||
)
|
||||
|
||||
# MKL on Mac OS doesn't ship with GNU thread versions, only Intel versions (see above)
|
||||
IF(NOT APPLE)
|
||||
FIND_LIBRARY(MKL_GNUTHREAD_LIBRARY
|
||||
mkl_gnu_thread
|
||||
PATHS
|
||||
${MKL_ROOT_DIR}/lib/${MKL_ARCH_DIR}
|
||||
${MKL_ROOT_DIR}/lib/
|
||||
)
|
||||
ENDIF()
|
||||
|
||||
# Intel Libraries
|
||||
IF("${MKL_ARCH_DIR}" STREQUAL "32")
|
||||
|
|
@ -226,7 +230,12 @@ ELSE() # UNIX and macOS
|
|||
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
|
||||
MKL_SEQUENTIAL_LIBRARY MKL_INTELTHREAD_LIBRARY MKL_GNUTHREAD_LIBRARY)
|
||||
ENDIF()
|
||||
|
|
|
|||
|
|
@ -0,0 +1 @@
|
|||
718.856 718.856 0.0 607.1928 185.2157 0.5371657189
|
||||
|
|
@ -0,0 +1 @@
|
|||
718.856 718.856 0.0 607.1928 185.2157 0.5371657189
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -120,15 +120,15 @@ int main(int argc, char** argv) {
|
|||
// 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));
|
||||
// Create odometry (Between) factors between consecutive poses
|
||||
graph.push_back(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>(1, 2, 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
|
||||
// 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
|
||||
graph.push_back(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
|
||||
graph.push_back(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>(1, 0.0, 0.0, unaryNoise));
|
||||
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
|
||||
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));
|
||||
graph.print("\nFactor Graph:\n"); // print
|
||||
|
||||
// 3. Create the data structure to hold the initialEstimate estimate to the solution
|
||||
|
|
|
|||
|
|
@ -65,15 +65,15 @@ int main(int argc, char** argv) {
|
|||
// A prior factor consists of a mean and a noise model (covariance matrix)
|
||||
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));
|
||||
graph.push_back(PriorFactor<Pose2>(1, priorMean, priorNoise));
|
||||
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
|
||||
|
||||
// Add odometry factors
|
||||
Pose2 odometry(2.0, 0.0, 0.0);
|
||||
// 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));
|
||||
// Create odometry (Between) factors between consecutive poses
|
||||
graph.push_back(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
|
||||
graph.push_back(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
|
||||
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
|
||||
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
|
||||
graph.print("\nFactor Graph:\n"); // print
|
||||
|
||||
// Create the data structure to hold the initialEstimate estimate to the solution
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
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
|
||||
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
|
||||
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
|
||||
graph.push_back(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
|
||||
graph.push_back(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
|
||||
graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
|
||||
graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
|
||||
|
||||
// Add Range-Bearing measurements to two different landmarks
|
||||
// create a noise model for the landmark measurements
|
||||
|
|
@ -101,9 +101,9 @@ int main(int argc, char** argv) {
|
|||
range32 = 2.0;
|
||||
|
||||
// Add Bearing-Range factors
|
||||
graph.push_back(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
|
||||
graph.push_back(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
|
||||
graph.push_back(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
|
||||
graph.add(BearingRangeFactor<Pose2, Point2>(x1, l1, bearing11, range11, measurementNoise));
|
||||
graph.add(BearingRangeFactor<Pose2, Point2>(x2, l1, bearing21, range21, measurementNoise));
|
||||
graph.add(BearingRangeFactor<Pose2, Point2>(x3, l2, bearing32, range32, measurementNoise));
|
||||
|
||||
// Print
|
||||
graph.print("Factor Graph:\n");
|
||||
|
|
|
|||
|
|
@ -72,23 +72,23 @@ int main(int argc, char** argv) {
|
|||
// 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)
|
||||
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
|
||||
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
|
||||
|
||||
// 2b. Add odometry factors
|
||||
// Create odometry (Between) factors between consecutive poses
|
||||
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
|
||||
graph.push_back(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.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2, 0, M_PI_2), model));
|
||||
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));
|
||||
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2, 0, M_PI_2), model));
|
||||
graph.add(BetweenFactor<Pose2>(3, 4, 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
|
||||
// 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
|
||||
// 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
|
||||
|
||||
// 3. Create the data structure to hold the initialEstimate estimate to the solution
|
||||
|
|
|
|||
|
|
@ -26,36 +26,72 @@
|
|||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber)
|
||||
int main(const int argc, const char *argv[]) {
|
||||
|
||||
// Read graph from file
|
||||
string g2oFile;
|
||||
if (argc < 2)
|
||||
g2oFile = findExampleDataFile("noisyToyGraph.txt");
|
||||
else
|
||||
g2oFile = argv[1];
|
||||
string kernelType = "none";
|
||||
int maxIterations = 100; // default
|
||||
string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default
|
||||
|
||||
// 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;
|
||||
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
|
||||
NonlinearFactorGraph graphWithPrior = *graph;
|
||||
noiseModel::Diagonal::shared_ptr priorModel = //
|
||||
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
|
||||
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
|
||||
std::cout << "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;
|
||||
GaussNewtonOptimizer optimizer(graphWithPrior, *initial);
|
||||
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);
|
||||
NonlinearFactorGraph::shared_ptr graphNoKernel;
|
||||
Values::shared_ptr initial2;
|
||||
boost::tie(graphNoKernel, initial2) = readG2o(g2oFile);
|
||||
writeG2o(*graphNoKernel, result, outputFile);
|
||||
std::cout << "done! " << std::endl;
|
||||
}
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -13,6 +13,22 @@
|
|||
* @brief Incremental and batch solving, timing, and accuracy comparisons
|
||||
* @author Richard Roberts
|
||||
* @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>
|
||||
|
|
|
|||
|
|
@ -14,6 +14,7 @@
|
|||
* @brief A visualSLAM example for the structure-from-motion problem on a simulated dataset
|
||||
* This version uses iSAM to solve the problem incrementally
|
||||
* @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));
|
||||
|
||||
// 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
|
||||
vector<Point3> points = createPoints();
|
||||
|
|
@ -69,7 +71,8 @@ int main(int argc, char* argv[]) {
|
|||
// Create the set of ground-truth poses
|
||||
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;
|
||||
NonlinearISAM isam(relinearizeInterval);
|
||||
|
||||
|
|
@ -82,32 +85,44 @@ int main(int argc, char* argv[]) {
|
|||
|
||||
// Add factors for each landmark observation
|
||||
for (size_t j = 0; j < points.size(); ++j) {
|
||||
// Create ground truth measurement
|
||||
SimpleCamera camera(poses[i], *K);
|
||||
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
|
||||
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
|
||||
// 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
|
||||
// adding it to iSAM.
|
||||
if( i == 0) {
|
||||
// Add a prior on pose x0
|
||||
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
|
||||
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
|
||||
if (i == 0) {
|
||||
// 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)));
|
||||
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
|
||||
|
||||
// Add a prior on landmark l0
|
||||
noiseModel::Isotropic::shared_ptr pointNoise = noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
|
||||
noiseModel::Isotropic::shared_ptr pointNoise =
|
||||
noiseModel::Isotropic::Sigma(3, 0.1);
|
||||
graph.add(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise));
|
||||
|
||||
// Add initial guesses to all observed landmarks
|
||||
// Intentionally initialize the variables off from the ground truth
|
||||
for (size_t j = 0; j < points.size(); ++j)
|
||||
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
|
||||
Point3 noise(-0.25, 0.20, 0.15);
|
||||
for (size_t j = 0; j < points.size(); ++j) {
|
||||
// Intentionally initialize the variables off from the ground truth
|
||||
Point3 initial_lj = points[j].compose(noise);
|
||||
initialEstimate.insert(Symbol('l', j), initial_lj);
|
||||
}
|
||||
|
||||
} else {
|
||||
// Update iSAM with the new factors
|
||||
|
|
|
|||
|
|
@ -16,8 +16,9 @@
|
|||
*/
|
||||
|
||||
#pragma once
|
||||
#include <boost/make_shared.hpp>
|
||||
|
||||
#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
|
||||
|
|
|
|||
|
|
@ -19,9 +19,9 @@
|
|||
|
||||
#include <cstdarg>
|
||||
|
||||
#include <gtsam/base/DerivedValue.h>
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/base/DerivedValue.h>
|
||||
#include <boost/serialization/nvp.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
|
@ -40,9 +40,12 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
|
|||
/** initialize from a normal matrix */
|
||||
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 */
|
||||
template<int M, int N>
|
||||
LieMatrix(const Eigen::Matrix<double, M, N>& v) : Matrix(v) {}
|
||||
#endif
|
||||
|
||||
/** constructor with size and initial data, row order ! */
|
||||
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 {
|
||||
if(v.size() != this->size())
|
||||
throw std::invalid_argument("LieMatrix::retract called with Vector of incorrect size");
|
||||
|
||||
return LieMatrix(*this +
|
||||
Eigen::Map<const Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic,Eigen::RowMajor> >(
|
||||
&v(0), this->rows(), this->cols()));
|
||||
|
|
@ -153,7 +157,7 @@ struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> {
|
|||
result.data(), p.rows(), p.cols()) = p;
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
/// @}
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -33,10 +33,13 @@ struct LieVector : public Vector, public DerivedValue<LieVector> {
|
|||
|
||||
/** initialize from a normal vector */
|
||||
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 */
|
||||
template<int N>
|
||||
LieVector(const Eigen::Matrix<double, N, 1>& v) : Vector(v) {}
|
||||
#endif
|
||||
|
||||
/** wrap a double */
|
||||
LieVector(double d) : Vector((Vector(1) << d)) {}
|
||||
|
|
|
|||
|
|
@ -36,18 +36,19 @@ namespace gtsam {
|
|||
* Values can operate generically on Value objects, retracting or computing
|
||||
* local coordinates for many Value objects of different types.
|
||||
*
|
||||
* When you implement retract_(), localCoordinates_(), and equals_(), we
|
||||
* suggest first implementing versions of these functions that work directly
|
||||
* with derived objects, then using the provided helper functions to
|
||||
* implement the generic Value versions. This makes your implementation
|
||||
* easier, and also improves performance in situations where the derived type
|
||||
* is in fact known, such as in most implementations of \c evaluateError() in
|
||||
* classes derived from NonlinearFactor.
|
||||
* Inheriting from the DerivedValue class templated provides a generic implementation of
|
||||
* the pure virtual functions retract_(), localCoordinates_(), and equals_(), eliminating
|
||||
* the need to implement these functions in your class. Note that you must inherit from
|
||||
* DerivedValue templated on the class you are defining. For example you cannot define
|
||||
* the following
|
||||
* \code
|
||||
* class Rot3 : public DerivedValue<Point3>{ \\classdef }
|
||||
* \endcode
|
||||
*
|
||||
* Using the above practice, here is an example of implementing a typical
|
||||
* class derived from Value:
|
||||
* \code
|
||||
class Rot3 : public Value {
|
||||
class GTSAM_EXPORT Rot3 : public DerivedValue<Rot3> {
|
||||
public:
|
||||
// Constructor, there is never a need to call the Value base class constructor.
|
||||
Rot3() { ... }
|
||||
|
|
@ -74,27 +75,6 @@ namespace gtsam {
|
|||
// Math to implement 3D rotation localCoordinates, e.g. logarithm map
|
||||
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
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -30,55 +30,11 @@
|
|||
|
||||
#include <gtsam/base/Vector.h>
|
||||
|
||||
//#ifdef WIN32
|
||||
//#include <Windows.h>
|
||||
//#endif
|
||||
|
||||
using namespace std;
|
||||
|
||||
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 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) {
|
||||
size_t n = v.size();
|
||||
odprintf_("%s [", stream, s.c_str());
|
||||
for(size_t i=0; i<n; i++)
|
||||
odprintf_("%g%s", stream, v[i], (i<n-1 ? "; " : ""));
|
||||
odprintf_("];\n", stream);
|
||||
|
||||
stream << s << "[";
|
||||
for(size_t i=0; i<n; i++) {
|
||||
stream << setprecision(9) << v(i) << (i<n-1 ? "; " : "");
|
||||
}
|
||||
stream << "];" << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -41,11 +41,6 @@ typedef Eigen::Matrix<double, 6, 1> Vector6;
|
|||
typedef Eigen::VectorBlock<Vector> SubVector;
|
||||
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
|
||||
* @param n is the size of the vector
|
||||
|
|
|
|||
|
|
@ -1127,6 +1127,12 @@ TEST( matrix, svd2 )
|
|||
|
||||
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(expected_s,s,1e-9));
|
||||
EXPECT(assert_equal(expectedV,V));
|
||||
|
|
@ -1143,6 +1149,13 @@ TEST( matrix, svd3 )
|
|||
Matrix expectedV = (Matrix(3, 2) << 0.,1.,0.,0.,-1.,0.);
|
||||
|
||||
svd(sampleAt, U, s, V);
|
||||
|
||||
// take care of sign ambiguity
|
||||
if (U(0, 0) > 0) {
|
||||
U = -U;
|
||||
V = -V;
|
||||
}
|
||||
|
||||
Matrix S = diag(s);
|
||||
Matrix t = U * S;
|
||||
Matrix Vt = trans(V);
|
||||
|
|
@ -1176,6 +1189,17 @@ TEST( matrix, svd4 )
|
|||
0.6723, 0.7403);
|
||||
|
||||
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);
|
||||
|
||||
EXPECT(assert_equal(A, reconstructed, 1e-4));
|
||||
|
|
|
|||
|
|
@ -299,6 +299,8 @@ namespace gtsam {
|
|||
|
||||
// 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>
|
||||
namespace std {
|
||||
template<typename T> inline int isfinite(T a) {
|
||||
|
|
@ -309,6 +311,8 @@ namespace std {
|
|||
return (int)boost::math::isinf(a); }
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
#include <boost/math/constants/constants.hpp>
|
||||
#ifndef M_PI
|
||||
#define M_PI (boost::math::constants::pi<double>())
|
||||
|
|
|
|||
|
|
@ -53,6 +53,8 @@ public:
|
|||
*/
|
||||
Cal3Bundler(double f, double k1, double k2, double u0 = 0, double v0 = 0);
|
||||
|
||||
virtual ~Cal3Bundler() {}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
/// @{
|
||||
|
|
|
|||
|
|
@ -23,24 +23,9 @@
|
|||
|
||||
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 {
|
||||
gtsam::print(K(), s_ + ".K");
|
||||
gtsam::print(Vector(k()), s_ + ".k");
|
||||
Base::print(s_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -52,135 +37,6 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
|
|||
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 {
|
||||
return Cal3DS2(vector() + d);
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@
|
|||
|
||||
/**
|
||||
* @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
|
||||
* @author ydjian
|
||||
*/
|
||||
|
|
@ -19,7 +19,7 @@
|
|||
#pragma once
|
||||
|
||||
#include <gtsam/base/DerivedValue.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Cal3DS2_Base.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -37,34 +37,29 @@ namespace gtsam {
|
|||
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
|
||||
* pi = K*pn
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3DS2 : public DerivedValue<Cal3DS2> {
|
||||
class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue<Cal3DS2> {
|
||||
|
||||
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
|
||||
typedef Cal3DS2_Base Base;
|
||||
|
||||
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() : 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,
|
||||
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
|
||||
/// @{
|
||||
|
||||
Cal3DS2(const Vector &v) ;
|
||||
Cal3DS2(const Vector &v) : Base(v) {}
|
||||
|
||||
/// @}
|
||||
/// @name Testable
|
||||
|
|
@ -76,57 +71,6 @@ public:
|
|||
/// assert equality up to a tolerance
|
||||
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
|
||||
/// @{
|
||||
|
|
@ -156,18 +100,10 @@ private:
|
|||
{
|
||||
ar & boost::serialization::make_nvp("Cal3DS2",
|
||||
boost::serialization::base_object<Value>(*this));
|
||||
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_);
|
||||
ar & boost::serialization::make_nvp("Cal3DS2",
|
||||
boost::serialization::base_object<Cal3DS2_Base>(*this));
|
||||
}
|
||||
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
||||
|
|
@ -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_);
|
||||
}
|
||||
|
||||
/// @}
|
||||
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -22,8 +22,8 @@
|
|||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Cal3DS2.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#include <gtsam/geometry/Cal3DS2_Base.h>
|
||||
#include <gtsam/base/DerivedValue.h>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -40,10 +40,10 @@ namespace gtsam {
|
|||
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
|
||||
* pi = K*pn
|
||||
*/
|
||||
class GTSAM_EXPORT Cal3Unified : public Cal3DS2 {
|
||||
class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue<Cal3Unified> {
|
||||
|
||||
typedef Cal3Unified This;
|
||||
typedef Cal3DS2 Base;
|
||||
typedef Cal3DS2_Base Base;
|
||||
|
||||
private:
|
||||
|
||||
|
|
@ -90,7 +90,7 @@ public:
|
|||
/**
|
||||
* convert intrinsic coordinates xy to image coordinates uv
|
||||
* @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
|
||||
* @return point in image coordinates
|
||||
*/
|
||||
|
|
@ -135,7 +135,9 @@ private:
|
|||
void serialize(Archive & ar, const unsigned int version)
|
||||
{
|
||||
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_);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -165,6 +165,16 @@ public:
|
|||
*/
|
||||
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
|
||||
/// @{
|
||||
|
|
|
|||
|
|
@ -240,7 +240,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
|
|||
return retractCayley(omega);
|
||||
} else if(mode == Rot3::SLOW_CAYLEY) {
|
||||
Matrix Omega = skewSymmetric(omega);
|
||||
return (*this)*Cayley<3>(-Omega/2);
|
||||
return (*this)*CayleyFixed<3>(-Omega/2);
|
||||
} else {
|
||||
assert(false);
|
||||
exit(1);
|
||||
|
|
@ -269,7 +269,7 @@ Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const
|
|||
// Create a fixed-size matrix
|
||||
Eigen::Matrix3d A(between(T).matrix());
|
||||
// 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));
|
||||
} else {
|
||||
assert(false);
|
||||
|
|
|
|||
|
|
@ -21,6 +21,7 @@
|
|||
|
||||
#include <boost/math/constants/constants.hpp>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#include <cmath>
|
||||
|
||||
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) {
|
||||
Eigen::AngleAxisd angleAxis(R.quaternion_);
|
||||
if(angleAxis.angle() > M_PI) // Important: use the smallest possible
|
||||
angleAxis.angle() -= 2.0*M_PI; // angle, e.g. no more than PI, to keep
|
||||
if(angleAxis.angle() < -M_PI) // error continuous.
|
||||
angleAxis.angle() += 2.0*M_PI;
|
||||
return angleAxis.axis() * angleAxis.angle();
|
||||
using std::acos;
|
||||
using std::sqrt;
|
||||
static const double twoPi = 2.0 * M_PI,
|
||||
// define these compile time constants to avoid std::abs:
|
||||
NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10;
|
||||
|
||||
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,7 +21,16 @@
|
|||
#include <gtsam/geometry/Unit3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
#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>
|
||||
#ifdef __clang__
|
||||
# pragma clang diagnostic pop
|
||||
#endif
|
||||
|
||||
#include <boost/random/variate_generator.hpp>
|
||||
#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
|
||||
if (B_.rows() == 3)
|
||||
return B_;
|
||||
if (B_)
|
||||
return *B_;
|
||||
|
||||
// Get the axis of rotation with the minimum projected length of the point
|
||||
Point3 axis;
|
||||
|
|
@ -83,9 +92,9 @@ const Matrix& Unit3::basis() const {
|
|||
b2 = b2 / b2.norm();
|
||||
|
||||
// Create the basis matrix
|
||||
B_ = Matrix(3, 2);
|
||||
B_ << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
|
||||
return B_;
|
||||
B_.reset(Unit3::Matrix32());
|
||||
(*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
|
||||
return *B_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -23,6 +23,7 @@
|
|||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/base/DerivedValue.h>
|
||||
#include <boost/random/mersenne_twister.hpp>
|
||||
#include <boost/optional.hpp>
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
|
|
@ -31,8 +32,10 @@ class GTSAM_EXPORT Unit3: public DerivedValue<Unit3> {
|
|||
|
||||
private:
|
||||
|
||||
typedef Eigen::Matrix<double,3,2> Matrix32;
|
||||
|
||||
Point3 p_; ///< The location of the point on the unit sphere
|
||||
mutable Matrix B_; ///< Cached basis
|
||||
mutable boost::optional<Matrix32> B_; ///< Cached basis
|
||||
|
||||
public:
|
||||
|
||||
|
|
@ -84,7 +87,7 @@ public:
|
|||
* It is a 3*2 matrix [b1 b2] composed of two orthogonal directions
|
||||
* 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
|
||||
Matrix skew() const;
|
||||
|
|
|
|||
|
|
@ -19,6 +19,9 @@
|
|||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/Cal3Unified.h>
|
||||
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
GTSAM_CONCEPT_TESTABLE_INST(Cal3Unified)
|
||||
|
|
@ -97,6 +100,19 @@ TEST( Cal3Unified, retract)
|
|||
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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -93,6 +93,17 @@ TEST( Cal3_S2, retract)
|
|||
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() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -173,6 +173,12 @@ TEST (EssentialMatrix, epipoles) {
|
|||
Vector S;
|
||||
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(fabs(S(2))<1e-10);
|
||||
|
||||
|
|
@ -182,8 +188,15 @@ TEST (EssentialMatrix, epipoles) {
|
|||
// Check epipoles
|
||||
|
||||
// Epipole in image 1 is just E.direction()
|
||||
Unit3 e1(U(0, 2), U(1, 2), U(2, 2));
|
||||
EXPECT(assert_equal(e1, E.epipole_a()));
|
||||
Unit3 e1(-U(0, 2), -U(1, 2), -U(2, 2));
|
||||
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())
|
||||
Unit3 e2(V(0, 2), V(1, 2), V(2, 2));
|
||||
|
|
|
|||
|
|
@ -183,25 +183,30 @@ TEST( PinholeCamera, Dproject)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
//static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) {
|
||||
// Point3 point(point2D.x(), point2D.y(), 1.0);
|
||||
// return Camera(pose,cal).projectPointAtInfinity(point);
|
||||
//}
|
||||
//
|
||||
//TEST( PinholeCamera, Dproject_Infinity)
|
||||
//{
|
||||
// Matrix Dpose, Dpoint, Dcal;
|
||||
// Point2 point2D(-0.08,-0.08);
|
||||
// Point3 point3D(point1.x(), point1.y(), 1.0);
|
||||
// Point2 result = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal);
|
||||
// Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point2D, K);
|
||||
// Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point2D, K);
|
||||
// Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point2D, K);
|
||||
// CHECK(assert_equal(numerical_pose, Dpose, 1e-7));
|
||||
// CHECK(assert_equal(numerical_point, Dpoint, 1e-7));
|
||||
// CHECK(assert_equal(numerical_cal, Dcal, 1e-7));
|
||||
//}
|
||||
//
|
||||
static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const Cal3_S2& cal) {
|
||||
return Camera(pose,cal).projectPointAtInfinity(point3D);
|
||||
}
|
||||
|
||||
TEST( PinholeCamera, Dproject_Infinity)
|
||||
{
|
||||
Matrix Dpose, Dpoint, Dcal;
|
||||
Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera
|
||||
|
||||
// test Projection
|
||||
Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal);
|
||||
Point2 expected(-5.0, 5.0);
|
||||
CHECK(assert_equal(actual, expected, 1e-7));
|
||||
|
||||
// test Jacobians
|
||||
Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point3D, K);
|
||||
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) {
|
||||
return camera.project2(point);
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -13,15 +13,16 @@
|
|||
* @file testRot3.cpp
|
||||
* @brief Unit tests for Rot3 class
|
||||
* @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 <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
|
||||
#include <boost/math/constants/constants.hpp>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
|
@ -33,206 +34,10 @@ using namespace gtsam;
|
|||
|
||||
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);
|
||||
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)
|
||||
TEST(Rot3, manifold_cayley)
|
||||
{
|
||||
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
|
||||
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 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
|
||||
|
|
@ -287,343 +92,6 @@ TEST(Rot3, manifold_slow_caley)
|
|||
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
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -11,478 +11,24 @@
|
|||
|
||||
/**
|
||||
* @file testRot3.cpp
|
||||
* @brief Unit tests for Rot3 class
|
||||
* @brief Unit tests for Rot3 class, Quaternion specific
|
||||
* @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/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
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
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");
|
||||
}
|
||||
// No quaternion only tests
|
||||
|
||||
#endif
|
||||
|
||||
|
|
|
|||
|
|
@ -25,6 +25,7 @@
|
|||
#include <gtsam/geometry/PinholeCamera.h>
|
||||
#include <gtsam/geometry/Cal3DS2.h>
|
||||
#include <gtsam/geometry/Cal3Bundler.h>
|
||||
#include <gtsam/geometry/Cal3Unified.h>
|
||||
#include <gtsam/geometry/StereoCamera.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::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4));
|
||||
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 StereoCamera cam2(pose3, cal4ptr);
|
||||
|
|
@ -66,6 +68,7 @@ TEST (Serialization, text_geometry) {
|
|||
EXPECT(equalsObj(cal3));
|
||||
EXPECT(equalsObj(cal4));
|
||||
EXPECT(equalsObj(cal5));
|
||||
EXPECT(equalsObj(cal6));
|
||||
|
||||
EXPECT(equalsObj(cam1));
|
||||
EXPECT(equalsObj(cam2));
|
||||
|
|
|
|||
|
|
@ -268,7 +268,7 @@ TEST( triangulation, TriangulationFactor ) {
|
|||
Key pointKey(1);
|
||||
SharedNoiseModel model;
|
||||
typedef TriangulationFactor<> Factor;
|
||||
Factor factor(camera1, z1, model, pointKey, sharedCal);
|
||||
Factor factor(camera1, z1, model, pointKey);
|
||||
|
||||
// Use the factor to calculate the Jacobians
|
||||
Matrix HActual;
|
||||
|
|
|
|||
|
|
@ -328,10 +328,6 @@ TEST(Unit3, localCoordinates_retract_expmap) {
|
|||
//*******************************************************************************
|
||||
TEST(Unit3, Random) {
|
||||
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
|
||||
Point3 expectedMean, actualMean;
|
||||
for (size_t i = 0; i < 100; i++)
|
||||
|
|
|
|||
|
|
@ -174,7 +174,7 @@ Point3 triangulateNonlinear(
|
|||
* @param poses A vector of camera poses
|
||||
* @param sharedCal shared pointer to single calibration object
|
||||
* @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
|
||||
* @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.
|
||||
* @param cameras pinhole cameras
|
||||
* @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
|
||||
* @return Returns a Point3
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -38,7 +38,7 @@ Errors::Errors(const VectorValues& V) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
void Errors::print(const std::string& s) const {
|
||||
odprintf("%s:\n", s.c_str());
|
||||
cout << s << endl;
|
||||
BOOST_FOREACH(const Vector& v, *this)
|
||||
gtsam::print(v);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -111,7 +111,7 @@ namespace gtsam {
|
|||
* assumed to have already been solved in and their values are read from \c x.
|
||||
* 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
|
||||
* variables of this conditional, this solve function computes
|
||||
* \f$ x_f = R^{-1} (d - S x_s) \f$ using back-substitution.
|
||||
|
|
|
|||
|
|
@ -85,7 +85,7 @@ namespace gtsam {
|
|||
dims_accumulated.resize(dims.size()+1,0);
|
||||
dims_accumulated[0]=0;
|
||||
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;
|
||||
}
|
||||
|
||||
|
|
@ -361,8 +361,8 @@ VectorValues GaussianFactorGraph::gradientAtZero(
|
|||
/* ************************************************************************* */
|
||||
void GaussianFactorGraph::multiplyHessianAdd(double alpha,
|
||||
const double* x, double* y) const {
|
||||
vector<size_t> FactorKeys = getkeydim();
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this)
|
||||
vector<size_t> FactorKeys = getkeydim();
|
||||
BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this)
|
||||
f->multiplyHessianAdd(alpha, x, y, FactorKeys);
|
||||
|
||||
}
|
||||
|
|
|
|||
|
|
@ -538,7 +538,7 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
|
|||
|
||||
// copy to yvalues
|
||||
for(DenseIndex i = 0; i < (DenseIndex)size(); ++i) {
|
||||
bool didNotExist;
|
||||
bool didNotExist;
|
||||
VectorValues::iterator it;
|
||||
boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector());
|
||||
if (didNotExist)
|
||||
|
|
@ -660,10 +660,11 @@ EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)
|
|||
// Do dense elimination
|
||||
GaussianConditional::shared_ptr conditional;
|
||||
try {
|
||||
VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(keys.size());
|
||||
conditional = boost::make_shared<GaussianConditional>(jointFactor->keys(), keys.size(), Ab);
|
||||
size_t numberOfKeysToEliminate = keys.size();
|
||||
VerticalBlockMatrix Ab = jointFactor->info_.choleskyPartial(numberOfKeysToEliminate);
|
||||
conditional = boost::make_shared<GaussianConditional>(jointFactor->keys(), numberOfKeysToEliminate, Ab);
|
||||
// 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&) {
|
||||
// std::cout << "Problematic Hessian: " << jointFactor->information() << std::endl;
|
||||
throw IndeterminantLinearSystemException(keys.front());
|
||||
|
|
|
|||
|
|
@ -15,6 +15,17 @@
|
|||
* @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++
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
|
|
@ -22,16 +33,6 @@
|
|||
#include <boost/tuple/tuple.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 gtsam;
|
||||
|
||||
|
|
|
|||
|
|
@ -25,13 +25,15 @@
|
|||
#include <gtsam/linear/GaussianConditional.h>
|
||||
#include <gtsam/linear/GaussianBayesNet.h>
|
||||
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <vector>
|
||||
#include <boost/assign/std/list.hpp>
|
||||
#include <boost/assign/std/vector.hpp>
|
||||
#include <boost/assign/list_inserter.hpp>
|
||||
#include <boost/make_shared.hpp>
|
||||
#include <boost/assign/list_of.hpp>
|
||||
|
||||
#include <iostream>
|
||||
#include <sstream>
|
||||
#include <vector>
|
||||
|
||||
using namespace gtsam;
|
||||
using namespace std;
|
||||
|
|
|
|||
|
|
@ -18,20 +18,21 @@
|
|||
* @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 +=
|
||||
using namespace boost::assign;
|
||||
|
||||
#include <gtsam/base/TestableAssertions.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 gtsam;
|
||||
|
||||
|
|
|
|||
|
|
@ -15,24 +15,25 @@
|
|||
* @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/JacobianFactor.h>
|
||||
#include <gtsam/linear/GaussianFactorGraph.h>
|
||||
#include <gtsam/linear/GaussianConditional.h>
|
||||
#include <gtsam/linear/VectorValues.h>
|
||||
|
||||
#include <gtsam/base/debug.h>
|
||||
#include <gtsam/base/TestableAssertions.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;
|
||||
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
const double tol = 1e-5;
|
||||
|
|
|
|||
|
|
@ -345,12 +345,12 @@ namespace gtsam {
|
|||
|
||||
/** Constructor */
|
||||
CombinedImuFactor(
|
||||
Key pose_i, ///< previous pose key
|
||||
Key vel_i, ///< previous velocity key
|
||||
Key pose_j, ///< current pose key
|
||||
Key vel_j, ///< current velocity key
|
||||
Key bias_i, ///< previous bias key
|
||||
Key bias_j, ///< current bias key
|
||||
Key pose_i, ///< previous pose key
|
||||
Key vel_i, ///< previous velocity key
|
||||
Key pose_j, ///< current pose key
|
||||
Key vel_j, ///< current velocity key
|
||||
Key bias_i, ///< previous bias key
|
||||
Key bias_j, ///< current bias key
|
||||
const CombinedPreintegratedMeasurements& preintegratedMeasurements, ///< Preintegrated IMU measurements
|
||||
const Vector3& gravity, ///< gravity vector
|
||||
const Vector3& omegaCoriolis, ///< rotation rate of inertial frame
|
||||
|
|
@ -480,33 +480,33 @@ namespace gtsam {
|
|||
Matrix3 dfPdPi;
|
||||
Matrix3 dfVdPi;
|
||||
if(use2ndOrderCoriolis_){
|
||||
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
|
||||
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
|
||||
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
|
||||
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
|
||||
}
|
||||
else{
|
||||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Matrix3::Zero();
|
||||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Matrix3::Zero();
|
||||
}
|
||||
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
|
||||
// dfR/dPi
|
||||
Matrix3::Zero(),
|
||||
//dBiasAcc/dPi
|
||||
Matrix3::Zero(), Matrix3::Zero(),
|
||||
//dBiasOmega/dPi
|
||||
Matrix3::Zero(), Matrix3::Zero();
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
|
||||
// dfR/dPi
|
||||
Matrix3::Zero(),
|
||||
//dBiasAcc/dPi
|
||||
Matrix3::Zero(), Matrix3::Zero(),
|
||||
//dBiasOmega/dPi
|
||||
Matrix3::Zero(), Matrix3::Zero();
|
||||
}
|
||||
|
||||
if(H2) {
|
||||
|
|
@ -517,13 +517,13 @@ namespace gtsam {
|
|||
+ skewSymmetric(omegaCoriolis_) * deltaTij * deltaTij, // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
// dfV/dVi
|
||||
- Matrix3::Identity()
|
||||
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
|
||||
// dfR/dVi
|
||||
Matrix3::Zero(),
|
||||
//dBiasAcc/dVi
|
||||
Matrix3::Zero(),
|
||||
//dBiasOmega/dVi
|
||||
Matrix3::Zero();
|
||||
+ 2 * skewSymmetric(omegaCoriolis_) * deltaTij, // Coriolis term
|
||||
// dfR/dVi
|
||||
Matrix3::Zero(),
|
||||
//dBiasAcc/dVi
|
||||
Matrix3::Zero(),
|
||||
//dBiasOmega/dVi
|
||||
Matrix3::Zero();
|
||||
}
|
||||
|
||||
if(H3) {
|
||||
|
|
@ -643,21 +643,21 @@ namespace gtsam {
|
|||
// Predict state at time j
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
|
||||
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
|
||||
+ vel_i * deltaTij
|
||||
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
+ 0.5 * gravity * deltaTij*deltaTij;
|
||||
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
|
||||
+ vel_i * deltaTij
|
||||
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
+ 0.5 * gravity * deltaTij*deltaTij;
|
||||
|
||||
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
|
||||
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
|
||||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
|
||||
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
|
||||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
|
||||
if(use2ndOrderCoriolis){
|
||||
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
|
||||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
|
||||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
}
|
||||
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
|
||||
|
|
|
|||
|
|
@ -308,11 +308,11 @@ namespace gtsam {
|
|||
|
||||
/** Constructor */
|
||||
ImuFactor(
|
||||
Key pose_i, ///< previous pose key
|
||||
Key vel_i, ///< previous velocity key
|
||||
Key pose_j, ///< current pose key
|
||||
Key vel_j, ///< current velocity key
|
||||
Key bias, ///< previous bias key
|
||||
Key pose_i, ///< previous pose key
|
||||
Key vel_i, ///< previous velocity key
|
||||
Key pose_j, ///< current pose key
|
||||
Key vel_j, ///< current velocity key
|
||||
Key bias, ///< previous bias key
|
||||
const PreintegratedMeasurements& preintegratedMeasurements, ///< preintegrated IMU measurements
|
||||
const Vector3& gravity, ///< gravity vector
|
||||
const Vector3& omegaCoriolis, ///< rotation rate of the inertial frame
|
||||
|
|
@ -419,29 +419,29 @@ namespace gtsam {
|
|||
Matrix3 dfPdPi;
|
||||
Matrix3 dfVdPi;
|
||||
if(use2ndOrderCoriolis_){
|
||||
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
|
||||
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
|
||||
dfPdPi = - Rot_i.matrix() + 0.5 * skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij*deltaTij;
|
||||
dfVdPi = skewSymmetric(omegaCoriolis_) * skewSymmetric(omegaCoriolis_) * Rot_i.matrix() * deltaTij;
|
||||
}
|
||||
else{
|
||||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Matrix3::Zero();
|
||||
dfPdPi = - Rot_i.matrix();
|
||||
dfVdPi = Matrix3::Zero();
|
||||
}
|
||||
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
|
||||
// dfR/dPi
|
||||
Matrix3::Zero();
|
||||
(*H1) <<
|
||||
// dfP/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaPij
|
||||
+ preintegratedMeasurements_.delPdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delPdelBiasAcc * biasAccIncr),
|
||||
// dfP/dPi
|
||||
dfPdPi,
|
||||
// dfV/dRi
|
||||
Rot_i.matrix() * skewSymmetric(preintegratedMeasurements_.deltaVij
|
||||
+ preintegratedMeasurements_.delVdelBiasOmega * biasOmegaIncr + preintegratedMeasurements_.delVdelBiasAcc * biasAccIncr),
|
||||
// dfV/dPi
|
||||
dfVdPi,
|
||||
// dfR/dRi
|
||||
Jrinv_fRhat * (- Rot_j.between(Rot_i).matrix() - fRhat.inverse().matrix() * Jtheta),
|
||||
// dfR/dPi
|
||||
Matrix3::Zero();
|
||||
}
|
||||
|
||||
if(H2) {
|
||||
|
|
@ -540,22 +540,22 @@ namespace gtsam {
|
|||
|
||||
// Predict state at time j
|
||||
/* ---------------------------------------------------------------------------------------------------- */
|
||||
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
|
||||
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
|
||||
+ vel_i * deltaTij
|
||||
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
+ 0.5 * gravity * deltaTij*deltaTij;
|
||||
Vector3 pos_j = pos_i + Rot_i.matrix() * (preintegratedMeasurements.deltaPij
|
||||
+ preintegratedMeasurements.delPdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements.delPdelBiasOmega * biasOmegaIncr)
|
||||
+ vel_i * deltaTij
|
||||
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
|
||||
+ 0.5 * gravity * deltaTij*deltaTij;
|
||||
|
||||
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
|
||||
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
|
||||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij
|
||||
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
|
||||
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
|
||||
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term
|
||||
+ gravity * deltaTij);
|
||||
|
||||
if(use2ndOrderCoriolis){
|
||||
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
|
||||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
pos_j += - 0.5 * skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij*deltaTij; // 2nd order coriolis term for position
|
||||
vel_j += - skewSymmetric(omegaCoriolis) * skewSymmetric(omegaCoriolis) * pos_i * deltaTij; // 2nd order term for velocity
|
||||
}
|
||||
|
||||
const Rot3 deltaRij_biascorrected = preintegratedMeasurements.deltaRij.retract(preintegratedMeasurements.delRdelBiasOmega * biasOmegaIncr, Rot3::EXPMAP);
|
||||
|
|
|
|||
|
|
@ -37,7 +37,15 @@ class MagFactor: public NoiseModelFactor1<Rot2> {
|
|||
|
||||
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,
|
||||
const Unit3& direction, const Point3& bias,
|
||||
const SharedNoiseModel& model) :
|
||||
|
|
|
|||
|
|
@ -117,7 +117,7 @@ public:
|
|||
|
||||
// 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
|
||||
|
|
|
|||
|
|
@ -176,11 +176,11 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
|
|||
stm << "];\n";
|
||||
|
||||
if (firstTimePoses) {
|
||||
lastKey = key;
|
||||
firstTimePoses = false;
|
||||
lastKey = key;
|
||||
firstTimePoses = false;
|
||||
} else {
|
||||
stm << " var" << key << "--" << "var" << lastKey << ";\n";
|
||||
lastKey = key;
|
||||
stm << " var" << key << "--" << "var" << lastKey << ";\n";
|
||||
lastKey = key;
|
||||
}
|
||||
}
|
||||
stm << "\n";
|
||||
|
|
@ -219,37 +219,37 @@ void NonlinearFactorGraph::saveGraph(std::ostream &stm, const Values& values,
|
|||
// Create factors and variable connections
|
||||
for(size_t i = 0; i < this->size(); ++i) {
|
||||
if(graphvizFormatting.plotFactorPoints){
|
||||
// Make each factor a dot
|
||||
stm << " factor" << i << "[label=\"\", shape=point";
|
||||
{
|
||||
map<size_t, Point2>::const_iterator pos = graphvizFormatting.factorPositions.find(i);
|
||||
if(pos != graphvizFormatting.factorPositions.end())
|
||||
stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\"";
|
||||
}
|
||||
stm << "];\n";
|
||||
// Make each factor a dot
|
||||
stm << " factor" << i << "[label=\"\", shape=point";
|
||||
{
|
||||
map<size_t, Point2>::const_iterator pos = graphvizFormatting.factorPositions.find(i);
|
||||
if(pos != graphvizFormatting.factorPositions.end())
|
||||
stm << ", pos=\"" << graphvizFormatting.scale*(pos->second.x() - minX) << "," << graphvizFormatting.scale*(pos->second.y() - minY) << "!\"";
|
||||
}
|
||||
stm << "];\n";
|
||||
|
||||
// Make factor-variable connections
|
||||
if(graphvizFormatting.connectKeysToFactor && this->at(i)) {
|
||||
BOOST_FOREACH(Key key, *this->at(i)) {
|
||||
stm << " var" << key << "--" << "factor" << i << ";\n";
|
||||
}
|
||||
}
|
||||
// Make factor-variable connections
|
||||
if(graphvizFormatting.connectKeysToFactor && this->at(i)) {
|
||||
BOOST_FOREACH(Key key, *this->at(i)) {
|
||||
stm << " var" << key << "--" << "factor" << i << ";\n";
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
else {
|
||||
if(this->at(i)) {
|
||||
Key k;
|
||||
bool firstTime = true;
|
||||
BOOST_FOREACH(Key key, *this->at(i)) {
|
||||
if(firstTime){
|
||||
k = key;
|
||||
firstTime = false;
|
||||
continue;
|
||||
}
|
||||
stm << " var" << key << "--" << "var" << k << ";\n";
|
||||
k = key;
|
||||
}
|
||||
}
|
||||
if(this->at(i)) {
|
||||
Key k;
|
||||
bool firstTime = true;
|
||||
BOOST_FOREACH(Key key, *this->at(i)) {
|
||||
if(firstTime){
|
||||
k = key;
|
||||
firstTime = false;
|
||||
continue;
|
||||
}
|
||||
stm << " var" << key << "--" << "var" << k << ";\n";
|
||||
k = key;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -77,9 +77,11 @@ void NonlinearOptimizer::defaultOptimize() {
|
|||
params.errorTol, currentError, this->error(), params.verbosity));
|
||||
|
||||
// Printing if verbose
|
||||
if (params.verbosity >= NonlinearOptimizerParams::TERMINATION &&
|
||||
this->iterations() >= params.maxIterations)
|
||||
cout << "Terminating because reached maximum iterations" << endl;
|
||||
if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) {
|
||||
cout << "iterations: " << this->iterations() << " >? " << params.maxIterations << endl;
|
||||
if (this->iterations() >= params.maxIterations)
|
||||
cout << "Terminating because reached maximum iterations" << endl;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -29,6 +29,7 @@ public:
|
|||
|
||||
/**
|
||||
* Constructor
|
||||
* @param key Essential Matrix variable key
|
||||
* @param pA point in first 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
|
||||
|
|
@ -42,6 +43,7 @@ public:
|
|||
|
||||
/**
|
||||
* Constructor
|
||||
* @param key Essential Matrix variable key
|
||||
* @param pA point in first 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
|
||||
|
|
@ -99,6 +101,8 @@ public:
|
|||
|
||||
/**
|
||||
* Constructor
|
||||
* @param key1 Essential Matrix variable key
|
||||
* @param key2 Inverse depth variable key
|
||||
* @param pA point in first camera, in calibrated coordinates
|
||||
* @param pB point in second camera, in calibrated coordinates
|
||||
* @param model noise model should be in pixels, as well
|
||||
|
|
@ -113,6 +117,8 @@ public:
|
|||
|
||||
/**
|
||||
* Constructor
|
||||
* @param key1 Essential Matrix variable key
|
||||
* @param key2 Inverse depth variable key
|
||||
* @param pA point in first camera, in pixel coordinates
|
||||
* @param pB point in second camera, in pixel coordinates
|
||||
* @param K calibration object, will be used only in constructor
|
||||
|
|
@ -216,6 +222,8 @@ public:
|
|||
|
||||
/**
|
||||
* Constructor
|
||||
* @param key1 Essential Matrix variable key
|
||||
* @param key2 Inverse depth variable key
|
||||
* @param pA point in first camera, in calibrated coordinates
|
||||
* @param pB point in second camera, in calibrated coordinates
|
||||
* @param bRc extra rotation between "body" and "camera" frame
|
||||
|
|
@ -228,6 +236,8 @@ public:
|
|||
|
||||
/**
|
||||
* Constructor
|
||||
* @param key1 Essential Matrix variable key
|
||||
* @param key2 Inverse depth variable key
|
||||
* @param pA point in first camera, in pixel coordinates
|
||||
* @param pB point in second camera, in pixel coordinates
|
||||
* @param K calibration object, will be used only in constructor
|
||||
|
|
|
|||
|
|
@ -81,7 +81,7 @@ public:
|
|||
}
|
||||
|
||||
/// Get matrix P
|
||||
inline const Matrix& getPointCovariance() const {
|
||||
inline const Matrix3& getPointCovariance() const {
|
||||
return PointCovariance_;
|
||||
}
|
||||
|
||||
|
|
@ -286,26 +286,27 @@ public:
|
|||
return 0.5 * (result + f);
|
||||
}
|
||||
|
||||
/// needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
|
||||
// This is wrong and does not match the definition in Hessian
|
||||
// virtual double error(const VectorValues& x) const {
|
||||
//
|
||||
// // resize does not do malloc if correct size
|
||||
// e1.resize(size());
|
||||
// e2.resize(size());
|
||||
//
|
||||
// // e1 = F * x - b = (2m*dm)*dm
|
||||
// for (size_t k = 0; k < size(); ++k)
|
||||
// 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)
|
||||
// result += dot(e2[k], e2[k]);
|
||||
//
|
||||
// std::cout << "implicitFactor::error result " << result << std::endl;
|
||||
// return 0.5 * result;
|
||||
// }
|
||||
// needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
|
||||
// This is wrong and does not match the definition in Hessian,
|
||||
// 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());
|
||||
e2.resize(size());
|
||||
|
||||
// e1 = F * x - b = (2m*dm)*dm
|
||||
for (size_t k = 0; k < size(); ++k)
|
||||
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)
|
||||
result += dot(e2[k], e2[k]);
|
||||
|
||||
// std::cout << "implicitFactor::error result " << result << std::endl;
|
||||
return 0.5 * result;
|
||||
}
|
||||
/**
|
||||
* @brief Calculate corrected error Q*e = (I - E*P*E')*e
|
||||
*/
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -77,7 +77,7 @@ public:
|
|||
(*H).middleCols(rotInterval.first, rDim).setIdentity(rDim, rDim);
|
||||
}
|
||||
|
||||
return Rotation::Logmap(newR) - Rotation::Logmap(measured_);
|
||||
return measured_.localCoordinates(newR);
|
||||
}
|
||||
|
||||
private:
|
||||
|
|
|
|||
|
|
@ -572,7 +572,7 @@ public:
|
|||
|
||||
FastMap<Key,size_t> KeySlotMap;
|
||||
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
|
||||
size_t numKeys = this->keys_.size(); // cameras observing current point
|
||||
|
|
|
|||
|
|
@ -85,7 +85,6 @@ string createRewrittenFileName(const string& name) {
|
|||
return newpath.string();
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -103,6 +102,25 @@ static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
|
|||
double 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
|
||||
Matrix M(3, 3);
|
||||
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,
|
||||
KernelFunctionType kernelFunctionType) {
|
||||
|
||||
|
|
@ -178,12 +196,12 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID,
|
|||
string tag;
|
||||
|
||||
// load the poses
|
||||
while (is) {
|
||||
while (!is.eof()) {
|
||||
if (!(is >> tag))
|
||||
break;
|
||||
|
||||
if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
|
||||
int id;
|
||||
Key id;
|
||||
double 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
|
||||
int id1, id2;
|
||||
Key id1, id2;
|
||||
bool haveLandmark = false;
|
||||
while (is) {
|
||||
while (!is.eof()) {
|
||||
if (!(is >> tag))
|
||||
break;
|
||||
|
||||
|
|
@ -250,7 +268,6 @@ GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID,
|
|||
new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
|
||||
graph->push_back(factor);
|
||||
}
|
||||
|
||||
// Parse measurements
|
||||
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) {
|
||||
// just call load2D
|
||||
int maxID = 0;
|
||||
bool addNoise = false;
|
||||
bool smart = true;
|
||||
|
||||
if(is3D)
|
||||
return load3D(g2oFile);
|
||||
|
||||
return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
|
||||
NoiseFormatG2O, kernelFunctionType);
|
||||
}
|
||||
|
|
@ -374,44 +395,97 @@ void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
|
|||
|
||||
fstream stream(filename.c_str(), fstream::out);
|
||||
|
||||
// save poses
|
||||
// save 2D & 3D poses
|
||||
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() << " "
|
||||
<< pose.y() << " " << pose.theta() << endl;
|
||||
|
||||
const Pose2* pose2D = dynamic_cast<const Pose2*>(&key_value.value);
|
||||
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::shared_ptr<BetweenFactor<Pose2> > factor =
|
||||
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
|
||||
if (!factor)
|
||||
continue;
|
||||
if (factor){
|
||||
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<noiseModel::Diagonal> diagonalModel =
|
||||
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
|
||||
if (!diagonalModel)
|
||||
throw invalid_argument(
|
||||
"writeG2o: invalid noise model (current version assumes diagonal noise model)!");
|
||||
boost::shared_ptr< BetweenFactor<Pose3> > factor3D =
|
||||
boost::dynamic_pointer_cast< BetweenFactor<Pose3> >(factor_);
|
||||
|
||||
Pose2 pose = factor->measured(); //.inverse();
|
||||
stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
|
||||
<< pose.x() << " " << pose.y() << " " << pose.theta() << " "
|
||||
<< diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " "
|
||||
<< diagonalModel->precision(1) << " " << 0.0 << " "
|
||||
<< diagonalModel->precision(2) << endl;
|
||||
if (factor3D){
|
||||
SharedNoiseModel model = factor3D->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();
|
||||
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();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool load3D(const string& filename) {
|
||||
GraphAndValues load3D(const string& filename) {
|
||||
|
||||
ifstream is(filename.c_str());
|
||||
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];
|
||||
is.getline(buf, LINESIZE);
|
||||
istringstream ls(buf);
|
||||
|
|
@ -419,15 +493,26 @@ bool load3D(const string& filename) {
|
|||
ls >> tag;
|
||||
|
||||
if (tag == "VERTEX3") {
|
||||
int id;
|
||||
Key id;
|
||||
double 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.seekg(0, ios::beg);
|
||||
|
||||
while (is) {
|
||||
while (!is.eof()) {
|
||||
char buf[LINESIZE];
|
||||
is.getline(buf, LINESIZE);
|
||||
istringstream ls(buf);
|
||||
|
|
@ -435,16 +520,46 @@ bool load3D(const string& filename) {
|
|||
ls >> tag;
|
||||
|
||||
if (tag == "EDGE3") {
|
||||
int id1, id2;
|
||||
Key id1, id2;
|
||||
double 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);
|
||||
for (int i = 0; i < 6; i++)
|
||||
for (int j = i; j < 6; 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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -57,7 +57,8 @@ enum NoiseFormat {
|
|||
NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33
|
||||
NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr
|
||||
NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix !
|
||||
NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33
|
||||
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
|
||||
|
|
@ -79,7 +80,7 @@ GTSAM_EXPORT GraphAndValues load2D(
|
|||
std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0,
|
||||
bool addNoise = false,
|
||||
bool smart = true, //
|
||||
NoiseFormat noiseFormat = NoiseFormatGRAPH,
|
||||
NoiseFormat noiseFormat = NoiseFormatAUTO,
|
||||
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
|
||||
|
||||
/**
|
||||
|
|
@ -94,8 +95,8 @@ GTSAM_EXPORT GraphAndValues load2D(
|
|||
* @return graph and initial values
|
||||
*/
|
||||
GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
|
||||
SharedNoiseModel model = SharedNoiseModel(), int maxID = 0, bool addNoise =
|
||||
false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatGRAPH, //
|
||||
SharedNoiseModel model = SharedNoiseModel(), Key maxID = 0, bool addNoise =
|
||||
false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatAUTO, //
|
||||
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
|
||||
|
||||
/// @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
|
||||
* 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
|
||||
* @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);
|
||||
|
||||
/**
|
||||
|
|
@ -130,7 +132,7 @@ GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& 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
|
||||
typedef std::pair<size_t, Point2> SfM_Measurement;
|
||||
|
|
|
|||
|
|
@ -56,6 +56,17 @@ TEST( dataSet, load2D)
|
|||
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)
|
||||
{
|
||||
|
|
@ -116,13 +127,116 @@ TEST( dataSet, readG2o)
|
|||
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)
|
||||
{
|
||||
const string g2oFile = findExampleDataFile("pose2example");
|
||||
NonlinearFactorGraph::shared_ptr actualGraph;
|
||||
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));
|
||||
SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel);
|
||||
|
|
@ -149,7 +263,8 @@ TEST( dataSet, readG2oTukey)
|
|||
const string g2oFile = findExampleDataFile("pose2example");
|
||||
NonlinearFactorGraph::shared_ptr actualGraph;
|
||||
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));
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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)
|
||||
{
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -36,7 +36,7 @@ using namespace boost::assign;
|
|||
static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3);
|
||||
static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
|
||||
|
||||
namespace simple {
|
||||
namespace simpleLago {
|
||||
// We consider a small graph:
|
||||
// symbolic FG
|
||||
// x2 0 1
|
||||
|
|
@ -67,7 +67,7 @@ NonlinearFactorGraph graph() {
|
|||
|
||||
/* *************************************************************************** */
|
||||
TEST( Lago, checkSTandChords ) {
|
||||
NonlinearFactorGraph g = simple::graph();
|
||||
NonlinearFactorGraph g = simpleLago::graph();
|
||||
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
|
||||
BetweenFactor<Pose2> >(g);
|
||||
|
||||
|
|
@ -84,7 +84,7 @@ TEST( Lago, checkSTandChords ) {
|
|||
|
||||
/* *************************************************************************** */
|
||||
TEST( Lago, orientationsOverSpanningTree ) {
|
||||
NonlinearFactorGraph g = simple::graph();
|
||||
NonlinearFactorGraph g = simpleLago::graph();
|
||||
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
|
||||
BetweenFactor<Pose2> >(g);
|
||||
|
||||
|
|
@ -115,7 +115,7 @@ TEST( Lago, orientationsOverSpanningTree ) {
|
|||
|
||||
/* *************************************************************************** */
|
||||
TEST( Lago, regularizedMeasurements ) {
|
||||
NonlinearFactorGraph g = simple::graph();
|
||||
NonlinearFactorGraph g = simpleLago::graph();
|
||||
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
|
||||
BetweenFactor<Pose2> >(g);
|
||||
|
||||
|
|
@ -141,7 +141,7 @@ TEST( Lago, regularizedMeasurements ) {
|
|||
/* *************************************************************************** */
|
||||
TEST( Lago, smallGraphVectorValues ) {
|
||||
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
|
||||
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
|
||||
|
|
@ -153,7 +153,7 @@ TEST( Lago, smallGraphVectorValues ) {
|
|||
/* *************************************************************************** */
|
||||
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
|
||||
EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
|
||||
|
|
@ -165,8 +165,8 @@ TEST( Lago, smallGraphVectorValuesSP ) {
|
|||
/* *************************************************************************** */
|
||||
TEST( Lago, multiplePosePriors ) {
|
||||
bool useOdometricPath = false;
|
||||
NonlinearFactorGraph g = simple::graph();
|
||||
g.add(PriorFactor<Pose2>(x1, simple::pose1, model));
|
||||
NonlinearFactorGraph g = simpleLago::graph();
|
||||
g.add(PriorFactor<Pose2>(x1, simpleLago::pose1, model));
|
||||
VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
|
||||
|
||||
// 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 ) {
|
||||
NonlinearFactorGraph g = simple::graph();
|
||||
g.add(PriorFactor<Pose2>(x1, simple::pose1, model));
|
||||
NonlinearFactorGraph g = simpleLago::graph();
|
||||
g.add(PriorFactor<Pose2>(x1, simpleLago::pose1, model));
|
||||
VectorValues initial = lago::initializeOrientations(g);
|
||||
|
||||
// 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 ) {
|
||||
bool useOdometricPath = false;
|
||||
NonlinearFactorGraph g = simple::graph();
|
||||
g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model));
|
||||
NonlinearFactorGraph g = simpleLago::graph();
|
||||
g.add(PriorFactor<Rot2>(x1, simpleLago::pose1.theta(), model));
|
||||
VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
|
||||
|
||||
// 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 ) {
|
||||
NonlinearFactorGraph g = simple::graph();
|
||||
g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model));
|
||||
NonlinearFactorGraph g = simpleLago::graph();
|
||||
g.add(PriorFactor<Rot2>(x1, simpleLago::pose1.theta(), model));
|
||||
VectorValues initial = lago::initializeOrientations(g);
|
||||
|
||||
// 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
|
||||
Values initialGuess;
|
||||
initialGuess.insert(x0,Pose2(simple::pose0.x(),simple::pose0.y(),0.0));
|
||||
initialGuess.insert(x1,Pose2(simple::pose1.x(),simple::pose1.y(),0.0));
|
||||
initialGuess.insert(x2,Pose2(simple::pose2.x(),simple::pose2.y(),0.0));
|
||||
initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.y(),0.0));
|
||||
initialGuess.insert(x0,Pose2(simpleLago::pose0.x(),simpleLago::pose0.y(),0.0));
|
||||
initialGuess.insert(x1,Pose2(simpleLago::pose1.x(),simpleLago::pose1.y(),0.0));
|
||||
initialGuess.insert(x2,Pose2(simpleLago::pose2.x(),simpleLago::pose2.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
|
||||
Values actual = lago::initialize(simple::graph(), initialGuess);
|
||||
Values actual = lago::initialize(simpleLago::graph(), initialGuess);
|
||||
|
||||
// we are in a noiseless case
|
||||
Values expected;
|
||||
expected.insert(x0,simple::pose0);
|
||||
expected.insert(x1,simple::pose1);
|
||||
expected.insert(x2,simple::pose2);
|
||||
expected.insert(x3,simple::pose3);
|
||||
expected.insert(x0,simpleLago::pose0);
|
||||
expected.insert(x1,simpleLago::pose1);
|
||||
expected.insert(x2,simpleLago::pose2);
|
||||
expected.insert(x3,simpleLago::pose3);
|
||||
|
||||
EXPECT(assert_equal(expected, actual, 1e-6));
|
||||
}
|
||||
|
|
@ -243,14 +243,14 @@ TEST( Lago, smallGraphValues ) {
|
|||
TEST( Lago, smallGraph2 ) {
|
||||
|
||||
// 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
|
||||
Values expected;
|
||||
expected.insert(x0,simple::pose0);
|
||||
expected.insert(x1,simple::pose1);
|
||||
expected.insert(x2,simple::pose2);
|
||||
expected.insert(x3,simple::pose3);
|
||||
expected.insert(x0,simpleLago::pose0);
|
||||
expected.insert(x1,simpleLago::pose1);
|
||||
expected.insert(x2,simpleLago::pose2);
|
||||
expected.insert(x3,simpleLago::pose3);
|
||||
|
||||
EXPECT(assert_equal(expected, actual, 1e-6));
|
||||
}
|
||||
|
|
|
|||
|
|
@ -35,6 +35,7 @@ const Rot3 rot3A, rot3B = Rot3::pitch(-M_PI_2), rot3C = Rot3::Expmap((Vector(3)
|
|||
// Pose2 examples
|
||||
const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);
|
||||
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) {
|
||||
|
|
@ -62,10 +63,18 @@ TEST( testPoseRotationFactor, level3_error ) {
|
|||
Pose3 pose1(rot3A, point3A);
|
||||
Pose3RotationPrior factor(poseKey, rot3C, model3);
|
||||
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>(
|
||||
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
|
||||
EXPECT(assert_equal(expH1, actH1, tol));
|
||||
boost::bind(evalFactorError3, factor, _1), pose1, 1e-2);
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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); }
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -251,7 +251,7 @@ TEST( BayesTree, shortcutCheck )
|
|||
// Check if all the cached shortcuts are cleared
|
||||
rootClique->deleteCachedShortcuts();
|
||||
BOOST_FOREACH(SymbolicBayesTree::sharedClique& clique, allCliques) {
|
||||
bool notCleared = clique->cachedSeparatorMarginal();
|
||||
bool notCleared = clique->cachedSeparatorMarginal().is_initialized();
|
||||
CHECK( notCleared == false);
|
||||
}
|
||||
EXPECT_LONGS_EQUAL(0, (long)rootClique->numCachedSeparatorMarginals());
|
||||
|
|
|
|||
|
|
@ -34,89 +34,89 @@ class DSFMap {
|
|||
|
||||
protected:
|
||||
|
||||
/// We store the forest in an STL map, but parents are done with pointers
|
||||
struct Entry {
|
||||
typename std::map<KEY, Entry>::iterator parent_;
|
||||
size_t rank_;
|
||||
Entry() {}
|
||||
};
|
||||
/// We store the forest in an STL map, but parents are done with pointers
|
||||
struct Entry {
|
||||
typename std::map<KEY, Entry>::iterator parent_;
|
||||
size_t rank_;
|
||||
Entry() {}
|
||||
};
|
||||
|
||||
typedef typename std::map<KEY, Entry> Map;
|
||||
typedef typename Map::iterator iterator;
|
||||
mutable Map entries_;
|
||||
typedef typename Map::iterator iterator;
|
||||
mutable Map entries_;
|
||||
|
||||
/// Given key, find iterator to initial entry
|
||||
iterator find__(const KEY& key) const {
|
||||
static const Entry empty;
|
||||
iterator it = entries_.find(key);
|
||||
// if key does not exist, create and return itself
|
||||
if (it == entries_.end()) {
|
||||
it = entries_.insert(std::make_pair(key, empty)).first;
|
||||
it->second.parent_ = it;
|
||||
it->second.rank_ = 0;
|
||||
}
|
||||
return it;
|
||||
}
|
||||
/// Given key, find iterator to initial entry
|
||||
iterator find__(const KEY& key) const {
|
||||
static const Entry empty;
|
||||
iterator it = entries_.find(key);
|
||||
// if key does not exist, create and return itself
|
||||
if (it == entries_.end()) {
|
||||
it = entries_.insert(std::make_pair(key, empty)).first;
|
||||
it->second.parent_ = it;
|
||||
it->second.rank_ = 0;
|
||||
}
|
||||
return it;
|
||||
}
|
||||
|
||||
/// Given iterator to initial entry, find the root Entry
|
||||
iterator find_(const iterator& it) const {
|
||||
// follow parent pointers until we reach set representative
|
||||
iterator& parent = it->second.parent_;
|
||||
if (parent != it)
|
||||
parent = find_(parent); // not yet, recurse!
|
||||
return parent;
|
||||
}
|
||||
/// Given iterator to initial entry, find the root Entry
|
||||
iterator find_(const iterator& it) const {
|
||||
// follow parent pointers until we reach set representative
|
||||
iterator& parent = it->second.parent_;
|
||||
if (parent != it)
|
||||
parent = find_(parent); // not yet, recurse!
|
||||
return parent;
|
||||
}
|
||||
|
||||
/// Given key, find the root Entry
|
||||
inline iterator find_(const KEY& key) const {
|
||||
iterator initial = find__(key);
|
||||
return find_(initial);
|
||||
}
|
||||
/// Given key, find the root Entry
|
||||
inline iterator find_(const KEY& key) const {
|
||||
iterator initial = find__(key);
|
||||
return find_(initial);
|
||||
}
|
||||
|
||||
public:
|
||||
|
||||
typedef std::set<KEY> Set;
|
||||
typedef std::set<KEY> Set;
|
||||
|
||||
/// constructor
|
||||
DSFMap() {
|
||||
}
|
||||
/// constructor
|
||||
DSFMap() {
|
||||
}
|
||||
|
||||
/// Given key, find the representative key for the set in which it lives
|
||||
inline KEY find(const KEY& key) const {
|
||||
iterator root = find_(key);
|
||||
return root->first;
|
||||
}
|
||||
/// Given key, find the representative key for the set in which it lives
|
||||
inline KEY find(const KEY& key) const {
|
||||
iterator root = find_(key);
|
||||
return root->first;
|
||||
}
|
||||
|
||||
/// Merge two sets
|
||||
void merge(const KEY& x, const KEY& y) {
|
||||
/// Merge two sets
|
||||
void merge(const KEY& x, const KEY& y) {
|
||||
|
||||
// straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure
|
||||
iterator xRoot = find_(x);
|
||||
iterator yRoot = find_(y);
|
||||
if (xRoot == yRoot)
|
||||
return;
|
||||
// straight from http://en.wikipedia.org/wiki/Disjoint-set_data_structure
|
||||
iterator xRoot = find_(x);
|
||||
iterator yRoot = find_(y);
|
||||
if (xRoot == yRoot)
|
||||
return;
|
||||
|
||||
// Merge sets
|
||||
if (xRoot->second.rank_ < yRoot->second.rank_)
|
||||
xRoot->second.parent_ = yRoot;
|
||||
else if (xRoot->second.rank_ > yRoot->second.rank_)
|
||||
yRoot->second.parent_ = xRoot;
|
||||
else {
|
||||
yRoot->second.parent_ = xRoot;
|
||||
xRoot->second.rank_ = xRoot->second.rank_ + 1;
|
||||
}
|
||||
}
|
||||
// Merge sets
|
||||
if (xRoot->second.rank_ < yRoot->second.rank_)
|
||||
xRoot->second.parent_ = yRoot;
|
||||
else if (xRoot->second.rank_ > yRoot->second.rank_)
|
||||
yRoot->second.parent_ = xRoot;
|
||||
else {
|
||||
yRoot->second.parent_ = xRoot;
|
||||
xRoot->second.rank_ = xRoot->second.rank_ + 1;
|
||||
}
|
||||
}
|
||||
|
||||
/// return all sets, i.e. a partition of all elements
|
||||
std::map<KEY, Set> sets() const {
|
||||
std::map<KEY, Set> sets;
|
||||
iterator it = entries_.begin();
|
||||
for (; it != entries_.end(); it++) {
|
||||
iterator root = find_(it);
|
||||
sets[root->first].insert(it->first);
|
||||
}
|
||||
return sets;
|
||||
}
|
||||
/// return all sets, i.e. a partition of all elements
|
||||
std::map<KEY, Set> sets() const {
|
||||
std::map<KEY, Set> sets;
|
||||
iterator it = entries_.begin();
|
||||
for (; it != entries_.end(); it++) {
|
||||
iterator root = find_(it);
|
||||
sets[root->first].insert(it->first);
|
||||
}
|
||||
return sets;
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
|
|
|||
|
|
@ -1,8 +1,8 @@
|
|||
/**
|
||||
* @file testLoopyBelief.cpp
|
||||
* @file testLoopyBelief.cpp
|
||||
* @brief
|
||||
* @author Duy-Nguyen Ta
|
||||
* @date Oct 11, 2013
|
||||
* @date Oct 11, 2013
|
||||
*/
|
||||
|
||||
#include <gtsam/inference/VariableIndex.h>
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -24,6 +24,7 @@ virtual class gtsam::GaussianFactor;
|
|||
virtual class gtsam::HessianFactor;
|
||||
virtual class gtsam::JacobianFactor;
|
||||
virtual class gtsam::Cal3_S2;
|
||||
virtual class gtsam::Cal3DS2;
|
||||
class gtsam::GaussianFactorGraph;
|
||||
class gtsam::NonlinearFactorGraph;
|
||||
class gtsam::Ordering;
|
||||
|
|
@ -358,6 +359,11 @@ virtual class TransformBtwRobotsUnaryFactorEM : gtsam::NonlinearFactor {
|
|||
Vector calcIndicatorProb(const gtsam::Values& x);
|
||||
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
|
||||
};
|
||||
|
||||
|
|
@ -745,4 +751,45 @@ virtual class OdometryFactorBase : gtsam::NoiseModelFactor {
|
|||
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
|
||||
|
|
|
|||
|
|
@ -19,459 +19,459 @@ using namespace std;
|
|||
|
||||
namespace gtsam { namespace partition {
|
||||
|
||||
/**
|
||||
* 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,
|
||||
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
|
||||
{
|
||||
typedef pair<int, int> IntPair;
|
||||
typedef list<sharedGenericFactor2D> FactorList;
|
||||
typedef map<IntPair, FactorList::iterator> Connections;
|
||||
/**
|
||||
* 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,
|
||||
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark)
|
||||
{
|
||||
typedef pair<int, int> IntPair;
|
||||
typedef list<sharedGenericFactor2D> FactorList;
|
||||
typedef map<IntPair, FactorList::iterator> Connections;
|
||||
|
||||
// create disjoin set forest
|
||||
DSFVector dsf(workspace.dsf, keys);
|
||||
// create disjoin set forest
|
||||
DSFVector dsf(workspace.dsf, keys);
|
||||
|
||||
FactorList factors(graph.begin(), graph.end());
|
||||
size_t nrFactors = factors.size();
|
||||
FactorList::iterator itEnd;
|
||||
workspace.prepareDictionary(keys);
|
||||
while (nrFactors) {
|
||||
Connections connections;
|
||||
bool succeed = false;
|
||||
itEnd = factors.end();
|
||||
list<FactorList::iterator> toErase;
|
||||
for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {
|
||||
FactorList factors(graph.begin(), graph.end());
|
||||
size_t nrFactors = factors.size();
|
||||
FactorList::iterator itEnd;
|
||||
workspace.prepareDictionary(keys);
|
||||
while (nrFactors) {
|
||||
Connections connections;
|
||||
bool succeed = false;
|
||||
itEnd = factors.end();
|
||||
list<FactorList::iterator> toErase;
|
||||
for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {
|
||||
|
||||
// remove invalid factors
|
||||
GenericNode2D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
|
||||
if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
|
||||
toErase.push_back(itFactor); nrFactors--; continue;
|
||||
}
|
||||
// remove invalid factors
|
||||
GenericNode2D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
|
||||
if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
|
||||
toErase.push_back(itFactor); nrFactors--; continue;
|
||||
}
|
||||
|
||||
size_t label1 = dsf.findSet(key1.index);
|
||||
size_t label2 = dsf.findSet(key2.index);
|
||||
if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; }
|
||||
size_t label1 = dsf.findSet(key1.index);
|
||||
size_t label2 = dsf.findSet(key2.index);
|
||||
if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; }
|
||||
|
||||
// merge two trees if the connection is strong enough, otherwise cache it
|
||||
// an odometry factor always merges two islands
|
||||
if (key1.type == NODE_POSE_2D && key2.type == NODE_POSE_2D) {
|
||||
toErase.push_back(itFactor); nrFactors--;
|
||||
dsf.makeUnionInPlace(label1, label2);
|
||||
succeed = true;
|
||||
break;
|
||||
}
|
||||
// merge two trees if the connection is strong enough, otherwise cache it
|
||||
// an odometry factor always merges two islands
|
||||
if (key1.type == NODE_POSE_2D && key2.type == NODE_POSE_2D) {
|
||||
toErase.push_back(itFactor); nrFactors--;
|
||||
dsf.makeUnionInPlace(label1, label2);
|
||||
succeed = true;
|
||||
break;
|
||||
}
|
||||
|
||||
// single landmark island only need one measurement
|
||||
if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) ||
|
||||
(dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) {
|
||||
toErase.push_back(itFactor); nrFactors--;
|
||||
dsf.makeUnionInPlace(label1, label2);
|
||||
succeed = true;
|
||||
break;
|
||||
}
|
||||
// single landmark island only need one measurement
|
||||
if ((dsf.isSingleton(label1)==1 && key1.type == NODE_LANDMARK_2D) ||
|
||||
(dsf.isSingleton(label2)==1 && key2.type == NODE_LANDMARK_2D)) {
|
||||
toErase.push_back(itFactor); nrFactors--;
|
||||
dsf.makeUnionInPlace(label1, label2);
|
||||
succeed = true;
|
||||
break;
|
||||
}
|
||||
|
||||
// stack the current factor with the cached constraint
|
||||
IntPair labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1);
|
||||
Connections::iterator itCached = connections.find(labels);
|
||||
if (itCached == connections.end()) {
|
||||
connections.insert(make_pair(labels, itFactor));
|
||||
continue;
|
||||
} else {
|
||||
GenericNode2D key21 = (*itCached->second)->key1, key22 = (*itCached->second)->key2;
|
||||
// if observe the same landmark, we can not merge, abandon the current factor
|
||||
if ((key1.index == key21.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 == key22.index && key2.type == NODE_LANDMARK_2D)) {
|
||||
toErase.push_back(itFactor); nrFactors--;
|
||||
continue;
|
||||
} else {
|
||||
toErase.push_back(itFactor); nrFactors--;
|
||||
toErase.push_back(itCached->second); nrFactors--;
|
||||
dsf.makeUnionInPlace(label1, label2);
|
||||
connections.erase(itCached);
|
||||
succeed = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
// stack the current factor with the cached constraint
|
||||
IntPair labels = (label1 < label2) ? make_pair(label1, label2) : make_pair(label2, label1);
|
||||
Connections::iterator itCached = connections.find(labels);
|
||||
if (itCached == connections.end()) {
|
||||
connections.insert(make_pair(labels, itFactor));
|
||||
continue;
|
||||
} else {
|
||||
GenericNode2D key21 = (*itCached->second)->key1, key22 = (*itCached->second)->key2;
|
||||
// if observe the same landmark, we can not merge, abandon the current factor
|
||||
if ((key1.index == key21.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 == key22.index && key2.type == NODE_LANDMARK_2D)) {
|
||||
toErase.push_back(itFactor); nrFactors--;
|
||||
continue;
|
||||
} else {
|
||||
toErase.push_back(itFactor); nrFactors--;
|
||||
toErase.push_back(itCached->second); nrFactors--;
|
||||
dsf.makeUnionInPlace(label1, label2);
|
||||
connections.erase(itCached);
|
||||
succeed = true;
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// erase unused factors
|
||||
BOOST_FOREACH(const FactorList::iterator& it, toErase)
|
||||
factors.erase(it);
|
||||
// erase unused factors
|
||||
BOOST_FOREACH(const FactorList::iterator& it, toErase)
|
||||
factors.erase(it);
|
||||
|
||||
if (!succeed) break;
|
||||
}
|
||||
if (!succeed) break;
|
||||
}
|
||||
|
||||
list<vector<size_t> > islands;
|
||||
map<size_t, vector<size_t> > arrays = dsf.arrays();
|
||||
size_t key; vector<size_t> array;
|
||||
BOOST_FOREACH(boost::tie(key, array), arrays)
|
||||
islands.push_back(array);
|
||||
return islands;
|
||||
}
|
||||
list<vector<size_t> > islands;
|
||||
map<size_t, vector<size_t> > arrays = dsf.arrays();
|
||||
size_t key; vector<size_t> array;
|
||||
BOOST_FOREACH(boost::tie(key, array), arrays)
|
||||
islands.push_back(array);
|
||||
return islands;
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
void print(const GenericGraph2D& graph, const std::string name) {
|
||||
cout << name << endl;
|
||||
BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph)
|
||||
cout << factor_->key1.index << " " << factor_->key2.index << endl;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void print(const GenericGraph2D& graph, const std::string name) {
|
||||
cout << name << endl;
|
||||
BOOST_FOREACH(const sharedGenericFactor2D& factor_, graph)
|
||||
cout << factor_->key1.index << " " << factor_->key2.index << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void print(const GenericGraph3D& graph, const std::string name) {
|
||||
cout << name << endl;
|
||||
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph)
|
||||
cout << factor_->key1.index << " " << factor_->key2.index << " (" <<
|
||||
factor_->key1.type << ", " << factor_->key2.type <<")" << endl;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
void print(const GenericGraph3D& graph, const std::string name) {
|
||||
cout << name << endl;
|
||||
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph)
|
||||
cout << factor_->key1.index << " " << factor_->key2.index << " (" <<
|
||||
factor_->key1.type << ", " << factor_->key2.type <<")" << endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// create disjoin set forest
|
||||
DSFVector createDSF(const GenericGraph3D& graph, const vector<size_t>& keys, const WorkSpace& workspace) {
|
||||
DSFVector dsf(workspace.dsf, keys);
|
||||
typedef list<sharedGenericFactor3D> FactorList;
|
||||
/* ************************************************************************* */
|
||||
// create disjoin set forest
|
||||
DSFVector createDSF(const GenericGraph3D& graph, const vector<size_t>& keys, const WorkSpace& workspace) {
|
||||
DSFVector dsf(workspace.dsf, keys);
|
||||
typedef list<sharedGenericFactor3D> FactorList;
|
||||
|
||||
FactorList factors(graph.begin(), graph.end());
|
||||
size_t nrFactors = factors.size();
|
||||
FactorList::iterator itEnd;
|
||||
while (nrFactors) {
|
||||
FactorList factors(graph.begin(), graph.end());
|
||||
size_t nrFactors = factors.size();
|
||||
FactorList::iterator itEnd;
|
||||
while (nrFactors) {
|
||||
|
||||
bool succeed = false;
|
||||
itEnd = factors.end();
|
||||
list<FactorList::iterator> toErase;
|
||||
for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {
|
||||
bool succeed = false;
|
||||
itEnd = factors.end();
|
||||
list<FactorList::iterator> toErase;
|
||||
for (FactorList::iterator itFactor=factors.begin(); itFactor!=itEnd; itFactor++) {
|
||||
|
||||
// remove invalid factors
|
||||
if (graph.size() == 178765) cout << "kai21" << endl;
|
||||
GenericNode3D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
|
||||
if (graph.size() == 178765) cout << "kai21: " << key1.index << " " << key2.index << endl;
|
||||
if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
|
||||
toErase.push_back(itFactor); nrFactors--; continue;
|
||||
}
|
||||
// remove invalid factors
|
||||
if (graph.size() == 178765) cout << "kai21" << endl;
|
||||
GenericNode3D key1 = (*itFactor)->key1, key2 = (*itFactor)->key2;
|
||||
if (graph.size() == 178765) cout << "kai21: " << key1.index << " " << key2.index << endl;
|
||||
if (workspace.dictionary[key1.index]==-1 || workspace.dictionary[key2.index]==-1) {
|
||||
toErase.push_back(itFactor); nrFactors--; continue;
|
||||
}
|
||||
|
||||
if (graph.size() == 178765) cout << "kai22" << endl;
|
||||
size_t label1 = dsf.findSet(key1.index);
|
||||
size_t label2 = dsf.findSet(key2.index);
|
||||
if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; }
|
||||
if (graph.size() == 178765) cout << "kai22" << endl;
|
||||
size_t label1 = dsf.findSet(key1.index);
|
||||
size_t label2 = dsf.findSet(key2.index);
|
||||
if (label1 == label2) { toErase.push_back(itFactor); nrFactors--; continue; }
|
||||
|
||||
if (graph.size() == 178765) cout << "kai23" << endl;
|
||||
// merge two trees if the connection is strong enough, otherwise cache it
|
||||
// an odometry factor always merges two islands
|
||||
if ((key1.type == NODE_POSE_3D && key2.type == NODE_LANDMARK_3D) ||
|
||||
(key1.type == NODE_POSE_3D && key2.type == NODE_POSE_3D)) {
|
||||
toErase.push_back(itFactor); nrFactors--;
|
||||
dsf.makeUnionInPlace(label1, label2);
|
||||
succeed = true;
|
||||
break;
|
||||
}
|
||||
if (graph.size() == 178765) cout << "kai23" << endl;
|
||||
// merge two trees if the connection is strong enough, otherwise cache it
|
||||
// an odometry factor always merges two islands
|
||||
if ((key1.type == NODE_POSE_3D && key2.type == NODE_LANDMARK_3D) ||
|
||||
(key1.type == NODE_POSE_3D && key2.type == NODE_POSE_3D)) {
|
||||
toErase.push_back(itFactor); nrFactors--;
|
||||
dsf.makeUnionInPlace(label1, label2);
|
||||
succeed = true;
|
||||
break;
|
||||
}
|
||||
|
||||
if (graph.size() == 178765) cout << "kai24" << endl;
|
||||
if (graph.size() == 178765) cout << "kai24" << endl;
|
||||
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
// erase unused factors
|
||||
BOOST_FOREACH(const FactorList::iterator& it, toErase)
|
||||
factors.erase(it);
|
||||
// erase unused factors
|
||||
BOOST_FOREACH(const FactorList::iterator& it, toErase)
|
||||
factors.erase(it);
|
||||
|
||||
if (!succeed) break;
|
||||
}
|
||||
return dsf;
|
||||
}
|
||||
if (!succeed) break;
|
||||
}
|
||||
return dsf;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// 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) {
|
||||
switch(node.type) {
|
||||
case NODE_POSE_3D:
|
||||
return singularCameras.find(node.index) != singularCameras.end(); break;
|
||||
case NODE_LANDMARK_3D:
|
||||
return singularLandmarks.find(node.index) != singularLandmarks.end(); break;
|
||||
default:
|
||||
throw runtime_error("unrecognized key type!");
|
||||
}
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
// 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) {
|
||||
switch(node.type) {
|
||||
case NODE_POSE_3D:
|
||||
return singularCameras.find(node.index) != singularCameras.end(); break;
|
||||
case NODE_LANDMARK_3D:
|
||||
return singularLandmarks.find(node.index) != singularLandmarks.end(); break;
|
||||
default:
|
||||
throw runtime_error("unrecognized key type!");
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void findSingularCamerasLandmarks(const GenericGraph3D& graph, const WorkSpace& workspace,
|
||||
const vector<bool>& isCamera, const vector<bool>& isLandmark,
|
||||
set<size_t>& singularCameras, set<size_t>& singularLandmarks, vector<int>& nrConstraints,
|
||||
bool& foundSingularCamera, bool& foundSingularLandmark,
|
||||
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {
|
||||
/* ************************************************************************* */
|
||||
void findSingularCamerasLandmarks(const GenericGraph3D& graph, const WorkSpace& workspace,
|
||||
const vector<bool>& isCamera, const vector<bool>& isLandmark,
|
||||
set<size_t>& singularCameras, set<size_t>& singularLandmarks, vector<int>& nrConstraints,
|
||||
bool& foundSingularCamera, bool& foundSingularLandmark,
|
||||
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) {
|
||||
|
||||
// compute the constraint number per camera
|
||||
std::fill(nrConstraints.begin(), nrConstraints.end(), 0);
|
||||
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
|
||||
const int& key1 = factor_->key1.index;
|
||||
const int& key2 = factor_->key2.index;
|
||||
if (workspace.dictionary[key1] != -1 && workspace.dictionary[key2] != -1 &&
|
||||
!isSingular(singularCameras, singularLandmarks, factor_->key1) &&
|
||||
!isSingular(singularCameras, singularLandmarks, factor_->key2)) {
|
||||
nrConstraints[key1]++;
|
||||
nrConstraints[key2]++;
|
||||
// compute the constraint number per camera
|
||||
std::fill(nrConstraints.begin(), nrConstraints.end(), 0);
|
||||
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
|
||||
const int& key1 = factor_->key1.index;
|
||||
const int& key2 = factor_->key2.index;
|
||||
if (workspace.dictionary[key1] != -1 && workspace.dictionary[key2] != -1 &&
|
||||
!isSingular(singularCameras, singularLandmarks, factor_->key1) &&
|
||||
!isSingular(singularCameras, singularLandmarks, factor_->key2)) {
|
||||
nrConstraints[key1]++;
|
||||
nrConstraints[key2]++;
|
||||
|
||||
// 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
|
||||
if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
|
||||
nrConstraints[key1]+=2;
|
||||
nrConstraints[key2]+=2;
|
||||
}
|
||||
}
|
||||
}
|
||||
// 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
|
||||
if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
|
||||
nrConstraints[key1]+=2;
|
||||
nrConstraints[key2]+=2;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// find singular cameras and landmarks
|
||||
foundSingularCamera = false;
|
||||
foundSingularLandmark = false;
|
||||
for (size_t i=0; i<nrConstraints.size(); i++) {
|
||||
if (isCamera[i] && nrConstraints[i] < minNrConstraintsPerCamera &&
|
||||
singularCameras.find(i) == singularCameras.end()) {
|
||||
singularCameras.insert(i);
|
||||
foundSingularCamera = true;
|
||||
}
|
||||
if (isLandmark[i] && nrConstraints[i] < minNrConstraintsPerLandmark &&
|
||||
singularLandmarks.find(i) == singularLandmarks.end()) {
|
||||
singularLandmarks.insert(i);
|
||||
foundSingularLandmark = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
// find singular cameras and landmarks
|
||||
foundSingularCamera = false;
|
||||
foundSingularLandmark = false;
|
||||
for (size_t i=0; i<nrConstraints.size(); i++) {
|
||||
if (isCamera[i] && nrConstraints[i] < minNrConstraintsPerCamera &&
|
||||
singularCameras.find(i) == singularCameras.end()) {
|
||||
singularCameras.insert(i);
|
||||
foundSingularCamera = true;
|
||||
}
|
||||
if (isLandmark[i] && nrConstraints[i] < minNrConstraintsPerLandmark &&
|
||||
singularLandmarks.find(i) == singularLandmarks.end()) {
|
||||
singularLandmarks.insert(i);
|
||||
foundSingularLandmark = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
list<vector<size_t> > findIslands(const GenericGraph3D& graph, const vector<size_t>& keys, WorkSpace& workspace,
|
||||
const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
|
||||
/* ************************************************************************* */
|
||||
list<vector<size_t> > findIslands(const GenericGraph3D& graph, const vector<size_t>& keys, WorkSpace& workspace,
|
||||
const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
|
||||
|
||||
// create disjoint set forest
|
||||
workspace.prepareDictionary(keys);
|
||||
DSFVector dsf = createDSF(graph, keys, workspace);
|
||||
// create disjoint set forest
|
||||
workspace.prepareDictionary(keys);
|
||||
DSFVector dsf = createDSF(graph, keys, workspace);
|
||||
|
||||
const bool verbose = false;
|
||||
bool foundSingularCamera = true;
|
||||
bool foundSingularLandmark = true;
|
||||
const bool verbose = false;
|
||||
bool foundSingularCamera = true;
|
||||
bool foundSingularLandmark = true;
|
||||
|
||||
list<vector<size_t> > islands;
|
||||
set<size_t> singularCameras, singularLandmarks;
|
||||
vector<bool> isCamera(workspace.dictionary.size(), false);
|
||||
vector<bool> isLandmark(workspace.dictionary.size(), false);
|
||||
list<vector<size_t> > islands;
|
||||
set<size_t> singularCameras, singularLandmarks;
|
||||
vector<bool> isCamera(workspace.dictionary.size(), false);
|
||||
vector<bool> isLandmark(workspace.dictionary.size(), false);
|
||||
|
||||
// check the constraint number of every variable
|
||||
// find the camera and landmark keys
|
||||
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
|
||||
//assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM
|
||||
if (workspace.dictionary[factor_->key1.index] != -1) {
|
||||
if (factor_->key1.type == NODE_POSE_3D)
|
||||
isCamera[factor_->key1.index] = true;
|
||||
else
|
||||
isLandmark[factor_->key1.index] = true;
|
||||
}
|
||||
// check the constraint number of every variable
|
||||
// find the camera and landmark keys
|
||||
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
|
||||
//assert(factor_->key2.type == NODE_LANDMARK_3D); // only VisualSLAM should come here, not StereoSLAM
|
||||
if (workspace.dictionary[factor_->key1.index] != -1) {
|
||||
if (factor_->key1.type == NODE_POSE_3D)
|
||||
isCamera[factor_->key1.index] = true;
|
||||
else
|
||||
isLandmark[factor_->key1.index] = true;
|
||||
}
|
||||
if (workspace.dictionary[factor_->key2.index] != -1) {
|
||||
if (factor_->key2.type == NODE_POSE_3D)
|
||||
isCamera[factor_->key2.index] = true;
|
||||
else
|
||||
isLandmark[factor_->key2.index] = true;
|
||||
if (factor_->key2.type == NODE_POSE_3D)
|
||||
isCamera[factor_->key2.index] = true;
|
||||
else
|
||||
isLandmark[factor_->key2.index] = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
vector<int> nrConstraints(workspace.dictionary.size(), 0);
|
||||
// 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
|
||||
while (foundSingularCamera || foundSingularLandmark) {
|
||||
findSingularCamerasLandmarks(graph, workspace, isCamera, isLandmark, // input
|
||||
singularCameras, singularLandmarks, nrConstraints, // output
|
||||
foundSingularCamera, foundSingularLandmark, // output
|
||||
minNrConstraintsPerCamera, minNrConstraintsPerLandmark); // input
|
||||
}
|
||||
vector<int> nrConstraints(workspace.dictionary.size(), 0);
|
||||
// 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
|
||||
while (foundSingularCamera || foundSingularLandmark) {
|
||||
findSingularCamerasLandmarks(graph, workspace, isCamera, isLandmark, // input
|
||||
singularCameras, singularLandmarks, nrConstraints, // output
|
||||
foundSingularCamera, foundSingularLandmark, // output
|
||||
minNrConstraintsPerCamera, minNrConstraintsPerLandmark); // input
|
||||
}
|
||||
|
||||
// add singular variables directly as islands
|
||||
if (!singularCameras.empty()) {
|
||||
if (verbose) cout << "singular cameras:";
|
||||
BOOST_FOREACH(const size_t i, singularCameras) {
|
||||
islands.push_back(vector<size_t>(1, i)); // <---------------------------
|
||||
if (verbose) cout << i << " ";
|
||||
}
|
||||
if (verbose) cout << endl;
|
||||
}
|
||||
if (!singularLandmarks.empty()) {
|
||||
if (verbose) cout << "singular landmarks:";
|
||||
BOOST_FOREACH(const size_t i, singularLandmarks) {
|
||||
islands.push_back(vector<size_t>(1, i)); // <---------------------------
|
||||
if (verbose) cout << i << " ";
|
||||
}
|
||||
if (verbose) cout << endl;
|
||||
}
|
||||
// add singular variables directly as islands
|
||||
if (!singularCameras.empty()) {
|
||||
if (verbose) cout << "singular cameras:";
|
||||
BOOST_FOREACH(const size_t i, singularCameras) {
|
||||
islands.push_back(vector<size_t>(1, i)); // <---------------------------
|
||||
if (verbose) cout << i << " ";
|
||||
}
|
||||
if (verbose) cout << endl;
|
||||
}
|
||||
if (!singularLandmarks.empty()) {
|
||||
if (verbose) cout << "singular landmarks:";
|
||||
BOOST_FOREACH(const size_t i, singularLandmarks) {
|
||||
islands.push_back(vector<size_t>(1, i)); // <---------------------------
|
||||
if (verbose) cout << i << " ";
|
||||
}
|
||||
if (verbose) cout << endl;
|
||||
}
|
||||
|
||||
|
||||
// regenerating islands
|
||||
map<size_t, vector<size_t> > labelIslands = dsf.arrays();
|
||||
size_t label; vector<size_t> island;
|
||||
BOOST_FOREACH(boost::tie(label, island), labelIslands) {
|
||||
vector<size_t> filteredIsland; // remove singular cameras from array
|
||||
filteredIsland.reserve(island.size());
|
||||
BOOST_FOREACH(const size_t key, island) {
|
||||
if ((isCamera[key] && singularCameras.find(key) == singularCameras.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
|
||||
filteredIsland.push_back(key);
|
||||
}
|
||||
}
|
||||
islands.push_back(filteredIsland);
|
||||
}
|
||||
// regenerating islands
|
||||
map<size_t, vector<size_t> > labelIslands = dsf.arrays();
|
||||
size_t label; vector<size_t> island;
|
||||
BOOST_FOREACH(boost::tie(label, island), labelIslands) {
|
||||
vector<size_t> filteredIsland; // remove singular cameras from array
|
||||
filteredIsland.reserve(island.size());
|
||||
BOOST_FOREACH(const size_t key, island) {
|
||||
if ((isCamera[key] && singularCameras.find(key) == singularCameras.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
|
||||
filteredIsland.push_back(key);
|
||||
}
|
||||
}
|
||||
islands.push_back(filteredIsland);
|
||||
}
|
||||
|
||||
// sanity check
|
||||
size_t nrKeys = 0;
|
||||
BOOST_FOREACH(const vector<size_t>& island, islands)
|
||||
nrKeys += island.size();
|
||||
if (nrKeys != keys.size()) {
|
||||
cout << nrKeys << " vs " << keys.size() << endl;
|
||||
throw runtime_error("findIslands: the number of keys is inconsistent!");
|
||||
}
|
||||
// sanity check
|
||||
size_t nrKeys = 0;
|
||||
BOOST_FOREACH(const vector<size_t>& island, islands)
|
||||
nrKeys += island.size();
|
||||
if (nrKeys != keys.size()) {
|
||||
cout << nrKeys << " vs " << keys.size() << endl;
|
||||
throw runtime_error("findIslands: the number of keys is inconsistent!");
|
||||
}
|
||||
|
||||
|
||||
if (verbose) cout << "found " << islands.size() << " islands!" << endl;
|
||||
return islands;
|
||||
}
|
||||
if (verbose) cout << "found " << islands.size() << " islands!" << endl;
|
||||
return islands;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// return the number of intersection between two **sorted** landmark vectors
|
||||
inline int getNrCommonLandmarks(const vector<size_t>& landmarks1, const vector<size_t>& landmarks2){
|
||||
size_t i1 = 0, i2 = 0;
|
||||
int nrCommonLandmarks = 0;
|
||||
while (i1 < landmarks1.size() && i2 < landmarks2.size()) {
|
||||
if (landmarks1[i1] < landmarks2[i2])
|
||||
i1 ++;
|
||||
else if (landmarks1[i1] > landmarks2[i2])
|
||||
i2 ++;
|
||||
else {
|
||||
i1++; i2++;
|
||||
nrCommonLandmarks ++;
|
||||
}
|
||||
}
|
||||
return nrCommonLandmarks;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
// return the number of intersection between two **sorted** landmark vectors
|
||||
inline int getNrCommonLandmarks(const vector<size_t>& landmarks1, const vector<size_t>& landmarks2){
|
||||
size_t i1 = 0, i2 = 0;
|
||||
int nrCommonLandmarks = 0;
|
||||
while (i1 < landmarks1.size() && i2 < landmarks2.size()) {
|
||||
if (landmarks1[i1] < landmarks2[i2])
|
||||
i1 ++;
|
||||
else if (landmarks1[i1] > landmarks2[i2])
|
||||
i2 ++;
|
||||
else {
|
||||
i1++; i2++;
|
||||
nrCommonLandmarks ++;
|
||||
}
|
||||
}
|
||||
return nrCommonLandmarks;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
|
||||
const std::vector<int>& dictionary, GenericGraph3D& reducedGraph) {
|
||||
/* ************************************************************************* */
|
||||
void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
|
||||
const std::vector<int>& dictionary, GenericGraph3D& reducedGraph) {
|
||||
|
||||
typedef size_t CameraKey;
|
||||
typedef pair<CameraKey, CameraKey> CameraPair;
|
||||
typedef size_t LandmarkKey;
|
||||
// get a mapping from each landmark to its connected cameras
|
||||
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
|
||||
vector<int> cameraToCamera(dictionary.size(), -1);
|
||||
size_t key_i, key_j;
|
||||
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
|
||||
if (factor_->key1.type == NODE_POSE_3D) {
|
||||
if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor
|
||||
cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index);
|
||||
}
|
||||
else { // odometry factor
|
||||
if (factor_->key1.index < factor_->key2.index) {
|
||||
key_i = factor_->key1.index;
|
||||
key_j = factor_->key2.index;
|
||||
} else {
|
||||
key_i = factor_->key2.index;
|
||||
key_j = factor_->key1.index;
|
||||
}
|
||||
cameraToCamera[key_i] = key_j;
|
||||
}
|
||||
}
|
||||
}
|
||||
typedef size_t CameraKey;
|
||||
typedef pair<CameraKey, CameraKey> CameraPair;
|
||||
typedef size_t LandmarkKey;
|
||||
// get a mapping from each landmark to its connected cameras
|
||||
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
|
||||
vector<int> cameraToCamera(dictionary.size(), -1);
|
||||
size_t key_i, key_j;
|
||||
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
|
||||
if (factor_->key1.type == NODE_POSE_3D) {
|
||||
if (factor_->key2.type == NODE_LANDMARK_3D) {// projection factor
|
||||
cameraToLandmarks[factor_->key1.index].push_back(factor_->key2.index);
|
||||
}
|
||||
else { // odometry factor
|
||||
if (factor_->key1.index < factor_->key2.index) {
|
||||
key_i = factor_->key1.index;
|
||||
key_j = factor_->key2.index;
|
||||
} else {
|
||||
key_i = factor_->key2.index;
|
||||
key_j = factor_->key1.index;
|
||||
}
|
||||
cameraToCamera[key_i] = key_j;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// sort the landmark keys for the late getNrCommonLandmarks call
|
||||
BOOST_FOREACH(vector<LandmarkKey> &landmarks, cameraToLandmarks){
|
||||
if (!landmarks.empty())
|
||||
std::sort(landmarks.begin(), landmarks.end());
|
||||
}
|
||||
// sort the landmark keys for the late getNrCommonLandmarks call
|
||||
BOOST_FOREACH(vector<LandmarkKey> &landmarks, cameraToLandmarks){
|
||||
if (!landmarks.empty())
|
||||
std::sort(landmarks.begin(), landmarks.end());
|
||||
}
|
||||
|
||||
// generate the reduced graph
|
||||
reducedGraph.clear();
|
||||
int factorIndex = 0;
|
||||
int camera1, camera2, nrTotalConstraints;
|
||||
bool hasOdometry;
|
||||
for (size_t i1=0; i1<cameraKeys.size()-1; ++i1) {
|
||||
for (size_t i2=i1+1; i2<cameraKeys.size(); ++i2) {
|
||||
camera1 = cameraKeys[i1];
|
||||
camera2 = cameraKeys[i2];
|
||||
int nrCommonLandmarks = getNrCommonLandmarks(cameraToLandmarks[camera1], cameraToLandmarks[camera2]);
|
||||
hasOdometry = cameraToCamera[camera1] == camera2;
|
||||
if (nrCommonLandmarks > 0 || hasOdometry) {
|
||||
nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0);
|
||||
reducedGraph.push_back(boost::make_shared<GenericFactor3D>(camera1, camera2,
|
||||
factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
// generate the reduced graph
|
||||
reducedGraph.clear();
|
||||
int factorIndex = 0;
|
||||
int camera1, camera2, nrTotalConstraints;
|
||||
bool hasOdometry;
|
||||
for (size_t i1=0; i1<cameraKeys.size()-1; ++i1) {
|
||||
for (size_t i2=i1+1; i2<cameraKeys.size(); ++i2) {
|
||||
camera1 = cameraKeys[i1];
|
||||
camera2 = cameraKeys[i2];
|
||||
int nrCommonLandmarks = getNrCommonLandmarks(cameraToLandmarks[camera1], cameraToLandmarks[camera2]);
|
||||
hasOdometry = cameraToCamera[camera1] == camera2;
|
||||
if (nrCommonLandmarks > 0 || hasOdometry) {
|
||||
nrTotalConstraints = 2 * nrCommonLandmarks + (hasOdometry ? 6 : 0);
|
||||
reducedGraph.push_back(boost::make_shared<GenericFactor3D>(camera1, camera2,
|
||||
factorIndex++, NODE_POSE_3D, NODE_POSE_3D, nrTotalConstraints));
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
|
||||
WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
|
||||
workspace.prepareDictionary(frontals);
|
||||
vector<size_t> nrConstraints(workspace.dictionary.size(), 0);
|
||||
/* ************************************************************************* */
|
||||
void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
|
||||
WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark) {
|
||||
workspace.prepareDictionary(frontals);
|
||||
vector<size_t> nrConstraints(workspace.dictionary.size(), 0);
|
||||
|
||||
// summarize the constraint number
|
||||
const vector<int>& dictionary = workspace.dictionary;
|
||||
vector<bool> isValidCamera(workspace.dictionary.size(), false);
|
||||
vector<bool> isValidLandmark(workspace.dictionary.size(), false);
|
||||
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
|
||||
assert(factor_->key1.type == NODE_POSE_3D);
|
||||
//assert(factor_->key2.type == NODE_LANDMARK_3D);
|
||||
const size_t& key1 = factor_->key1.index;
|
||||
const size_t& key2 = factor_->key2.index;
|
||||
if (dictionary[key1] == -1 || dictionary[key2] == -1)
|
||||
continue;
|
||||
// summarize the constraint number
|
||||
const vector<int>& dictionary = workspace.dictionary;
|
||||
vector<bool> isValidCamera(workspace.dictionary.size(), false);
|
||||
vector<bool> isValidLandmark(workspace.dictionary.size(), false);
|
||||
BOOST_FOREACH(const sharedGenericFactor3D& factor_, graph) {
|
||||
assert(factor_->key1.type == NODE_POSE_3D);
|
||||
//assert(factor_->key2.type == NODE_LANDMARK_3D);
|
||||
const size_t& key1 = factor_->key1.index;
|
||||
const size_t& key2 = factor_->key2.index;
|
||||
if (dictionary[key1] == -1 || dictionary[key2] == -1)
|
||||
continue;
|
||||
|
||||
isValidCamera[key1] = true;
|
||||
if(factor_->key2.type == NODE_LANDMARK_3D)
|
||||
isValidLandmark[key2] = true;
|
||||
else
|
||||
isValidCamera[key2] = true;
|
||||
isValidCamera[key1] = true;
|
||||
if(factor_->key2.type == NODE_LANDMARK_3D)
|
||||
isValidLandmark[key2] = true;
|
||||
else
|
||||
isValidCamera[key2] = true;
|
||||
|
||||
nrConstraints[key1]++;
|
||||
nrConstraints[key2]++;
|
||||
nrConstraints[key1]++;
|
||||
nrConstraints[key2]++;
|
||||
|
||||
// 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
|
||||
if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
|
||||
nrConstraints[key1]+=2;
|
||||
nrConstraints[key2]+=2;
|
||||
}
|
||||
}
|
||||
// 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
|
||||
if(factor_->key1.type == NODE_POSE_3D && factor_->key2.type == NODE_POSE_3D){
|
||||
nrConstraints[key1]+=2;
|
||||
nrConstraints[key2]+=2;
|
||||
}
|
||||
}
|
||||
|
||||
// find the minimum constraint for cameras and landmarks
|
||||
size_t minFoundConstraintsPerCamera = 10000;
|
||||
size_t minFoundConstraintsPerLandmark = 10000;
|
||||
// find the minimum constraint for cameras and landmarks
|
||||
size_t minFoundConstraintsPerCamera = 10000;
|
||||
size_t minFoundConstraintsPerLandmark = 10000;
|
||||
|
||||
for (size_t i=0; i<isValidCamera.size(); i++) {
|
||||
if (isValidCamera[i]) {
|
||||
minFoundConstraintsPerCamera = std::min(nrConstraints[i], minFoundConstraintsPerCamera);
|
||||
if (nrConstraints[i] < minNrConstraintsPerCamera)
|
||||
cout << "!!!!!!!!!!!!!!!!!!! camera with " << nrConstraints[i] << " constraint: " << i << endl;
|
||||
}
|
||||
for (size_t i=0; i<isValidCamera.size(); i++) {
|
||||
if (isValidCamera[i]) {
|
||||
minFoundConstraintsPerCamera = std::min(nrConstraints[i], minFoundConstraintsPerCamera);
|
||||
if (nrConstraints[i] < minNrConstraintsPerCamera)
|
||||
cout << "!!!!!!!!!!!!!!!!!!! camera with " << nrConstraints[i] << " constraint: " << i << endl;
|
||||
}
|
||||
|
||||
}
|
||||
for (size_t j=0; j<isValidLandmark.size(); j++) {
|
||||
if (isValidLandmark[j]) {
|
||||
minFoundConstraintsPerLandmark = std::min(nrConstraints[j], minFoundConstraintsPerLandmark);
|
||||
if (nrConstraints[j] < minNrConstraintsPerLandmark)
|
||||
cout << "!!!!!!!!!!!!!!!!!!! landmark with " << nrConstraints[j] << " constraint: " << j << endl;
|
||||
}
|
||||
}
|
||||
}
|
||||
for (size_t j=0; j<isValidLandmark.size(); j++) {
|
||||
if (isValidLandmark[j]) {
|
||||
minFoundConstraintsPerLandmark = std::min(nrConstraints[j], minFoundConstraintsPerLandmark);
|
||||
if (nrConstraints[j] < minNrConstraintsPerLandmark)
|
||||
cout << "!!!!!!!!!!!!!!!!!!! landmark with " << nrConstraints[j] << " constraint: " << j << endl;
|
||||
}
|
||||
}
|
||||
|
||||
// debug info
|
||||
BOOST_FOREACH(const size_t key, frontals) {
|
||||
if (isValidCamera[key] && nrConstraints[key] < minNrConstraintsPerCamera)
|
||||
cout << "singular camera:" << key << " with " << nrConstraints[key] << " constraints" << endl;
|
||||
}
|
||||
// debug info
|
||||
BOOST_FOREACH(const size_t key, frontals) {
|
||||
if (isValidCamera[key] && nrConstraints[key] < minNrConstraintsPerCamera)
|
||||
cout << "singular camera:" << key << " with " << nrConstraints[key] << " constraints" << endl;
|
||||
}
|
||||
|
||||
if (minFoundConstraintsPerCamera < minNrConstraintsPerCamera)
|
||||
throw runtime_error("checkSingularity:minConstraintsPerCamera < " + boost::lexical_cast<string>(minFoundConstraintsPerCamera));
|
||||
if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark)
|
||||
throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + boost::lexical_cast<string>(minFoundConstraintsPerLandmark));
|
||||
}
|
||||
if (minFoundConstraintsPerCamera < minNrConstraintsPerCamera)
|
||||
throw runtime_error("checkSingularity:minConstraintsPerCamera < " + boost::lexical_cast<string>(minFoundConstraintsPerCamera));
|
||||
if (minFoundConstraintsPerLandmark < minNrConstraintsPerLandmark)
|
||||
throw runtime_error("checkSingularity:minConstraintsPerLandmark < " + boost::lexical_cast<string>(minFoundConstraintsPerLandmark));
|
||||
}
|
||||
|
||||
}} // namespace
|
||||
|
|
|
|||
|
|
@ -17,133 +17,133 @@
|
|||
|
||||
namespace gtsam { namespace partition {
|
||||
|
||||
/***************************************************
|
||||
* 2D generic factors and their factor graph
|
||||
***************************************************/
|
||||
enum GenericNode2DType { NODE_POSE_2D, NODE_LANDMARK_2D };
|
||||
/***************************************************
|
||||
* 2D generic factors and their factor graph
|
||||
***************************************************/
|
||||
enum GenericNode2DType { NODE_POSE_2D, NODE_LANDMARK_2D };
|
||||
|
||||
/** the index of the node and the type of the node */
|
||||
struct GenericNode2D {
|
||||
std::size_t index;
|
||||
GenericNode2DType type;
|
||||
GenericNode2D (const std::size_t& index_in, const GenericNode2DType& type_in) : index(index_in), type(type_in) {}
|
||||
};
|
||||
/** the index of the node and the type of the node */
|
||||
struct GenericNode2D {
|
||||
std::size_t index;
|
||||
GenericNode2DType type;
|
||||
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 */
|
||||
struct GenericFactor2D {
|
||||
GenericNode2D key1;
|
||||
GenericNode2D key2;
|
||||
int index; // the factor index in the original nonlinear factor graph
|
||||
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)
|
||||
: 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)
|
||||
: key1(index1, type1 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D),
|
||||
key2(index2, type2 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_), weight(weight_) {}
|
||||
};
|
||||
/** a factor always involves two nodes/variables for now */
|
||||
struct GenericFactor2D {
|
||||
GenericNode2D key1;
|
||||
GenericNode2D key2;
|
||||
int index; // the factor index in the original nonlinear factor graph
|
||||
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)
|
||||
: 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)
|
||||
: key1(index1, type1 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D),
|
||||
key2(index2, type2 == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_), weight(weight_) {}
|
||||
};
|
||||
|
||||
/** graph is a collection of factors */
|
||||
typedef boost::shared_ptr<GenericFactor2D> sharedGenericFactor2D;
|
||||
typedef std::vector<sharedGenericFactor2D> GenericGraph2D;
|
||||
/** graph is a collection of factors */
|
||||
typedef boost::shared_ptr<GenericFactor2D> sharedGenericFactor2D;
|
||||
typedef std::vector<sharedGenericFactor2D> GenericGraph2D;
|
||||
|
||||
/** 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,
|
||||
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark);
|
||||
/** 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,
|
||||
const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark);
|
||||
|
||||
/** eliminate the sensors from generic graph */
|
||||
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) {
|
||||
throw std::runtime_error("reduceGenericGraph 2d not implemented");
|
||||
}
|
||||
/** eliminate the sensors from generic graph */
|
||||
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) {
|
||||
throw std::runtime_error("reduceGenericGraph 2d not implemented");
|
||||
}
|
||||
|
||||
/** 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,
|
||||
WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { return; }
|
||||
/** 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,
|
||||
WorkSpace& workspace, const int minNrConstraintsPerCamera, const int minNrConstraintsPerLandmark) { return; }
|
||||
|
||||
/** print the graph **/
|
||||
void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D");
|
||||
/** print the graph **/
|
||||
void print(const GenericGraph2D& graph, const std::string name = "GenericGraph2D");
|
||||
|
||||
/***************************************************
|
||||
* 3D generic factors and their factor graph
|
||||
***************************************************/
|
||||
enum GenericNode3DType { NODE_POSE_3D, NODE_LANDMARK_3D };
|
||||
/***************************************************
|
||||
* 3D generic factors and their factor graph
|
||||
***************************************************/
|
||||
enum GenericNode3DType { NODE_POSE_3D, NODE_LANDMARK_3D };
|
||||
|
||||
// const int minNrConstraintsPerCamera = 7;
|
||||
// const int minNrConstraintsPerLandmark = 2;
|
||||
// const int minNrConstraintsPerCamera = 7;
|
||||
// const int minNrConstraintsPerLandmark = 2;
|
||||
|
||||
/** the index of the node and the type of the node */
|
||||
struct GenericNode3D {
|
||||
std::size_t index;
|
||||
GenericNode3DType type;
|
||||
GenericNode3D (const std::size_t& index_in, const GenericNode3DType& type_in) : index(index_in), type(type_in) {}
|
||||
};
|
||||
/** the index of the node and the type of the node */
|
||||
struct GenericNode3D {
|
||||
std::size_t index;
|
||||
GenericNode3DType type;
|
||||
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 */
|
||||
struct GenericFactor3D {
|
||||
GenericNode3D key1;
|
||||
GenericNode3D key2;
|
||||
int index; // the index in the entire graph, 0-based
|
||||
int weight; // the weight of the edge
|
||||
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,
|
||||
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_) {}
|
||||
};
|
||||
/** a factor always involves two nodes/variables for now */
|
||||
struct GenericFactor3D {
|
||||
GenericNode3D key1;
|
||||
GenericNode3D key2;
|
||||
int index; // the index in the entire graph, 0-based
|
||||
int weight; // the weight of the edge
|
||||
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,
|
||||
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_) {}
|
||||
};
|
||||
|
||||
/** graph is a collection of factors */
|
||||
typedef boost::shared_ptr<GenericFactor3D> sharedGenericFactor3D;
|
||||
typedef std::vector<sharedGenericFactor3D> GenericGraph3D;
|
||||
/** graph is a collection of factors */
|
||||
typedef boost::shared_ptr<GenericFactor3D> sharedGenericFactor3D;
|
||||
typedef std::vector<sharedGenericFactor3D> GenericGraph3D;
|
||||
|
||||
/** 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,
|
||||
const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
|
||||
/** 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,
|
||||
const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
|
||||
|
||||
/** eliminate the sensors from generic graph */
|
||||
void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
|
||||
const std::vector<int>& dictionary, GenericGraph3D& reducedGraph);
|
||||
/** eliminate the sensors from generic graph */
|
||||
void reduceGenericGraph(const GenericGraph3D& graph, const std::vector<size_t>& cameraKeys, const std::vector<size_t>& landmarkKeys,
|
||||
const std::vector<int>& dictionary, GenericGraph3D& reducedGraph);
|
||||
|
||||
/** check whether the 3D graph is singular (under constrained) */
|
||||
void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
|
||||
WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
|
||||
/** check whether the 3D graph is singular (under constrained) */
|
||||
void checkSingularity(const GenericGraph3D& graph, const std::vector<size_t>& frontals,
|
||||
WorkSpace& workspace, const size_t minNrConstraintsPerCamera, const size_t minNrConstraintsPerLandmark);
|
||||
|
||||
|
||||
/** print the graph **/
|
||||
void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D");
|
||||
/** print the graph **/
|
||||
void print(const GenericGraph3D& graph, const std::string name = "GenericGraph3D");
|
||||
|
||||
/***************************************************
|
||||
* unary generic factors and their factor graph
|
||||
***************************************************/
|
||||
/** a factor involves a single variable */
|
||||
struct GenericUnaryFactor {
|
||||
GenericNode2D key;
|
||||
int index; // the factor index in the original nonlinear factor graph
|
||||
GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_ = -1)
|
||||
: key(key_, type_), index(index_) {}
|
||||
GenericUnaryFactor(const size_t key_, const char type_, const int index_ = -1)
|
||||
: key(key_, type_ == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_) {}
|
||||
};
|
||||
/***************************************************
|
||||
* unary generic factors and their factor graph
|
||||
***************************************************/
|
||||
/** a factor involves a single variable */
|
||||
struct GenericUnaryFactor {
|
||||
GenericNode2D key;
|
||||
int index; // the factor index in the original nonlinear factor graph
|
||||
GenericUnaryFactor(const size_t key_, const GenericNode2DType type_, const int index_ = -1)
|
||||
: key(key_, type_), index(index_) {}
|
||||
GenericUnaryFactor(const size_t key_, const char type_, const int index_ = -1)
|
||||
: key(key_, type_ == 'x' ? NODE_POSE_2D : NODE_LANDMARK_2D), index(index_) {}
|
||||
};
|
||||
|
||||
/** graph is a collection of factors */
|
||||
typedef boost::shared_ptr<GenericUnaryFactor> sharedGenericUnaryFactor;
|
||||
typedef std::vector<sharedGenericUnaryFactor> GenericUnaryGraph;
|
||||
/** graph is a collection of factors */
|
||||
typedef boost::shared_ptr<GenericUnaryFactor> sharedGenericUnaryFactor;
|
||||
typedef std::vector<sharedGenericUnaryFactor> GenericUnaryGraph;
|
||||
|
||||
/***************************************************
|
||||
* utility functions
|
||||
***************************************************/
|
||||
inline bool hasCommonCamera(const std::set<size_t>& cameras1, const std::set<size_t>& cameras2) {
|
||||
if (cameras1.empty() || cameras2.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 it2 = cameras2.begin();
|
||||
while (it1 != cameras1.end() && it2 != cameras2.end()) {
|
||||
if (*it1 == *it2)
|
||||
return true;
|
||||
else if (*it1 < *it2)
|
||||
it1++;
|
||||
else
|
||||
it2++;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
/***************************************************
|
||||
* utility functions
|
||||
***************************************************/
|
||||
inline bool hasCommonCamera(const std::set<size_t>& cameras1, const std::set<size_t>& cameras2) {
|
||||
if (cameras1.empty() || cameras2.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 it2 = cameras2.begin();
|
||||
while (it1 != cameras1.end() && it2 != cameras2.end()) {
|
||||
if (*it1 == *it2)
|
||||
return true;
|
||||
else if (*it1 < *it2)
|
||||
it1++;
|
||||
else
|
||||
it2++;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
}} // namespace
|
||||
|
|
|
|||
|
|
@ -16,236 +16,236 @@
|
|||
|
||||
namespace gtsam { namespace partition {
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
|
||||
const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose) :
|
||||
fg_(fg), ordering_(ordering){
|
||||
GenericUnaryGraph unaryFactors;
|
||||
GenericGraph gfg;
|
||||
boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
|
||||
const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose) :
|
||||
fg_(fg), ordering_(ordering){
|
||||
GenericUnaryGraph unaryFactors;
|
||||
GenericGraph gfg;
|
||||
boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
|
||||
|
||||
// build reverse mapping from integer to symbol
|
||||
int numNodes = ordering.size();
|
||||
int2symbol_.resize(numNodes);
|
||||
Ordering::const_iterator it = ordering.begin(), itLast = ordering.end();
|
||||
while(it != itLast)
|
||||
int2symbol_[it->second] = (it++)->first;
|
||||
// build reverse mapping from integer to symbol
|
||||
int numNodes = ordering.size();
|
||||
int2symbol_.resize(numNodes);
|
||||
Ordering::const_iterator it = ordering.begin(), itLast = ordering.end();
|
||||
while(it != itLast)
|
||||
int2symbol_[it->second] = (it++)->first;
|
||||
|
||||
vector<size_t> keys;
|
||||
keys.reserve(numNodes);
|
||||
for(int i=0; i<ordering.size(); ++i)
|
||||
keys.push_back(i);
|
||||
vector<size_t> keys;
|
||||
keys.reserve(numNodes);
|
||||
for(int i=0; i<ordering.size(); ++i)
|
||||
keys.push_back(i);
|
||||
|
||||
WorkSpace workspace(numNodes);
|
||||
root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), numNodeStopPartition, minNodesPerMap, boost::shared_ptr<SubNLG>(), workspace, verbose);
|
||||
}
|
||||
WorkSpace workspace(numNodes);
|
||||
root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), numNodeStopPartition, minNodesPerMap, boost::shared_ptr<SubNLG>(), workspace, verbose);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
|
||||
const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& cuts, const bool verbose) : fg_(fg), ordering_(ordering){
|
||||
GenericUnaryGraph unaryFactors;
|
||||
GenericGraph gfg;
|
||||
boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
NestedDissection<NLG, SubNLG, GenericGraph>::NestedDissection(
|
||||
const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& cuts, const bool verbose) : fg_(fg), ordering_(ordering){
|
||||
GenericUnaryGraph unaryFactors;
|
||||
GenericGraph gfg;
|
||||
boost::tie(unaryFactors, gfg) = fg.createGenericGraph(ordering);
|
||||
|
||||
// build reverse mapping from integer to symbol
|
||||
int numNodes = ordering.size();
|
||||
int2symbol_.resize(numNodes);
|
||||
Ordering::const_iterator it = ordering.begin(), itLast = ordering.end();
|
||||
while(it != itLast)
|
||||
int2symbol_[it->second] = (it++)->first;
|
||||
// build reverse mapping from integer to symbol
|
||||
int numNodes = ordering.size();
|
||||
int2symbol_.resize(numNodes);
|
||||
Ordering::const_iterator it = ordering.begin(), itLast = ordering.end();
|
||||
while(it != itLast)
|
||||
int2symbol_[it->second] = (it++)->first;
|
||||
|
||||
vector<size_t> keys;
|
||||
keys.reserve(numNodes);
|
||||
for(int i=0; i<ordering.size(); ++i)
|
||||
keys.push_back(i);
|
||||
vector<size_t> keys;
|
||||
keys.reserve(numNodes);
|
||||
for(int i=0; i<ordering.size(); ++i)
|
||||
keys.push_back(i);
|
||||
|
||||
WorkSpace workspace(numNodes);
|
||||
root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), cuts, boost::shared_ptr<SubNLG>(), workspace, verbose);
|
||||
}
|
||||
WorkSpace workspace(numNodes);
|
||||
root_ = recursivePartition(gfg, unaryFactors, keys, vector<size_t>(), cuts, boost::shared_ptr<SubNLG>(), workspace, verbose);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
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 {
|
||||
OrderedSymbols frontalKeys;
|
||||
BOOST_FOREACH(const size_t index, frontals)
|
||||
frontalKeys.push_back(int2symbol_[index]);
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
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 {
|
||||
OrderedSymbols frontalKeys;
|
||||
BOOST_FOREACH(const size_t index, frontals)
|
||||
frontalKeys.push_back(int2symbol_[index]);
|
||||
|
||||
UnorderedSymbols sepKeys;
|
||||
BOOST_FOREACH(const size_t index, sep)
|
||||
sepKeys.insert(int2symbol_[index]);
|
||||
UnorderedSymbols sepKeys;
|
||||
BOOST_FOREACH(const size_t index, sep)
|
||||
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>
|
||||
void NestedDissection<NLG, SubNLG, GenericGraph>::processFactor(
|
||||
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
|
||||
typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques
|
||||
list<size_t> sep_; // the separator variables involved in the current factor
|
||||
int partition1 = partitionTable[factor->key1.index];
|
||||
int partition2 = partitionTable[factor->key2.index];
|
||||
if (partition1 <= 0 && partition2 <= 0) { // is a factor in the current clique
|
||||
sepFactors.push_back(fg_[factor->index]);
|
||||
}
|
||||
else if (partition1 > 0 && partition2 > 0 && partition1 != partition2) { // is a weeklink (factor between two child cliques)
|
||||
weeklinks.push_back(fg_[factor->index]);
|
||||
}
|
||||
else if (partition1 > 0 && partition2 > 0 && partition1 == partition2) { // is a local factor in one of the child cliques
|
||||
frontalFactors[partition1 - 1].push_back(factor);
|
||||
}
|
||||
else { // is a joint factor in the child clique (involving varaibles in the current clique)
|
||||
if (partition1 > 0 && partition2 <= 0) {
|
||||
frontalFactors[partition1 - 1].push_back(factor);
|
||||
childSeps[partition1 - 1].insert(factor->key2.index);
|
||||
} else if (partition1 <= 0 && partition2 > 0) {
|
||||
frontalFactors[partition2 - 1].push_back(factor);
|
||||
childSeps[partition2 - 1].insert(factor->key1.index);
|
||||
} else
|
||||
throw runtime_error("processFactor: unexpected entries in the partition table!");
|
||||
}
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
void NestedDissection<NLG, SubNLG, GenericGraph>::processFactor(
|
||||
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
|
||||
typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques
|
||||
list<size_t> sep_; // the separator variables involved in the current factor
|
||||
int partition1 = partitionTable[factor->key1.index];
|
||||
int partition2 = partitionTable[factor->key2.index];
|
||||
if (partition1 <= 0 && partition2 <= 0) { // is a factor in the current clique
|
||||
sepFactors.push_back(fg_[factor->index]);
|
||||
}
|
||||
else if (partition1 > 0 && partition2 > 0 && partition1 != partition2) { // is a weeklink (factor between two child cliques)
|
||||
weeklinks.push_back(fg_[factor->index]);
|
||||
}
|
||||
else if (partition1 > 0 && partition2 > 0 && partition1 == partition2) { // is a local factor in one of the child cliques
|
||||
frontalFactors[partition1 - 1].push_back(factor);
|
||||
}
|
||||
else { // is a joint factor in the child clique (involving varaibles in the current clique)
|
||||
if (partition1 > 0 && partition2 <= 0) {
|
||||
frontalFactors[partition1 - 1].push_back(factor);
|
||||
childSeps[partition1 - 1].insert(factor->key2.index);
|
||||
} else if (partition1 <= 0 && partition2 > 0) {
|
||||
frontalFactors[partition2 - 1].push_back(factor);
|
||||
childSeps[partition2 - 1].insert(factor->key1.index);
|
||||
} else
|
||||
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})
|
||||
* 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
|
||||
* the correspoding ordering in {childSeps}.
|
||||
*/
|
||||
// TODO: frontalFactors and localFrontals should be generated in findSeparator
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
void NestedDissection<NLG, SubNLG, GenericGraph>::partitionFactorsAndVariables(
|
||||
const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& keys, //input
|
||||
const std::vector<int>& partitionTable, const int numSubmaps, // input
|
||||
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
|
||||
typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques
|
||||
/* ************************************************************************* */
|
||||
/**
|
||||
* 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 ({localFrontals}). Those separator variables involved in {frontalFactors} are put into
|
||||
* the correspoding ordering in {childSeps}.
|
||||
*/
|
||||
// TODO: frontalFactors and localFrontals should be generated in findSeparator
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
void NestedDissection<NLG, SubNLG, GenericGraph>::partitionFactorsAndVariables(
|
||||
const GenericGraph& fg, const GenericUnaryGraph& unaryFactors, const std::vector<size_t>& keys, //input
|
||||
const std::vector<int>& partitionTable, const int numSubmaps, // input
|
||||
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
|
||||
typename SubNLG::Weeklinks& weeklinks) const { // the links between child cliques
|
||||
|
||||
// make three lists of variables A, B, and C
|
||||
int partition;
|
||||
childFrontals.resize(numSubmaps);
|
||||
BOOST_FOREACH(const size_t key, keys){
|
||||
partition = partitionTable[key];
|
||||
switch (partition) {
|
||||
case -1: break; // the separator of the separator variables
|
||||
case 0: localFrontals.push_back(key); break; // the separator variables
|
||||
default: childFrontals[partition-1].push_back(key); // the frontal variables
|
||||
}
|
||||
}
|
||||
// make three lists of variables A, B, and C
|
||||
int partition;
|
||||
childFrontals.resize(numSubmaps);
|
||||
BOOST_FOREACH(const size_t key, keys){
|
||||
partition = partitionTable[key];
|
||||
switch (partition) {
|
||||
case -1: break; // the separator of the separator variables
|
||||
case 0: localFrontals.push_back(key); break; // the separator variables
|
||||
default: childFrontals[partition-1].push_back(key); // the frontal variables
|
||||
}
|
||||
}
|
||||
|
||||
// group the factors to {frontalFactors} and {sepFactors},and find the joint variables
|
||||
vector<set<size_t> > childSeps_;
|
||||
childSeps_.resize(numSubmaps);
|
||||
childSeps.reserve(numSubmaps);
|
||||
frontalFactors.resize(numSubmaps);
|
||||
frontalUnaryFactors.resize(numSubmaps);
|
||||
BOOST_FOREACH(typename GenericGraph::value_type factor, fg)
|
||||
processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks);
|
||||
BOOST_FOREACH(const set<size_t>& childSep, childSeps_)
|
||||
childSeps.push_back(vector<size_t>(childSep.begin(), childSep.end()));
|
||||
// group the factors to {frontalFactors} and {sepFactors},and find the joint variables
|
||||
vector<set<size_t> > childSeps_;
|
||||
childSeps_.resize(numSubmaps);
|
||||
childSeps.reserve(numSubmaps);
|
||||
frontalFactors.resize(numSubmaps);
|
||||
frontalUnaryFactors.resize(numSubmaps);
|
||||
BOOST_FOREACH(typename GenericGraph::value_type factor, fg)
|
||||
processFactor(factor, partitionTable, frontalFactors, sepFactors, childSeps_, weeklinks);
|
||||
BOOST_FOREACH(const set<size_t>& childSep, childSeps_)
|
||||
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
|
||||
BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) {
|
||||
partition = partitionTable[unaryFactor_->key.index];
|
||||
if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]);
|
||||
else frontalUnaryFactors[partition-1].push_back(unaryFactor_);
|
||||
}
|
||||
}
|
||||
// add unary factor to the current cluster or pass it to one of the child clusters
|
||||
BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors) {
|
||||
partition = partitionTable[unaryFactor_->key.index];
|
||||
if (!partition) sepFactors.push_back(fg_[unaryFactor_->index]);
|
||||
else frontalUnaryFactors[partition-1].push_back(unaryFactor_);
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
NLG NestedDissection<NLG, SubNLG, GenericGraph>::collectOriginalFactors(
|
||||
const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const {
|
||||
NLG sepFactors;
|
||||
typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end();
|
||||
while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]);
|
||||
BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors)
|
||||
sepFactors.push_back(fg_[unaryFactor_->index]);
|
||||
return sepFactors;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
NLG NestedDissection<NLG, SubNLG, GenericGraph>::collectOriginalFactors(
|
||||
const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const {
|
||||
NLG sepFactors;
|
||||
typename GenericGraph::const_iterator it = gfg.begin(), itLast = gfg.end();
|
||||
while(it!=itLast) sepFactors.push_back(fg_[(*it++)->index]);
|
||||
BOOST_FOREACH(const sharedGenericUnaryFactor& unaryFactor_, unaryFactors)
|
||||
sepFactors.push_back(fg_[unaryFactor_->index]);
|
||||
return sepFactors;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
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 int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const {
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
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 int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const {
|
||||
|
||||
// if no split needed
|
||||
NLG sepFactors; // factors that should remain in the current cluster
|
||||
if (frontals.size() <= numNodeStopPartition || gfg.size() <= numNodeStopPartition) {
|
||||
sepFactors = collectOriginalFactors(gfg, unaryFactors);
|
||||
return makeSubNLG(sepFactors, frontals, sep, parent);
|
||||
}
|
||||
// if no split needed
|
||||
NLG sepFactors; // factors that should remain in the current cluster
|
||||
if (frontals.size() <= numNodeStopPartition || gfg.size() <= numNodeStopPartition) {
|
||||
sepFactors = collectOriginalFactors(gfg, unaryFactors);
|
||||
return makeSubNLG(sepFactors, frontals, sep, parent);
|
||||
}
|
||||
|
||||
// find the nested dissection separator
|
||||
int numSubmaps = findSeparator(gfg, frontals, minNodesPerMap, workspace, verbose, int2symbol_, NLG::reduceGraph(),
|
||||
NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
|
||||
partition::PartitionTable& partitionTable = workspace.partitionTable;
|
||||
if (numSubmaps == 0) throw runtime_error("recursivePartition: get zero submap after ND!");
|
||||
// find the nested dissection separator
|
||||
int numSubmaps = findSeparator(gfg, frontals, minNodesPerMap, workspace, verbose, int2symbol_, NLG::reduceGraph(),
|
||||
NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
|
||||
partition::PartitionTable& partitionTable = workspace.partitionTable;
|
||||
if (numSubmaps == 0) throw runtime_error("recursivePartition: get zero submap after ND!");
|
||||
|
||||
// split the factors between child cliques and the current clique
|
||||
vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
|
||||
vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
|
||||
partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
|
||||
frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
|
||||
// split the factors between child cliques and the current clique
|
||||
vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
|
||||
vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
|
||||
partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
|
||||
frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
|
||||
|
||||
// make a new cluster
|
||||
boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
|
||||
current->setWeeklinks(weeklinks);
|
||||
// make a new cluster
|
||||
boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
|
||||
current->setWeeklinks(weeklinks);
|
||||
|
||||
// check whether all the submaps are fully constrained
|
||||
for (int i=0; i<numSubmaps; i++) {
|
||||
checkSingularity(frontalFactors[i], childFrontals[i], workspace, NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
|
||||
}
|
||||
// check whether all the submaps are fully constrained
|
||||
for (int i=0; i<numSubmaps; i++) {
|
||||
checkSingularity(frontalFactors[i], childFrontals[i], workspace, NLG::minNrConstraintsPerCamera(),NLG::minNrConstraintsPerLandmark());
|
||||
}
|
||||
|
||||
// create child clusters
|
||||
for (int i=0; i<numSubmaps; i++) {
|
||||
boost::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i],
|
||||
numNodeStopPartition, minNodesPerMap, current, workspace, verbose);
|
||||
current->addChild(child);
|
||||
}
|
||||
// create child clusters
|
||||
for (int i=0; i<numSubmaps; i++) {
|
||||
boost::shared_ptr<SubNLG> child = recursivePartition(frontalFactors[i], frontalUnaryFactors[i], childFrontals[i], childSeps[i],
|
||||
numNodeStopPartition, minNodesPerMap, current, workspace, verbose);
|
||||
current->addChild(child);
|
||||
}
|
||||
|
||||
return current;
|
||||
}
|
||||
return current;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
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 boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const {
|
||||
/* ************************************************************************* */
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
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 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
|
||||
NLG sepFactors; // factors that should remain in the current cluster
|
||||
if (!cuts.get()) {
|
||||
sepFactors = collectOriginalFactors(gfg, unaryFactors);
|
||||
return makeSubNLG(sepFactors, frontals, sep, parent);
|
||||
}
|
||||
// if there is no need to cut any more
|
||||
NLG sepFactors; // factors that should remain in the current cluster
|
||||
if (!cuts.get()) {
|
||||
sepFactors = collectOriginalFactors(gfg, unaryFactors);
|
||||
return makeSubNLG(sepFactors, frontals, sep, parent);
|
||||
}
|
||||
|
||||
// retrieve the current partitioning info
|
||||
int numSubmaps = 2;
|
||||
partition::PartitionTable& partitionTable = cuts->partitionTable;
|
||||
// retrieve the current partitioning info
|
||||
int numSubmaps = 2;
|
||||
partition::PartitionTable& partitionTable = cuts->partitionTable;
|
||||
|
||||
// split the factors between child cliques and the current clique
|
||||
vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
|
||||
vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
|
||||
partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
|
||||
frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
|
||||
// split the factors between child cliques and the current clique
|
||||
vector<GenericGraph> frontalFactors; vector<GenericUnaryGraph> frontalUnaryFactors; typename SubNLG::Weeklinks weeklinks;
|
||||
vector<size_t> localFrontals; vector<vector<size_t> > childFrontals, childSeps;
|
||||
partitionFactorsAndVariables(gfg, unaryFactors, frontals, partitionTable, numSubmaps,
|
||||
frontalFactors, frontalUnaryFactors, sepFactors, childFrontals, childSeps, localFrontals, weeklinks);
|
||||
|
||||
// make a new cluster
|
||||
boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
|
||||
current->setWeeklinks(weeklinks);
|
||||
// make a new cluster
|
||||
boost::shared_ptr<SubNLG> current = makeSubNLG(sepFactors, localFrontals, sep, parent);
|
||||
current->setWeeklinks(weeklinks);
|
||||
|
||||
// create child clusters
|
||||
for (int i=0; i<2; 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);
|
||||
current->addChild(child);
|
||||
}
|
||||
return current;
|
||||
}
|
||||
// create child clusters
|
||||
for (int i=0; i<2; 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);
|
||||
current->addChild(child);
|
||||
}
|
||||
return current;
|
||||
}
|
||||
}} //namespace
|
||||
|
|
|
|||
|
|
@ -14,56 +14,56 @@
|
|||
|
||||
namespace gtsam { namespace partition {
|
||||
|
||||
/**
|
||||
* Apply nested dissection algorithm to nonlinear factor graphs
|
||||
*/
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
class NestedDissection {
|
||||
public:
|
||||
typedef boost::shared_ptr<SubNLG> sharedSubNLG;
|
||||
/**
|
||||
* Apply nested dissection algorithm to nonlinear factor graphs
|
||||
*/
|
||||
template <class NLG, class SubNLG, class GenericGraph>
|
||||
class NestedDissection {
|
||||
public:
|
||||
typedef boost::shared_ptr<SubNLG> sharedSubNLG;
|
||||
|
||||
private:
|
||||
NLG fg_; // the original nonlinear factor graph
|
||||
Ordering ordering_; // the variable ordering in the nonlinear factor graph
|
||||
std::vector<Symbol> int2symbol_; // the mapping from integer key to symbol
|
||||
sharedSubNLG root_; // the root of generated cluster tree
|
||||
private:
|
||||
NLG fg_; // the original nonlinear factor graph
|
||||
Ordering ordering_; // the variable ordering in the nonlinear factor graph
|
||||
std::vector<Symbol> int2symbol_; // the mapping from integer key to symbol
|
||||
sharedSubNLG root_; // the root of generated cluster tree
|
||||
|
||||
public:
|
||||
sharedSubNLG root() const { return root_; }
|
||||
public:
|
||||
sharedSubNLG root() const { return root_; }
|
||||
|
||||
public:
|
||||
/* constructor with post-determined partitoning*/
|
||||
NestedDissection(const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose = false);
|
||||
public:
|
||||
/* constructor with post-determined partitoning*/
|
||||
NestedDissection(const NLG& fg, const Ordering& ordering, const int numNodeStopPartition, const int minNodesPerMap, const bool verbose = false);
|
||||
|
||||
/* constructor with pre-determined cuts*/
|
||||
NestedDissection(const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& cuts, const bool verbose = false);
|
||||
/* constructor with pre-determined cuts*/
|
||||
NestedDissection(const NLG& fg, const Ordering& ordering, const boost::shared_ptr<Cuts>& cuts, const bool verbose = false);
|
||||
|
||||
private:
|
||||
/* 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;
|
||||
private:
|
||||
/* 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;
|
||||
|
||||
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
|
||||
typename SubNLG::Weeklinks& weeklinks) const;
|
||||
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
|
||||
typename SubNLG::Weeklinks& weeklinks) const;
|
||||
|
||||
/* recursively partition the generic graph */
|
||||
void partitionFactorsAndVariables(
|
||||
const GenericGraph& fg, const GenericUnaryGraph& unaryFactors,
|
||||
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<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;
|
||||
/* recursively partition the generic graph */
|
||||
void partitionFactorsAndVariables(
|
||||
const GenericGraph& fg, const GenericUnaryGraph& unaryFactors,
|
||||
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<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;
|
||||
|
||||
NLG collectOriginalFactors(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const;
|
||||
NLG collectOriginalFactors(const GenericGraph& gfg, const GenericUnaryGraph& unaryFactors) const;
|
||||
|
||||
/* 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,
|
||||
const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const;
|
||||
/* 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,
|
||||
const int numNodeStopPartition, const int minNodesPerMap, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const;
|
||||
|
||||
/* 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,
|
||||
const boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const;
|
||||
/* 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,
|
||||
const boost::shared_ptr<Cuts>& cuts, const boost::shared_ptr<SubNLG>& parent, WorkSpace& workspace, const bool verbose) const;
|
||||
|
||||
};
|
||||
};
|
||||
|
||||
}} //namespace
|
||||
|
|
|
|||
|
|
@ -13,32 +13,32 @@
|
|||
|
||||
namespace gtsam { namespace partition {
|
||||
|
||||
typedef std::vector<int> PartitionTable;
|
||||
typedef std::vector<int> PartitionTable;
|
||||
|
||||
// the work space, preallocated memory
|
||||
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
|
||||
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
|
||||
// the work space, preallocated memory
|
||||
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
|
||||
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
|
||||
|
||||
// constructor
|
||||
WorkSpace(const size_t numNodes) : dictionary(numNodes,0),
|
||||
dsf(new std::vector<size_t>(numNodes, 0)), partitionTable(numNodes, -1) { }
|
||||
// constructor
|
||||
WorkSpace(const size_t numNodes) : dictionary(numNodes,0),
|
||||
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
|
||||
inline void prepareDictionary(const std::vector<size_t>& keys) {
|
||||
int index = 0;
|
||||
std::fill(dictionary.begin(), dictionary.end(), -1);
|
||||
std::vector<size_t>::const_iterator it=keys.begin(), itLast=keys.end();
|
||||
while(it!=itLast) dictionary[*(it++)] = index++;
|
||||
}
|
||||
};
|
||||
// set up dictionary: -1: no such key, none-zero: the corresponding 0-based index
|
||||
inline void prepareDictionary(const std::vector<size_t>& keys) {
|
||||
int index = 0;
|
||||
std::fill(dictionary.begin(), dictionary.end(), -1);
|
||||
std::vector<size_t>::const_iterator it=keys.begin(), itLast=keys.end();
|
||||
while(it!=itLast) dictionary[*(it++)] = index++;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
// manually defined cuts
|
||||
struct Cuts {
|
||||
PartitionTable partitionTable;
|
||||
std::vector<boost::shared_ptr<Cuts> > children;
|
||||
};
|
||||
// manually defined cuts
|
||||
struct Cuts {
|
||||
PartitionTable partitionTable;
|
||||
std::vector<boost::shared_ptr<Cuts> > children;
|
||||
};
|
||||
|
||||
}} // namespace
|
||||
|
|
|
|||
|
|
@ -29,29 +29,29 @@ using namespace gtsam::partition;
|
|||
*/
|
||||
TEST ( GenerciGraph, findIslands )
|
||||
{
|
||||
GenericGraph2D graph;
|
||||
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>(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>(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>(5, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 9, NODE_LANDMARK_2D));
|
||||
GenericGraph2D graph;
|
||||
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>(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>(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>(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>(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>(4, NODE_POSE_2D, 5, 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;
|
||||
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>(4, NODE_POSE_2D, 5, 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;
|
||||
|
||||
WorkSpace workspace(10); // from 0 to 9
|
||||
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
|
||||
LONGS_EQUAL(2, islands.size());
|
||||
vector<size_t> island1; island1 += 1, 2, 3, 7, 8;
|
||||
vector<size_t> island2; island2 += 4, 5, 6, 9;
|
||||
CHECK(island1 == islands.front());
|
||||
CHECK(island2 == islands.back());
|
||||
WorkSpace workspace(10); // from 0 to 9
|
||||
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
|
||||
LONGS_EQUAL(2, islands.size());
|
||||
vector<size_t> island1; island1 += 1, 2, 3, 7, 8;
|
||||
vector<size_t> island2; island2 += 4, 5, 6, 9;
|
||||
CHECK(island1 == islands.front());
|
||||
CHECK(island2 == islands.back());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -62,27 +62,27 @@ TEST ( GenerciGraph, findIslands )
|
|||
*/
|
||||
TEST( GenerciGraph, findIslands2 )
|
||||
{
|
||||
GenericGraph2D graph;
|
||||
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>(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>(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>(5, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(6, NODE_POSE_2D, 8, NODE_LANDMARK_2D));
|
||||
GenericGraph2D graph;
|
||||
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>(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>(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>(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>(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>(4, NODE_POSE_2D, 5, 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;
|
||||
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>(4, NODE_POSE_2D, 5, 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;
|
||||
|
||||
WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here
|
||||
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
|
||||
LONGS_EQUAL(1, islands.size());
|
||||
vector<size_t> island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8;
|
||||
CHECK(island1 == islands.front());
|
||||
WorkSpace workspace(15); // from 0 to 8, but testing over-allocation here
|
||||
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
|
||||
LONGS_EQUAL(1, islands.size());
|
||||
vector<size_t> island1; island1 += 1, 2, 3, 4, 5, 6, 7, 8;
|
||||
CHECK(island1 == islands.front());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -92,21 +92,21 @@ TEST( GenerciGraph, findIslands2 )
|
|||
*/
|
||||
TEST ( GenerciGraph, findIslands3 )
|
||||
{
|
||||
GenericGraph2D graph;
|
||||
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));
|
||||
GenericGraph2D graph;
|
||||
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>(2, NODE_POSE_2D, 3, 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;
|
||||
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));
|
||||
std::vector<size_t> keys; keys += 1, 2, 3, 4, 5, 6;
|
||||
|
||||
WorkSpace workspace(7); // from 0 to 9
|
||||
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
|
||||
LONGS_EQUAL(2, islands.size());
|
||||
vector<size_t> island1; island1 += 1, 5;
|
||||
vector<size_t> island2; island2 += 2, 3, 4, 6;
|
||||
CHECK(island1 == islands.front());
|
||||
CHECK(island2 == islands.back());
|
||||
WorkSpace workspace(7); // from 0 to 9
|
||||
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
|
||||
LONGS_EQUAL(2, islands.size());
|
||||
vector<size_t> island1; island1 += 1, 5;
|
||||
vector<size_t> island2; island2 += 2, 3, 4, 6;
|
||||
CHECK(island1 == islands.front());
|
||||
CHECK(island2 == islands.back());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -115,18 +115,18 @@ TEST ( GenerciGraph, findIslands3 )
|
|||
*/
|
||||
TEST ( GenerciGraph, findIslands4 )
|
||||
{
|
||||
GenericGraph2D graph;
|
||||
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));
|
||||
std::vector<size_t> keys; keys += 3, 4, 7;
|
||||
GenericGraph2D graph;
|
||||
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));
|
||||
std::vector<size_t> keys; keys += 3, 4, 7;
|
||||
|
||||
WorkSpace workspace(8); // from 0 to 7
|
||||
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
|
||||
LONGS_EQUAL(2, islands.size());
|
||||
vector<size_t> island1; island1 += 3, 4;
|
||||
vector<size_t> island2; island2 += 7;
|
||||
CHECK(island1 == islands.front());
|
||||
CHECK(island2 == islands.back());
|
||||
WorkSpace workspace(8); // from 0 to 7
|
||||
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
|
||||
LONGS_EQUAL(2, islands.size());
|
||||
vector<size_t> island1; island1 += 3, 4;
|
||||
vector<size_t> island2; island2 += 7;
|
||||
CHECK(island1 == islands.front());
|
||||
CHECK(island2 == islands.back());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -137,24 +137,24 @@ TEST ( GenerciGraph, findIslands4 )
|
|||
*/
|
||||
TEST ( GenerciGraph, findIslands5 )
|
||||
{
|
||||
GenericGraph2D graph;
|
||||
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>(3, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
|
||||
graph.push_back(boost::make_shared<GenericFactor2D>(4, NODE_POSE_2D, 5, NODE_LANDMARK_2D));
|
||||
GenericGraph2D graph;
|
||||
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>(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>(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>(1, NODE_POSE_2D, 3, 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
|
||||
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
|
||||
LONGS_EQUAL(2, islands.size());
|
||||
vector<size_t> island1; island1 += 1, 3, 5;
|
||||
vector<size_t> island2; island2 += 2, 4;
|
||||
CHECK(island1 == islands.front());
|
||||
CHECK(island2 == islands.back());
|
||||
WorkSpace workspace(6); // from 0 to 5
|
||||
list<vector<size_t> > islands = findIslands(graph, keys, workspace, 7, 2);
|
||||
LONGS_EQUAL(2, islands.size());
|
||||
vector<size_t> island1; island1 += 1, 3, 5;
|
||||
vector<size_t> island2; island2 += 2, 4;
|
||||
CHECK(island1 == islands.front());
|
||||
CHECK(island2 == islands.back());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -165,31 +165,31 @@ TEST ( GenerciGraph, findIslands5 )
|
|||
*/
|
||||
TEST ( GenerciGraph, reduceGenericGraph )
|
||||
{
|
||||
GenericGraph3D graph;
|
||||
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, 5));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(2, 5));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(2, 6));
|
||||
GenericGraph3D graph;
|
||||
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, 5));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(2, 5));
|
||||
graph.push_back(boost::make_shared<GenericFactor3D>(2, 6));
|
||||
|
||||
std::vector<size_t> cameraKeys, landmarkKeys;
|
||||
cameraKeys.push_back(1);
|
||||
cameraKeys.push_back(2);
|
||||
landmarkKeys.push_back(3);
|
||||
landmarkKeys.push_back(4);
|
||||
landmarkKeys.push_back(5);
|
||||
landmarkKeys.push_back(6);
|
||||
std::vector<size_t> cameraKeys, landmarkKeys;
|
||||
cameraKeys.push_back(1);
|
||||
cameraKeys.push_back(2);
|
||||
landmarkKeys.push_back(3);
|
||||
landmarkKeys.push_back(4);
|
||||
landmarkKeys.push_back(5);
|
||||
landmarkKeys.push_back(6);
|
||||
|
||||
std::vector<int> dictionary;
|
||||
dictionary.resize(7, -1); // from 0 to 6
|
||||
dictionary[1] = 0;
|
||||
dictionary[2] = 1;
|
||||
std::vector<int> dictionary;
|
||||
dictionary.resize(7, -1); // from 0 to 6
|
||||
dictionary[1] = 0;
|
||||
dictionary[2] = 1;
|
||||
|
||||
GenericGraph3D reduced;
|
||||
std::map<size_t, vector<size_t> > cameraToLandmarks;
|
||||
reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
|
||||
LONGS_EQUAL(1, reduced.size());
|
||||
LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
|
||||
GenericGraph3D reduced;
|
||||
std::map<size_t, vector<size_t> > cameraToLandmarks;
|
||||
reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
|
||||
LONGS_EQUAL(1, reduced.size());
|
||||
LONGS_EQUAL(1, reduced[0]->key1.index); LONGS_EQUAL(2, reduced[0]->key2.index);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -200,53 +200,53 @@ TEST ( GenerciGraph, reduceGenericGraph )
|
|||
*/
|
||||
TEST ( GenericGraph, reduceGenericGraph2 )
|
||||
{
|
||||
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, 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>(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, 7, 5, NODE_POSE_3D, NODE_POSE_3D));
|
||||
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, 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>(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, 7, 5, NODE_POSE_3D, NODE_POSE_3D));
|
||||
|
||||
std::vector<size_t> cameraKeys, landmarkKeys;
|
||||
cameraKeys.push_back(1);
|
||||
cameraKeys.push_back(2);
|
||||
cameraKeys.push_back(7);
|
||||
landmarkKeys.push_back(3);
|
||||
landmarkKeys.push_back(4);
|
||||
landmarkKeys.push_back(5);
|
||||
landmarkKeys.push_back(6);
|
||||
std::vector<size_t> cameraKeys, landmarkKeys;
|
||||
cameraKeys.push_back(1);
|
||||
cameraKeys.push_back(2);
|
||||
cameraKeys.push_back(7);
|
||||
landmarkKeys.push_back(3);
|
||||
landmarkKeys.push_back(4);
|
||||
landmarkKeys.push_back(5);
|
||||
landmarkKeys.push_back(6);
|
||||
|
||||
std::vector<int> dictionary;
|
||||
dictionary.resize(8, -1); // from 0 to 7
|
||||
dictionary[1] = 0;
|
||||
dictionary[2] = 1;
|
||||
dictionary[7] = 6;
|
||||
std::vector<int> dictionary;
|
||||
dictionary.resize(8, -1); // from 0 to 7
|
||||
dictionary[1] = 0;
|
||||
dictionary[2] = 1;
|
||||
dictionary[7] = 6;
|
||||
|
||||
GenericGraph3D reduced;
|
||||
std::map<size_t, vector<size_t> > cameraToLandmarks;
|
||||
reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
|
||||
LONGS_EQUAL(2, reduced.size());
|
||||
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);
|
||||
GenericGraph3D reduced;
|
||||
std::map<size_t, vector<size_t> > cameraToLandmarks;
|
||||
reduceGenericGraph(graph, cameraKeys, landmarkKeys, dictionary, reduced);
|
||||
LONGS_EQUAL(2, reduced.size());
|
||||
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);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST ( GenerciGraph, hasCommonCamera )
|
||||
{
|
||||
std::set<size_t> cameras1; cameras1 += 1, 2, 3, 4, 5;
|
||||
std::set<size_t> cameras2; cameras2 += 8, 7, 6, 5;
|
||||
bool actual = hasCommonCamera(cameras1, cameras2);
|
||||
CHECK(actual);
|
||||
std::set<size_t> cameras1; cameras1 += 1, 2, 3, 4, 5;
|
||||
std::set<size_t> cameras2; cameras2 += 8, 7, 6, 5;
|
||||
bool actual = hasCommonCamera(cameras1, cameras2);
|
||||
CHECK(actual);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST ( GenerciGraph, hasCommonCamera2 )
|
||||
{
|
||||
std::set<size_t> cameras1; cameras1 += 1, 3, 5, 7;
|
||||
std::set<size_t> cameras2; cameras2 += 2, 4, 6, 8, 10;
|
||||
bool actual = hasCommonCamera(cameras1, cameras2);
|
||||
CHECK(!actual);
|
||||
std::set<size_t> cameras1; cameras1 += 1, 3, 5, 7;
|
||||
std::set<size_t> cameras2; cameras2 += 2, 4, 6, 8, 10;
|
||||
bool actual = hasCommonCamera(cameras1, cameras2);
|
||||
CHECK(!actual);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -32,22 +32,22 @@ using namespace gtsam::partition;
|
|||
// l1
|
||||
TEST ( NestedDissection, oneIsland )
|
||||
{
|
||||
using namespace submapPlanarSLAM;
|
||||
typedef TSAM2D::SubNLG SubNLG;
|
||||
Graph fg;
|
||||
fg.addOdometry(1, 2, Pose2(), odoNoise);
|
||||
fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise);
|
||||
fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise);
|
||||
fg.addPoseConstraint(1, Pose2());
|
||||
using namespace submapPlanarSLAM;
|
||||
typedef TSAM2D::SubNLG SubNLG;
|
||||
Graph fg;
|
||||
fg.addOdometry(1, 2, Pose2(), odoNoise);
|
||||
fg.addBearingRange(1, 1, Rot2(), 0., bearingRangeNoise);
|
||||
fg.addBearingRange(2, 1, Rot2(), 0., bearingRangeNoise);
|
||||
fg.addPoseConstraint(1, Pose2());
|
||||
|
||||
Ordering ordering; ordering += x1, x2, l1;
|
||||
Ordering ordering; ordering += x1, x2, l1;
|
||||
|
||||
int numNodeStopPartition = 1e3;
|
||||
int minNodesPerMap = 1e3;
|
||||
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
|
||||
LONGS_EQUAL(4, nd.root()->size());
|
||||
LONGS_EQUAL(3, nd.root()->frontal().size());
|
||||
LONGS_EQUAL(0, nd.root()->children().size());
|
||||
int numNodeStopPartition = 1e3;
|
||||
int minNodesPerMap = 1e3;
|
||||
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
|
||||
LONGS_EQUAL(4, nd.root()->size());
|
||||
LONGS_EQUAL(3, nd.root()->frontal().size());
|
||||
LONGS_EQUAL(0, nd.root()->children().size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -56,35 +56,35 @@ TEST ( NestedDissection, oneIsland )
|
|||
// x2/ \x5
|
||||
TEST ( NestedDissection, TwoIslands )
|
||||
{
|
||||
using namespace submapPlanarSLAM;
|
||||
typedef TSAM2D::SubNLG SubNLG;
|
||||
Graph fg;
|
||||
fg.addOdometry(1, 2, Pose2(), odoNoise);
|
||||
fg.addOdometry(1, 3, Pose2(), odoNoise);
|
||||
fg.addOdometry(2, 3, Pose2(), odoNoise);
|
||||
fg.addOdometry(3, 4, Pose2(), odoNoise);
|
||||
fg.addOdometry(4, 5, Pose2(), odoNoise);
|
||||
fg.addOdometry(3, 5, Pose2(), odoNoise);
|
||||
fg.addPoseConstraint(1, Pose2());
|
||||
fg.addPoseConstraint(4, Pose2());
|
||||
Ordering ordering; ordering += x1, x2, x3, x4, x5;
|
||||
using namespace submapPlanarSLAM;
|
||||
typedef TSAM2D::SubNLG SubNLG;
|
||||
Graph fg;
|
||||
fg.addOdometry(1, 2, Pose2(), odoNoise);
|
||||
fg.addOdometry(1, 3, Pose2(), odoNoise);
|
||||
fg.addOdometry(2, 3, Pose2(), odoNoise);
|
||||
fg.addOdometry(3, 4, Pose2(), odoNoise);
|
||||
fg.addOdometry(4, 5, Pose2(), odoNoise);
|
||||
fg.addOdometry(3, 5, Pose2(), odoNoise);
|
||||
fg.addPoseConstraint(1, Pose2());
|
||||
fg.addPoseConstraint(4, Pose2());
|
||||
Ordering ordering; ordering += x1, x2, x3, x4, x5;
|
||||
|
||||
int numNodeStopPartition = 2;
|
||||
int minNodesPerMap = 1;
|
||||
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
|
||||
// root submap
|
||||
LONGS_EQUAL(0, nd.root()->size());
|
||||
LONGS_EQUAL(1, nd.root()->frontal().size());
|
||||
LONGS_EQUAL(0, nd.root()->separator().size());
|
||||
LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
|
||||
int numNodeStopPartition = 2;
|
||||
int minNodesPerMap = 1;
|
||||
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
|
||||
// root submap
|
||||
LONGS_EQUAL(0, nd.root()->size());
|
||||
LONGS_EQUAL(1, nd.root()->frontal().size());
|
||||
LONGS_EQUAL(0, nd.root()->separator().size());
|
||||
LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
|
||||
|
||||
// the 1st submap
|
||||
LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size());
|
||||
LONGS_EQUAL(4, nd.root()->children()[0]->size());
|
||||
// the 1st submap
|
||||
LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size());
|
||||
LONGS_EQUAL(4, nd.root()->children()[0]->size());
|
||||
|
||||
// the 2nd submap
|
||||
LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size());
|
||||
LONGS_EQUAL(4, nd.root()->children()[1]->size());
|
||||
// the 2nd submap
|
||||
LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size());
|
||||
LONGS_EQUAL(4, nd.root()->children()[1]->size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -93,40 +93,40 @@ TEST ( NestedDissection, TwoIslands )
|
|||
// x2/ \x5
|
||||
TEST ( NestedDissection, FourIslands )
|
||||
{
|
||||
using namespace submapPlanarSLAM;
|
||||
typedef TSAM2D::SubNLG SubNLG;
|
||||
Graph fg;
|
||||
fg.addOdometry(1, 3, Pose2(), odoNoise);
|
||||
fg.addOdometry(2, 3, Pose2(), odoNoise);
|
||||
fg.addOdometry(3, 4, Pose2(), odoNoise);
|
||||
fg.addOdometry(3, 5, Pose2(), odoNoise);
|
||||
fg.addPoseConstraint(1, Pose2());
|
||||
fg.addPoseConstraint(4, Pose2());
|
||||
Ordering ordering; ordering += x1, x2, x3, x4, x5;
|
||||
using namespace submapPlanarSLAM;
|
||||
typedef TSAM2D::SubNLG SubNLG;
|
||||
Graph fg;
|
||||
fg.addOdometry(1, 3, Pose2(), odoNoise);
|
||||
fg.addOdometry(2, 3, Pose2(), odoNoise);
|
||||
fg.addOdometry(3, 4, Pose2(), odoNoise);
|
||||
fg.addOdometry(3, 5, Pose2(), odoNoise);
|
||||
fg.addPoseConstraint(1, Pose2());
|
||||
fg.addPoseConstraint(4, Pose2());
|
||||
Ordering ordering; ordering += x1, x2, x3, x4, x5;
|
||||
|
||||
int numNodeStopPartition = 2;
|
||||
int minNodesPerMap = 1;
|
||||
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
|
||||
LONGS_EQUAL(0, nd.root()->size());
|
||||
LONGS_EQUAL(1, nd.root()->frontal().size());
|
||||
LONGS_EQUAL(0, nd.root()->separator().size());
|
||||
LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps
|
||||
int numNodeStopPartition = 2;
|
||||
int minNodesPerMap = 1;
|
||||
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
|
||||
LONGS_EQUAL(0, nd.root()->size());
|
||||
LONGS_EQUAL(1, nd.root()->frontal().size());
|
||||
LONGS_EQUAL(0, nd.root()->separator().size());
|
||||
LONGS_EQUAL(4, nd.root()->children().size()); // 4 leaf submaps
|
||||
|
||||
// the 1st submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size());
|
||||
LONGS_EQUAL(2, nd.root()->children()[0]->size());
|
||||
// the 1st submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size());
|
||||
LONGS_EQUAL(2, nd.root()->children()[0]->size());
|
||||
|
||||
// the 2nd submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size());
|
||||
LONGS_EQUAL(2, nd.root()->children()[1]->size());
|
||||
// the 2nd submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size());
|
||||
LONGS_EQUAL(2, nd.root()->children()[1]->size());
|
||||
|
||||
// the 3rd submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size());
|
||||
LONGS_EQUAL(1, nd.root()->children()[2]->size());
|
||||
// the 3rd submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size());
|
||||
LONGS_EQUAL(1, nd.root()->children()[2]->size());
|
||||
|
||||
// the 4th submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size());
|
||||
LONGS_EQUAL(1, nd.root()->children()[3]->size());
|
||||
// the 4th submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[3]->frontal().size());
|
||||
LONGS_EQUAL(1, nd.root()->children()[3]->size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -137,41 +137,41 @@ TEST ( NestedDissection, FourIslands )
|
|||
// x5
|
||||
TEST ( NestedDissection, weekLinks )
|
||||
{
|
||||
using namespace submapPlanarSLAM;
|
||||
typedef TSAM2D::SubNLG SubNLG;
|
||||
Graph fg;
|
||||
fg.addOdometry(1, 2, Pose2(), odoNoise);
|
||||
fg.addOdometry(2, 3, Pose2(), odoNoise);
|
||||
fg.addOdometry(2, 4, Pose2(), odoNoise);
|
||||
fg.addOdometry(3, 4, Pose2(), odoNoise);
|
||||
fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise);
|
||||
fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise);
|
||||
fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise);
|
||||
fg.addPoseConstraint(1, Pose2());
|
||||
fg.addPoseConstraint(4, Pose2());
|
||||
fg.addPoseConstraint(5, Pose2());
|
||||
Ordering ordering; ordering += x1, x2, x3, x4, x5, l6;
|
||||
using namespace submapPlanarSLAM;
|
||||
typedef TSAM2D::SubNLG SubNLG;
|
||||
Graph fg;
|
||||
fg.addOdometry(1, 2, Pose2(), odoNoise);
|
||||
fg.addOdometry(2, 3, Pose2(), odoNoise);
|
||||
fg.addOdometry(2, 4, Pose2(), odoNoise);
|
||||
fg.addOdometry(3, 4, Pose2(), odoNoise);
|
||||
fg.addBearingRange(1, 6, Rot2(), 0., bearingRangeNoise);
|
||||
fg.addBearingRange(2, 6, Rot2(), 0., bearingRangeNoise);
|
||||
fg.addBearingRange(5, 6, Rot2(), 0., bearingRangeNoise);
|
||||
fg.addPoseConstraint(1, Pose2());
|
||||
fg.addPoseConstraint(4, Pose2());
|
||||
fg.addPoseConstraint(5, Pose2());
|
||||
Ordering ordering; ordering += x1, x2, x3, x4, x5, l6;
|
||||
|
||||
int numNodeStopPartition = 2;
|
||||
int minNodesPerMap = 1;
|
||||
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
|
||||
LONGS_EQUAL(0, nd.root()->size()); // one weeklink
|
||||
LONGS_EQUAL(1, nd.root()->frontal().size());
|
||||
LONGS_EQUAL(0, nd.root()->separator().size());
|
||||
LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps
|
||||
LONGS_EQUAL(1, nd.root()->weeklinks().size());
|
||||
int numNodeStopPartition = 2;
|
||||
int minNodesPerMap = 1;
|
||||
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, numNodeStopPartition, minNodesPerMap);
|
||||
LONGS_EQUAL(0, nd.root()->size()); // one weeklink
|
||||
LONGS_EQUAL(1, nd.root()->frontal().size());
|
||||
LONGS_EQUAL(0, nd.root()->separator().size());
|
||||
LONGS_EQUAL(3, nd.root()->children().size()); // 4 leaf submaps
|
||||
LONGS_EQUAL(1, nd.root()->weeklinks().size());
|
||||
|
||||
// the 1st submap
|
||||
LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4
|
||||
LONGS_EQUAL(4, nd.root()->children()[0]->size());
|
||||
// the 1st submap
|
||||
LONGS_EQUAL(2, nd.root()->children()[0]->frontal().size()); // x3 and x4
|
||||
LONGS_EQUAL(4, nd.root()->children()[0]->size());
|
||||
|
||||
// the 2nd submap
|
||||
LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6
|
||||
LONGS_EQUAL(4, nd.root()->children()[1]->size());
|
||||
//
|
||||
// the 3rd submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5
|
||||
LONGS_EQUAL(1, nd.root()->children()[2]->size());
|
||||
// the 2nd submap
|
||||
LONGS_EQUAL(2, nd.root()->children()[1]->frontal().size()); // x1 and l6
|
||||
LONGS_EQUAL(4, nd.root()->children()[1]->size());
|
||||
//
|
||||
// the 3rd submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[2]->frontal().size()); // x5
|
||||
LONGS_EQUAL(1, nd.root()->children()[2]->size());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -184,86 +184,86 @@ TEST ( NestedDissection, weekLinks )
|
|||
*/
|
||||
TEST ( NestedDissection, manual_cuts )
|
||||
{
|
||||
using namespace submapPlanarSLAM;
|
||||
typedef partition::Cuts Cuts;
|
||||
typedef TSAM2D::SubNLG SubNLG;
|
||||
typedef partition::PartitionTable PartitionTable;
|
||||
Graph fg;
|
||||
fg.addOdometry(x0, x1, Pose2(1.0, 0, 0), odoNoise);
|
||||
fg.addOdometry(x1, x2, Pose2(1.0, 0, 0), odoNoise);
|
||||
using namespace submapPlanarSLAM;
|
||||
typedef partition::Cuts Cuts;
|
||||
typedef TSAM2D::SubNLG SubNLG;
|
||||
typedef partition::PartitionTable PartitionTable;
|
||||
Graph fg;
|
||||
fg.addOdometry(x0, x1, 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, l4, Rot2::fromAngle(-M_PI_2), 1, 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, l1, 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, 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, l2, Rot2::fromAngle( M_PI_2), 1, 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, l5, Rot2::fromAngle( M_PI_2), 1, bearingRangeNoise);
|
||||
fg.addBearingRange(x1, l6, Rot2::fromAngle(-M_PI_4), 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, 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, l5, Rot2::fromAngle( M_PI_2), 1, 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, l5, Rot2::fromAngle(-M_PI_4 * 3), sqrt(2), 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, 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, l3, 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
|
||||
Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6;
|
||||
// generate ordering
|
||||
Ordering ordering; ordering += x0, x1, x2, l1, l2, l3, l4, l5, l6;
|
||||
|
||||
// define cuts
|
||||
boost::shared_ptr<Cuts> cuts(new Cuts());
|
||||
cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable;
|
||||
//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;
|
||||
// define cuts
|
||||
boost::shared_ptr<Cuts> cuts(new Cuts());
|
||||
cuts->partitionTable = PartitionTable(9, -1); PartitionTable* p = &cuts->partitionTable;
|
||||
//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;
|
||||
|
||||
cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
|
||||
cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable;
|
||||
//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;
|
||||
cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
|
||||
cuts->children[0]->partitionTable = PartitionTable(9, -1); p = &cuts->children[0]->partitionTable;
|
||||
//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;
|
||||
|
||||
cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
|
||||
cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable;
|
||||
//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;
|
||||
cuts->children.push_back(boost::shared_ptr<Cuts>(new Cuts()));
|
||||
cuts->children[1]->partitionTable = PartitionTable(9, -1); p = &cuts->children[1]->partitionTable;
|
||||
//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;
|
||||
|
||||
|
||||
// nested dissection
|
||||
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, cuts);
|
||||
LONGS_EQUAL(2, nd.root()->size());
|
||||
LONGS_EQUAL(3, nd.root()->frontal().size());
|
||||
LONGS_EQUAL(0, nd.root()->separator().size());
|
||||
LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
|
||||
LONGS_EQUAL(0, nd.root()->weeklinks().size());
|
||||
// nested dissection
|
||||
NestedDissection<Graph, SubNLG, GenericGraph2D> nd(fg, ordering, cuts);
|
||||
LONGS_EQUAL(2, nd.root()->size());
|
||||
LONGS_EQUAL(3, nd.root()->frontal().size());
|
||||
LONGS_EQUAL(0, nd.root()->separator().size());
|
||||
LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
|
||||
LONGS_EQUAL(0, nd.root()->weeklinks().size());
|
||||
|
||||
// the 1st submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0
|
||||
LONGS_EQUAL(4, nd.root()->children()[0]->size());
|
||||
LONGS_EQUAL(2, nd.root()->children()[0]->children().size());
|
||||
// the 1st submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[0]->frontal().size()); // x0
|
||||
LONGS_EQUAL(4, nd.root()->children()[0]->size());
|
||||
LONGS_EQUAL(2, nd.root()->children()[0]->children().size());
|
||||
|
||||
// the 1-1st submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1
|
||||
LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size());
|
||||
// the 1-1st submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[0]->children()[0]->frontal().size()); // l1
|
||||
LONGS_EQUAL(2, nd.root()->children()[0]->children()[0]->size());
|
||||
|
||||
// the 1-2nd submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4
|
||||
LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size());
|
||||
// the 1-2nd submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[0]->children()[1]->frontal().size()); // l4
|
||||
LONGS_EQUAL(2, nd.root()->children()[0]->children()[1]->size());
|
||||
|
||||
// the 2nd submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2
|
||||
LONGS_EQUAL(3, nd.root()->children()[1]->size());
|
||||
LONGS_EQUAL(2, nd.root()->children()[1]->children().size());
|
||||
// the 2nd submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[1]->frontal().size()); // x2
|
||||
LONGS_EQUAL(3, nd.root()->children()[1]->size());
|
||||
LONGS_EQUAL(2, nd.root()->children()[1]->children().size());
|
||||
|
||||
// the 2-1st submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3
|
||||
LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size());
|
||||
// the 2-1st submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[1]->children()[0]->frontal().size()); // l3
|
||||
LONGS_EQUAL(2, nd.root()->children()[1]->children()[0]->size());
|
||||
|
||||
// the 2-2nd submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6
|
||||
LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size());
|
||||
// the 2-2nd submap
|
||||
LONGS_EQUAL(1, nd.root()->children()[1]->children()[1]->frontal().size()); // l6
|
||||
LONGS_EQUAL(2, nd.root()->children()[1]->children()[1]->size());
|
||||
|
||||
}
|
||||
|
||||
|
|
@ -272,65 +272,65 @@ TEST ( NestedDissection, manual_cuts )
|
|||
// / | / \ | \
|
||||
// x0 x1 x2 x3
|
||||
TEST( NestedDissection, Graph3D) {
|
||||
using namespace gtsam::submapVisualSLAM;
|
||||
typedef TSAM3D::SubNLG SubNLG;
|
||||
typedef partition::PartitionTable PartitionTable;
|
||||
vector<GeneralCamera> cameras;
|
||||
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( 2., 0., 0.))));
|
||||
using namespace gtsam::submapVisualSLAM;
|
||||
typedef TSAM3D::SubNLG SubNLG;
|
||||
typedef partition::PartitionTable PartitionTable;
|
||||
vector<GeneralCamera> cameras;
|
||||
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( 2., 0., 0.))));
|
||||
|
||||
vector<Point3> points;
|
||||
for (int cube_index = 0; cube_index <= 3; cube_index++) {
|
||||
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));
|
||||
}
|
||||
vector<Point3> points;
|
||||
for (int cube_index = 0; cube_index <= 3; cube_index++) {
|
||||
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));
|
||||
}
|
||||
|
||||
Graph graph;
|
||||
SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.));
|
||||
SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.));
|
||||
for (int j=1; j<=8; j++)
|
||||
graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
|
||||
for (int j=1; j<=16; j++)
|
||||
graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
|
||||
for (int j=9; j<=24; j++)
|
||||
graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
|
||||
for (int j=17; j<=24; j++)
|
||||
graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
|
||||
Graph graph;
|
||||
SharedDiagonal measurementNoise(gtsam::Vector_(2, 1., 1.));
|
||||
SharedDiagonal measurementZeroNoise(gtsam::Vector_(2, 0., 0.));
|
||||
for (int j=1; j<=8; j++)
|
||||
graph.addMeasurement(0, j, cameras[0].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
|
||||
for (int j=1; j<=16; j++)
|
||||
graph.addMeasurement(1, j, cameras[1].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
|
||||
for (int j=9; j<=24; j++)
|
||||
graph.addMeasurement(2, j, cameras[2].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
|
||||
for (int j=17; j<=24; j++)
|
||||
graph.addMeasurement(3, j, cameras[3].project(points[j-1]).expmap(measurementZeroNoise->sample()), measurementNoise);
|
||||
|
||||
// make an easy ordering
|
||||
Ordering ordering; ordering += x0, x1, x2, x3;
|
||||
for (int j=1; j<=24; j++)
|
||||
ordering += Symbol('l', j);
|
||||
// make an easy ordering
|
||||
Ordering ordering; ordering += x0, x1, x2, x3;
|
||||
for (int j=1; j<=24; j++)
|
||||
ordering += Symbol('l', j);
|
||||
|
||||
// nested dissection
|
||||
const int numNodeStopPartition = 10;
|
||||
const int minNodesPerMap = 5;
|
||||
NestedDissection<Graph, SubNLG, GenericGraph3D> nd(graph, ordering, numNodeStopPartition, minNodesPerMap);
|
||||
// nested dissection
|
||||
const int numNodeStopPartition = 10;
|
||||
const int minNodesPerMap = 5;
|
||||
NestedDissection<Graph, SubNLG, GenericGraph3D> nd(graph, ordering, numNodeStopPartition, minNodesPerMap);
|
||||
|
||||
LONGS_EQUAL(0, nd.root()->size());
|
||||
LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16
|
||||
LONGS_EQUAL(0, nd.root()->separator().size());
|
||||
LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
|
||||
LONGS_EQUAL(0, nd.root()->weeklinks().size());
|
||||
LONGS_EQUAL(0, nd.root()->size());
|
||||
LONGS_EQUAL(8, nd.root()->frontal().size()); // l9-l16
|
||||
LONGS_EQUAL(0, nd.root()->separator().size());
|
||||
LONGS_EQUAL(2, nd.root()->children().size()); // 2 leaf submaps
|
||||
LONGS_EQUAL(0, nd.root()->weeklinks().size());
|
||||
|
||||
// the 1st submap
|
||||
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(0, nd.root()->children()[0]->children().size());
|
||||
// the 1st submap
|
||||
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(0, nd.root()->children()[0]->children().size());
|
||||
|
||||
// the 2nd submap
|
||||
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(0, nd.root()->children()[1]->children().size());
|
||||
// the 2nd submap
|
||||
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(0, nd.root()->children()[1]->children().size());
|
||||
}
|
||||
|
||||
|
||||
|
|
|
|||
|
|
@ -295,87 +295,87 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
SharedGaussian get_model_inlier() const {
|
||||
return model_inlier_;
|
||||
return model_inlier_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SharedGaussian get_model_outlier() const {
|
||||
return model_outlier_;
|
||||
return model_outlier_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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 {
|
||||
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){
|
||||
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
|
||||
* (note these are given in the E step, where indicator probabilities are calculated).
|
||||
*
|
||||
* Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
|
||||
* unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
|
||||
*
|
||||
* TODO: improve efficiency (info form)
|
||||
*/
|
||||
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
|
||||
* (note these are given in the E step, where indicator probabilities are calculated).
|
||||
*
|
||||
* Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
|
||||
* unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
|
||||
*
|
||||
* TODO: improve efficiency (info form)
|
||||
*/
|
||||
|
||||
// get joint covariance of the involved states
|
||||
std::vector<gtsam::Key> Keys;
|
||||
Keys.push_back(key1_);
|
||||
Keys.push_back(key2_);
|
||||
Marginals marginals( graph, values, Marginals::QR );
|
||||
JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
|
||||
Matrix cov1 = joint_marginal12(key1_, key1_);
|
||||
Matrix cov2 = joint_marginal12(key2_, key2_);
|
||||
Matrix cov12 = joint_marginal12(key1_, key2_);
|
||||
// get joint covariance of the involved states
|
||||
std::vector<gtsam::Key> Keys;
|
||||
Keys.push_back(key1_);
|
||||
Keys.push_back(key2_);
|
||||
Marginals marginals( graph, values, Marginals::QR );
|
||||
JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
|
||||
Matrix cov1 = joint_marginal12(key1_, key1_);
|
||||
Matrix cov2 = joint_marginal12(key2_, key2_);
|
||||
Matrix cov12 = joint_marginal12(key1_, key2_);
|
||||
|
||||
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){
|
||||
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
|
||||
* (note these are given in the E step, where indicator probabilities are calculated).
|
||||
*
|
||||
* Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
|
||||
* unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
|
||||
*
|
||||
* TODO: improve efficiency (info form)
|
||||
*/
|
||||
/* Update model_inlier_ and model_outlier_ to account for uncertainty in robot trajectories
|
||||
* (note these are given in the E step, where indicator probabilities are calculated).
|
||||
*
|
||||
* Principle: R += [H1 H2] * joint_cov12 * [H1 H2]', where H1, H2 are Jacobians of the
|
||||
* unwhitened error w.r.t. states, and R is the measurement covariance (inlier or outlier modes).
|
||||
*
|
||||
* TODO: improve efficiency (info form)
|
||||
*/
|
||||
|
||||
const T& p1 = values.at<T>(key1_);
|
||||
const T& p2 = values.at<T>(key2_);
|
||||
const T& p1 = values.at<T>(key1_);
|
||||
const T& p2 = values.at<T>(key2_);
|
||||
|
||||
Matrix H1, H2;
|
||||
T hx = p1.between(p2, H1, H2); // h(x)
|
||||
Matrix H1, H2;
|
||||
T hx = p1.between(p2, H1, H2); // h(x)
|
||||
|
||||
Matrix H;
|
||||
H.resize(H1.rows(), H1.rows()+H2.rows());
|
||||
H << H1, H2; // H = [H1 H2]
|
||||
Matrix H;
|
||||
H.resize(H1.rows(), H1.rows()+H2.rows());
|
||||
H << H1, H2; // H = [H1 H2]
|
||||
|
||||
Matrix joint_cov;
|
||||
joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols());
|
||||
joint_cov << cov1, cov12,
|
||||
cov12.transpose(), cov2;
|
||||
Matrix joint_cov;
|
||||
joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols());
|
||||
joint_cov << cov1, cov12,
|
||||
cov12.transpose(), cov2;
|
||||
|
||||
Matrix cov_state = H*joint_cov*H.transpose();
|
||||
Matrix cov_state = H*joint_cov*H.transpose();
|
||||
|
||||
// model_inlier_->print("before:");
|
||||
// model_inlier_->print("before:");
|
||||
|
||||
// update inlier and outlier noise models
|
||||
Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
|
||||
model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
|
||||
// update inlier and outlier noise models
|
||||
Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
|
||||
model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
|
||||
|
||||
Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
|
||||
model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
|
||||
Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
|
||||
model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
|
||||
|
||||
// model_inlier_->print("after:");
|
||||
// std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
|
||||
// model_inlier_->print("after:");
|
||||
// std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
|
||||
|
|
@ -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
Loading…
Reference in New Issue