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"
|
||||
|
|
@ -137,12 +138,15 @@ ELSE() # UNIX and macOS
|
|||
${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()
|
||||
|
||||
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));
|
||||
// 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
|
||||
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
|
||||
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 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()));
|
||||
|
|
|
|||
|
|
@ -34,9 +34,12 @@ 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.
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -77,10 +77,12 @@ void NonlinearOptimizer::defaultOptimize() {
|
|||
params.errorTol, currentError, this->error(), params.verbosity));
|
||||
|
||||
// Printing if verbose
|
||||
if (params.verbosity >= NonlinearOptimizerParams::TERMINATION &&
|
||||
this->iterations() >= params.maxIterations)
|
||||
if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) {
|
||||
cout << "iterations: " << this->iterations() << " >? " << params.maxIterations << endl;
|
||||
if (this->iterations() >= params.maxIterations)
|
||||
cout << "Terminating because reached maximum iterations" << endl;
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
const Values& NonlinearOptimizer::optimizeSafely() {
|
||||
|
|
|
|||
|
|
@ -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]);
|
||||
//
|
||||
// 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;
|
||||
// }
|
||||
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::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<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() << " "
|
||||
<< diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " "
|
||||
<< diagonalModel->precision(1) << " " << 0.0 << " "
|
||||
<< diagonalModel->precision(2) << endl;
|
||||
<< 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;
|
||||
}
|
||||
|
||||
boost::shared_ptr< BetweenFactor<Pose3> > factor3D =
|
||||
boost::dynamic_pointer_cast< BetweenFactor<Pose3> >(factor_);
|
||||
|
||||
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;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
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 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;
|
||||
#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());
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
@ -21,6 +21,8 @@
|
|||
#include <gtsam/base/Testable.h>
|
||||
#include <gtsam/base/Lie.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactor.h>
|
||||
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
|
||||
#include <gtsam/nonlinear/Marginals.h>
|
||||
#include <gtsam/linear/GaussianFactor.h>
|
||||
#include <gtsam/slam/BetweenFactor.h>
|
||||
|
||||
|
|
@ -302,6 +304,101 @@ namespace gtsam {
|
|||
return currA_T_currB_msr.localCoordinates(currA_T_currB_pred);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SharedGaussian get_model_inlier() const {
|
||||
return model_inlier_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
SharedGaussian get_model_outlier() const {
|
||||
return model_outlier_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix get_model_inlier_cov() const {
|
||||
return (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix get_model_outlier_cov() const {
|
||||
return (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
void updateNoiseModels(const gtsam::Values& values, const gtsam::Marginals& marginals) {
|
||||
/* given marginals version, don't need to marginal multiple times if update a lot */
|
||||
|
||||
std::vector<gtsam::Key> Keys;
|
||||
Keys.push_back(keyA_);
|
||||
Keys.push_back(keyB_);
|
||||
JointMarginal joint_marginal12 = marginals.jointMarginalCovariance(Keys);
|
||||
Matrix cov1 = joint_marginal12(keyA_, keyA_);
|
||||
Matrix cov2 = joint_marginal12(keyB_, keyB_);
|
||||
Matrix cov12 = joint_marginal12(keyA_, keyB_);
|
||||
|
||||
updateNoiseModels_givenCovs(values, cov1, cov2, cov12);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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)
|
||||
*/
|
||||
|
||||
// get joint covariance of the involved states
|
||||
|
||||
Marginals marginals(graph, values, Marginals::QR);
|
||||
|
||||
this->updateNoiseModels(values, marginals);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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)
|
||||
*/
|
||||
|
||||
const T& p1 = values.at<T>(keyA_);
|
||||
const T& p2 = values.at<T>(keyB_);
|
||||
|
||||
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 joint_cov;
|
||||
joint_cov.resize(cov1.rows()+cov2.rows(), cov1.cols()+cov2.cols());
|
||||
joint_cov << cov1, cov12,
|
||||
cov12.transpose(), cov2;
|
||||
|
||||
Matrix cov_state = H*joint_cov*H.transpose();
|
||||
|
||||
// model_inlier_->print("before:");
|
||||
|
||||
// update inlier and outlier noise models
|
||||
Matrix covRinlier = (model_inlier_->R().transpose()*model_inlier_->R()).inverse();
|
||||
model_inlier_ = gtsam::noiseModel::Gaussian::Covariance(covRinlier + cov_state);
|
||||
|
||||
Matrix covRoutlier = (model_outlier_->R().transpose()*model_outlier_->R()).inverse();
|
||||
model_outlier_ = gtsam::noiseModel::Gaussian::Covariance(covRoutlier + cov_state);
|
||||
|
||||
// model_inlier_->print("after:");
|
||||
// std::cout<<"covRinlier + cov_state: "<<covRinlier + cov_state<<std::endl;
|
||||
}
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
/** number of variables attached to this factor */
|
||||
|
|
|
|||
|
|
@ -0,0 +1,85 @@
|
|||
/**
|
||||
* @file testBiasedGPSFactor.cpp
|
||||
* @brief
|
||||
* @author Luca Carlone
|
||||
* @date July 30, 2014
|
||||
*/
|
||||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam_unstable/slam/BiasedGPSFactor.h>
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
using namespace gtsam;
|
||||
using namespace gtsam::symbol_shorthand;
|
||||
using namespace gtsam::noiseModel;
|
||||
// Convenience for named keys
|
||||
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::B;
|
||||
|
||||
TEST(BiasedGPSFactor, errorNoiseless) {
|
||||
|
||||
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
|
||||
Point3 t(1.0, 0.5, 0.2);
|
||||
Pose3 pose(R,t);
|
||||
Point3 bias(0.0,0.0,0.0);
|
||||
Point3 noise(0.0,0.0,0.0);
|
||||
Point3 measured = t + noise;
|
||||
|
||||
BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05));
|
||||
Vector expectedError = (Vector(3) << 0.0, 0.0, 0.0 );
|
||||
Vector actualError = factor.evaluateError(pose,bias);
|
||||
EXPECT(assert_equal(expectedError,actualError, 1E-5));
|
||||
}
|
||||
|
||||
TEST(BiasedGPSFactor, errorNoisy) {
|
||||
|
||||
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
|
||||
Point3 t(1.0, 0.5, 0.2);
|
||||
Pose3 pose(R,t);
|
||||
Point3 bias(0.0,0.0,0.0);
|
||||
Point3 noise(1.0,2.0,3.0);
|
||||
Point3 measured = t - noise;
|
||||
|
||||
BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05));
|
||||
Vector expectedError = (Vector(3) << 1.0, 2.0, 3.0 );
|
||||
Vector actualError = factor.evaluateError(pose,bias);
|
||||
EXPECT(assert_equal(expectedError,actualError, 1E-5));
|
||||
}
|
||||
|
||||
TEST(BiasedGPSFactor, jacobian) {
|
||||
|
||||
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
|
||||
Point3 t(1.0, 0.5, 0.2);
|
||||
Pose3 pose(R,t);
|
||||
Point3 bias(0.0,0.0,0.0);
|
||||
|
||||
Point3 noise(0.0,0.0,0.0);
|
||||
Point3 measured = t + noise;
|
||||
|
||||
BiasedGPSFactor factor(X(1), B(1), measured, Isotropic::Sigma(3, 0.05));
|
||||
|
||||
Matrix actualH1, actualH2;
|
||||
factor.evaluateError(pose,bias, actualH1, actualH2);
|
||||
|
||||
Matrix numericalH1 = numericalDerivative21(
|
||||
boost::function<Vector(const Pose3&, const Point3&)>(boost::bind(
|
||||
&BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none,
|
||||
boost::none)), pose, bias, 1e-5);
|
||||
EXPECT(assert_equal(numericalH1,actualH1, 1E-5));
|
||||
|
||||
Matrix numericalH2 = numericalDerivative22(
|
||||
boost::function<Vector(const Pose3&, const Point3&)>(boost::bind(
|
||||
&BiasedGPSFactor::evaluateError, factor, _1, _2, boost::none,
|
||||
boost::none)), pose, bias, 1e-5);
|
||||
EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -0,0 +1,122 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testGaussMarkov1stOrderFactor.cpp
|
||||
* @brief Unit tests for the GaussMarkov1stOrder factor
|
||||
* @author Vadim Indelman
|
||||
* @date Jan 17, 2012
|
||||
*/
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
#include <gtsam_unstable/slam/GaussMarkov1stOrderFactor.h>
|
||||
#include <gtsam/nonlinear/Values.h>
|
||||
#include <gtsam/inference/Key.h>
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
//! Factors
|
||||
typedef GaussMarkov1stOrderFactor<gtsam::LieVector> GaussMarkovFactor;
|
||||
|
||||
/* ************************************************************************* */
|
||||
gtsam::LieVector predictionError(const gtsam::LieVector& v1, const gtsam::LieVector& v2, const GaussMarkovFactor factor) {
|
||||
return factor.evaluateError(v1, v2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussMarkovFactor, equals )
|
||||
{
|
||||
// Create two identical factors and make sure they're equal
|
||||
gtsam::Key x1(1);
|
||||
gtsam::Key x2(2);
|
||||
double delta_t = 0.10;
|
||||
Vector tau = (Vector(3) << 100.0, 150.0, 10.0);
|
||||
gtsam::SharedGaussian model = gtsam::noiseModel::Isotropic::Sigma(3, 1.0);
|
||||
|
||||
GaussMarkovFactor factor1(x1, x2, delta_t, tau, model);
|
||||
GaussMarkovFactor factor2(x1, x2, delta_t, tau, model);
|
||||
|
||||
CHECK(gtsam::assert_equal(factor1, factor2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( GaussMarkovFactor, error )
|
||||
{
|
||||
gtsam::Values linPoint;
|
||||
gtsam::Key x1(1);
|
||||
gtsam::Key x2(2);
|
||||
double delta_t = 0.10;
|
||||
Vector tau = (Vector(3) << 100.0, 150.0, 10.0);
|
||||
gtsam::SharedGaussian model = gtsam::noiseModel::Isotropic::Sigma(3, 1.0);
|
||||
|
||||
gtsam::LieVector v1 = gtsam::LieVector((gtsam::Vector(3) << 10.0, 12.0, 13.0));
|
||||
gtsam::LieVector v2 = gtsam::LieVector((gtsam::Vector(3) << 10.0, 15.0, 14.0));
|
||||
|
||||
// Create two nodes
|
||||
linPoint.insert(x1, v1);
|
||||
linPoint.insert(x2, v2);
|
||||
|
||||
GaussMarkovFactor factor(x1, x2, delta_t, tau, model);
|
||||
gtsam::Vector Err1( factor.evaluateError(v1, v2) );
|
||||
|
||||
// Manually calculate the error
|
||||
Vector alpha(tau.size());
|
||||
Vector alpha_v1(tau.size());
|
||||
for(int i=0; i<tau.size(); i++){
|
||||
alpha(i) = exp(- 1/tau(i)*delta_t );
|
||||
alpha_v1(i) = alpha(i) * v1(i);
|
||||
}
|
||||
gtsam::Vector Err2( v2 - alpha_v1 );
|
||||
|
||||
CHECK(gtsam::assert_equal(Err1, Err2, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST (GaussMarkovFactor, jacobian ) {
|
||||
|
||||
gtsam::Values linPoint;
|
||||
gtsam::Key x1(1);
|
||||
gtsam::Key x2(2);
|
||||
double delta_t = 0.10;
|
||||
Vector tau = (Vector(3) << 100.0, 150.0, 10.0);
|
||||
gtsam::SharedGaussian model = gtsam::noiseModel::Isotropic::Sigma(3, 1.0);
|
||||
|
||||
GaussMarkovFactor factor(x1, x2, delta_t, tau, model);
|
||||
|
||||
// Update the linearization point
|
||||
gtsam::LieVector v1_upd = gtsam::LieVector((gtsam::Vector(3) << 0.5, -0.7, 0.3));
|
||||
gtsam::LieVector v2_upd = gtsam::LieVector((gtsam::Vector(3) << -0.7, 0.4, 0.9));
|
||||
|
||||
// Calculate the Jacobian matrix using the factor
|
||||
Matrix computed_H1, computed_H2;
|
||||
factor.evaluateError(v1_upd, v2_upd, computed_H1, computed_H2);
|
||||
|
||||
// Calculate the Jacobian matrices H1 and H2 using the numerical derivative function
|
||||
gtsam::Matrix numerical_H1, numerical_H2;
|
||||
numerical_H1 = gtsam::numericalDerivative21<gtsam::LieVector, gtsam::LieVector,
|
||||
gtsam::LieVector>(boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd);
|
||||
numerical_H2 = gtsam::numericalDerivative22<gtsam::LieVector, gtsam::LieVector,
|
||||
gtsam::LieVector>(boost::bind(&predictionError, _1, _2, factor), v1_upd, v2_upd);
|
||||
|
||||
// Verify they are equal for this choice of state
|
||||
CHECK( gtsam::assert_equal(numerical_H1, computed_H1, 1e-9));
|
||||
CHECK( gtsam::assert_equal(numerical_H2, computed_H2, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main()
|
||||
{
|
||||
TestResult tr; return TestRegistry::runAllTests(tr);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -95,13 +95,20 @@ TEST( PoseBetweenFactor, Error ) {
|
|||
|
||||
// The expected error
|
||||
Vector expectedError(6);
|
||||
// The solution depends on choice of Pose3 and Rot3 Expmap mode!
|
||||
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
|
||||
expectedError << -0.0298135267953815,
|
||||
0.0131341515747393,
|
||||
0.0968868439682154,
|
||||
#if defined(GTSAM_POSE3_EXPMAP)
|
||||
-0.145701634472172,
|
||||
-0.134898525569125,
|
||||
-0.0421026389164264;
|
||||
#else
|
||||
-0.13918755,
|
||||
-0.142346243,
|
||||
-0.0390885321;
|
||||
#endif
|
||||
#else
|
||||
expectedError << -0.029839512616488,
|
||||
0.013145599455949,
|
||||
|
|
@ -132,14 +139,20 @@ TEST( PoseBetweenFactor, ErrorWithTransform ) {
|
|||
|
||||
// The expected error
|
||||
Vector expectedError(6);
|
||||
// TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode!
|
||||
#if defined(GTSAM_ROT3_EXPMAP)
|
||||
// The solution depends on choice of Pose3 and Rot3 Expmap mode!
|
||||
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
|
||||
expectedError << 0.0173358202010741,
|
||||
0.0222210698409755,
|
||||
-0.0125032003886145,
|
||||
#if defined(GTSAM_POSE3_EXPMAP)
|
||||
0.0263800787416566,
|
||||
0.00540285006310398,
|
||||
0.000175859555693563;
|
||||
#else
|
||||
0.0264132886,
|
||||
0.0052376953,
|
||||
-7.16127036e-05;
|
||||
#endif
|
||||
#else
|
||||
expectedError << 0.017337193670445,
|
||||
0.022222830355243,
|
||||
|
|
|
|||
|
|
@ -90,14 +90,20 @@ TEST( PosePriorFactor, Error ) {
|
|||
|
||||
// The expected error
|
||||
Vector expectedError(6);
|
||||
// TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode!
|
||||
#if defined(GTSAM_ROT3_EXPMAP)
|
||||
// The solution depends on choice of Pose3 and Rot3 Expmap mode!
|
||||
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
|
||||
expectedError << -0.182948257976108,
|
||||
0.13851858011118,
|
||||
-0.157375974517456,
|
||||
#if defined(GTSAM_POSE3_EXPMAP)
|
||||
0.766913166076379,
|
||||
-1.22976117053126,
|
||||
0.949345561430261;
|
||||
#else
|
||||
0.740211734,
|
||||
-1.19821028,
|
||||
1.00815609;
|
||||
#endif
|
||||
#else
|
||||
expectedError << -0.184137861505414,
|
||||
0.139419283914526,
|
||||
|
|
@ -115,7 +121,7 @@ TEST( PosePriorFactor, Error ) {
|
|||
Vector actualError(factor.evaluateError(pose));
|
||||
|
||||
// Verify we get the expected error
|
||||
CHECK(assert_equal(expectedError, actualError, 1e-9));
|
||||
CHECK(assert_equal(expectedError, actualError, 1e-8));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
@ -127,14 +133,20 @@ TEST( PosePriorFactor, ErrorWithTransform ) {
|
|||
|
||||
// The expected error
|
||||
Vector expectedError(6);
|
||||
// TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode!
|
||||
#if defined(GTSAM_ROT3_EXPMAP)
|
||||
// The solution depends on choice of Pose3 and Rot3 Expmap mode!
|
||||
#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS)
|
||||
expectedError << -0.0224998729281528,
|
||||
0.191947887288328,
|
||||
0.273826035236257,
|
||||
#if defined(GTSAM_POSE3_EXPMAP)
|
||||
1.36483391560855,
|
||||
-0.754590051075035,
|
||||
0.585710674473659;
|
||||
#else
|
||||
1.49751986,
|
||||
-0.549375791,
|
||||
0.452761203;
|
||||
#endif
|
||||
#else
|
||||
expectedError << -0.022712885347328,
|
||||
0.193765110165872,
|
||||
|
|
@ -151,7 +163,7 @@ TEST( PosePriorFactor, ErrorWithTransform ) {
|
|||
Vector actualError(factor.evaluateError(pose));
|
||||
|
||||
// Verify we get the expected error
|
||||
CHECK(assert_equal(expectedError, actualError, 1e-9));
|
||||
CHECK(assert_equal(expectedError, actualError, 1e-8));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
|
|
|||
|
|
@ -0,0 +1,221 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testProjectionFactor.cpp
|
||||
* @brief Unit tests for ProjectionFactorPPP Class
|
||||
* @author Chris Beall
|
||||
* @date July 2014
|
||||
*/
|
||||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam_unstable/slam/ProjectionFactorPPP.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/geometry/Cal3DS2.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// make a realistic calibration matrix
|
||||
static double fov = 60; // degrees
|
||||
static size_t w=640,h=480;
|
||||
static Cal3_S2::shared_ptr K(new Cal3_S2(fov,w,h));
|
||||
|
||||
// Create a noise model for the pixel error
|
||||
static SharedNoiseModel model(noiseModel::Unit::Create(2));
|
||||
|
||||
// Convenience for named keys
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::L;
|
||||
using symbol_shorthand::T;
|
||||
|
||||
typedef ProjectionFactorPPP<Pose3, Point3> TestProjectionFactor;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ProjectionFactorPPP, nonStandard ) {
|
||||
ProjectionFactorPPP<Pose3, Point3, Cal3DS2> f;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ProjectionFactorPPP, Constructor) {
|
||||
Key poseKey(X(1));
|
||||
Key transformKey(T(1));
|
||||
Key pointKey(L(1));
|
||||
|
||||
Point2 measurement(323.0, 240.0);
|
||||
|
||||
TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ProjectionFactorPPP, ConstructorWithTransform) {
|
||||
Key poseKey(X(1));
|
||||
Key transformKey(T(1));
|
||||
Key pointKey(L(1));
|
||||
|
||||
Point2 measurement(323.0, 240.0);
|
||||
TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ProjectionFactorPPP, Equals ) {
|
||||
// Create two identical factors and make sure they're equal
|
||||
Point2 measurement(323.0, 240.0);
|
||||
|
||||
TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K);
|
||||
TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K);
|
||||
|
||||
CHECK(assert_equal(factor1, factor2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ProjectionFactorPPP, EqualsWithTransform ) {
|
||||
// Create two identical factors and make sure they're equal
|
||||
Point2 measurement(323.0, 240.0);
|
||||
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
|
||||
|
||||
TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K);
|
||||
TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K);
|
||||
|
||||
CHECK(assert_equal(factor1, factor2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ProjectionFactorPPP, Error ) {
|
||||
// Create the factor with a measurement that is 3 pixels off in x
|
||||
Key poseKey(X(1));
|
||||
Key transformKey(T(1));
|
||||
Key pointKey(L(1));
|
||||
Point2 measurement(323.0, 240.0);
|
||||
TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
|
||||
|
||||
// Set the linearization point
|
||||
Pose3 pose(Rot3(), Point3(0,0,-6));
|
||||
Point3 point(0.0, 0.0, 0.0);
|
||||
|
||||
// Use the factor to calculate the error
|
||||
Vector actualError(factor.evaluateError(pose, Pose3(), point));
|
||||
|
||||
// The expected error is (-3.0, 0.0) pixels / UnitCovariance
|
||||
Vector expectedError = (Vector(2) << -3.0, 0.0);
|
||||
|
||||
// Verify we get the expected error
|
||||
CHECK(assert_equal(expectedError, actualError, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ProjectionFactorPPP, ErrorWithTransform ) {
|
||||
// Create the factor with a measurement that is 3 pixels off in x
|
||||
Key poseKey(X(1));
|
||||
Key transformKey(T(1));
|
||||
Key pointKey(L(1));
|
||||
Point2 measurement(323.0, 240.0);
|
||||
Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
|
||||
TestProjectionFactor factor(measurement, model, poseKey,transformKey, pointKey, K);
|
||||
|
||||
// Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
|
||||
Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
|
||||
Point3 point(0.0, 0.0, 0.0);
|
||||
|
||||
// Use the factor to calculate the error
|
||||
Vector actualError(factor.evaluateError(pose, transform, point));
|
||||
|
||||
// The expected error is (-3.0, 0.0) pixels / UnitCovariance
|
||||
Vector expectedError = (Vector(2) << -3.0, 0.0);
|
||||
|
||||
// Verify we get the expected error
|
||||
CHECK(assert_equal(expectedError, actualError, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ProjectionFactorPPP, Jacobian ) {
|
||||
// Create the factor with a measurement that is 3 pixels off in x
|
||||
Key poseKey(X(1));
|
||||
Key transformKey(T(1));
|
||||
Key pointKey(L(1));
|
||||
Point2 measurement(323.0, 240.0);
|
||||
TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
|
||||
|
||||
// Set the linearization point
|
||||
Pose3 pose(Rot3(), Point3(0,0,-6));
|
||||
Point3 point(0.0, 0.0, 0.0);
|
||||
|
||||
// Use the factor to calculate the Jacobians
|
||||
Matrix H1Actual, H2Actual, H3Actual;
|
||||
factor.evaluateError(pose, Pose3(), point, H1Actual, H2Actual, H3Actual);
|
||||
|
||||
// The expected Jacobians
|
||||
Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.);
|
||||
Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.);
|
||||
|
||||
// Verify the Jacobians are correct
|
||||
CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
|
||||
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
|
||||
|
||||
// Verify H2 with numerical derivative
|
||||
Matrix H2Expected = numericalDerivative32<Pose3, Pose3, Point3>(
|
||||
boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
|
||||
boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
|
||||
boost::none, boost::none, boost::none)), pose, Pose3(), point);
|
||||
|
||||
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ProjectionFactorPPP, JacobianWithTransform ) {
|
||||
// Create the factor with a measurement that is 3 pixels off in x
|
||||
Key poseKey(X(1));
|
||||
Key transformKey(T(1));
|
||||
Key pointKey(L(1));
|
||||
Point2 measurement(323.0, 240.0);
|
||||
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
|
||||
TestProjectionFactor factor(measurement, model, poseKey, transformKey, pointKey, K);
|
||||
|
||||
// Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
|
||||
Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
|
||||
Point3 point(0.0, 0.0, 0.0);
|
||||
|
||||
// Use the factor to calculate the Jacobians
|
||||
Matrix H1Actual, H2Actual, H3Actual;
|
||||
factor.evaluateError(pose, body_P_sensor, point, H1Actual, H2Actual, H3Actual);
|
||||
|
||||
// The expected Jacobians
|
||||
Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376);
|
||||
Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376);
|
||||
|
||||
// Verify the Jacobians are correct
|
||||
CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
|
||||
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
|
||||
|
||||
// Verify H2 with numerical derivative
|
||||
Matrix H2Expected = numericalDerivative32<Pose3, Pose3, Point3>(
|
||||
boost::function<Vector(const Pose3&, const Pose3&, const Point3&)>(
|
||||
boost::bind(&TestProjectionFactor::evaluateError, &factor, _1, _2, _3,
|
||||
boost::none, boost::none, boost::none)), pose, body_P_sensor, point);
|
||||
|
||||
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
|
||||
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -0,0 +1,191 @@
|
|||
/* ----------------------------------------------------------------------------
|
||||
|
||||
* 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 testProjectionFactorPPPC.cpp
|
||||
* @brief Unit tests for Pose+Transform+Calibration ProjectionFactor Class
|
||||
* @author Chris Beall
|
||||
* @date Jul 29, 2014
|
||||
*/
|
||||
|
||||
#include <gtsam/base/numericalDerivative.h>
|
||||
#include <gtsam/base/TestableAssertions.h>
|
||||
#include <gtsam_unstable/slam/ProjectionFactorPPPC.h>
|
||||
#include <gtsam/inference/Symbol.h>
|
||||
#include <gtsam/geometry/Cal3DS2.h>
|
||||
#include <gtsam/geometry/Cal3_S2.h>
|
||||
#include <gtsam/geometry/Pose3.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Point2.h>
|
||||
|
||||
#include <CppUnitLite/TestHarness.h>
|
||||
|
||||
#include <boost/bind.hpp>
|
||||
|
||||
using namespace std;
|
||||
using namespace gtsam;
|
||||
|
||||
// make a realistic calibration matrix
|
||||
static double fov = 60; // degrees
|
||||
static size_t w=640,h=480;
|
||||
static Cal3_S2::shared_ptr K1(new Cal3_S2(fov,w,h));
|
||||
|
||||
// Create a noise model for the pixel error
|
||||
static SharedNoiseModel model(noiseModel::Unit::Create(2));
|
||||
|
||||
// Convenience for named keys
|
||||
using symbol_shorthand::X;
|
||||
using symbol_shorthand::L;
|
||||
using symbol_shorthand::T;
|
||||
using symbol_shorthand::K;
|
||||
|
||||
typedef ProjectionFactorPPPC<Pose3, Point3, Cal3_S2> TestProjectionFactor;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ProjectionFactorPPPC, nonStandard ) {
|
||||
ProjectionFactorPPPC<Pose3, Point3, Cal3DS2> f;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ProjectionFactorPPPC, Constructor) {
|
||||
Point2 measurement(323.0, 240.0);
|
||||
TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
|
||||
// TODO: Actually check something
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ProjectionFactorPPPC, Equals ) {
|
||||
// Create two identical factors and make sure they're equal
|
||||
Point2 measurement(323.0, 240.0);
|
||||
|
||||
TestProjectionFactor factor1(measurement, model, X(1), T(1), L(1), K(1));
|
||||
TestProjectionFactor factor2(measurement, model, X(1), T(1), L(1), K(1));
|
||||
|
||||
CHECK(assert_equal(factor1, factor2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ProjectionFactorPPPC, Error ) {
|
||||
// Create the factor with a measurement that is 3 pixels off in x
|
||||
Point2 measurement(323.0, 240.0);
|
||||
TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
|
||||
|
||||
// Set the linearization point
|
||||
Pose3 pose(Rot3(), Point3(0,0,-6));
|
||||
Point3 point(0.0, 0.0, 0.0);
|
||||
|
||||
// Use the factor to calculate the error
|
||||
Vector actualError(factor.evaluateError(pose, Pose3(), point, *K1));
|
||||
|
||||
// The expected error is (-3.0, 0.0) pixels / UnitCovariance
|
||||
Vector expectedError = (Vector(2) << -3.0, 0.0);
|
||||
|
||||
// Verify we get the expected error
|
||||
CHECK(assert_equal(expectedError, actualError, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ProjectionFactorPPPC, ErrorWithTransform ) {
|
||||
// Create the factor with a measurement that is 3 pixels off in x
|
||||
Point2 measurement(323.0, 240.0);
|
||||
Pose3 transform(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
|
||||
TestProjectionFactor factor(measurement, model, X(1),T(1), L(1), K(1));
|
||||
|
||||
// Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
|
||||
Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
|
||||
Point3 point(0.0, 0.0, 0.0);
|
||||
|
||||
// Use the factor to calculate the error
|
||||
Vector actualError(factor.evaluateError(pose, transform, point, *K1));
|
||||
|
||||
// The expected error is (-3.0, 0.0) pixels / UnitCovariance
|
||||
Vector expectedError = (Vector(2) << -3.0, 0.0);
|
||||
|
||||
// Verify we get the expected error
|
||||
CHECK(assert_equal(expectedError, actualError, 1e-9));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ProjectionFactorPPPC, Jacobian ) {
|
||||
// Create the factor with a measurement that is 3 pixels off in x
|
||||
Point2 measurement(323.0, 240.0);
|
||||
TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
|
||||
|
||||
// Set the linearization point
|
||||
Pose3 pose(Rot3(), Point3(0,0,-6));
|
||||
Point3 point(0.0, 0.0, 0.0);
|
||||
|
||||
// Use the factor to calculate the Jacobians
|
||||
Matrix H1Actual, H2Actual, H3Actual, H4Actual;
|
||||
factor.evaluateError(pose, Pose3(), point, *K1, H1Actual, H2Actual, H3Actual, H4Actual);
|
||||
|
||||
// The expected Jacobians
|
||||
Matrix H1Expected = (Matrix(2, 6) << 0., -554.256, 0., -92.376, 0., 0., 554.256, 0., 0., 0., -92.376, 0.);
|
||||
Matrix H3Expected = (Matrix(2, 3) << 92.376, 0., 0., 0., 92.376, 0.);
|
||||
|
||||
// Verify the Jacobians are correct
|
||||
CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
|
||||
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
|
||||
|
||||
// Verify H2 and H4 with numerical derivatives
|
||||
Matrix H2Expected = numericalDerivative11<LieVector, Pose3>(
|
||||
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
|
||||
*K1, boost::none, boost::none, boost::none, boost::none), Pose3());
|
||||
|
||||
Matrix H4Expected = numericalDerivative11<LieVector, Cal3_S2>(
|
||||
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, Pose3(), point,
|
||||
_1, boost::none, boost::none, boost::none, boost::none), *K1);
|
||||
|
||||
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
|
||||
CHECK(assert_equal(H4Expected, H4Actual, 1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( ProjectionFactorPPPC, JacobianWithTransform ) {
|
||||
// Create the factor with a measurement that is 3 pixels off in x
|
||||
Point2 measurement(323.0, 240.0);
|
||||
Pose3 body_P_sensor(Rot3::RzRyRx(-M_PI_2, 0.0, -M_PI_2), Point3(0.25, -0.10, 1.0));
|
||||
TestProjectionFactor factor(measurement, model, X(1), T(1), L(1), K(1));
|
||||
|
||||
// Set the linearization point. The vehicle pose has been selected to put the camera at (-6, 0, 0)
|
||||
Pose3 pose(Rot3(), Point3(-6.25, 0.10 , -1.0));
|
||||
Point3 point(0.0, 0.0, 0.0);
|
||||
|
||||
// Use the factor to calculate the Jacobians
|
||||
Matrix H1Actual, H2Actual, H3Actual, H4Actual;
|
||||
factor.evaluateError(pose, body_P_sensor, point, *K1, H1Actual, H2Actual, H3Actual, H4Actual);
|
||||
|
||||
// The expected Jacobians
|
||||
Matrix H1Expected = (Matrix(2, 6) << -92.376, 0., 577.350, 0., 92.376, 0., -9.2376, -577.350, 0., 0., 0., 92.376);
|
||||
Matrix H3Expected = (Matrix(2, 3) << 0., -92.376, 0., 0., 0., -92.376);
|
||||
|
||||
// Verify the Jacobians are correct
|
||||
CHECK(assert_equal(H1Expected, H1Actual, 1e-3));
|
||||
CHECK(assert_equal(H3Expected, H3Actual, 1e-3));
|
||||
|
||||
// Verify H2 and H4 with numerical derivatives
|
||||
Matrix H2Expected = numericalDerivative11<LieVector, Pose3>(
|
||||
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, _1, point,
|
||||
*K1, boost::none, boost::none, boost::none, boost::none), body_P_sensor);
|
||||
|
||||
Matrix H4Expected = numericalDerivative11<LieVector, Cal3_S2>(
|
||||
boost::bind(&TestProjectionFactor::evaluateError, &factor, pose, body_P_sensor, point,
|
||||
_1, boost::none, boost::none, boost::none, boost::none), *K1);
|
||||
|
||||
CHECK(assert_equal(H2Expected, H2Actual, 1e-5));
|
||||
CHECK(assert_equal(H4Expected, H4Actual, 1e-5));
|
||||
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
|
||||
/* ************************************************************************* */
|
||||
|
||||
|
|
@ -0,0 +1,304 @@
|
|||
clear all;
|
||||
clf;
|
||||
|
||||
import gtsam.*;
|
||||
|
||||
write_video = true;
|
||||
|
||||
use_camera = true;
|
||||
use_camera_transform_noise = true;
|
||||
gps_noise = 0.5; % normally distributed (meters)
|
||||
landmark_noise = 0.2; % normally distributed (meters)
|
||||
nrLandmarks = 1000; % Number of randomly generated landmarks
|
||||
|
||||
% ground-truth IMU-camera transform
|
||||
camera_transform = Pose3(Rot3.RzRyRx(-pi, 0, -pi/2),Point3());
|
||||
|
||||
% noise to compose onto the above for initialization
|
||||
camera_transform_noise = Pose3(Rot3.RzRyRx(0.1,0.1,0.1),Point3(0,0.02,0));
|
||||
|
||||
if(write_video)
|
||||
videoObj = VideoWriter('FlightCameraIMU_transform_GPS0_5_lm0_2_robust.avi');
|
||||
videoObj.Quality = 100;
|
||||
videoObj.FrameRate = 10;
|
||||
open(videoObj);
|
||||
end
|
||||
|
||||
% IMU parameters
|
||||
IMU_metadata.AccelerometerSigma = 1e-2;
|
||||
IMU_metadata.GyroscopeSigma = 1e-2;
|
||||
IMU_metadata.AccelerometerBiasSigma = 1e-6;
|
||||
IMU_metadata.GyroscopeBiasSigma = 1e-6;
|
||||
IMU_metadata.IntegrationSigma = 1e-1;
|
||||
|
||||
transformKey = 1000;
|
||||
calibrationKey = 2000;
|
||||
|
||||
fg = NonlinearFactorGraph;
|
||||
initial = Values;
|
||||
|
||||
%% some noise models
|
||||
trans_cov = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 20; 20; 0.1]);
|
||||
GPS_trans_cov = noiseModel.Diagonal.Sigmas([3; 3; 4]);
|
||||
K_cov = noiseModel.Diagonal.Sigmas([20; 20; 0.001; 20; 20]);
|
||||
|
||||
l_cov = noiseModel.Diagonal.Sigmas([landmark_noise; landmark_noise; landmark_noise]);
|
||||
z_cov = noiseModel.Diagonal.Sigmas([1.0;1.0]);
|
||||
% z_cov = noiseModel.Robust(noiseModel.mEstimator.Huber(1.0), noiseModel.Diagonal.Sigmas([1.0;1.0]));
|
||||
|
||||
%% calibration initialization
|
||||
K = Cal3_S2(20,1280,960);
|
||||
|
||||
% initialize K incorrectly
|
||||
K_corrupt = Cal3_S2(K.fx()+10,K.fy()+10,0,K.px(),K.py());
|
||||
|
||||
isamParams = gtsam.ISAM2Params;
|
||||
isamParams.setFactorization('QR');
|
||||
isam = ISAM2(isamParams);
|
||||
|
||||
%% Get initial conditions for the estimated trajectory
|
||||
currentVelocityGlobal = LieVector([10;0;0]); % (This is slightly wrong!)
|
||||
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
|
||||
|
||||
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0);
|
||||
sigma_init_b = noiseModel.Isotropic.Sigmas([ 0.100; 0.100; 0.100; 5.00e-05; 5.00e-05; 5.00e-05 ]);
|
||||
sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadata.GyroscopeBiasSigma * ones(3,1) ];
|
||||
g = [0;0;-9.8];
|
||||
w_coriolis = [0;0;0];
|
||||
|
||||
%% generate trajectory and landmarks
|
||||
trajectory = flight_trajectory();
|
||||
landmarks = ground_landmarks(nrLandmarks);
|
||||
|
||||
figure(1);
|
||||
% 3D map subplot
|
||||
a1 = subplot(2,2,1);
|
||||
grid on;
|
||||
|
||||
plot3DTrajectory(trajectory,'-b',true,5);
|
||||
plot3DPoints(landmarks,'*g');
|
||||
axis([-800 800 -800 800 0 1600]);
|
||||
axis equal;
|
||||
hold on;
|
||||
view(-37,40);
|
||||
|
||||
% camera subplot
|
||||
a2 = subplot(2,2,2);
|
||||
if ~use_camera
|
||||
title('Camera Off');
|
||||
end
|
||||
|
||||
% IMU-cam transform subplot
|
||||
a3 = subplot(2,2,3);
|
||||
view(-37,40);
|
||||
axis([-1 1 -1 1 -1 1]);
|
||||
grid on;
|
||||
xlabel('x');
|
||||
ylabel('y');
|
||||
zlabel('z');
|
||||
title('Estimated vs. actual IMU-cam transform');
|
||||
axis equal;
|
||||
|
||||
for i=1:size(trajectory)-1
|
||||
xKey = symbol('x',i);
|
||||
pose = trajectory.at(xKey); % GT pose
|
||||
pose_t = pose.translation(); % GT pose-translation
|
||||
|
||||
if exist('h_cursor','var')
|
||||
delete(h_cursor);
|
||||
end
|
||||
|
||||
% current ground-truth position indicator
|
||||
h_cursor = plot3(a1, pose_t.x,pose_t.y,pose_t.z,'*');
|
||||
|
||||
camera_pose = pose.compose(camera_transform);
|
||||
|
||||
axes(a2);
|
||||
if use_camera
|
||||
% project (and plot 2D camera view inside)
|
||||
measurements = project_landmarks(camera_pose,landmarks, K);
|
||||
% plot red landmarks in 3D plot
|
||||
plot_projected_landmarks(a1, landmarks, measurements);
|
||||
else
|
||||
measurements = Values;
|
||||
end
|
||||
|
||||
%% ISAM stuff
|
||||
currentVelKey = symbol('v',i);
|
||||
currentBiasKey = symbol('b',i);
|
||||
|
||||
initial.insert(currentVelKey, currentVelocityGlobal);
|
||||
initial.insert(currentBiasKey, currentBias);
|
||||
|
||||
% prior on translation, sort of like GPS with noise!
|
||||
gps_pose = pose.retract([0; 0; 0; normrnd(0,gps_noise,3,1)]);
|
||||
fg.add(PoseTranslationPrior3D(xKey, gps_pose, GPS_trans_cov));
|
||||
|
||||
if i==1
|
||||
% camera transform
|
||||
if use_camera_transform_noise
|
||||
camera_transform_init = camera_transform.compose(camera_transform_noise);
|
||||
else
|
||||
camera_transform_init = camera_transform;
|
||||
end
|
||||
initial.insert(transformKey,camera_transform_init);
|
||||
fg.add(PriorFactorPose3(transformKey,camera_transform_init,trans_cov));
|
||||
|
||||
% calibration
|
||||
initial.insert(2000, K_corrupt);
|
||||
fg.add(PriorFactorCal3_S2(calibrationKey,K_corrupt,K_cov));
|
||||
|
||||
initial.insert(xKey, pose);
|
||||
|
||||
result = initial;
|
||||
end
|
||||
|
||||
% priors on first two poses
|
||||
if i < 3
|
||||
% fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
|
||||
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
|
||||
|
||||
|
||||
end
|
||||
|
||||
%% the 'normal' case
|
||||
if i > 1
|
||||
|
||||
xKey_prev = symbol('x',i-1);
|
||||
pose_prev = trajectory.at(xKey_prev);
|
||||
|
||||
step = pose_prev.between(pose);
|
||||
|
||||
% insert estimate for current pose with some normal noise on
|
||||
% translation
|
||||
initial.insert(xKey,result.at(xKey_prev).compose(step.retract([0; 0; 0; normrnd(0,0.2,3,1)])));
|
||||
|
||||
% visual measurements
|
||||
if measurements.size > 0 && use_camera
|
||||
measurementKeys = KeyVector(measurements.keys);
|
||||
|
||||
for zz = 0:measurementKeys.size-1
|
||||
zKey = measurementKeys.at(zz);
|
||||
lKey = symbol('l',symbolIndex(zKey));
|
||||
|
||||
fg.add(TransformCalProjectionFactorCal3_S2(measurements.at(zKey), ...
|
||||
z_cov, xKey, transformKey, lKey, calibrationKey, false, true));
|
||||
|
||||
% only add landmark to values if doesn't exist yet
|
||||
if ~result.exists(lKey)
|
||||
noisy_landmark = landmarks.at(lKey).compose(Point3(normrnd(0,landmark_noise,3,1)));
|
||||
initial.insert(lKey, noisy_landmark);
|
||||
|
||||
% and add a prior since its position is known
|
||||
fg.add(PriorFactorPoint3(lKey, noisy_landmark,l_cov));
|
||||
end
|
||||
end
|
||||
end % end landmark observations
|
||||
|
||||
|
||||
%% IMU
|
||||
deltaT = 1;
|
||||
logmap = Pose3.Logmap(step);
|
||||
omega = logmap(1:3);
|
||||
velocity = logmap(4:6);
|
||||
|
||||
|
||||
% Simulate IMU measurements, considering Coriolis effect
|
||||
% (in this simple example we neglect gravity and there are no other forces acting on the body)
|
||||
acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
|
||||
omega, omega, velocity, velocity, deltaT);
|
||||
|
||||
% [ currentIMUPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
|
||||
% currentIMUPoseGlobal, omega, velocity, velocity, deltaT);
|
||||
|
||||
currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
|
||||
currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
|
||||
IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3));
|
||||
|
||||
accMeas = acc_omega(1:3)-g;
|
||||
omegaMeas = acc_omega(4:6);
|
||||
currentSummarizedMeasurement.integrateMeasurement(accMeas, omegaMeas, deltaT);
|
||||
|
||||
%% create IMU factor
|
||||
fg.add(ImuFactor( ...
|
||||
xKey_prev, currentVelKey-1, ...
|
||||
xKey, currentVelKey, ...
|
||||
currentBiasKey, currentSummarizedMeasurement, g, w_coriolis));
|
||||
|
||||
% Bias evolution as given in the IMU metadata
|
||||
fg.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ...
|
||||
noiseModel.Diagonal.Sigmas(sqrt(10) * sigma_between_b)));
|
||||
|
||||
% ISAM update
|
||||
isam.update(fg, initial);
|
||||
result = isam.calculateEstimate();
|
||||
|
||||
%% reset
|
||||
initial = Values;
|
||||
fg = NonlinearFactorGraph;
|
||||
|
||||
currentVelocityGlobal = result.at(currentVelKey);
|
||||
currentBias = result.at(currentBiasKey);
|
||||
|
||||
%% plot current pose result
|
||||
isam_pose = result.at(xKey);
|
||||
pose_t = isam_pose.translation();
|
||||
|
||||
if exist('h_result','var')
|
||||
delete(h_result);
|
||||
end
|
||||
|
||||
h_result = plot3(a1, pose_t.x,pose_t.y,pose_t.z,'^b', 'MarkerSize', 10);
|
||||
title(a1, sprintf('Step %d', i));
|
||||
|
||||
if exist('h_text1(1)', 'var')
|
||||
delete(h_text1(1));
|
||||
% delete(h_text2(1));
|
||||
end
|
||||
ty = result.at(transformKey).translation().y();
|
||||
K_estimate = result.at(calibrationKey);
|
||||
K_errors = K.localCoordinates(K_estimate);
|
||||
|
||||
camera_transform_estimate = result.at(transformKey);
|
||||
|
||||
fx = result.at(calibrationKey).fx();
|
||||
fy = result.at(calibrationKey).fy();
|
||||
% h_text1 = text(-600,0,0,sprintf('Y-Transform(0.0): %0.2f',ty));
|
||||
text(0,1300,0,sprintf('Calibration and IMU-cam transform errors:'));
|
||||
|
||||
entries = [{' f_x', ' f_y', ' s', 'p_x', 'p_y'}; num2cell(K_errors')];
|
||||
h_text1 = text(0,1750,0,sprintf('%s = %0.1f\n', entries{:}));
|
||||
|
||||
camera_transform_errors = camera_transform.localCoordinates(camera_transform_estimate);
|
||||
entries1 = [{'ax', 'ay', 'az', 'tx', 'ty', 'tz'}; num2cell(camera_transform_errors')];
|
||||
h_text2 = text(600,1700,0,sprintf('%s = %0.2f\n', entries1{:}));
|
||||
|
||||
% marginal is really huge
|
||||
% marginal_camera_transform = isam.marginalCovariance(transformKey);
|
||||
% plot transform
|
||||
axes(a3);
|
||||
cla;
|
||||
|
||||
plotPose3(camera_transform,[],1);
|
||||
plotPose3(camera_transform_estimate,[],0.5);
|
||||
|
||||
end
|
||||
|
||||
drawnow;
|
||||
if(write_video)
|
||||
currFrame = getframe(gcf);
|
||||
writeVideo(videoObj, currFrame)
|
||||
else
|
||||
pause(0.00001);
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
% print out final camera transform
|
||||
result.at(transformKey);
|
||||
|
||||
|
||||
if(write_video)
|
||||
close(videoObj);
|
||||
end
|
||||
|
|
@ -0,0 +1,189 @@
|
|||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
% Atlanta, Georgia 30332-0415
|
||||
% All Rights Reserved
|
||||
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
%
|
||||
% See LICENSE for the license information
|
||||
%
|
||||
% @brief Read graph from file and perform GraphSLAM
|
||||
% @author Frank Dellaert
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
clear all;
|
||||
clc;
|
||||
import gtsam.*
|
||||
|
||||
write_video = false;
|
||||
|
||||
if(write_video)
|
||||
videoObj = VideoWriter('test.avi');
|
||||
videoObj.Quality = 100;
|
||||
videoObj.FrameRate = 2;
|
||||
open(videoObj);
|
||||
end
|
||||
|
||||
%% generate some landmarks
|
||||
nrPoints = 8;
|
||||
landmarks = {Point3([20 15 1]'),...
|
||||
Point3([22 7 -1]'),...
|
||||
Point3([20 20 6]'),...
|
||||
Point3([24 19 -4]'),...
|
||||
Point3([26 17 -2]'),...
|
||||
Point3([12 15 4]'),...
|
||||
Point3([25 11 -6]'),...
|
||||
Point3([23 10 4]')};
|
||||
|
||||
curvature = 5.0;
|
||||
transformKey = 1000;
|
||||
calibrationKey = 2000;
|
||||
|
||||
fg = NonlinearFactorGraph;
|
||||
initial = Values;
|
||||
|
||||
%% intial landmarks and camera trajectory shifted in + y-direction
|
||||
y_shift = Point3(0,1,0);
|
||||
|
||||
% insert shifted points
|
||||
for i=1:nrPoints
|
||||
initial.insert(100+i,landmarks{i}.compose(y_shift));
|
||||
end
|
||||
|
||||
figure(1);
|
||||
cla
|
||||
hold on;
|
||||
|
||||
%% initial pose priors
|
||||
pose_cov = noiseModel.Diagonal.Sigmas([1*pi/180; 1*pi/180; 1*pi/180; 0.1; 0.1; 0.1]);
|
||||
fg.add(PriorFactorPose3(1, Pose3(),pose_cov));
|
||||
fg.add(PriorFactorPose3(2, Pose3(Rot3(),Point3(1,0,0)),pose_cov));
|
||||
|
||||
%% Actual camera translation coincides with odometry, but -90deg Z-X rotation
|
||||
camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift);
|
||||
actual_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),Point3());
|
||||
initial.insert(transformKey,camera_transform);
|
||||
|
||||
trans_cov = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 20; 20; 20]);
|
||||
fg.add(PriorFactorPose3(transformKey,camera_transform,trans_cov));
|
||||
|
||||
|
||||
%% insert poses
|
||||
initial.insert(1, Pose3());
|
||||
|
||||
|
||||
move_forward = Pose3(Rot3(),Point3(1,0,0));
|
||||
move_circle = Pose3(Rot3.RzRyRx(0.0,0.0,curvature*pi/180),Point3(1,0,0));
|
||||
covariance = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]);
|
||||
z_cov = noiseModel.Diagonal.Sigmas([1.0;1.0]);
|
||||
|
||||
%% calibration initialization
|
||||
K = Cal3_S2(900,900,0,640,480);
|
||||
K_corrupt = Cal3_S2(910,890,0,650,470);
|
||||
initial.insert(2000, K_corrupt);
|
||||
|
||||
K_cov = noiseModel.Diagonal.Sigmas([20; 20; 0.001; 20; 20]);
|
||||
fg.add(PriorFactorCal3_S2(calibrationKey,K_corrupt,K_cov));
|
||||
|
||||
|
||||
cheirality_exception_count = 0;
|
||||
|
||||
isamParams = gtsam.ISAM2Params;
|
||||
isamParams.setFactorization('QR');
|
||||
isam = ISAM2(isamParams);
|
||||
|
||||
result = initial
|
||||
|
||||
for i=1:20
|
||||
|
||||
if i > 1
|
||||
if i < 11
|
||||
initial.insert(i,result.at(i-1).compose(move_forward));
|
||||
fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance));
|
||||
else
|
||||
initial.insert(i,result.at(i-1).compose(move_circle));
|
||||
fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance));
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
% generate some camera measurements
|
||||
cam_pose = initial.at(i).compose(actual_transform);
|
||||
% gtsam.plotPose3(cam_pose);
|
||||
cam = SimpleCamera(cam_pose,K);
|
||||
i
|
||||
% result
|
||||
for j=1:nrPoints
|
||||
% All landmarks seen in every frame
|
||||
try
|
||||
z = cam.project(landmarks{j});
|
||||
fg.add(TransformCalProjectionFactorCal3_S2(z, z_cov, i, transformKey, 100+j, calibrationKey));
|
||||
catch
|
||||
cheirality_exception_count = cheirality_exception_count + 1;
|
||||
end % end try/catch
|
||||
end
|
||||
|
||||
if i > 2
|
||||
disp('ISAM Update');
|
||||
isam.update(fg, initial);
|
||||
result = isam.calculateEstimate();
|
||||
|
||||
%% reset
|
||||
initial = Values;
|
||||
fg = NonlinearFactorGraph;
|
||||
end
|
||||
|
||||
hold off;
|
||||
|
||||
clf;
|
||||
hold on;
|
||||
|
||||
%% plot results
|
||||
result_camera_transform = result.at(transformKey);
|
||||
for j=1:i
|
||||
gtsam.plotPose3(result.at(j),[],0.5);
|
||||
gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5);
|
||||
end
|
||||
|
||||
xlabel('x (m)');
|
||||
ylabel('y (m)');
|
||||
|
||||
title(sprintf('Curvature %g deg, iteration %g', curvature, i));
|
||||
|
||||
axis([0 20 0 20 -10 10]);
|
||||
view(-37,40);
|
||||
% axis equal
|
||||
|
||||
for l=101:100+nrPoints
|
||||
plotPoint3(result.at(l),'g');
|
||||
end
|
||||
|
||||
ty = result.at(transformKey).translation().y();
|
||||
fx = result.at(calibrationKey).fx();
|
||||
fy = result.at(calibrationKey).fy();
|
||||
text(1,5,5,sprintf('Y-Transform(0.0): %0.2f',ty));
|
||||
text(1,5,3,sprintf('fx(900): %.0f',fx));
|
||||
text(1,5,1,sprintf('fy(900): %.0f',fy));
|
||||
|
||||
if(write_video)
|
||||
currFrame = getframe(gcf);
|
||||
writeVideo(videoObj, currFrame)
|
||||
else
|
||||
pause(0.1);
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
if(write_video)
|
||||
close(videoObj);
|
||||
end
|
||||
|
||||
fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
|
||||
|
||||
disp('Transform after optimization');
|
||||
result.at(transformKey)
|
||||
|
||||
disp('Calibration after optimization');
|
||||
result.at(calibrationKey)
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,354 @@
|
|||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
% Atlanta, Georgia 30332-0415
|
||||
% All Rights Reserved
|
||||
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
%
|
||||
% See LICENSE for the license information
|
||||
%
|
||||
% @brief Estimate trajectory, calibration, landmarks, body-camera offset,
|
||||
% IMU
|
||||
% @author Chris Beall
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
clear all;
|
||||
clc;
|
||||
import gtsam.*
|
||||
|
||||
write_video = false;
|
||||
|
||||
if(write_video)
|
||||
videoObj = VideoWriter('test.avi');
|
||||
videoObj.Quality = 100;
|
||||
videoObj.FrameRate = 2;
|
||||
open(videoObj);
|
||||
end
|
||||
|
||||
%% generate some landmarks
|
||||
nrPoints = 8;
|
||||
landmarks = {Point3([20 15 1]'),...
|
||||
Point3([22 7 -1]'),...
|
||||
Point3([20 20 6]'),...
|
||||
Point3([24 19 -4]'),...
|
||||
Point3([26 17 -2]'),...
|
||||
Point3([12 15 4]'),...
|
||||
Point3([25 11 -6]'),...
|
||||
Point3([23 10 4]')};
|
||||
|
||||
IMU_metadata.AccelerometerSigma = 1e-2;
|
||||
IMU_metadata.GyroscopeSigma = 1e-2;
|
||||
IMU_metadata.AccelerometerBiasSigma = 1e-6;
|
||||
IMU_metadata.GyroscopeBiasSigma = 1e-6;
|
||||
IMU_metadata.IntegrationSigma = 1e-1;
|
||||
|
||||
curvature = 5.0;
|
||||
transformKey = 1000;
|
||||
calibrationKey = 2000;
|
||||
steps = 50;
|
||||
|
||||
fg = NonlinearFactorGraph;
|
||||
initial = Values;
|
||||
|
||||
%% intial landmarks and camera trajectory shifted in + y-direction
|
||||
y_shift = Point3(0,0.5,0);
|
||||
|
||||
% insert shifted points
|
||||
for i=1:nrPoints
|
||||
initial.insert(100+i,landmarks{i}.compose(y_shift));
|
||||
end
|
||||
|
||||
figure(1);
|
||||
cla
|
||||
hold on;
|
||||
|
||||
%% initial pose priors
|
||||
pose_cov = noiseModel.Diagonal.Sigmas([0.1*pi/180; 0.1*pi/180; 0.1*pi/180; 1e-4; 1e-4; 1e-4]);
|
||||
|
||||
%% Actual camera translation coincides with odometry, but -90deg Z-X rotation
|
||||
camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift);
|
||||
actual_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),Point3());
|
||||
|
||||
trans_cov = noiseModel.Diagonal.Sigmas([1*pi/180; 1*pi/180; 1*pi/180; 20; 1e-6; 1e-6]);
|
||||
|
||||
|
||||
move_forward = Pose3(Rot3(),Point3(1,0,0));
|
||||
move_circle = Pose3(Rot3.RzRyRx(0.0,0.0,curvature*pi/180),Point3(1,0,0));
|
||||
covariance = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]);
|
||||
z_cov = noiseModel.Diagonal.Sigmas([1.0;1.0]);
|
||||
|
||||
%% calibration initialization
|
||||
K = Cal3_S2(900,900,0,640,480);
|
||||
K_corrupt = Cal3_S2(910,890,0,650,470);
|
||||
K_cov = noiseModel.Diagonal.Sigmas([20; 20; 0.001; 20; 20]);
|
||||
|
||||
cheirality_exception_count = 0;
|
||||
|
||||
isamParams = gtsam.ISAM2Params;
|
||||
isamParams.setFactorization('QR');
|
||||
isam = ISAM2(isamParams);
|
||||
|
||||
currentIMUPoseGlobal = Pose3();
|
||||
|
||||
%% Get initial conditions for the estimated trajectory
|
||||
currentVelocityGlobal = LieVector([1;0;0]); % the vehicle is stationary at the beginning
|
||||
currentBias = imuBias.ConstantBias(zeros(3,1), zeros(3,1));
|
||||
|
||||
sigma_init_v = noiseModel.Isotropic.Sigma(3, 1.0);
|
||||
sigma_init_b = noiseModel.Isotropic.Sigmas([ 0.100; 0.100; 0.100; 5.00e-05; 5.00e-05; 5.00e-05 ]);
|
||||
sigma_between_b = [ IMU_metadata.AccelerometerBiasSigma * ones(3,1); IMU_metadata.GyroscopeBiasSigma * ones(3,1) ];
|
||||
g = [0;0;-9.8];
|
||||
w_coriolis = [0;0;0];
|
||||
|
||||
|
||||
for i=1:steps
|
||||
|
||||
t = i-1;
|
||||
|
||||
currentVelKey = symbol('v',i);
|
||||
currentBiasKey = symbol('b',i);
|
||||
|
||||
initial.insert(currentVelKey, currentVelocityGlobal);
|
||||
initial.insert(currentBiasKey, currentBias);
|
||||
|
||||
if i==1
|
||||
|
||||
% Pose Priors
|
||||
fg.add(PriorFactorPose3(1, Pose3(),pose_cov));
|
||||
fg.add(PriorFactorPose3(2, Pose3(Rot3(),Point3(1,0,0)),pose_cov));
|
||||
|
||||
% insert first
|
||||
initial.insert(1, Pose3());
|
||||
|
||||
% camera transform
|
||||
initial.insert(transformKey,camera_transform);
|
||||
fg.add(PriorFactorPose3(transformKey,camera_transform,trans_cov));
|
||||
|
||||
% calibration
|
||||
initial.insert(2000, K_corrupt);
|
||||
fg.add(PriorFactorCal3_S2(calibrationKey,K_corrupt,K_cov));
|
||||
|
||||
% velocity and bias evolution
|
||||
fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
|
||||
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
|
||||
|
||||
result = initial;
|
||||
end
|
||||
if i == 2
|
||||
fg.add(PriorFactorPose3(2, Pose3(Rot3(),Point3(1,0,0)),pose_cov));
|
||||
fg.add(PriorFactorLieVector(currentVelKey, currentVelocityGlobal, sigma_init_v));
|
||||
fg.add(PriorFactorConstantBias(currentBiasKey, currentBias, sigma_init_b));
|
||||
end
|
||||
if i > 1
|
||||
if i < 11
|
||||
step = move_forward;
|
||||
else
|
||||
step = move_circle;
|
||||
end
|
||||
|
||||
initial.insert(i,result.at(i-1).compose(step));
|
||||
fg.add(BetweenFactorPose3(i-1,i, step, covariance));
|
||||
|
||||
deltaT = 1;
|
||||
logmap = Pose3.Logmap(step);
|
||||
omega = logmap(1:3);
|
||||
velocity = logmap(4:6);
|
||||
%% Simulate IMU measurements, considering Coriolis effect
|
||||
% (in this simple example we neglect gravity and there are no other forces acting on the body)
|
||||
acc_omega = imuSimulator.calculateIMUMeas_coriolis( ...
|
||||
omega, omega, velocity, velocity, deltaT);
|
||||
|
||||
[ currentIMUPoseGlobal, currentVelocityGlobal ] = imuSimulator.integrateTrajectory( ...
|
||||
currentIMUPoseGlobal, omega, velocity, velocity, deltaT);
|
||||
|
||||
currentSummarizedMeasurement = gtsam.ImuFactorPreintegratedMeasurements( ...
|
||||
currentBias, IMU_metadata.AccelerometerSigma.^2 * eye(3), ...
|
||||
IMU_metadata.GyroscopeSigma.^2 * eye(3), IMU_metadata.IntegrationSigma.^2 * eye(3));
|
||||
|
||||
accMeas = acc_omega(1:3)-g;
|
||||
omegaMeas = acc_omega(4:6);
|
||||
currentSummarizedMeasurement.integrateMeasurement(accMeas, omegaMeas, deltaT);
|
||||
|
||||
%% create IMU factor
|
||||
fg.add(ImuFactor( ...
|
||||
i-1, currentVelKey-1, ...
|
||||
i, currentVelKey, ...
|
||||
currentBiasKey, currentSummarizedMeasurement, g, w_coriolis));
|
||||
|
||||
% Bias evolution as given in the IMU metadata
|
||||
fg.add(BetweenFactorConstantBias(currentBiasKey-1, currentBiasKey, imuBias.ConstantBias(zeros(3,1), zeros(3,1)), ...
|
||||
noiseModel.Diagonal.Sigmas(sqrt(steps) * sigma_between_b)));
|
||||
|
||||
end
|
||||
|
||||
% generate some camera measurements
|
||||
cam_pose = currentIMUPoseGlobal.compose(actual_transform);
|
||||
% gtsam.plotPose3(cam_pose);
|
||||
cam = SimpleCamera(cam_pose,K);
|
||||
i
|
||||
% result
|
||||
for j=1:nrPoints
|
||||
% All landmarks seen in every frame
|
||||
try
|
||||
z = cam.project(landmarks{j});
|
||||
fg.add(TransformCalProjectionFactorCal3_S2(z, z_cov, i, transformKey, 100+j, calibrationKey, false, true));
|
||||
catch
|
||||
cheirality_exception_count = cheirality_exception_count + 1;
|
||||
end % end try/catch
|
||||
end
|
||||
|
||||
if i > 1
|
||||
disp('ISAM Update');
|
||||
isam.update(fg, initial);
|
||||
result = isam.calculateEstimate();
|
||||
|
||||
%% reset
|
||||
initial = Values;
|
||||
fg = NonlinearFactorGraph;
|
||||
|
||||
currentVelocityGlobal = isam.calculateEstimate(currentVelKey);
|
||||
currentBias = isam.calculateEstimate(currentBiasKey);
|
||||
|
||||
%% Compute some marginals
|
||||
marginal = isam.marginalCovariance(calibrationKey);
|
||||
marginal_fx(i)=sqrt(marginal(1,1));
|
||||
marginal_fy(i)=sqrt(marginal(2,2));
|
||||
|
||||
%% Compute condition number
|
||||
isam_fg = isam.getFactorsUnsafe();
|
||||
isam_values = isam.getLinearizationPoint();
|
||||
gfg = isam_fg.linearize(isam_values);
|
||||
mat = gfg.jacobian();
|
||||
c(i) = cond(mat, 2);
|
||||
mat = gfg.augmentedJacobian();
|
||||
augmented_c(i)= cond(mat, 2);
|
||||
|
||||
for f=0:isam_fg.size()-1
|
||||
nonlinear_factor = isam_fg.at(f);
|
||||
if strcmp(class(nonlinear_factor),'gtsam.TransformCalProjectionFactorCal3_S2')
|
||||
gaussian_factor = nonlinear_factor.linearize(isam_values);
|
||||
A = gaussian_factor.getA();
|
||||
b = gaussian_factor.getb();
|
||||
% Column 17 (fy) in jacobian
|
||||
A_col = A(:,17);
|
||||
if A_col(2) == 0
|
||||
% pause
|
||||
disp('Cheirality Exception!');
|
||||
end
|
||||
end
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
hold off;
|
||||
|
||||
clf;
|
||||
figure(1);
|
||||
subplot(5,1,1:2);
|
||||
hold on;
|
||||
|
||||
%% plot the integrated IMU frame (not from
|
||||
gtsam.plotPose3(currentIMUPoseGlobal, [], 2);
|
||||
|
||||
%% plot results
|
||||
result_camera_transform = result.at(transformKey);
|
||||
for j=1:i
|
||||
gtsam.plotPose3(result.at(j),[],0.5);
|
||||
gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5);
|
||||
end
|
||||
|
||||
xlabel('x (m)');
|
||||
ylabel('y (m)');
|
||||
|
||||
title(sprintf('Curvature %g deg, iteration %g', curvature, i));
|
||||
|
||||
axis([0 20 0 20 -10 10]);
|
||||
view(-37,40);
|
||||
% axis equal
|
||||
|
||||
for l=101:100+nrPoints
|
||||
plotPoint3(result.at(l),'g');
|
||||
end
|
||||
|
||||
ty = result.at(transformKey).translation().y();
|
||||
fx = result.at(calibrationKey).fx();
|
||||
fy = result.at(calibrationKey).fy();
|
||||
px = result.at(calibrationKey).px();
|
||||
py = result.at(calibrationKey).py();
|
||||
text(1,5,5,sprintf('Y-Transform(0.0): %0.2f',ty));
|
||||
text(1,5,3,sprintf('fx(900): %.0f',fx));
|
||||
text(1,5,1,sprintf('fy(900): %.0f',fy));
|
||||
|
||||
fxs(i) = fx;
|
||||
fys(i) = fy;
|
||||
pxs(i) = px;
|
||||
pys(i) = py;
|
||||
subplot(5,1,3);
|
||||
hold on;
|
||||
plot(1:steps,repmat(K.fx,1,steps),'r--');
|
||||
p(1) = plot(1:i,fxs,'r','LineWidth',2);
|
||||
|
||||
plot(1:steps,repmat(K.fy,1,steps),'g--');
|
||||
p(2) = plot(1:i,fys,'g','LineWidth',2);
|
||||
|
||||
if i > 1
|
||||
plot(2:i,fxs(2:i) + marginal_fx(2:i),'r-.');
|
||||
plot(2:i,fxs(2:i) - marginal_fx(2:i),'r-.');
|
||||
|
||||
plot(2:i,fys(2:i) + marginal_fy(2:i),'g-.');
|
||||
plot(2:i,fys(2:i) - marginal_fy(2:i),'g-.');
|
||||
|
||||
|
||||
|
||||
subplot(5,1,5);
|
||||
hold on;
|
||||
title('Condition Number');
|
||||
plot(2:i,c(2:i),'b-');
|
||||
plot(2:i,augmented_c(2:i),'r-');
|
||||
axis([0 steps 0 max(c(2:i))*1.1]);
|
||||
|
||||
|
||||
% figure(2);
|
||||
% plotBayesTree(isam);
|
||||
|
||||
end
|
||||
legend(p, 'f_x', 'f_y', 'Location', 'SouthWest');
|
||||
|
||||
% legend(p, 'f_x', 'f_x''', 'f_y', 'f_y''', 'Location', 'SouthWest');
|
||||
|
||||
%% plot principal points
|
||||
subplot(5,1,4);
|
||||
hold on;
|
||||
plot(1:steps,repmat(K.px,1,steps),'r--');
|
||||
pp(1) = plot(1:i,pxs,'r','LineWidth',2);
|
||||
|
||||
plot(1:steps,repmat(K.py,1,steps),'g--');
|
||||
pp(2) = plot(1:i,pys,'g','LineWidth',2);
|
||||
title('Principal Point');
|
||||
legend(pp, 'p_x', 'p_y', 'Location', 'SouthWest');
|
||||
|
||||
if(write_video)
|
||||
currFrame = getframe(gcf);
|
||||
writeVideo(videoObj, currFrame)
|
||||
else
|
||||
pause(0.1);
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
if(write_video)
|
||||
close(videoObj);
|
||||
end
|
||||
|
||||
fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
|
||||
|
||||
disp('Transform after optimization');
|
||||
result.at(transformKey)
|
||||
|
||||
disp('Calibration after optimization');
|
||||
result.at(calibrationKey)
|
||||
|
||||
disp('Bias after optimization');
|
||||
currentBias
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,120 @@
|
|||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
% Atlanta, Georgia 30332-0415
|
||||
% All Rights Reserved
|
||||
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
%
|
||||
% See LICENSE for the license information
|
||||
%
|
||||
% @brief Read graph from file and perform GraphSLAM
|
||||
% @author Frank Dellaert
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
clear all;
|
||||
clc;
|
||||
import gtsam.*
|
||||
|
||||
%% generate some landmarks
|
||||
nrPoints = 8;
|
||||
landmarks = {Point3([20 15 1]'),...
|
||||
Point3([22 7 1]'),...
|
||||
Point3([20 20 6]'),...
|
||||
Point3([24 19 4]'),...
|
||||
Point3([26 17 2]'),...
|
||||
Point3([12 15 4]'),...
|
||||
Point3([25 11 6]'),...
|
||||
Point3([23 10 4]')};
|
||||
|
||||
fg = NonlinearFactorGraph;
|
||||
fg.add(NonlinearEqualityPose3(1, Pose3()));
|
||||
initial = Values;
|
||||
|
||||
%% intial landmarks and camera trajectory shifted in + y-direction
|
||||
y_shift = Point3(0,1,0);
|
||||
|
||||
% insert shifted points
|
||||
for i=1:nrPoints
|
||||
initial.insert(100+i,landmarks{i}.compose(y_shift));
|
||||
end
|
||||
|
||||
figure(1);
|
||||
cla
|
||||
hold on;
|
||||
plot3DPoints(initial);
|
||||
|
||||
%% Actual camera translation coincides with odometry, but -90deg Z-X rotation
|
||||
camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift);
|
||||
actual_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),Point3());
|
||||
initial.insert(1000,camera_transform);
|
||||
|
||||
%% insert poses
|
||||
initial.insert(1, Pose3());
|
||||
|
||||
|
||||
move_forward = Pose3(Rot3(),Point3(1,0,0));
|
||||
move_circle = Pose3(Rot3.RzRyRx(0.0,0.0,0.2),Point3(1,0,0));
|
||||
covariance = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]);
|
||||
z_cov = noiseModel.Diagonal.Sigmas([1.0;1.0]);
|
||||
|
||||
K = Cal3_S2(900,900,0,640,480);
|
||||
cheirality_exception_count = 0;
|
||||
|
||||
for i=1:20
|
||||
if i > 1
|
||||
if i < 11
|
||||
initial.insert(i,initial.at(i-1).compose(move_forward));
|
||||
fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance));
|
||||
else
|
||||
initial.insert(i,initial.at(i-1).compose(move_circle));
|
||||
fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance));
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
% generate some camera measurements
|
||||
cam_pose = initial.at(i).compose(actual_transform);
|
||||
gtsam.plotPose3(cam_pose);
|
||||
cam = SimpleCamera(cam_pose,K);
|
||||
i
|
||||
for j=1:nrPoints
|
||||
% All landmarks seen in every frame
|
||||
try
|
||||
z = cam.project(landmarks{j});
|
||||
fg.add(TransformProjectionFactorCal3_S2(z, z_cov, i, 1000, 100+j, K));
|
||||
catch
|
||||
cheirality_exception_count = cheirality_exception_count + 1;
|
||||
end % end try/catch
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
|
||||
|
||||
% plot3DTrajectory(initial, 'g-*');
|
||||
|
||||
%% camera plotting
|
||||
for i=1:20
|
||||
gtsam.plotPose3(initial.at(i).compose(camera_transform));
|
||||
end
|
||||
|
||||
xlabel('x (m)');
|
||||
ylabel('y (m)');
|
||||
|
||||
disp('Transform before optimization');
|
||||
initial.at(1000)
|
||||
|
||||
params = LevenbergMarquardtParams;
|
||||
params.setAbsoluteErrorTol(1e-15);
|
||||
params.setRelativeErrorTol(1e-15);
|
||||
params.setVerbosity('ERROR');
|
||||
params.setVerbosityLM('VERBOSE');
|
||||
|
||||
optimizer = LevenbergMarquardtOptimizer(fg, initial, params);
|
||||
result = optimizer.optimizeSafely();
|
||||
|
||||
disp('Transform after optimization');
|
||||
result.at(1000)
|
||||
|
||||
axis([0 25 0 25 0 10]);
|
||||
axis equal
|
||||
view(-37,40)
|
||||
|
||||
|
|
@ -0,0 +1,174 @@
|
|||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
% GTSAM Copyright 2010, Georgia Tech Research Corporation,
|
||||
% Atlanta, Georgia 30332-0415
|
||||
% All Rights Reserved
|
||||
% Authors: Frank Dellaert, et al. (see THANKS for the full author list)
|
||||
%
|
||||
% See LICENSE for the license information
|
||||
%
|
||||
% @brief Read graph from file and perform GraphSLAM
|
||||
% @author Frank Dellaert
|
||||
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
|
||||
clear all;
|
||||
clc;
|
||||
import gtsam.*
|
||||
|
||||
write_video = true;
|
||||
|
||||
if(write_video)
|
||||
videoObj = VideoWriter('test.avi');
|
||||
videoObj.Quality = 100;
|
||||
videoObj.FrameRate = 2;
|
||||
open(videoObj);
|
||||
end
|
||||
|
||||
%% generate some landmarks
|
||||
nrPoints = 8;
|
||||
landmarks = {Point3([20 15 1]'),...
|
||||
Point3([22 7 -1]'),...
|
||||
Point3([20 20 6]'),...
|
||||
Point3([24 19 -4]'),...
|
||||
Point3([26 17 -2]'),...
|
||||
Point3([12 15 4]'),...
|
||||
Point3([25 11 -6]'),...
|
||||
Point3([23 10 4]')};
|
||||
|
||||
fg = NonlinearFactorGraph;
|
||||
pose_cov = noiseModel.Diagonal.Sigmas([1*pi/180; 1*pi/180; 1*pi/180; 0.1; 0.1; 0.1]);
|
||||
fg.add(PriorFactorPose3(1, Pose3(),pose_cov));
|
||||
fg.add(PriorFactorPose3(2, Pose3(Rot3(),Point3(1,0,0)),pose_cov));
|
||||
|
||||
curvature = 0.5;
|
||||
|
||||
initial = Values;
|
||||
|
||||
%% intial landmarks and camera trajectory shifted in + y-direction
|
||||
y_shift = Point3(0,1,0);
|
||||
|
||||
% insert shifted points
|
||||
for i=1:nrPoints
|
||||
initial.insert(100+i,landmarks{i}.compose(y_shift));
|
||||
end
|
||||
|
||||
figure(1);
|
||||
cla
|
||||
hold on;
|
||||
|
||||
|
||||
%% Actual camera translation coincides with odometry, but -90deg Z-X rotation
|
||||
camera_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),y_shift);
|
||||
actual_transform = Pose3(Rot3.RzRyRx(-pi/2, 0, -pi/2),Point3());
|
||||
initial.insert(1000,camera_transform);
|
||||
|
||||
trans_cov = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 20; 20; 20]);
|
||||
fg.add(PriorFactorPose3(1000,camera_transform,trans_cov));
|
||||
|
||||
|
||||
%% insert poses
|
||||
initial.insert(1, Pose3());
|
||||
|
||||
|
||||
move_forward = Pose3(Rot3(),Point3(1,0,0));
|
||||
move_circle = Pose3(Rot3.RzRyRx(0.0,0.0,curvature*pi/180),Point3(1,0,0));
|
||||
covariance = noiseModel.Diagonal.Sigmas([5*pi/180; 5*pi/180; 5*pi/180; 0.05; 0.05; 0.05]);
|
||||
z_cov = noiseModel.Diagonal.Sigmas([1.0;1.0]);
|
||||
|
||||
K = Cal3_S2(900,900,0,640,480);
|
||||
cheirality_exception_count = 0;
|
||||
|
||||
isamParams = gtsam.ISAM2Params;
|
||||
isamParams.setFactorization('QR');
|
||||
isam = ISAM2(isamParams);
|
||||
|
||||
result = initial
|
||||
|
||||
for i=1:20
|
||||
|
||||
if i > 1
|
||||
if i < 11
|
||||
initial.insert(i,result.at(i-1).compose(move_forward));
|
||||
fg.add(BetweenFactorPose3(i-1,i, move_forward, covariance));
|
||||
else
|
||||
initial.insert(i,result.at(i-1).compose(move_circle));
|
||||
fg.add(BetweenFactorPose3(i-1,i, move_circle, covariance));
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
% generate some camera measurements
|
||||
cam_pose = initial.at(i).compose(actual_transform);
|
||||
% gtsam.plotPose3(cam_pose);
|
||||
cam = SimpleCamera(cam_pose,K);
|
||||
i
|
||||
% result
|
||||
for j=1:nrPoints
|
||||
% All landmarks seen in every frame
|
||||
try
|
||||
z = cam.project(landmarks{j});
|
||||
fg.add(TransformProjectionFactorCal3_S2(z, z_cov, i, 1000, 100+j, K));
|
||||
catch
|
||||
cheirality_exception_count = cheirality_exception_count + 1;
|
||||
end % end try/catch
|
||||
end
|
||||
|
||||
if i > 2
|
||||
disp('ISAM Update');
|
||||
isam.update(fg, initial);
|
||||
result = isam.calculateEstimate();
|
||||
|
||||
%% reset
|
||||
initial = Values;
|
||||
fg = NonlinearFactorGraph;
|
||||
end
|
||||
|
||||
hold off;
|
||||
|
||||
clf;
|
||||
hold on;
|
||||
|
||||
%% plot results
|
||||
result_camera_transform = result.at(1000);
|
||||
for j=1:i
|
||||
gtsam.plotPose3(result.at(j));
|
||||
gtsam.plotPose3(result.at(j).compose(result_camera_transform),[],0.5);
|
||||
end
|
||||
|
||||
xlabel('x (m)');
|
||||
ylabel('y (m)');
|
||||
|
||||
title(sprintf('Curvature %g deg, iteration %g', curvature, i));
|
||||
|
||||
axis([0 20 0 20 -10 10]);
|
||||
view(-37,40);
|
||||
% axis equal
|
||||
|
||||
for l=101:100+nrPoints
|
||||
plotPoint3(result.at(l),'g');
|
||||
end
|
||||
|
||||
ty = result.at(1000).translation().y();
|
||||
text(5,5,5,sprintf('Y-Transform: %0.2g',ty));
|
||||
|
||||
if(write_video)
|
||||
currFrame = getframe(gcf);
|
||||
writeVideo(videoObj, currFrame)
|
||||
else
|
||||
pause(0.001);
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
if(write_video)
|
||||
close(videoObj);
|
||||
end
|
||||
|
||||
fprintf('Cheirality Exception count: %d\n', cheirality_exception_count);
|
||||
|
||||
|
||||
|
||||
disp('Transform after optimization');
|
||||
result.at(1000)
|
||||
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,56 @@
|
|||
function [ values ] = flight_trajectory( input_args )
|
||||
%UNTITLED2 Summary of this function goes here
|
||||
% Detailed explanation goes here
|
||||
|
||||
import gtsam.*;
|
||||
|
||||
values = Values;
|
||||
curvature = 2;
|
||||
|
||||
|
||||
forward = Pose3(Rot3(),Point3(10,0,0));
|
||||
left = Pose3(Rot3.RzRyRx(0.0,0.0,curvature*pi/180),Point3(10,0,0));
|
||||
right = Pose3(Rot3.RzRyRx(0.0,0.0,-curvature*pi/180),Point3(10,0,0));
|
||||
|
||||
pose = Pose3(Rot3.RzRyRx(0,0,0),Point3(0,0,1000));
|
||||
|
||||
plan(1).direction = right;
|
||||
plan(1).steps = 20;
|
||||
|
||||
plan(2).direction = forward;
|
||||
plan(2).steps = 5;
|
||||
|
||||
plan(3).direction = left;
|
||||
plan(3).steps = 100;
|
||||
|
||||
plan(4).direction = forward;
|
||||
plan(4).steps = 50;
|
||||
|
||||
plan(5).direction = left;
|
||||
plan(5).steps = 80;
|
||||
|
||||
plan(6).direction = forward;
|
||||
plan(6).steps = 50;
|
||||
|
||||
plan(7).direction = right;
|
||||
plan(7).steps = 100;
|
||||
|
||||
plan_steps = numel(plan);
|
||||
|
||||
values_i = 0;
|
||||
|
||||
for i=1:plan_steps
|
||||
direction = plan(i).direction;
|
||||
segment_steps = plan(i).steps;
|
||||
|
||||
for j=1:segment_steps
|
||||
pose = pose.compose(direction);
|
||||
values.insert(symbol('x',values_i), pose);
|
||||
values_i = values_i + 1;
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
|
||||
end
|
||||
|
||||
|
|
@ -0,0 +1,19 @@
|
|||
function [ values ] = ground_landmarks( nrPoints )
|
||||
%UNTITLED2 Summary of this function goes here
|
||||
% Detailed explanation goes here
|
||||
|
||||
import gtsam.*;
|
||||
|
||||
values = Values;
|
||||
|
||||
x = -800+1600.*rand(nrPoints,1);
|
||||
y = -800+1600.*rand(nrPoints,1);
|
||||
z = 3 * rand(nrPoints,1);
|
||||
|
||||
for i=1:nrPoints
|
||||
values.insert(symbol('l',i),gtsam.Point3(x(i),y(i),z(i)));
|
||||
|
||||
end
|
||||
|
||||
end
|
||||
|
||||
Some files were not shown because too many files have changed in this diff Show More
Loading…
Reference in New Issue