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

Merge remote-tracking branch 'origin/develop' into feature/AHRSFactor 

Conflicts:
	gtsam.h
	gtsam/navigation/CombinedImuFactor.h
	gtsam/navigation/ImuFactor.h
release/4.3a0
krunalchande 2014-11-19 13:10:33 -05:00
parent 708d114b3c d4b868aa12
commit 881ecebfc9
242 changed files with 14246 additions and 3992 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

306
.cproject
View File

@ -1,17 +1,19 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?> <?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"> <storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544"> <cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC"> <storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC">
<externalSettings/> <externalSettings/>
<extensions> <extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions> </extensions>
</storageModule> </storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> <storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -60,13 +62,13 @@
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890" moduleId="org.eclipse.cdt.core.settings" name="Timing"> <storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890" moduleId="org.eclipse.cdt.core.settings" name="Timing">
<externalSettings/> <externalSettings/>
<extensions> <extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions> </extensions>
</storageModule> </storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> <storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -116,13 +118,13 @@
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" moduleId="org.eclipse.cdt.core.settings" name="fast"> <storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" moduleId="org.eclipse.cdt.core.settings" name="fast">
<externalSettings/> <externalSettings/>
<extensions> <extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/> <extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions> </extensions>
</storageModule> </storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> <storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -516,6 +518,22 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="SFMExampleExpressions.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>SFMExampleExpressions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMExampleExpressions.run" path="build/gtsam_unstable/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>Pose2SLAMExampleExpressions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthCamera3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testInvDepthCamera3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -718,14 +736,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testImuFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testImuFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthFactor3.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testInvDepthFactor3.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -782,7 +792,71 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testGaussianFactorGraphUnordered.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testInertialNavFactor_GlobalVelocity.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInertialNavFactor_GlobalVelocity.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthFactorVariant3.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInvDepthFactorVariant3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthFactorVariant1.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInvDepthFactorVariant1.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEquivInertialNavFactor_GlobalVel.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testEquivInertialNavFactor_GlobalVel.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthFactorVariant2.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInvDepthFactorVariant2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRelativeElevationFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testRelativeElevationFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoseBetweenFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPoseBetweenFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussMarkov1stOrderFactor.run" path="build/gtsam_unstable/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="testGaussianFactorGraph.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testGaussianFactorGraphUnordered.run</buildTarget> <buildTarget>testGaussianFactorGraphUnordered.run</buildTarget>
@ -878,6 +952,46 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testGaussianBayesTree.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeCameraExpression.run" path="build/gtsam_unstable/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeCameraExpression.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeOneCameraExpression.run" path="build/gtsam_unstable/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeOneCameraExpression.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeSFMExpressions.run" path="build/gtsam_unstable/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeSFMExpressions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeAdaptAutoDiff.run" path="build/gtsam_unstable/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeAdaptAutoDiff.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCombinedImuFactor.run" path="build/gtsam/navigation/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testCombinedImuFactor.run" path="build/gtsam/navigation/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -2055,6 +2169,30 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testRot2.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testRot2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot3Q.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testRot3Q.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot3.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testRot3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -2151,6 +2289,22 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testVelocityConstraint.run" path="build/gtsam_unstable/dynamics/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVelocityConstraint.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVelocityConstraint3.run" path="build/gtsam_unstable/dynamics/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVelocityConstraint3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDiscreteBayesTree.run" path="build/gtsam/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testDiscreteBayesTree.run" path="build/gtsam/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j1</buildArguments> <buildArguments>-j1</buildArguments>
@ -2231,6 +2385,38 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testSpirit.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSpirit.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.wrap" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.wrap</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testMethod.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testMethod.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testClass.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testClass.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="schedulingExample.run" path="build/gtsam_unstable/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="schedulingExample.run" path="build/gtsam_unstable/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -2311,6 +2497,22 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testNumericalDerivative.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testNumericalDerivative.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVerticalBlockMatrix.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVerticalBlockMatrix.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -2524,6 +2726,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testManifold.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testManifold.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testParticleFactor.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testParticleFactor.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -2532,6 +2742,38 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testExpressionFactor.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testExpressionFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testExpression.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testExpression.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testExpressionMeta.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testExpressionMeta.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testAdaptAutoDiff.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testAdaptAutoDiff.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -2596,10 +2838,10 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testBetweenFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testPriorFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testBetweenFactor.run</buildTarget> <buildTarget>testPriorFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -2652,18 +2894,10 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testGPSFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testPoseRotationPrior.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testGPSFactor.run</buildTarget> <buildTarget>testPoseRotationPrior.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> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -3052,22 +3286,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSpirit.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.wrap</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>

4
CMakeLists.txt
View File

@ -9,8 +9,8 @@ if(NOT DEFINED CMAKE_MACOSX_RPATH)
endif() endif()
# Set the version number for the library # Set the version number for the library
set (GTSAM_VERSION_MAJOR 3) set (GTSAM_VERSION_MAJOR 4)
set (GTSAM_VERSION_MINOR 1) set (GTSAM_VERSION_MINOR 0)
set (GTSAM_VERSION_PATCH 0) set (GTSAM_VERSION_PATCH 0)
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}") math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}") set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")

2
examples/CameraResectioning.cpp
View File

@ -48,7 +48,7 @@ public:
virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H = virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const { boost::none) const {
SimpleCamera camera(pose, *K_); SimpleCamera camera(pose, *K_);
Point2 reprojectionError(camera.project(P_, H) - p_); Point2 reprojectionError(camera.project(P_, H, boost::none, boost::none) - p_);
return reprojectionError.vector(); return reprojectionError.vector();
} }
}; };

4
examples/SFMExample.cpp
View File

@ -85,8 +85,8 @@ int main(int argc, char* argv[]) {
// Simulated measurements from each camera pose, adding them to the factor graph // Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) { for (size_t i = 0; i < poses.size(); ++i) {
SimpleCamera camera(poses[i], *K);
for (size_t j = 0; j < points.size(); ++j) { for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]); Point2 measurement = camera.project(points[j]);
graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K)); graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
} }
@ -111,6 +111,8 @@ int main(int argc, char* argv[]) {
/* Optimize the graph and print results */ /* Optimize the graph and print results */
Values result = DoglegOptimizer(graph, initialEstimate).optimize(); Values result = DoglegOptimizer(graph, initialEstimate).optimize();
result.print("Final results:\n"); result.print("Final results:\n");
cout << "initial error = " << graph.error(initialEstimate) << endl;
cout << "final error = " << graph.error(result) << endl;
return 0; return 0;
} }

128
gtsam.h
View File

@ -156,8 +156,14 @@ virtual class Value {
size_t dim() const; size_t dim() const;
}; };
class Vector3 {
Vector3(Vector v);
};
class Vector6 {
Vector6(Vector v);
};
#include <gtsam/base/LieScalar.h> #include <gtsam/base/LieScalar.h>
virtual class LieScalar : gtsam::Value { class LieScalar {
// Standard constructors // Standard constructors
LieScalar(); LieScalar();
LieScalar(double d); LieScalar(double d);
@ -186,7 +192,7 @@ virtual class LieScalar : gtsam::Value {
}; };
#include <gtsam/base/LieVector.h> #include <gtsam/base/LieVector.h>
virtual class LieVector : gtsam::Value { class LieVector {
// Standard constructors // Standard constructors
LieVector(); LieVector();
LieVector(Vector v); LieVector(Vector v);
@ -218,7 +224,7 @@ virtual class LieVector : gtsam::Value {
}; };
#include <gtsam/base/LieMatrix.h> #include <gtsam/base/LieMatrix.h>
virtual class LieMatrix : gtsam::Value { class LieMatrix {
// Standard constructors // Standard constructors
LieMatrix(); LieMatrix();
LieMatrix(Matrix v); LieMatrix(Matrix v);
@ -253,7 +259,7 @@ virtual class LieMatrix : gtsam::Value {
// geometry // geometry
//************************************************************************* //*************************************************************************
virtual class Point2 : gtsam::Value { class Point2 {
// Standard Constructors // Standard Constructors
Point2(); Point2();
Point2(double x, double y); Point2(double x, double y);
@ -290,7 +296,7 @@ virtual class Point2 : gtsam::Value {
void serialize() const; void serialize() const;
}; };
virtual class StereoPoint2 : gtsam::Value { class StereoPoint2 {
// Standard Constructors // Standard Constructors
StereoPoint2(); StereoPoint2();
StereoPoint2(double uL, double uR, double v); StereoPoint2(double uL, double uR, double v);
@ -325,7 +331,7 @@ virtual class StereoPoint2 : gtsam::Value {
void serialize() const; void serialize() const;
}; };
virtual class Point3 : gtsam::Value { class Point3 {
// Standard Constructors // Standard Constructors
Point3(); Point3();
Point3(double x, double y, double z); Point3(double x, double y, double z);
@ -361,7 +367,7 @@ virtual class Point3 : gtsam::Value {
void serialize() const; void serialize() const;
}; };
virtual class Rot2 : gtsam::Value { class Rot2 {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
Rot2(); Rot2();
Rot2(double theta); Rot2(double theta);
@ -406,7 +412,7 @@ virtual class Rot2 : gtsam::Value {
void serialize() const; void serialize() const;
}; };
virtual class Rot3 : gtsam::Value { class Rot3 {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
Rot3(); Rot3();
Rot3(Matrix R); Rot3(Matrix R);
@ -462,7 +468,7 @@ virtual class Rot3 : gtsam::Value {
void serialize() const; void serialize() const;
}; };
virtual class Pose2 : gtsam::Value { class Pose2 {
// Standard Constructor // Standard Constructor
Pose2(); Pose2();
Pose2(const gtsam::Pose2& pose); Pose2(const gtsam::Pose2& pose);
@ -512,7 +518,7 @@ virtual class Pose2 : gtsam::Value {
void serialize() const; void serialize() const;
}; };
virtual class Pose3 : gtsam::Value { class Pose3 {
// Standard Constructors // Standard Constructors
Pose3(); Pose3();
Pose3(const gtsam::Pose3& pose); Pose3(const gtsam::Pose3& pose);
@ -564,7 +570,7 @@ virtual class Pose3 : gtsam::Value {
}; };
#include <gtsam/geometry/Unit3.h> #include <gtsam/geometry/Unit3.h>
virtual class Unit3 : gtsam::Value { class Unit3 {
// Standard Constructors // Standard Constructors
Unit3(); Unit3();
Unit3(const gtsam::Point3& pose); Unit3(const gtsam::Point3& pose);
@ -585,7 +591,7 @@ virtual class Unit3 : gtsam::Value {
}; };
#include <gtsam/geometry/EssentialMatrix.h> #include <gtsam/geometry/EssentialMatrix.h>
virtual class EssentialMatrix : gtsam::Value { class EssentialMatrix {
// Standard Constructors // Standard Constructors
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb); EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
@ -606,7 +612,7 @@ virtual class EssentialMatrix : gtsam::Value {
double error(Vector vA, Vector vB); double error(Vector vA, Vector vB);
}; };
virtual class Cal3_S2 : gtsam::Value { class Cal3_S2 {
// Standard Constructors // Standard Constructors
Cal3_S2(); Cal3_S2();
Cal3_S2(double fx, double fy, double s, double u0, double v0); Cal3_S2(double fx, double fy, double s, double u0, double v0);
@ -643,7 +649,7 @@ virtual class Cal3_S2 : gtsam::Value {
}; };
#include <gtsam/geometry/Cal3DS2.h> #include <gtsam/geometry/Cal3DS2.h>
virtual class Cal3DS2 : gtsam::Value { class Cal3DS2 {
// Standard Constructors // Standard Constructors
Cal3DS2(); Cal3DS2();
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4); Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
@ -699,7 +705,43 @@ class Cal3_S2Stereo {
double baseline() const; double baseline() const;
}; };
virtual class CalibratedCamera : gtsam::Value { #include <gtsam/geometry/Cal3Bundler.h>
class Cal3Bundler {
// Standard Constructors
Cal3Bundler();
Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
// Testable
void print(string s) const;
bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3Bundler retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
// Standard Interface
double fx() const;
double fy() const;
double k1() const;
double k2() const;
double u0() const;
double v0() const;
Vector vector() const;
Vector k() const;
//Matrix K() const; //FIXME: Uppercase
// enabling serialization functionality
void serialize() const;
};
class CalibratedCamera {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
CalibratedCamera(); CalibratedCamera();
CalibratedCamera(const gtsam::Pose3& pose); CalibratedCamera(const gtsam::Pose3& pose);
@ -732,7 +774,7 @@ virtual class CalibratedCamera : gtsam::Value {
void serialize() const; void serialize() const;
}; };
virtual class SimpleCamera : gtsam::Value { class SimpleCamera {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
SimpleCamera(); SimpleCamera();
SimpleCamera(const gtsam::Pose3& pose); SimpleCamera(const gtsam::Pose3& pose);
@ -771,7 +813,7 @@ virtual class SimpleCamera : gtsam::Value {
}; };
template<CALIBRATION = {gtsam::Cal3DS2}> template<CALIBRATION = {gtsam::Cal3DS2}>
virtual class PinholeCamera : gtsam::Value { class PinholeCamera {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
PinholeCamera(); PinholeCamera();
PinholeCamera(const gtsam::Pose3& pose); PinholeCamera(const gtsam::Pose3& pose);
@ -809,7 +851,7 @@ virtual class PinholeCamera : gtsam::Value {
void serialize() const; void serialize() const;
}; };
virtual class StereoCamera : gtsam::Value { class StereoCamera {
// Standard Constructors and Named Constructors // Standard Constructors and Named Constructors
StereoCamera(); StereoCamera();
StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K); StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K);
@ -862,7 +904,7 @@ virtual class SymbolicFactor {
}; };
#include <gtsam/symbolic/SymbolicFactorGraph.h> #include <gtsam/symbolic/SymbolicFactorGraph.h>
class SymbolicFactorGraph { virtual class SymbolicFactorGraph {
SymbolicFactorGraph(); SymbolicFactorGraph();
SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet); SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet);
SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree); SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree);
@ -1664,15 +1706,12 @@ class Values {
void print(string s) const; void print(string s) const;
bool equals(const gtsam::Values& other, double tol) const; bool equals(const gtsam::Values& other, double tol) const;
void insert(size_t j, const gtsam::Value& value);
void insert(const gtsam::Values& values); void insert(const gtsam::Values& values);
void update(size_t j, const gtsam::Value& val);
void update(const gtsam::Values& values); void update(const gtsam::Values& values);
void erase(size_t j); void erase(size_t j);
void swap(gtsam::Values& values); void swap(gtsam::Values& values);
bool exists(size_t j) const; bool exists(size_t j) const;
gtsam::Value at(size_t j) const;
gtsam::KeyList keys() const; gtsam::KeyList keys() const;
gtsam::VectorValues zeroVectors() const; gtsam::VectorValues zeroVectors() const;
@ -1682,6 +1721,37 @@ class Values {
// enabling serialization functionality // enabling serialization functionality
void serialize() const; void serialize() const;
// New in 4.0, we have to specialize every insert/update/at to generate wrappers
// Instead of the old:
// void insert(size_t j, const gtsam::Value& value);
// void update(size_t j, const gtsam::Value& val);
// gtsam::Value at(size_t j) const;
void insert(size_t j, const gtsam::Point2& t);
void insert(size_t j, const gtsam::Point3& t);
void insert(size_t j, const gtsam::Rot2& t);
void insert(size_t j, const gtsam::Pose2& t);
void insert(size_t j, const gtsam::Rot3& t);
void insert(size_t j, const gtsam::Pose3& t);
void insert(size_t j, const gtsam::Cal3_S2& t);
void insert(size_t j, const gtsam::Cal3DS2& t);
void insert(size_t j, const gtsam::Cal3Bundler& t);
void insert(size_t j, const gtsam::EssentialMatrix& t);
void update(size_t j, const gtsam::Point2& t);
void update(size_t j, const gtsam::Point3& t);
void update(size_t j, const gtsam::Rot2& t);
void update(size_t j, const gtsam::Pose2& t);
void update(size_t j, const gtsam::Rot3& t);
void update(size_t j, const gtsam::Pose3& t);
void update(size_t j, const gtsam::Cal3_S2& t);
void update(size_t j, const gtsam::Cal3DS2& t);
void update(size_t j, const gtsam::Cal3Bundler& t);
void update(size_t j, const gtsam::EssentialMatrix& t);
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2,
gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2}>
T at(size_t j);
}; };
// Actually a FastList<Key> // Actually a FastList<Key>
@ -2080,7 +2150,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h> #include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h> #include <gtsam/slam/PriorFactor.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}> template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class PriorFactor : gtsam::NoiseModelFactor { virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel); PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const; T prior() const;
@ -2091,7 +2161,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}> template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
virtual class BetweenFactor : gtsam::NoiseModelFactor { virtual class BetweenFactor : gtsam::NoiseModelFactor {
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel); BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
T measured() const; T measured() const;
@ -2103,7 +2173,7 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
#include <gtsam/nonlinear/NonlinearEquality.h> #include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}> template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class NonlinearEquality : gtsam::NoiseModelFactor { virtual class NonlinearEquality : gtsam::NoiseModelFactor {
// Constructor - forces exact evaluation // Constructor - forces exact evaluation
NonlinearEquality(size_t j, const T& feasible); NonlinearEquality(size_t j, const T& feasible);
@ -2284,7 +2354,7 @@ void writeG2o(const gtsam::NonlinearFactorGraph& graph,
namespace imuBias { namespace imuBias {
#include <gtsam/navigation/ImuBias.h> #include <gtsam/navigation/ImuBias.h>
virtual class ConstantBias : gtsam::Value { class ConstantBias {
// Standard Constructor // Standard Constructor
ConstantBias(); ConstantBias();
ConstantBias(Vector biasAcc, Vector biasGyro); ConstantBias(Vector biasAcc, Vector biasGyro);
@ -2351,7 +2421,7 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
// Standard Interface // Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const; gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j, void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j,
const gtsam::imuBias::ConstantBias& bias, const gtsam::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const; Vector gravity, Vector omegaCoriolis) const;
@ -2440,9 +2510,7 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
// Standard Interface // Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const; gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::Vector3& vel_i, gtsam::Pose3& pose_j, gtsam::Vector3& vel_j,
static void predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j, const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements, const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis); Vector gravity, Vector omegaCoriolis);

BIN
gtsam/3rdparty/CCOLAMD/Lib/ccolamd.o vendored
View File

Binary file not shown.

BIN
gtsam/3rdparty/CCOLAMD/Lib/ccolamd_global.o vendored
View File

Binary file not shown.

BIN
gtsam/3rdparty/CCOLAMD/Lib/ccolamd_l.o vendored
View File

Binary file not shown.

4
gtsam/3rdparty/gtsam_eigen_includes.h.in vendored
View File

@ -27,3 +27,7 @@
#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/LU> #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/LU>
#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/SVD> #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/SVD>
#include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Geometry> #include <@GTSAM_EIGEN_INCLUDE_PREFIX@Eigen/Geometry>

221
gtsam/base/ChartValue.h Normal file
View File

@ -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 ChartValue.h
* @brief
* @date October, 2014
* @author Michael Bosse, Abel Gawel, Renaud Dube
* based on DerivedValue.h by Duy Nguyen Ta
*/
#pragma once
#include <gtsam/base/GenericValue.h>
#include <gtsam/base/Manifold.h>
#include <boost/make_shared.hpp>
//////////////////
// The following includes windows.h in some MSVC versions, so we undef min, max, and ERROR
#include <boost/pool/singleton_pool.hpp>
#ifdef min
#undef min
#endif
#ifdef max
#undef max
#endif
#ifdef ERROR
#undef ERROR
#endif
//////////////////
namespace gtsam {
/**
* ChartValue is derived from GenericValue<T> and Chart so that
* Chart can be zero sized (as in DefaultChart<T>)
* if the Chart is a member variable then it won't ever be zero sized.
*/
template<class T, class Chart_ = DefaultChart<T> >
class ChartValue: public GenericValue<T>, public Chart_ {
BOOST_CONCEPT_ASSERT((ChartConcept<Chart_>));
public:
typedef T type;
typedef Chart_ Chart;
public:
/// Default Constructor. TODO might not make sense for some types
ChartValue() :
GenericValue<T>(T()) {
}
/// Construct froma value
ChartValue(const T& value) :
GenericValue<T>(value) {
}
/// Construct from a value and initialize the chart
template<typename C>
ChartValue(const T& value, C chart_initializer) :
GenericValue<T>(value), Chart(chart_initializer) {
}
/// Destructor
virtual ~ChartValue() {
}
/**
* Create a duplicate object returned as a pointer to the generic Value interface.
* For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator.
* The result must be deleted with Value::deallocate_, not with the 'delete' operator.
*/
virtual Value* clone_() const {
void *place = boost::singleton_pool<PoolTag, sizeof(ChartValue)>::malloc();
ChartValue* ptr = new (place) ChartValue(*this); // calls copy constructor to fill in
return ptr;
}
/**
* Destroy and deallocate this object, only if it was originally allocated using clone_().
*/
virtual void deallocate_() const {
this->~ChartValue(); // Virtual destructor cleans up the derived object
boost::singleton_pool<PoolTag, sizeof(ChartValue)>::free((void*) this); // Release memory from pool
}
/**
* Clone this value (normal clone on the heap, delete with 'delete' operator)
*/
virtual boost::shared_ptr<Value> clone() const {
return boost::make_shared<ChartValue>(*this);
}
/// Chart Value interface version of retract
virtual Value* retract_(const Vector& delta) const {
// Call retract on the derived class using the retract trait function
const T retractResult = Chart::retract(GenericValue<T>::value(), delta);
// Create a Value pointer copy of the result
void* resultAsValuePlace =
boost::singleton_pool<PoolTag, sizeof(ChartValue)>::malloc();
Value* resultAsValue = new (resultAsValuePlace) ChartValue(retractResult,
static_cast<const Chart&>(*this));
// Return the pointer to the Value base class
return resultAsValue;
}
/// Generic Value interface version of localCoordinates
virtual Vector localCoordinates_(const Value& value2) const {
// Cast the base class Value pointer to a templated generic class pointer
const GenericValue<T>& genericValue2 =
static_cast<const GenericValue<T>&>(value2);
// Return the result of calling localCoordinates trait on the derived class
return Chart::local(GenericValue<T>::value(), genericValue2.value());
}
/// Non-virtual version of retract
ChartValue retract(const Vector& delta) const {
return ChartValue(Chart::retract(GenericValue<T>::value(), delta),
static_cast<const Chart&>(*this));
}
/// Non-virtual version of localCoordinates
Vector localCoordinates(const ChartValue& value2) const {
return localCoordinates_(value2);
}
/// Return run-time dimensionality
virtual size_t dim() const {
// need functional form here since the dimension may be dynamic
return Chart::getDimension(GenericValue<T>::value());
}
/// Assignment operator
virtual Value& operator=(const Value& rhs) {
// Cast the base class Value pointer to a derived class pointer
const ChartValue& derivedRhs = static_cast<const ChartValue&>(rhs);
// Do the assignment and return the result
*this = ChartValue(derivedRhs); // calls copy constructor
return *this;
}
protected:
// implicit assignment operator for (const ChartValue& rhs) works fine here
/// Assignment operator, protected because only the Value or DERIVED
/// assignment operators should be used.
// DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) {
// // Nothing to do, do not call base class assignment operator
// return *this;
// }
private:
/// Fake Tag struct for singleton pool allocator. In fact, it is never used!
struct PoolTag {
};
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
// ar & boost::serialization::make_nvp("value",);
// todo: implement a serialization for charts
ar
& boost::serialization::make_nvp("GenericValue",
boost::serialization::base_object<GenericValue<T> >(*this));
}
};
// Define
namespace traits {
/// The dimension of a ChartValue is the dimension of the chart
template<typename T, typename Chart>
struct dimension<ChartValue<T, Chart> > : public dimension<Chart> {
// TODO Frank thinks dimension is a property of type, chart should conform
};
} // \ traits
/// Get the chart from a Value
template<typename Chart>
const Chart& Value::getChart() const {
return dynamic_cast<const Chart&>(*this);
}
/// Convenience function that can be used to make an expression to convert a value to a chart
template<typename T>
ChartValue<T> convertToChartValue(const T& value,
boost::optional<
Eigen::Matrix<double, traits::dimension<T>::value,
traits::dimension<T>::value>&> H = boost::none) {
if (H) {
*H = Eigen::Matrix<double, traits::dimension<T>::value,
traits::dimension<T>::value>::Identity();
}
return ChartValue<T>(value);
}
} /* namespace gtsam */

168
gtsam/base/GenericValue.h Normal file
View File

@ -0,0 +1,168 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/*
* @file GenericValue.h
* @brief Wraps any type T so it can play as a Value
* @date October, 2014
* @author Michael Bosse, Abel Gawel, Renaud Dube
* based on DerivedValue.h by Duy Nguyen Ta
*/
#pragma once
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Value.h>
#include <cmath>
#include <iostream>
namespace gtsam {
// To play as a GenericValue, we need the following traits
namespace traits {
// trait to wrap the default equals of types
template<typename T>
struct equals {
typedef T type;
typedef bool result_type;
bool operator()(const T& a, const T& b, double tol) {
return a.equals(b, tol);
}
};
// trait to wrap the default print of types
template<typename T>
struct print {
typedef T type;
typedef void result_type;
void operator()(const T& obj, const std::string& str) {
obj.print(str);
}
};
// equals for scalars
template<>
struct equals<double> {
typedef double type;
typedef bool result_type;
bool operator()(double a, double b, double tol) {
return std::abs(a - b) <= tol;
}
};
// print for scalars
template<>
struct print<double> {
typedef double type;
typedef void result_type;
void operator()(double a, const std::string& str) {
std::cout << str << ": " << a << std::endl;
}
};
// equals for Matrix types
template<int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
struct equals<Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> > {
typedef Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> type;
typedef bool result_type;
bool operator()(const type& A, const type& B, double tol) {
return equal_with_abs_tol(A, B, tol);
}
};
// print for Matrix types
template<int _Rows, int _Cols, int _Options, int _MaxRows, int _MaxCols>
struct print<Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> > {
typedef Eigen::Matrix<double, _Rows, _Cols, _Options, _MaxRows, _MaxCols> type;
typedef void result_type;
void operator()(const type& A, const std::string& str) {
std::cout << str << ": " << A << std::endl;
}
};
}
/**
* Wraps any type T so it can play as a Value
*/
template<class T>
class GenericValue: public Value {
public:
typedef T type;
protected:
T value_; ///< The wrapped value
public:
/// Construct from value
GenericValue(const T& value) :
value_(value) {
}
/// Return a constant value
const T& value() const {
return value_;
}
/// Return the value
T& value() {
return value_;
}
/// Destructor
virtual ~GenericValue() {
}
/// equals implementing generic Value interface
virtual bool equals_(const Value& p, double tol = 1e-9) const {
// Cast the base class Value pointer to a templated generic class pointer
const GenericValue& genericValue2 = static_cast<const GenericValue&>(p);
// Return the result of using the equals traits for the derived class
return traits::equals<T>()(this->value_, genericValue2.value_, tol);
}
/// non virtual equals function, uses traits
bool equals(const GenericValue &other, double tol = 1e-9) const {
return traits::equals<T>()(this->value(), other.value(), tol);
}
/// Virtual print function, uses traits
virtual void print(const std::string& str) const {
traits::print<T>()(value_, str);
}
// Serialization below:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value);
ar & BOOST_SERIALIZATION_NVP(value_);
}
protected:
// Assignment operator for this class not needed since GenericValue<T> is an abstract class
};
// define Value::cast here since now GenericValue has been declared
template<typename ValueType>
const ValueType& Value::cast() const {
return dynamic_cast<const GenericValue<ValueType>&>(*this).value();
}
} /* namespace gtsam */

18
gtsam/base/LieMatrix.h
View File

@ -29,7 +29,7 @@ namespace gtsam {
/** /**
* LieVector is a wrapper around vector to allow it to be a Lie type * LieVector is a wrapper around vector to allow it to be a Lie type
*/ */
struct LieMatrix : public Matrix, public DerivedValue<LieMatrix> { struct LieMatrix : public Matrix {
/// @name Constructors /// @name Constructors
/// @{ /// @{
@ -166,12 +166,24 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) { void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("LieMatrix",
boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("Matrix", ar & boost::serialization::make_nvp("Matrix",
boost::serialization::base_object<Matrix>(*this)); boost::serialization::base_object<Matrix>(*this));
} }
}; };
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<LieMatrix> : public boost::true_type {
};
template<>
struct dimension<LieMatrix> : public Dynamic {
};
}
} // \namespace gtsam } // \namespace gtsam

20
gtsam/base/LieScalar.h
View File

@ -26,7 +26,7 @@ namespace gtsam {
/** /**
* LieScalar is a wrapper around double to allow it to be a Lie type * LieScalar is a wrapper around double to allow it to be a Lie type
*/ */
struct GTSAM_EXPORT LieScalar : public DerivedValue<LieScalar> { struct GTSAM_EXPORT LieScalar {
/** default constructor */ /** default constructor */
LieScalar() : d_(0.0) {} LieScalar() : d_(0.0) {}
@ -111,4 +111,22 @@ namespace gtsam {
private: private:
double d_; double d_;
}; };
// Define GTSAM traits
namespace traits {
template<>
struct is_group<LieScalar> : public boost::true_type {
};
template<>
struct is_manifold<LieScalar> : public boost::true_type {
};
template<>
struct dimension<LieScalar> : public boost::integral_constant<int, 1> {
};
}
} // \namespace gtsam } // \namespace gtsam

19
gtsam/base/LieVector.h
View File

@ -26,7 +26,7 @@ namespace gtsam {
/** /**
* LieVector is a wrapper around vector to allow it to be a Lie type * LieVector is a wrapper around vector to allow it to be a Lie type
*/ */
struct LieVector : public Vector, public DerivedValue<LieVector> { struct LieVector : public Vector {
/** default constructor - should be unnecessary */ /** default constructor - should be unnecessary */
LieVector() {} LieVector() {}
@ -123,11 +123,22 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) { void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("LieVector",
boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("Vector", ar & boost::serialization::make_nvp("Vector",
boost::serialization::base_object<Vector>(*this)); boost::serialization::base_object<Vector>(*this));
} }
}; };
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<LieVector> : public boost::true_type {
};
template<>
struct dimension<LieVector> : public Dynamic {
};
}
} // \namespace gtsam } // \namespace gtsam

269
gtsam/base/Manifold.h
View File

@ -13,25 +13,19 @@
* @file Manifold.h * @file Manifold.h
* @brief Base class and basic functions for Manifold types * @brief Base class and basic functions for Manifold types
* @author Alex Cunningham * @author Alex Cunningham
* @author Frank Dellaert
*/ */
#pragma once #pragma once
#include <string>
#include <gtsam/base/Matrix.h> #include <gtsam/base/Matrix.h>
#include <boost/static_assert.hpp>
#include <boost/type_traits.hpp>
#include <string>
namespace gtsam { namespace gtsam {
/** /**
* Concept check class for Manifold types
* Requires a mapping between a linear tangent space and the underlying
* manifold, of which Lie is a specialization.
*
* The necessary functions to implement for Manifold are defined
* below with additional details as to the interface. The
* concept checking function in class Manifold will check whether or not
* the function exists and throw compile-time errors.
*
* A manifold defines a space in which there is a notion of a linear tangent space * A manifold defines a space in which there is a notion of a linear tangent space
* that can be centered around a given point on the manifold. These nonlinear * that can be centered around a given point on the manifold. These nonlinear
* spaces may have such properties as wrapping around (as is the case with rotations), * spaces may have such properties as wrapping around (as is the case with rotations),
@ -45,7 +39,256 @@ namespace gtsam {
* There may be multiple possible retractions for a given manifold, which can be chosen * There may be multiple possible retractions for a given manifold, which can be chosen
* between depending on the computational complexity. The important criteria for * between depending on the computational complexity. The important criteria for
* the creation for the retract and localCoordinates functions is that they be * the creation for the retract and localCoordinates functions is that they be
* inverse operations. * inverse operations. The new notion of a Chart guarantees that.
*
*/
// Traits, same style as Boost.TypeTraits
// All meta-functions below ever only declare a single type
// or a type/value/value_type
namespace traits {
// is group, by default this is false
template<typename T>
struct is_group: public boost::false_type {
};
// identity, no default provided, by default given by default constructor
template<typename T>
struct identity {
static T value() {
return T();
}
};
// is manifold, by default this is false
template<typename T>
struct is_manifold: public boost::false_type {
};
// dimension, can return Eigen::Dynamic (-1) if not known at compile time
// defaults to dynamic, TODO makes sense ?
typedef boost::integral_constant<int, Eigen::Dynamic> Dynamic;
template<typename T>
struct dimension: public Dynamic {
};
/**
* zero<T>::value is intended to be the origin of a canonical coordinate system
* with canonical(t) == DefaultChart<T>::local(zero<T>::value, t)
* Below we provide the group identity as zero *in case* it is a group
*/
template<typename T> struct zero: public identity<T> {
BOOST_STATIC_ASSERT(is_group<T>::value);
};
// double
template<>
struct is_group<double> : public boost::true_type {
};
template<>
struct is_manifold<double> : public boost::true_type {
};
template<>
struct dimension<double> : public boost::integral_constant<int, 1> {
};
template<>
struct zero<double> {
static double value() {
return 0;
}
};
// Fixed size Eigen::Matrix type
template<int M, int N, int Options>
struct is_group<Eigen::Matrix<double, M, N, Options> > : public boost::true_type {
};
template<int M, int N, int Options>
struct is_manifold<Eigen::Matrix<double, M, N, Options> > : public boost::true_type {
};
template<int M, int N, int Options>
struct dimension<Eigen::Matrix<double, M, N, Options> > : public boost::integral_constant<int,
M == Eigen::Dynamic ? Eigen::Dynamic : (N == Eigen::Dynamic ? Eigen::Dynamic : M * N)> {
//TODO after switch to c++11 : the above should should be extracted to a constexpr function
// for readability and to reduce code duplication
};
template<int M, int N, int Options>
struct zero<Eigen::Matrix<double, M, N, Options> > {
BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
"traits::zero is only supported for fixed-size matrices");
static Eigen::Matrix<double, M, N, Options> value() {
return Eigen::Matrix<double, M, N, Options>::Zero();
}
};
template<int M, int N, int Options>
struct identity<Eigen::Matrix<double, M, N, Options> > : public zero<Eigen::Matrix<double, M, N, Options> > {
};
template<typename T> struct is_chart: public boost::false_type {
};
} // \ namespace traits
// Chart is a map from T -> vector, retract is its inverse
template<typename T>
struct DefaultChart {
//BOOST_STATIC_ASSERT(traits::is_manifold<T>::value);
typedef T type;
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;
static vector local(const T& origin, const T& other) {
return origin.localCoordinates(other);
}
static T retract(const T& origin, const vector& d) {
return origin.retract(d);
}
static int getDimension(const T& origin) {
return origin.dim();
}
};
namespace traits {
// populate default traits
template<typename T> struct is_chart<DefaultChart<T> > : public boost::true_type {
};
template<typename T> struct dimension<DefaultChart<T> > : public dimension<T> {
};
}
template<class C>
struct ChartConcept {
public:
typedef typename C::type type;
typedef typename C::vector vector;
BOOST_CONCEPT_USAGE(ChartConcept) {
// is_chart trait should be true
BOOST_STATIC_ASSERT((traits::is_chart<C>::value));
/**
* Returns Retraction update of val_
*/
type retract_ret = C::retract(val_, vec_);
/**
* Returns local coordinates of another object
*/
vec_ = C::local(val_, retract_ret);
// a way to get the dimension that is compatible with dynamically sized types
dim_ = C::getDimension(val_);
}
private:
type val_;
vector vec_;
int dim_;
};
/**
* CanonicalChart<Chart<T> > is a chart around zero<T>::value
* Canonical<T> is CanonicalChart<DefaultChart<T> >
* An example is Canonical<Rot3>
*/
template<typename C> struct CanonicalChart {
BOOST_CONCEPT_ASSERT((ChartConcept<C>));
typedef C Chart;
typedef typename Chart::type type;
typedef typename Chart::vector vector;
// Convert t of type T into canonical coordinates
vector local(const type& t) {
return Chart::local(traits::zero<type>::value(), t);
}
// Convert back from canonical coordinates to T
type retract(const vector& v) {
return Chart::retract(traits::zero<type>::value(), v);
}
};
template<typename T> struct Canonical: public CanonicalChart<DefaultChart<T> > {
};
// double
template<>
struct DefaultChart<double> {
typedef double type;
typedef Eigen::Matrix<double, 1, 1> vector;
static vector local(double origin, double other) {
vector d;
d << other - origin;
return d;
}
static double retract(double origin, const vector& d) {
return origin + d[0];
}
static const int getDimension(double) {
return 1;
}
};
// Fixed size Eigen::Matrix type
template<int M, int N, int Options>
struct DefaultChart<Eigen::Matrix<double, M, N, Options> > {
/**
* This chart for the vector space of M x N matrices (represented by Eigen matrices) chooses as basis the one with respect to which the coordinates are exactly the matrix entries as laid out in memory (as determined by Options).
* Computing coordinates for a matrix is then simply a reshape to the row vector of appropriate size.
*/
typedef Eigen::Matrix<double, M, N, Options> type;
typedef type T;
typedef Eigen::Matrix<double, traits::dimension<T>::value, 1> vector;BOOST_STATIC_ASSERT_MSG((M!=Eigen::Dynamic && N!=Eigen::Dynamic),
"DefaultChart has not been implemented yet for dynamically sized matrices");
static vector local(const T& origin, const T& other) {
return reshape<vector::RowsAtCompileTime, 1, vector::Options>(other) - reshape<vector::RowsAtCompileTime, 1, vector::Options>(origin);
}
static T retract(const T& origin, const vector& d) {
return origin + reshape<M, N, Options>(d);
}
static int getDimension(const T&origin) {
return origin.rows() * origin.cols();
}
};
// Dynamically sized Vector
template<>
struct DefaultChart<Vector> {
typedef Vector T;
typedef T type;
typedef T vector;
static vector local(const T& origin, const T& other) {
return other - origin;
}
static T retract(const T& origin, const vector& d) {
return origin + d;
}
static int getDimension(const T& origin) {
return origin.size();
}
};
/**
* Old Concept check class for Manifold types
* Requires a mapping between a linear tangent space and the underlying
* manifold, of which Lie is a specialization.
*
* The necessary functions to implement for Manifold are defined
* below with additional details as to the interface. The
* concept checking function in class Manifold will check whether or not
* the function exists and throw compile-time errors.
* *
* Returns dimensionality of the tangent space, which may be smaller than the number * Returns dimensionality of the tangent space, which may be smaller than the number
* of nonlinear parameters. * of nonlinear parameters.
@ -61,7 +304,7 @@ namespace gtsam {
* By convention, we use capital letters to designate a static function * By convention, we use capital letters to designate a static function
* @tparam T is a Lie type, like Point2, Pose3, etc. * @tparam T is a Lie type, like Point2, Pose3, etc.
*/ */
template <class T> template<class T>
class ManifoldConcept { class ManifoldConcept {
private: private:
/** concept checking function - implement the functions this demands */ /** concept checking function - implement the functions this demands */
@ -89,7 +332,7 @@ private:
} }
}; };
} // namespace gtsam } // \ namespace gtsam
/** /**
* Macros for using the ManifoldConcept * Macros for using the ManifoldConcept

2
gtsam/base/Matrix.cpp
View File

@ -667,7 +667,7 @@ Matrix expm(const Matrix& A, size_t K) {
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Cayley(const Matrix& A) { Matrix Cayley(const Matrix& A) {
size_t n = A.cols(); Matrix::Index n = A.cols();
assert(A.rows() == n); assert(A.rows() == n);
// original // original

61
gtsam/base/Matrix.h
View File

@ -37,10 +37,28 @@ namespace gtsam {
typedef Eigen::MatrixXd Matrix; typedef Eigen::MatrixXd Matrix;
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor; typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
typedef Eigen::Matrix2d Matrix2;
typedef Eigen::Matrix3d Matrix3; typedef Eigen::Matrix3d Matrix3;
typedef Eigen::Matrix4d Matrix4; typedef Eigen::Matrix4d Matrix4;
typedef Eigen::Matrix<double,5,5> Matrix5;
typedef Eigen::Matrix<double,6,6> Matrix6; typedef Eigen::Matrix<double,6,6> Matrix6;
typedef Eigen::Matrix<double,2,3> Matrix23;
typedef Eigen::Matrix<double,2,4> Matrix24;
typedef Eigen::Matrix<double,2,5> Matrix25;
typedef Eigen::Matrix<double,2,6> Matrix26;
typedef Eigen::Matrix<double,2,7> Matrix27;
typedef Eigen::Matrix<double,2,8> Matrix28;
typedef Eigen::Matrix<double,2,9> Matrix29;
typedef Eigen::Matrix<double,3,2> Matrix32;
typedef Eigen::Matrix<double,3,4> Matrix34;
typedef Eigen::Matrix<double,3,5> Matrix35;
typedef Eigen::Matrix<double,3,6> Matrix36;
typedef Eigen::Matrix<double,3,7> Matrix37;
typedef Eigen::Matrix<double,3,8> Matrix38;
typedef Eigen::Matrix<double,3,9> Matrix39;
// Matrix expressions for accessing parts of matrices // Matrix expressions for accessing parts of matrices
typedef Eigen::Block<Matrix> SubMatrix; typedef Eigen::Block<Matrix> SubMatrix;
typedef Eigen::Block<const Matrix> ConstSubMatrix; typedef Eigen::Block<const Matrix> ConstSubMatrix;
@ -275,6 +293,49 @@ void zeroBelowDiagonal(MATRIX& A, size_t cols=0) {
*/ */
inline Matrix trans(const Matrix& A) { return A.transpose(); } inline Matrix trans(const Matrix& A) { return A.transpose(); }
/// Reshape functor
template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
struct Reshape {
//TODO replace this with Eigen's reshape function as soon as available. (There is a PR already pending : https://bitbucket.org/eigen/eigen/pull-request/41/reshape/diff)
typedef Eigen::Map<const Eigen::Matrix<double, OutM, OutN, OutOptions> > ReshapedType;
static inline ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & in) {
return in.data();
}
};
/// Reshape specialization that does nothing as shape stays the same (needed to not be ambiguous for square input equals square output)
template <int M, int InOptions>
struct Reshape<M, M, InOptions, M, M, InOptions> {
typedef const Eigen::Matrix<double, M, M, InOptions> & ReshapedType;
static inline ReshapedType reshape(const Eigen::Matrix<double, M, M, InOptions> & in) {
return in;
}
};
/// Reshape specialization that does nothing as shape stays the same
template <int M, int N, int InOptions>
struct Reshape<M, N, InOptions, M, N, InOptions> {
typedef const Eigen::Matrix<double, M, N, InOptions> & ReshapedType;
static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
return in;
}
};
/// Reshape specialization that does transpose
template <int M, int N, int InOptions>
struct Reshape<N, M, InOptions, M, N, InOptions> {
typedef typename Eigen::Matrix<double, M, N, InOptions>::ConstTransposeReturnType ReshapedType;
static inline ReshapedType reshape(const Eigen::Matrix<double, M, N, InOptions> & in) {
return in.transpose();
}
};
template <int OutM, int OutN, int OutOptions, int InM, int InN, int InOptions>
inline typename Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::ReshapedType reshape(const Eigen::Matrix<double, InM, InN, InOptions> & m){
BOOST_STATIC_ASSERT(InM * InN == OutM * OutN);
return Reshape<OutM, OutN, OutOptions, InM, InN, InOptions>::reshape(m);
}
/** /**
* solve AX=B via in-place Lu factorization and backsubstitution * solve AX=B via in-place Lu factorization and backsubstitution
* After calling, A contains LU, B the solved RHS vectors * After calling, A contains LU, B the solved RHS vectors

12
gtsam/base/Value.h
View File

@ -120,7 +120,17 @@ namespace gtsam {
virtual Vector localCoordinates_(const Value& value) const = 0; virtual Vector localCoordinates_(const Value& value) const = 0;
/** Assignment operator */ /** Assignment operator */
virtual Value& operator=(const Value& rhs) = 0; virtual Value& operator=(const Value& rhs) {
//needs a empty definition so recursion in implicit derived assignment operators work
return *this;
}
/** Cast to known ValueType */
template<typename ValueType>
const ValueType& cast() const;
template<typename Chart>
const Chart& getChart() const;
/** Virutal destructor */ /** Virutal destructor */
virtual ~Value() {} virtual ~Value() {}

5
gtsam/base/Vector.h
View File

@ -36,7 +36,12 @@ typedef Eigen::VectorXd Vector;
// Commonly used fixed size vectors // Commonly used fixed size vectors
typedef Eigen::Vector2d Vector2; typedef Eigen::Vector2d Vector2;
typedef Eigen::Vector3d Vector3; typedef Eigen::Vector3d Vector3;
typedef Eigen::Matrix<double, 4, 1> Vector4;
typedef Eigen::Matrix<double, 5, 1> Vector5;
typedef Eigen::Matrix<double, 6, 1> Vector6; typedef Eigen::Matrix<double, 6, 1> Vector6;
typedef Eigen::Matrix<double, 7, 1> Vector7;
typedef Eigen::Matrix<double, 8, 1> Vector8;
typedef Eigen::Matrix<double, 9, 1> Vector9;
typedef Eigen::VectorBlock<Vector> SubVector; typedef Eigen::VectorBlock<Vector> SubVector;
typedef Eigen::VectorBlock<const Vector> ConstSubVector; typedef Eigen::VectorBlock<const Vector> ConstSubVector;

41
gtsam/base/VerticalBlockMatrix.h
View File

@ -65,36 +65,39 @@ namespace gtsam {
/** Construct from a container of the sizes of each vertical block. */ /** Construct from a container of the sizes of each vertical block. */
template<typename CONTAINER> template<typename CONTAINER>
VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height, bool appendOneDimension = false) : VerticalBlockMatrix(const CONTAINER& dimensions, DenseIndex height,
rowStart_(0), rowEnd_(height), blockStart_(0) bool appendOneDimension = false) :
{ variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
rowStart_(0), rowEnd_(height), blockStart_(0) {
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
matrix_.resize(height, variableColOffsets_.back()); matrix_.resize(height, variableColOffsets_.back());
assertInvariants(); assertInvariants();
} }
/** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */ /** Construct from a container of the sizes of each vertical block and a pre-prepared matrix. */
template<typename CONTAINER> template<typename CONTAINER, typename DERIVED>
VerticalBlockMatrix(const CONTAINER& dimensions, const Matrix& matrix, bool appendOneDimension = false) : VerticalBlockMatrix(const CONTAINER& dimensions,
matrix_(matrix), rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) const Eigen::MatrixBase<DERIVED>& matrix, bool appendOneDimension = false) :
{ matrix_(matrix), variableColOffsets_(dimensions.size() + (appendOneDimension ? 2 : 1)),
rowStart_(0), rowEnd_(matrix.rows()), blockStart_(0) {
fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension); fillOffsets(dimensions.begin(), dimensions.end(), appendOneDimension);
if(variableColOffsets_.back() != matrix_.cols()) if (variableColOffsets_.back() != matrix_.cols())
throw std::invalid_argument("Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix."); throw std::invalid_argument(
"Requested to create a VerticalBlockMatrix with dimensions that do not sum to the total columns of the provided matrix.");
assertInvariants(); assertInvariants();
} }
/** /** Construct from iterator over the sizes of each vertical block. */
* Construct from iterator over the sizes of each vertical block. */
template<typename ITERATOR> template<typename ITERATOR>
VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim, DenseIndex height, bool appendOneDimension = false) : VerticalBlockMatrix(ITERATOR firstBlockDim, ITERATOR lastBlockDim,
rowStart_(0), rowEnd_(height), blockStart_(0) DenseIndex height, bool appendOneDimension = false) :
{ variableColOffsets_((lastBlockDim-firstBlockDim) + (appendOneDimension ? 2 : 1)),
rowStart_(0), rowEnd_(height), blockStart_(0) {
fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension); fillOffsets(firstBlockDim, lastBlockDim, appendOneDimension);
matrix_.resize(height, variableColOffsets_.back()); matrix_.resize(height, variableColOffsets_.back());
assertInvariants(); assertInvariants();
} }
/** Copy the block structure and resize the underlying matrix, but do not copy the matrix data. /** Copy the block structure and resize the underlying matrix, but do not copy the matrix data.
* If blockStart(), rowStart(), and/or rowEnd() have been modified, this copies the structure of * If blockStart(), rowStart(), and/or rowEnd() have been modified, this copies the structure of
* the corresponding matrix view. In the destination VerticalBlockView, blockStart() and * the corresponding matrix view. In the destination VerticalBlockView, blockStart() and
@ -203,18 +206,12 @@ namespace gtsam {
template<typename ITERATOR> template<typename ITERATOR>
void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) { void fillOffsets(ITERATOR firstBlockDim, ITERATOR lastBlockDim, bool appendOneDimension) {
variableColOffsets_.resize((lastBlockDim-firstBlockDim) + 1 + (appendOneDimension ? 1 : 0));
variableColOffsets_[0] = 0; variableColOffsets_[0] = 0;
DenseIndex j=0; DenseIndex j=0;
for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim) { for(ITERATOR dim=firstBlockDim; dim!=lastBlockDim; ++dim, ++j)
variableColOffsets_[j+1] = variableColOffsets_[j] + *dim; variableColOffsets_[j+1] = variableColOffsets_[j] + *dim;
++ j;
}
if(appendOneDimension) if(appendOneDimension)
{
variableColOffsets_[j+1] = variableColOffsets_[j] + 1; variableColOffsets_[j+1] = variableColOffsets_[j] + 1;
++ j;
}
} }
friend class SymmetricBlockMatrix; friend class SymmetricBlockMatrix;

1066
gtsam/base/numericalDerivative.h
View File

File diff suppressed because it is too large Load Diff

108
gtsam/base/tests/testNumericalDerivative.cpp
View File

@ -15,115 +15,123 @@
* @date Apr 8, 2011 * @date Apr 8, 2011
*/ */
#include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/numericalDerivative.h> using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
double f(const LieVector& x) { double f(const Vector2& x) {
assert(x.size() == 2); assert(x.size() == 2);
return sin(x(0)) + cos(x(1)); return sin(x(0)) + cos(x(1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(testNumericalDerivative, numericalHessian) { //
LieVector center = ones(2); TEST(testNumericalDerivative, numericalGradient) {
Vector2 x(1, 1);
Matrix expected = (Matrix(2,2) << Vector expected(2);
-sin(center(0)), 0.0, expected << cos(x(1)), -sin(x(0));
0.0, -cos(center(1)));
Matrix actual = numericalHessian(f, center); Vector actual = numericalGradient<Vector2>(f, x);
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
double f2(const LieVector& x) { TEST(testNumericalDerivative, numericalHessian) {
Vector2 x(1, 1);
Matrix expected(2, 2);
expected << -sin(x(0)), 0.0, 0.0, -cos(x(1));
Matrix actual = numericalHessian<Vector2>(f, x);
EXPECT(assert_equal(expected, actual, 1e-5));
}
/* ************************************************************************* */
double f2(const Vector2& x) {
assert(x.size() == 2); assert(x.size() == 2);
return sin(x(0)) * cos(x(1)); return sin(x(0)) * cos(x(1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
//
TEST(testNumericalDerivative, numericalHessian2) { TEST(testNumericalDerivative, numericalHessian2) {
Vector v_center = (Vector(2) << 0.5, 1.0); Vector2 v(0.5, 1.0);
LieVector center(v_center); Vector2 x(v);
Matrix expected = (Matrix(2,2) << Matrix expected = (Matrix(2, 2) << -cos(x(1)) * sin(x(0)), -sin(x(1))
-cos(center(1))*sin(center(0)), -sin(center(1))*cos(center(0)), * cos(x(0)), -cos(x(0)) * sin(x(1)), -sin(x(0)) * cos(x(1)));
-cos(center(0))*sin(center(1)), -sin(center(0))*cos(center(1)));
Matrix actual = numericalHessian(f2, center); Matrix actual = numericalHessian(f2, x);
EXPECT(assert_equal(expected, actual, 1e-5)); EXPECT(assert_equal(expected, actual, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
double f3(const LieVector& x1, const LieVector& x2) { double f3(double x1, double x2) {
assert(x1.size() == 1 && x2.size() == 1); return sin(x1) * cos(x2);
return sin(x1(0)) * cos(x2(0));
} }
/* ************************************************************************* */ /* ************************************************************************* */
//
TEST(testNumericalDerivative, numericalHessian211) { TEST(testNumericalDerivative, numericalHessian211) {
Vector v_center1 = (Vector(1) << 1.0); double x1 = 1, x2 = 5;
Vector v_center2 = (Vector(1) << 5.0);
LieVector center1(v_center1), center2(v_center2);
Matrix expected11 = (Matrix(1, 1) << -sin(center1(0))*cos(center2(0))); Matrix expected11 = (Matrix(1, 1) << -sin(x1) * cos(x2));
Matrix actual11 = numericalHessian211(f3, center1, center2); Matrix actual11 = numericalHessian211<double, double>(f3, x1, x2);
EXPECT(assert_equal(expected11, actual11, 1e-5)); EXPECT(assert_equal(expected11, actual11, 1e-5));
Matrix expected12 = (Matrix(1, 1) <<-cos(center1(0))*sin(center2(0))); Matrix expected12 = (Matrix(1, 1) << -cos(x1) * sin(x2));
Matrix actual12 = numericalHessian212(f3, center1, center2); Matrix actual12 = numericalHessian212<double, double>(f3, x1, x2);
EXPECT(assert_equal(expected12, actual12, 1e-5)); EXPECT(assert_equal(expected12, actual12, 1e-5));
Matrix expected22 = (Matrix(1, 1) <<-sin(center1(0))*cos(center2(0))); Matrix expected22 = (Matrix(1, 1) << -sin(x1) * cos(x2));
Matrix actual22 = numericalHessian222(f3, center1, center2); Matrix actual22 = numericalHessian222<double, double>(f3, x1, x2);
EXPECT(assert_equal(expected22, actual22, 1e-5)); EXPECT(assert_equal(expected22, actual22, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
double f4(const LieVector& x, const LieVector& y, const LieVector& z) { double f4(double x, double y, double z) {
assert(x.size() == 1 && y.size() == 1 && z.size() == 1); return sin(x) * cos(y) * z * z;
return sin(x(0)) * cos(y(0)) * z(0)*z(0);
} }
/* ************************************************************************* */ /* ************************************************************************* */
//
TEST(testNumericalDerivative, numericalHessian311) { TEST(testNumericalDerivative, numericalHessian311) {
Vector v_center1 = (Vector(1) << 1.0); double x = 1, y = 2, z = 3;
Vector v_center2 = (Vector(1) << 2.0); Matrix expected11 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z);
Vector v_center3 = (Vector(1) << 3.0); Matrix actual11 = numericalHessian311<double, double, double>(f4, x, y, z);
LieVector center1(v_center1), center2(v_center2), center3(v_center3);
double x = center1(0), y = center2(0), z = center3(0);
Matrix expected11 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z);
Matrix actual11 = numericalHessian311(f4, center1, center2, center3);
EXPECT(assert_equal(expected11, actual11, 1e-5)); EXPECT(assert_equal(expected11, actual11, 1e-5));
Matrix expected12 = (Matrix(1, 1) << -cos(x)*sin(y)*z*z); Matrix expected12 = (Matrix(1, 1) << -cos(x) * sin(y) * z * z);
Matrix actual12 = numericalHessian312(f4, center1, center2, center3); Matrix actual12 = numericalHessian312<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected12, actual12, 1e-5)); EXPECT(assert_equal(expected12, actual12, 1e-5));
Matrix expected13 = (Matrix(1, 1) << cos(x)*cos(y)*2*z); Matrix expected13 = (Matrix(1, 1) << cos(x) * cos(y) * 2 * z);
Matrix actual13 = numericalHessian313(f4, center1, center2, center3); Matrix actual13 = numericalHessian313<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected13, actual13, 1e-5)); EXPECT(assert_equal(expected13, actual13, 1e-5));
Matrix expected22 = (Matrix(1, 1) << -sin(x)*cos(y)*z*z); Matrix expected22 = (Matrix(1, 1) << -sin(x) * cos(y) * z * z);
Matrix actual22 = numericalHessian322(f4, center1, center2, center3); Matrix actual22 = numericalHessian322<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected22, actual22, 1e-5)); EXPECT(assert_equal(expected22, actual22, 1e-5));
Matrix expected23 = (Matrix(1, 1) << -sin(x)*sin(y)*2*z); Matrix expected23 = (Matrix(1, 1) << -sin(x) * sin(y) * 2 * z);
Matrix actual23 = numericalHessian323(f4, center1, center2, center3); Matrix actual23 = numericalHessian323<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected23, actual23, 1e-5)); EXPECT(assert_equal(expected23, actual23, 1e-5));
Matrix expected33 = (Matrix(1, 1) << sin(x)*cos(y)*2); Matrix expected33 = (Matrix(1, 1) << sin(x) * cos(y) * 2);
Matrix actual33 = numericalHessian333(f4, center1, center2, center3); Matrix actual33 = numericalHessian333<double, double, double>(f4, x, y, z);
EXPECT(assert_equal(expected33, actual33, 1e-5)); EXPECT(assert_equal(expected33, actual33, 1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); } int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */ /* ************************************************************************* */

23
gtsam/base/tests/testVerticalBlockMatrix.cpp
View File

@ -24,9 +24,20 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
using boost::assign::list_of; using boost::assign::list_of;
list<size_t> L = list_of(3)(2)(1);
vector<size_t> dimensions(L.begin(),L.end());
//***************************************************************************** //*****************************************************************************
TEST(VerticalBlockMatrix, constructor) { TEST(VerticalBlockMatrix, Constructor1) {
VerticalBlockMatrix actual(list_of(3)(2)(1), VerticalBlockMatrix actual(dimensions,6);
EXPECT_LONGS_EQUAL(6,actual.rows());
EXPECT_LONGS_EQUAL(6,actual.cols());
EXPECT_LONGS_EQUAL(3,actual.nBlocks());
}
//*****************************************************************************
TEST(VerticalBlockMatrix, Constructor2) {
VerticalBlockMatrix actual(dimensions,
(Matrix(6, 6) << 1, 2, 3, 4, 5, 6, // (Matrix(6, 6) << 1, 2, 3, 4, 5, 6, //
2, 8, 9, 10, 11, 12, // 2, 8, 9, 10, 11, 12, //
3, 9, 15, 16, 17, 18, // 3, 9, 15, 16, 17, 18, //
@ -38,6 +49,14 @@ TEST(VerticalBlockMatrix, constructor) {
EXPECT_LONGS_EQUAL(3,actual.nBlocks()); EXPECT_LONGS_EQUAL(3,actual.nBlocks());
} }
//*****************************************************************************
TEST(VerticalBlockMatrix, Constructor3) {
VerticalBlockMatrix actual(dimensions.begin(),dimensions.end(),6);
EXPECT_LONGS_EQUAL(6,actual.rows());
EXPECT_LONGS_EQUAL(6,actual.cols());
EXPECT_LONGS_EQUAL(3,actual.nBlocks());
}
//***************************************************************************** //*****************************************************************************
int main() { int main() {
TestResult tr; TestResult tr;

43
gtsam/geometry/Cal3Bundler.cpp
View File

@ -46,7 +46,7 @@ Vector Cal3Bundler::k() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Cal3Bundler::vector() const { Vector3 Cal3Bundler::vector() const {
return (Vector(3) << f_, k1_, k2_); return (Vector(3) << f_, k1_, k2_);
} }
@ -64,6 +64,45 @@ bool Cal3Bundler::equals(const Cal3Bundler& K, double tol) const {
return true; return true;
} }
/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p) const {
// r = x^2 + y^2;
// g = (1 + k(1)*r + k(2)*r^2);
// pi(:,i) = g * pn(:,i)
const double x = p.x(), y = p.y();
const double r = x * x + y * y;
const double g = 1. + (k1_ + k2_ * r) * r;
const double u = g * x, v = g * y;
return Point2(u0_ + f_ * u, v0_ + f_ * v);
}
/* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p, //
boost::optional<Matrix23&> Dcal, boost::optional<Matrix2&> Dp) const {
// r = x^2 + y^2;
// g = (1 + k(1)*r + k(2)*r^2);
// pi(:,i) = g * pn(:,i)
const double x = p.x(), y = p.y();
const double r = x * x + y * y;
const double g = 1. + (k1_ + k2_ * r) * r;
const double u = g * x, v = g * y;
// Derivatives make use of intermediate variables above
if (Dcal) {
double rx = r * x, ry = r * y;
*Dcal << u, f_ * rx, f_ * r * rx, v, f_ * ry, f_ * r * ry;
}
if (Dp) {
const double a = 2. * (k1_ + 2. * k2_ * r);
const double axx = a * x * x, axy = a * x * y, ayy = a * y * y;
*Dp << g + axx, axy, axy, g + ayy;
*Dp *= f_;
}
return Point2(u0_ + f_ * u, v0_ + f_ * v);
}
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3Bundler::uncalibrate(const Point2& p, // Point2 Cal3Bundler::uncalibrate(const Point2& p, //
boost::optional<Matrix&> Dcal, boost::optional<Matrix&> Dp) const { boost::optional<Matrix&> Dcal, boost::optional<Matrix&> Dp) const {
@ -150,7 +189,7 @@ Cal3Bundler Cal3Bundler::retract(const Vector& d) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const { Vector3 Cal3Bundler::localCoordinates(const Cal3Bundler& T2) const {
return T2.vector() - vector(); return T2.vector() - vector();
} }

54
gtsam/geometry/Cal3Bundler.h
View File

@ -28,7 +28,7 @@ namespace gtsam {
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT Cal3Bundler: public DerivedValue<Cal3Bundler> { class GTSAM_EXPORT Cal3Bundler {
private: private:
double f_; ///< focal length double f_; ///< focal length
@ -72,7 +72,7 @@ public:
Matrix K() const; ///< Standard 3*3 calibration matrix Matrix K() const; ///< Standard 3*3 calibration matrix
Vector k() const; ///< Radial distortion parameters (4 of them, 2 0) Vector k() const; ///< Radial distortion parameters (4 of them, 2 0)
Vector vector() const; Vector3 vector() const;
/// focal length x /// focal length x
inline double fx() const { inline double fx() const {
@ -108,12 +108,29 @@ public:
/** /**
* convert intrinsic coordinates xy to image coordinates uv * convert intrinsic coordinates xy to image coordinates uv
* @param p point in intrinsic coordinates * @param p point in intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p) const;
/**
* Version of uncalibrate with fixed derivatives
* @param p point in intrinsic coordinates
* @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters * @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates * @return point in image coordinates
*/ */
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal = Point2 uncalibrate(const Point2& p, boost::optional<Matrix23&> Dcal,
boost::none, boost::optional<Matrix&> Dp = boost::none) const; boost::optional<Matrix2&> Dp) const;
/**
* Version of uncalibrate with dynamic derivatives
* @param p point in intrinsic coordinates
* @param Dcal optional 2*3 Jacobian wrpt CalBundler parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const;
/// Conver a pixel coordinate to ideal coordinate /// Conver a pixel coordinate to ideal coordinate
Point2 calibrate(const Point2& pi, const double tol = 1e-5) const; Point2 calibrate(const Point2& pi, const double tol = 1e-5) const;
@ -135,7 +152,7 @@ public:
Cal3Bundler retract(const Vector& d) const; Cal3Bundler retract(const Vector& d) const;
/// Calculate local coordinates to another calibration /// Calculate local coordinates to another calibration
Vector localCoordinates(const Cal3Bundler& T2) const; Vector3 localCoordinates(const Cal3Bundler& T2) const;
/// dimensionality /// dimensionality
virtual size_t dim() const { virtual size_t dim() const {
@ -157,9 +174,6 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) { void serialize(Archive & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("Cal3Bundler",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(f_); ar & BOOST_SERIALIZATION_NVP(f_);
ar & BOOST_SERIALIZATION_NVP(k1_); ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_); ar & BOOST_SERIALIZATION_NVP(k2_);
@ -169,6 +183,26 @@ private:
/// @} /// @}
}; };
} // namespace gtsam // Define GTSAM traits
namespace traits {
template<>
struct is_manifold<Cal3Bundler> : public boost::true_type {
};
template<>
struct dimension<Cal3Bundler> : public boost::integral_constant<int, 3> {
};
template<>
struct zero<Cal3Bundler> {
static Cal3Bundler value() {
return Cal3Bundler(0, 0, 0);
}
};
}
} // namespace gtsam

18
gtsam/geometry/Cal3DS2.h
View File

@ -37,7 +37,7 @@ namespace gtsam {
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn * pi = K*pn
*/ */
class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue<Cal3DS2> { class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base {
typedef Cal3DS2_Base Base; typedef Cal3DS2_Base Base;
@ -87,6 +87,7 @@ public:
/// Return dimensions of calibration manifold object /// Return dimensions of calibration manifold object
static size_t Dim() { return 9; } //TODO: make a final dimension variable static size_t Dim() { return 9; } //TODO: make a final dimension variable
private: private:
/// @} /// @}
@ -98,8 +99,6 @@ private:
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) void serialize(Archive & ar, const unsigned int version)
{ {
ar & boost::serialization::make_nvp("Cal3DS2",
boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("Cal3DS2", ar & boost::serialization::make_nvp("Cal3DS2",
boost::serialization::base_object<Cal3DS2_Base>(*this)); boost::serialization::base_object<Cal3DS2_Base>(*this));
} }
@ -108,5 +107,18 @@ private:
}; };
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<Cal3DS2> : public boost::true_type {
};
template<>
struct dimension<Cal3DS2> : public boost::integral_constant<int, 9> {
};
}
} }

47
gtsam/geometry/Cal3DS2_Base.cpp
View File

@ -53,23 +53,23 @@ bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Eigen::Matrix<double, 2, 9> D2dcalibration(double x, double y, double xx, static Matrix29 D2dcalibration(double x, double y, double xx,
double yy, double xy, double rr, double r4, double pnx, double pny, double yy, double xy, double rr, double r4, double pnx, double pny,
const Eigen::Matrix<double, 2, 2>& DK) { const Matrix2& DK) {
Eigen::Matrix<double, 2, 5> DR1; Matrix25 DR1;
DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0;
Eigen::Matrix<double, 2, 4> DR2; Matrix24 DR2;
DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, //
y * rr, y * r4, rr + 2 * yy, 2 * xy; y * rr, y * r4, rr + 2 * yy, 2 * xy;
Eigen::Matrix<double, 2, 9> D; Matrix29 D;
D << DR1, DK * DR2; D << DR1, DK * DR2;
return D; return D;
} }
/* ************************************************************************* */ /* ************************************************************************* */
static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr, static Matrix2 D2dintrinsic(double x, double y, double rr,
double g, double k1, double k2, double p1, double p2, double g, double k1, double k2, double p1, double p2,
const Eigen::Matrix<double, 2, 2>& DK) { const Matrix2& DK) {
const double drdx = 2. * x; const double drdx = 2. * x;
const double drdy = 2. * y; const double drdy = 2. * y;
const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; const double dgdx = k1 * drdx + k2 * 2. * rr * drdx;
@ -82,7 +82,7 @@ static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
const double dDydx = 2. * p2 * y + p1 * drdx; const double dDydx = 2. * p2 * y + p1 * drdx;
const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y);
Eigen::Matrix<double, 2, 2> DR; Matrix2 DR;
DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, //
y * dgdx + dDydx, g + y * dgdy + dDydy; y * dgdx + dDydx, g + y * dgdy + dDydy;
@ -90,8 +90,9 @@ static Eigen::Matrix<double, 2, 2> D2dintrinsic(double x, double y, double rr,
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1, Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
boost::optional<Matrix&> H2) const { boost::optional<Matrix29&> H1,
boost::optional<Matrix2&> H2) const {
// rr = x^2 + y^2; // rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2); // g = (1 + k(1)*rr + k(2)*rr^2);
@ -110,7 +111,7 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
const double pnx = g * x + dx; const double pnx = g * x + dx;
const double pny = g * y + dy; const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK; Matrix2 DK;
if (H1 || H2) DK << fx_, s_, 0.0, fy_; if (H1 || H2) DK << fx_, s_, 0.0, fy_;
// Derivative for calibration // Derivative for calibration
@ -125,6 +126,26 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional<Matrix&> H1,
return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
} }
/* ************************************************************************* */
Point2 Cal3DS2_Base::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const {
if (H1 || H2) {
Matrix29 D1;
Matrix2 D2;
Point2 pu = uncalibrate(p, D1, D2);
if (H1)
*H1 = D1;
if (H2)
*H2 = D2;
return pu;
} else {
return uncalibrate(p);
}
}
/* ************************************************************************* */ /* ************************************************************************* */
Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const {
// Use the following fixed point iteration to invert the radial distortion. // Use the following fixed point iteration to invert the radial distortion.
@ -161,7 +182,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const {
const double rr = xx + yy; const double rr = xx + yy;
const double r4 = rr * rr; const double r4 = rr * rr;
const double g = (1 + k1_ * rr + k2_ * r4); const double g = (1 + k1_ * rr + k2_ * r4);
Eigen::Matrix<double, 2, 2> DK; Matrix2 DK;
DK << fx_, s_, 0.0, fy_; DK << fx_, s_, 0.0, fy_;
return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK);
} }
@ -176,7 +197,7 @@ Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const {
const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy);
const double pnx = g * x + dx; const double pnx = g * x + dx;
const double pny = g * y + dy; const double pny = g * y + dy;
Eigen::Matrix<double, 2, 2> DK; Matrix2 DK;
DK << fx_, s_, 0.0, fy_; DK << fx_, s_, 0.0, fy_;
return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK);
} }

11
gtsam/geometry/Cal3DS2_Base.h
View File

@ -18,7 +18,6 @@
#pragma once #pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
namespace gtsam { namespace gtsam {
@ -114,9 +113,14 @@ public:
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in (distorted) image coordinates * @return point in (distorted) image coordinates
*/ */
Point2 uncalibrate(const Point2& p, Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal = boost::none, boost::optional<Matrix29&> Dcal = boost::none,
boost::optional<Matrix&> Dp = boost::none) const ; boost::optional<Matrix2&> Dp = boost::none) const ;
Point2 uncalibrate(const Point2& p,
boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const ;
/// Convert (distorted) image coordinates uv to intrinsic coordinates xy /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const; Point2 calibrate(const Point2& p, const double tol=1e-5) const;
@ -127,7 +131,6 @@ public:
/// Derivative of uncalibrate wrpt the calibration parameters /// Derivative of uncalibrate wrpt the calibration parameters
Matrix D2d_calibration(const Point2& p) const ; Matrix D2d_calibration(const Point2& p) const ;
private: private:
/// @} /// @}

21
gtsam/geometry/Cal3Unified.cpp
View File

@ -50,6 +50,7 @@ bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
// todo: make a fixed sized jacobian version of this
Point2 Cal3Unified::uncalibrate(const Point2& p, Point2 Cal3Unified::uncalibrate(const Point2& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {
@ -70,26 +71,30 @@ Point2 Cal3Unified::uncalibrate(const Point2& p,
// Part2: project NPlane point to pixel plane: use Cal3DS2 // Part2: project NPlane point to pixel plane: use Cal3DS2
Point2 m(x,y); Point2 m(x,y);
Matrix H1base, H2base; // jacobians from Base class Matrix29 H1base;
Matrix2 H2base; // jacobians from Base class
Point2 puncalib = Base::uncalibrate(m, H1base, H2base); Point2 puncalib = Base::uncalibrate(m, H1base, H2base);
// Inlined derivative for calibration // Inlined derivative for calibration
if (H1) { if (H1) {
// part1 // part1
Matrix DU = (Matrix(2,1) << -xs * sqrt_nx * xi_sqrt_nx2, Vector2 DU;
-ys * sqrt_nx * xi_sqrt_nx2); DU << -xs * sqrt_nx * xi_sqrt_nx2, //
Matrix DDS2U = H2base * DU; -ys * sqrt_nx * xi_sqrt_nx2;
*H1 = collect(2, &H1base, &DDS2U); H1->resize(2,10);
H1->block<2,9>(0,0) = H1base;
H1->block<2,1>(0,9) = H2base * DU;
} }
// Inlined derivative for points // Inlined derivative for points
if (H2) { if (H2) {
// part1 // part1
const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx; const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx;
const double mid = -(xi * xs*ys) * denom; const double mid = -(xi * xs*ys) * denom;
Matrix DU = (Matrix(2, 2) << Matrix2 DU;
(sqrt_nx + xi*(ys*ys + 1)) * denom, mid, DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, //
mid, (sqrt_nx + xi*(xs*xs + 1)) * denom); mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;
*H2 = H2base * DU; *H2 = H2base * DU;
} }

31
gtsam/geometry/Cal3Unified.h
View File

@ -40,7 +40,7 @@ namespace gtsam {
* k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
* pi = K*pn * pi = K*pn
*/ */
class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue<Cal3Unified> { class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base {
typedef Cal3Unified This; typedef Cal3Unified This;
typedef Cal3DS2_Base Base; typedef Cal3DS2_Base Base;
@ -50,9 +50,8 @@ private:
double xi_; // mirror parameter double xi_; // mirror parameter
public: public:
//Matrix K() const ;
//Eigen::Vector4d k() const { return Base::k(); } Vector vector() const ;
Vector vector() const ;
/// @name Standard Constructors /// @name Standard Constructors
/// @{ /// @{
@ -125,25 +124,35 @@ public:
private: private:
/// @}
/// @name Advanced Interface
/// @{
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) void serialize(Archive & ar, const unsigned int version)
{ {
ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Value>(*this));
ar & boost::serialization::make_nvp("Cal3Unified", ar & boost::serialization::make_nvp("Cal3Unified",
boost::serialization::base_object<Cal3DS2_Base>(*this)); boost::serialization::base_object<Cal3DS2_Base>(*this));
ar & BOOST_SERIALIZATION_NVP(xi_); ar & BOOST_SERIALIZATION_NVP(xi_);
} }
/// @} };
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<Cal3Unified> : public boost::true_type {
};
template<>
struct dimension<Cal3Unified> : public boost::integral_constant<int, 10> {
};
template<>
struct zero<Cal3Unified> {
static Cal3Unified value() { return Cal3Unified();}
}; };
} }
}

27
gtsam/geometry/Cal3_S2.cpp
View File

@ -75,10 +75,29 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const {
Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal, Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const { boost::optional<Matrix&> Dp) const {
const double x = p.x(), y = p.y(); const double x = p.x(), y = p.y();
if (Dcal) if (Dcal) {
*Dcal = (Matrix(2, 5) << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0); Dcal->resize(2,5);
if (Dp) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
*Dp = (Matrix(2, 2) << fx_, s_, 0.000, fy_); }
if (Dp) {
Dp->resize(2,2);
*Dp << fx_, s_, 0.0, fy_;
}
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}
/* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
boost::optional<Matrix2&> Dp) const {
const double x = p.x(), y = p.y();
if (Dcal) *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0;
if (Dp) *Dp << fx_, s_, 0.0, fy_;
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
}
/* ************************************************************************* */
Point2 Cal3_S2::uncalibrate(const Point2& p) const {
const double x = p.x(), y = p.y();
return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_);
} }

44
gtsam/geometry/Cal3_S2.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT Cal3_S2: public DerivedValue<Cal3_S2> { class GTSAM_EXPORT Cal3_S2 {
private: private:
double fx_, fy_, s_, u0_, v0_; double fx_, fy_, s_, u0_, v0_;
@ -144,12 +144,29 @@ public:
/** /**
* convert intrinsic coordinates xy to image coordinates uv * convert intrinsic coordinates xy to image coordinates uv
* @param p point in intrinsic coordinates * @param p point in intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p) const;
/**
* convert intrinsic coordinates xy to image coordinates uv, fixed derivaitves
* @param p point in intrinsic coordinates
* @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates * @return point in image coordinates
*/ */
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal = Point2 uncalibrate(const Point2& p, boost::optional<Matrix25&> Dcal,
boost::none, boost::optional<Matrix&> Dp = boost::none) const; boost::optional<Matrix2&> Dp) const;
/**
* convert intrinsic coordinates xy to image coordinates uv, dynamic derivaitves
* @param p point in intrinsic coordinates
* @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
* @return point in image coordinates
*/
Point2 uncalibrate(const Point2& p, boost::optional<Matrix&> Dcal,
boost::optional<Matrix&> Dp) const;
/** /**
* convert image coordinates uv to intrinsic coordinates xy * convert image coordinates uv to intrinsic coordinates xy
@ -209,9 +226,6 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) { void serialize(Archive & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("Cal3_S2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_); ar & BOOST_SERIALIZATION_NVP(s_);
@ -223,4 +237,22 @@ private:
}; };
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<Cal3_S2> : public boost::true_type {
};
template<>
struct dimension<Cal3_S2> : public boost::integral_constant<int, 5> {
};
template<>
struct zero<Cal3_S2> {
static Cal3_S2 value() { return Cal3_S2();}
};
}
} // \ namespace gtsam } // \ namespace gtsam

26
gtsam/geometry/CalibratedCamera.h
View File

@ -39,7 +39,7 @@ public:
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT CalibratedCamera: public DerivedValue<CalibratedCamera> { class GTSAM_EXPORT CalibratedCamera {
private: private:
Pose3 pose_; // 6DOF pose Pose3 pose_; // 6DOF pose
@ -214,12 +214,28 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) { void serialize(Archive & ar, const unsigned int version) {
ar
& boost::serialization::make_nvp("CalibratedCamera",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(pose_); ar & BOOST_SERIALIZATION_NVP(pose_);
} }
/// @} /// @}
};} };
// Define GTSAM traits
namespace traits {
template<>
struct is_group<CalibratedCamera> : public boost::true_type {
};
template<>
struct is_manifold<CalibratedCamera> : public boost::true_type {
};
template<>
struct dimension<CalibratedCamera> : public boost::integral_constant<int, 6> {
};
}
}

24
gtsam/geometry/EssentialMatrix.h
View File

@ -20,7 +20,7 @@ namespace gtsam {
* but here we choose instead to parameterize it as a (Rot3,Unit3) pair. * but here we choose instead to parameterize it as a (Rot3,Unit3) pair.
* We can then non-linearly optimize immediately on this 5-dimensional manifold. * We can then non-linearly optimize immediately on this 5-dimensional manifold.
*/ */
class GTSAM_EXPORT EssentialMatrix: public DerivedValue<EssentialMatrix> { class GTSAM_EXPORT EssentialMatrix {
private: private:
@ -58,6 +58,8 @@ public:
return EssentialMatrix(Rot3::Random(rng), Unit3::Random(rng)); return EssentialMatrix(Rot3::Random(rng), Unit3::Random(rng));
} }
virtual ~EssentialMatrix() {}
/// @} /// @}
/// @name Testable /// @name Testable
@ -176,8 +178,6 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("EssentialMatrix",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(aRb_); ar & BOOST_SERIALIZATION_NVP(aRb_);
ar & BOOST_SERIALIZATION_NVP(aTb_); ar & BOOST_SERIALIZATION_NVP(aTb_);
@ -196,5 +196,23 @@ private:
}; };
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<EssentialMatrix> : public boost::true_type {
};
template<>
struct dimension<EssentialMatrix> : public boost::integral_constant<int, 5> {
};
template<>
struct zero<EssentialMatrix> {
static EssentialMatrix value() { return EssentialMatrix();}
};
}
} // gtsam } // gtsam

183
gtsam/geometry/PinholeCamera.h
View File

@ -37,11 +37,13 @@ namespace gtsam {
* \nosubgrouping * \nosubgrouping
*/ */
template<typename Calibration> template<typename Calibration>
class PinholeCamera: public DerivedValue<PinholeCamera<Calibration> > { class PinholeCamera {
private: private:
Pose3 pose_; Pose3 pose_;
Calibration K_; Calibration K_;
static const int DimK = traits::dimension<Calibration>::value;
public: public:
/// @name Standard Constructors /// @name Standard Constructors
@ -240,12 +242,13 @@ public:
calibration().retract(d.tail(calibration().dim()))); calibration().retract(d.tail(calibration().dim())));
} }
typedef Eigen::Matrix<double,6+DimK,1> VectorK6;
/// return canonical coordinate /// return canonical coordinate
Vector localCoordinates(const PinholeCamera& T2) const { VectorK6 localCoordinates(const PinholeCamera& T2) const {
Vector d(dim()); VectorK6 d; // TODO: why does d.head<6>() not compile??
d.head(pose().dim()) = pose().localCoordinates(T2.pose()); d.head(6) = pose().localCoordinates(T2.pose());
d.tail(calibration().dim()) = calibration().localCoordinates( d.tail(DimK) = calibration().localCoordinates(T2.calibration());
T2.calibration());
return d; return d;
} }
@ -270,16 +273,15 @@ public:
* @param Dpoint is the 2*3 Jacobian w.r.t. P * @param Dpoint is the 2*3 Jacobian w.r.t. P
*/ */
inline static Point2 project_to_camera(const Point3& P, inline static Point2 project_to_camera(const Point3& P,
boost::optional<Matrix&> Dpoint = boost::none) { boost::optional<Matrix23&> Dpoint = boost::none) {
#ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION #ifdef GTSAM_THROW_CHEIRALITY_EXCEPTION
if (P.z() <= 0) if (P.z() <= 0)
throw CheiralityException(); throw CheiralityException();
#endif #endif
double d = 1.0 / P.z(); double d = 1.0 / P.z();
const double u = P.x() * d, v = P.y() * d; const double u = P.x() * d, v = P.y() * d;
if (Dpoint) { if (Dpoint)
*Dpoint = (Matrix(2, 3) << d, 0.0, -u * d, 0.0, d, -v * d); *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d;
}
return Point2(u, v); return Point2(u, v);
} }
@ -290,6 +292,22 @@ public:
return std::make_pair(K_.uncalibrate(pn), pc.z() > 0); return std::make_pair(K_.uncalibrate(pn), pc.z() > 0);
} }
/** project a point from world coordinate to the image
* @param pw is a point in world coordinates
*/
inline Point2 project(const Point3& pw) const {
// Transform to camera coordinates and check cheirality
const Point3 pc = pose_.transform_to(pw);
// Project to normalized image coordinates
const Point2 pn = project_to_camera(pc);
return K_.uncalibrate(pn);
}
typedef Eigen::Matrix<double,2,DimK> Matrix2K;
/** project a point from world coordinate to the image /** project a point from world coordinate to the image
* @param pw is a point in world coordinates * @param pw is a point in world coordinates
* @param Dpose is the Jacobian w.r.t. pose3 * @param Dpose is the Jacobian w.r.t. pose3
@ -298,9 +316,44 @@ public:
*/ */
inline Point2 project( inline Point2 project(
const Point3& pw, // const Point3& pw, //
boost::optional<Matrix&> Dpose = boost::none, boost::optional<Matrix26&> Dpose,
boost::optional<Matrix&> Dpoint = boost::none, boost::optional<Matrix23&> Dpoint,
boost::optional<Matrix&> Dcal = boost::none) const { boost::optional<Matrix2K&> Dcal) const {
// Transform to camera coordinates and check cheirality
const Point3 pc = pose_.transform_to(pw);
// Project to normalized image coordinates
const Point2 pn = project_to_camera(pc);
if (Dpose || Dpoint) {
const double z = pc.z(), d = 1.0 / z;
// uncalibration
Matrix2 Dpi_pn;
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
// chain the Jacobian matrices
if (Dpose)
calculateDpose(pn, d, Dpi_pn, *Dpose);
if (Dpoint)
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
return pi;
} else
return K_.uncalibrate(pn, Dcal, boost::none);
}
/** project a point from world coordinate to the image
* @param pw is a point in world coordinates
* @param Dpose is the Jacobian w.r.t. pose3
* @param Dpoint is the Jacobian w.r.t. point3
* @param Dcal is the Jacobian w.r.t. calibration
*/
inline Point2 project(
const Point3& pw, //
boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint,
boost::optional<Matrix&> Dcal) const {
// Transform to camera coordinates and check cheirality // Transform to camera coordinates and check cheirality
const Point3 pc = pose_.transform_to(pw); const Point3 pc = pose_.transform_to(pw);
@ -326,7 +379,7 @@ public:
} }
return pi; return pi;
} else } else
return K_.uncalibrate(pn, Dcal); return K_.uncalibrate(pn, Dcal, boost::none);
} }
/** project a point at infinity from world coordinate to the image /** project a point at infinity from world coordinate to the image
@ -355,7 +408,7 @@ public:
Dpc_pose.block(0, 0, 3, 3) = Dpc_rot; Dpc_pose.block(0, 0, 3, 3) = Dpc_rot;
// camera to normalized image coordinate // camera to normalized image coordinate
Matrix Dpn_pc; // 2*3 Matrix23 Dpn_pc; // 2*3
const Point2 pn = project_to_camera(pc, Dpn_pc); const Point2 pn = project_to_camera(pc, Dpn_pc);
// uncalibration // uncalibration
@ -371,15 +424,26 @@ public:
return pi; return pi;
} }
typedef Eigen::Matrix<double,2,6+DimK> Matrix2K6;
/** project a point from world coordinate to the image /** project a point from world coordinate to the image
* @param pw is a point in the world coordinate
*/
Point2 project2(const Point3& pw) const {
const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc);
return K_.uncalibrate(pn);
}
/** project a point from world coordinate to the image, fixed Jacobians
* @param pw is a point in the world coordinate * @param pw is a point in the world coordinate
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration] * @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
* @param Dpoint is the Jacobian w.r.t. point3 * @param Dpoint is the Jacobian w.r.t. point3
*/ */
inline Point2 project2( Point2 project2(
const Point3& pw, // const Point3& pw, //
boost::optional<Matrix&> Dcamera = boost::none, boost::optional<Matrix2K6&> Dcamera,
boost::optional<Matrix&> Dpoint = boost::none) const { boost::optional<Matrix23&> Dpoint) const {
const Point3 pc = pose_.transform_to(pw); const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc); const Point2 pn = project_to_camera(pc);
@ -390,7 +454,40 @@ public:
const double z = pc.z(), d = 1.0 / z; const double z = pc.z(), d = 1.0 / z;
// uncalibration // uncalibration
Matrix Dcal, Dpi_pn(2, 2); Matrix2K Dcal;
Matrix2 Dpi_pn;
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
if (Dcamera) { // TODO why does leftCols<6>() not compile ??
calculateDpose(pn, d, Dpi_pn, Dcamera->leftCols(6));
Dcamera->rightCols(K_.dim()) = Dcal; // Jacobian wrt calib
}
if (Dpoint) {
calculateDpoint(pn, d, pose_.rotation().matrix(), Dpi_pn, *Dpoint);
}
return pi;
}
}
/** project a point from world coordinate to the image
* @param pw is a point in the world coordinate
* @param Dcamera is the Jacobian w.r.t. [pose3 calibration]
* @param Dpoint is the Jacobian w.r.t. point3
*/
Point2 project2(const Point3& pw, //
boost::optional<Matrix&> Dcamera, boost::optional<Matrix&> Dpoint) const {
const Point3 pc = pose_.transform_to(pw);
const Point2 pn = project_to_camera(pc);
if (!Dcamera && !Dpoint) {
return K_.uncalibrate(pn);
} else {
const double z = pc.z(), d = 1.0 / z;
// uncalibration
Matrix2K Dcal;
Matrix2 Dpi_pn;
const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn); const Point2 pi = K_.uncalibrate(pn, Dcal, Dpi_pn);
if (Dcamera) { if (Dcamera) {
@ -517,16 +614,16 @@ private:
* See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
*/ */
template<typename Derived> template<typename Derived>
static void calculateDpose(const Point2& pn, double d, const Matrix& Dpi_pn, static void calculateDpose(const Point2& pn, double d, const Matrix2& Dpi_pn,
Eigen::MatrixBase<Derived> const & Dpose) { Eigen::MatrixBase<Derived> const & Dpose) {
// optimized version of derivatives, see CalibratedCamera.nb // optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y(); const double u = pn.x(), v = pn.y();
double uv = u * v, uu = u * u, vv = v * v; double uv = u * v, uu = u * u, vv = v * v;
Eigen::Matrix<double, 2, 6> Dpn_pose; Matrix26 Dpn_pose;
Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v; Dpn_pose << uv, -1 - uu, v, -d, 0, d * u, 1 + vv, -uv, -u, 0, -d, d * v;
assert(Dpose.rows()==2 && Dpose.cols()==6); assert(Dpose.rows()==2 && Dpose.cols()==6);
const_cast<Eigen::MatrixBase<Derived>&>(Dpose) = // const_cast<Eigen::MatrixBase<Derived>&>(Dpose) = //
Dpi_pn.block<2, 2>(0, 0) * Dpn_pose; Dpi_pn * Dpn_pose;
} }
/** /**
@ -538,32 +635,50 @@ private:
* See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html * See http://eigen.tuxfamily.org/dox/TopicFunctionTakingEigenTypes.html
*/ */
template<typename Derived> template<typename Derived>
static void calculateDpoint(const Point2& pn, double d, const Matrix& R, static void calculateDpoint(const Point2& pn, double d, const Matrix3& R,
const Matrix& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) { const Matrix2& Dpi_pn, Eigen::MatrixBase<Derived> const & Dpoint) {
// optimized version of derivatives, see CalibratedCamera.nb // optimized version of derivatives, see CalibratedCamera.nb
const double u = pn.x(), v = pn.y(); const double u = pn.x(), v = pn.y();
Eigen::Matrix<double, 2, 3> Dpn_point; Matrix23 Dpn_point;
Dpn_point << // Dpn_point << //
R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), // R(0, 0) - u * R(0, 2), R(1, 0) - u * R(1, 2), R(2, 0) - u * R(2, 2), //
/**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2); /**/R(0, 1) - v * R(0, 2), R(1, 1) - v * R(1, 2), R(2, 1) - v * R(2, 2);
Dpn_point *= d; Dpn_point *= d;
assert(Dpoint.rows()==2 && Dpoint.cols()==3); assert(Dpoint.rows()==2 && Dpoint.cols()==3);
const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) = // const_cast<Eigen::MatrixBase<Derived>&>(Dpoint) = //
Dpi_pn.block<2, 2>(0, 0) * Dpn_point; Dpi_pn * Dpn_point;
} }
/// @}
/// @name Advanced Interface
/// @{
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) { void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value);
ar & BOOST_SERIALIZATION_NVP(pose_); ar & BOOST_SERIALIZATION_NVP(pose_);
ar & BOOST_SERIALIZATION_NVP(K_); ar & BOOST_SERIALIZATION_NVP(K_);
} }
/// @}
} };
;}
// Define GTSAM traits
namespace traits {
template<typename Calibration>
struct is_manifold<PinholeCamera<Calibration> > : public boost::true_type {
};
template<typename Calibration>
struct dimension<PinholeCamera<Calibration> > : public boost::integral_constant<
int, dimension<Pose3>::value + dimension<Calibration>::value> {
};
template<typename Calibration>
struct zero<PinholeCamera<Calibration> > {
static PinholeCamera<Calibration> value() {
return PinholeCamera<Calibration>(zero<Pose3>::value(),
zero<Calibration>::value());
}
};
}
} // \ gtsam

16
gtsam/geometry/Point2.cpp
View File

@ -27,8 +27,6 @@ namespace gtsam {
/** Explicit instantiation of base class to export members */ /** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Point2); INSTANTIATE_LIE(Point2);
static const Matrix oneOne = (Matrix(1, 2) << 1.0, 1.0);
/* ************************************************************************* */ /* ************************************************************************* */
void Point2::print(const string& s) const { void Point2::print(const string& s) const {
cout << s << *this << endl; cout << s << *this << endl;
@ -39,16 +37,20 @@ bool Point2::equals(const Point2& q, double tol) const {
return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol); return (fabs(x_ - q.x()) < tol && fabs(y_ - q.y()) < tol);
} }
/* ************************************************************************* */
double Point2::norm() const {
return sqrt(x_ * x_ + y_ * y_);
}
/* ************************************************************************* */ /* ************************************************************************* */
double Point2::norm(boost::optional<Matrix&> H) const { double Point2::norm(boost::optional<Matrix&> H) const {
double r = sqrt(x_ * x_ + y_ * y_); double r = norm();
if (H) { if (H) {
Matrix D_result_d; H->resize(1,2);
if (fabs(r) > 1e-10) if (fabs(r) > 1e-10)
D_result_d = (Matrix(1, 2) << x_ / r, y_ / r); *H << x_ / r, y_ / r;
else else
D_result_d = oneOne; // TODO: really infinity, why 1 here?? *H << 1, 1; // really infinity, why 1 ?
*H = D_result_d;
} }
return r; return r;
} }

35
gtsam/geometry/Point2.h
View File

@ -32,11 +32,10 @@ namespace gtsam {
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT Point2 : public DerivedValue<Point2> { class GTSAM_EXPORT Point2 {
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 2;
private: private:
double x_, y_; double x_, y_;
public: public:
@ -153,10 +152,10 @@ public:
/// @{ /// @{
/// dimension of the variable - used to autodetect sizes /// dimension of the variable - used to autodetect sizes
inline static size_t Dim() { return dimension; } inline static size_t Dim() { return 2; }
/// Dimensionality of tangent space = 2 DOF /// Dimensionality of tangent space = 2 DOF
inline size_t dim() const { return dimension; } inline size_t dim() const { return 2; }
/// Updates a with tangent space delta /// Updates a with tangent space delta
inline Point2 retract(const Vector& v) const { return *this + Point2(v); } inline Point2 retract(const Vector& v) const { return *this + Point2(v); }
@ -182,7 +181,10 @@ public:
Point2 unit() const { return *this/norm(); } Point2 unit() const { return *this/norm(); }
/** norm of point */ /** norm of point */
double norm(boost::optional<Matrix&> H = boost::none) const; double norm() const;
/** norm of point, with derivative */
double norm(boost::optional<Matrix&> H) const;
/** distance between two points */ /** distance between two points */
double distance(const Point2& p2, boost::optional<Matrix&> H1 = boost::none, double distance(const Point2& p2, boost::optional<Matrix&> H1 = boost::none,
@ -235,8 +237,6 @@ private:
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) void serialize(ARCHIVE & ar, const unsigned int version)
{ {
ar & boost::serialization::make_nvp("Point2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(x_);
ar & BOOST_SERIALIZATION_NVP(y_); ar & BOOST_SERIALIZATION_NVP(y_);
} }
@ -248,5 +248,22 @@ private:
/// multiply with scalar /// multiply with scalar
inline Point2 operator*(double s, const Point2& p) {return p*s;} inline Point2 operator*(double s, const Point2& p) {return p*s;}
// Define GTSAM traits
namespace traits {
template<>
struct is_group<Point2> : public boost::true_type {
};
template<>
struct is_manifold<Point2> : public boost::true_type {
};
template<>
struct dimension<Point2> : public boost::integral_constant<int, 2> {
};
}
} }

13
gtsam/geometry/Point3.cpp
View File

@ -98,6 +98,19 @@ double Point3::norm() const {
return sqrt(x_ * x_ + y_ * y_ + z_ * z_); return sqrt(x_ * x_ + y_ * y_ + z_ * z_);
} }
/* ************************************************************************* */
double Point3::norm(boost::optional<Matrix&> H) const {
double r = norm();
if (H) {
H->resize(1,3);
if (fabs(r) > 1e-10)
*H << x_ / r, y_ / r, z_ / r;
else
*H << 1, 1, 1; // really infinity, why 1 ?
}
return r;
}
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Point3::normalize(boost::optional<Matrix&> H) const { Point3 Point3::normalize(boost::optional<Matrix&> H) const {
Point3 normalized = *this / norm(); Point3 normalized = *this / norm();

31
gtsam/geometry/Point3.h
View File

@ -36,12 +36,10 @@ namespace gtsam {
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT Point3 : public DerivedValue<Point3> { class GTSAM_EXPORT Point3 {
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 3;
private: private:
double x_, y_, z_; double x_, y_, z_;
public: public:
@ -122,10 +120,10 @@ namespace gtsam {
/// @{ /// @{
/// dimension of the variable - used to autodetect sizes /// dimension of the variable - used to autodetect sizes
inline static size_t Dim() { return dimension; } inline static size_t Dim() { return 3; }
/// return dimensionality of tangent space, DOF = 3 /// return dimensionality of tangent space, DOF = 3
inline size_t dim() const { return dimension; } inline size_t dim() const { return 3; }
/// Updates a with tangent space delta /// Updates a with tangent space delta
inline Point3 retract(const Vector& v) const { return Point3(*this + v); } inline Point3 retract(const Vector& v) const { return Point3(*this + v); }
@ -185,6 +183,9 @@ namespace gtsam {
/** Distance of the point from the origin */ /** Distance of the point from the origin */
double norm() const; double norm() const;
/** Distance of the point from the origin, with Jacobian */
double norm(boost::optional<Matrix&> H) const;
/** normalize, with optional Jacobian */ /** normalize, with optional Jacobian */
Point3 normalize(boost::optional<Matrix&> H = boost::none) const; Point3 normalize(boost::optional<Matrix&> H = boost::none) const;
@ -236,8 +237,6 @@ namespace gtsam {
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) void serialize(ARCHIVE & ar, const unsigned int version)
{ {
ar & boost::serialization::make_nvp("Point3",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(x_); ar & BOOST_SERIALIZATION_NVP(x_);
ar & BOOST_SERIALIZATION_NVP(y_); ar & BOOST_SERIALIZATION_NVP(y_);
ar & BOOST_SERIALIZATION_NVP(z_); ar & BOOST_SERIALIZATION_NVP(z_);
@ -250,4 +249,20 @@ namespace gtsam {
/// Syntactic sugar for multiplying coordinates by a scalar s*p /// Syntactic sugar for multiplying coordinates by a scalar s*p
inline Point3 operator*(double s, const Point3& p) { return p*s;} inline Point3 operator*(double s, const Point3& p) { return p*s;}
// Define GTSAM traits
namespace traits {
template<>
struct is_group<Point3> : public boost::true_type {
};
template<>
struct is_manifold<Point3> : public boost::true_type {
};
template<>
struct dimension<Point3> : public boost::integral_constant<int, 3> {
};
}
} }

77
gtsam/geometry/Pose2.cpp
View File

@ -123,6 +123,27 @@ Pose2 Pose2::inverse(boost::optional<Matrix&> H1) const {
return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y())));
} }
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point) const {
Point2 d = point - t_;
return r_.unrotate(d);
}
/* ************************************************************************* */
// see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point,
boost::optional<Matrix23&> H1, boost::optional<Matrix2&> H2) const {
Point2 d = point - t_;
Point2 q = r_.unrotate(d);
if (!H1 && !H2) return q;
if (H1) *H1 <<
-1.0, 0.0, q.y(),
0.0, -1.0, -q.x();
if (H2) *H2 = r_.transpose();
return q;
}
/* ************************************************************************* */ /* ************************************************************************* */
// see doc/math.lyx, SE(2) section // see doc/math.lyx, SE(2) section
Point2 Pose2::transform_to(const Point2& point, Point2 Pose2::transform_to(const Point2& point,
@ -161,6 +182,62 @@ Point2 Pose2::transform_from(const Point2& p,
return q + t_; return q + t_;
} }
/* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2) const {
// get cosines and sines from rotation matrices
const Rot2& R1 = r_, R2 = p2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Assert that R1 and R2 are normalized
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = p2.t() - t_;
double x = dt.x(), y = dt.y();
// t = R1' * (t2-t1)
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
return Pose2(R,t);
}
/* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const {
// get cosines and sines from rotation matrices
const Rot2& R1 = r_, R2 = p2.r();
double c1=R1.c(), s1=R1.s(), c2=R2.c(), s2=R2.s();
// Assert that R1 and R2 are normalized
assert(std::abs(c1*c1 + s1*s1 - 1.0) < 1e-5 && std::abs(c2*c2 + s2*s2 - 1.0) < 1e-5);
// Calculate delta rotation = between(R1,R2)
double c = c1 * c2 + s1 * s2, s = -s1 * c2 + c1 * s2;
Rot2 R(Rot2::atan2(s,c)); // normalizes
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = p2.t() - t_;
double x = dt.x(), y = dt.y();
// t = R1' * (t2-t1)
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above
if (H1) {
double dt1 = -s2 * x + c2 * y;
double dt2 = -c2 * x - s2 * y;
*H1 <<
-c, -s, dt1,
s, -c, dt2,
0.0, 0.0,-1.0;
}
if (H2) *H2 = I3;
return Pose2(R,t);
}
/* ************************************************************************* */ /* ************************************************************************* */
Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1, Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H2) const {

51
gtsam/geometry/Pose2.h
View File

@ -33,10 +33,9 @@ namespace gtsam {
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT Pose2 : public DerivedValue<Pose2> { class GTSAM_EXPORT Pose2 {
public: public:
static const size_t dimension = 3;
/** Pose Concept requirements */ /** Pose Concept requirements */
typedef Rot2 Rotation; typedef Rot2 Rotation;
@ -123,20 +122,29 @@ public:
/** /**
* Return relative pose between p1 and p2, in p1 coordinate frame * Return relative pose between p1 and p2, in p1 coordinate frame
*/ */
Pose2 between(const Pose2& p2, Pose2 between(const Pose2& p2) const;
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p2, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const;
/**
* Return relative pose between p1 and p2, in p1 coordinate frame
*/
Pose2 between(const Pose2& p2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const;
/// @} /// @}
/// @name Manifold /// @name Manifold
/// @{ /// @{
/// Dimensionality of tangent space = 3 DOF - used to autodetect sizes /// Dimensionality of tangent space = 3 DOF - used to autodetect sizes
inline static size_t Dim() { return dimension; } inline static size_t Dim() { return 3; }
/// Dimensionality of tangent space = 3 DOF /// Dimensionality of tangent space = 3 DOF
inline size_t dim() const { return dimension; } inline size_t dim() const { return 3; }
/// Retraction from R^3 \f$ [T_x,T_y,\theta] \f$ to Pose2 manifold neighborhood around current pose /// Retraction from R^3 \f$ [T_x,T_y,\theta] \f$ to Pose2 manifold neighborhood around current pose
Pose2 retract(const Vector& v) const; Pose2 retract(const Vector& v) const;
@ -182,10 +190,18 @@ public:
/// @name Group Action on Point2 /// @name Group Action on Point2
/// @{ /// @{
/** Return point coordinates in pose coordinate frame */
Point2 transform_to(const Point2& point) const;
/** Return point coordinates in pose coordinate frame */ /** Return point coordinates in pose coordinate frame */
Point2 transform_to(const Point2& point, Point2 transform_to(const Point2& point,
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix23&> H1,
boost::optional<Matrix&> H2=boost::none) const; boost::optional<Matrix2&> H2) const;
/** Return point coordinates in pose coordinate frame */
Point2 transform_to(const Point2& point,
boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const;
/** Return point coordinates in global frame */ /** Return point coordinates in global frame */
Point2 transform_from(const Point2& point, Point2 transform_from(const Point2& point,
@ -277,14 +293,14 @@ public:
*/ */
static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(2, 2); } static std::pair<size_t, size_t> rotationInterval() { return std::make_pair(2, 2); }
/// @}
private: private:
// Serialization function // Serialization function
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) { void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Pose2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(t_); ar & BOOST_SERIALIZATION_NVP(t_);
ar & BOOST_SERIALIZATION_NVP(r_); ar & BOOST_SERIALIZATION_NVP(r_);
} }
@ -303,7 +319,18 @@ inline Matrix wedge<Pose2>(const Vector& xi) {
typedef std::pair<Point2,Point2> Point2Pair; typedef std::pair<Point2,Point2> Point2Pair;
GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs); GTSAM_EXPORT boost::optional<Pose2> align(const std::vector<Point2Pair>& pairs);
/// @} // Define GTSAM traits
namespace traits {
template<>
struct is_manifold<Pose2> : public boost::true_type {
};
template<>
struct dimension<Pose2> : public boost::integral_constant<int, 3> {
};
}
} // namespace gtsam } // namespace gtsam

49
gtsam/geometry/Pose3.cpp
View File

@ -243,8 +243,8 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose, Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint) const { boost::optional<Matrix&> Dpoint) const {
if (Dpose) { if (Dpose) {
const Matrix R = R_.matrix(); const Matrix3 R = R_.matrix();
Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
Dpose->resize(3, 6); Dpose->resize(3, 6);
(*Dpose) << DR, R; (*Dpose) << DR, R;
} }
@ -254,18 +254,45 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose, Point3 Pose3::transform_to(const Point3& p) const {
boost::optional<Matrix&> Dpoint) const { return R_.unrotate(p - t_);
const Point3 result = R_.unrotate(p - t_); }
/* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
boost::optional<Matrix3&> Dpoint) const {
// Only get transpose once, to avoid multiple allocations,
// as well as multiple conversions in the Quaternion case
const Matrix3 Rt = R_.transpose();
const Point3 q(Rt*(p - t_).vector());
if (Dpose) { if (Dpose) {
const Point3& q = result; const double wx = q.x(), wy = q.y(), wz = q.z();
Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); (*Dpose) <<
Dpose->resize(3, 6); 0.0, -wz, +wy,-1.0, 0.0, 0.0,
(*Dpose) << DR, _I3; +wz, 0.0, -wx, 0.0,-1.0, 0.0,
-wy, +wx, 0.0, 0.0, 0.0,-1.0;
} }
if (Dpoint) if (Dpoint)
*Dpoint = R_.transpose(); *Dpoint = Rt;
return result; return q;
}
/* ************************************************************************* */
Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
boost::optional<Matrix&> Dpoint) const {
const Matrix3 Rt = R_.transpose();
const Point3 q(Rt*(p - t_).vector());
if (Dpose) {
const double wx = q.x(), wy = q.y(), wz = q.z();
Dpose->resize(3, 6);
(*Dpose) <<
0.0, -wz, +wy,-1.0, 0.0, 0.0,
+wz, 0.0, -wx, 0.0,-1.0, 0.0,
-wy, +wx, 0.0, 0.0, 0.0,-1.0;
}
if (Dpoint)
*Dpoint = Rt;
return q;
} }
/* ************************************************************************* */ /* ************************************************************************* */

33
gtsam/geometry/Pose3.h
View File

@ -39,9 +39,8 @@ class Pose2;
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT Pose3: public DerivedValue<Pose3> { class GTSAM_EXPORT Pose3{
public: public:
static const size_t dimension = 6;
/** Pose Concept requirements */ /** Pose Concept requirements */
typedef Rot3 Rotation; typedef Rot3 Rotation;
@ -132,12 +131,12 @@ public:
/// Dimensionality of tangent space = 6 DOF - used to autodetect sizes /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes
static size_t Dim() { static size_t Dim() {
return dimension; return 6;
} }
/// Dimensionality of the tangent space = 6 DOF /// Dimensionality of the tangent space = 6 DOF
size_t dim() const { size_t dim() const {
return dimension; return 6;
} }
/// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map
@ -250,8 +249,13 @@ public:
* @param Dpoint optional 3*3 Jacobian wrpt point * @param Dpoint optional 3*3 Jacobian wrpt point
* @return point in Pose coordinates * @return point in Pose coordinates
*/ */
Point3 transform_to(const Point3& p) const;
Point3 transform_to(const Point3& p, Point3 transform_to(const Point3& p,
boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const; boost::optional<Matrix36&> Dpose, boost::optional<Matrix3&> Dpoint) const;
Point3 transform_to(const Point3& p,
boost::optional<Matrix&> Dpose, boost::optional<Matrix&> Dpoint) const;
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
@ -322,8 +326,6 @@ public:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) { void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Pose3",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(R_); ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_); ar & BOOST_SERIALIZATION_NVP(t_);
} }
@ -350,4 +352,21 @@ inline Matrix wedge<Pose3>(const Vector& xi) {
typedef std::pair<Point3, Point3> Point3Pair; typedef std::pair<Point3, Point3> Point3Pair;
GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs); GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs);
// Define GTSAM traits
namespace traits {
template<>
struct is_group<Pose3> : public boost::true_type {
};
template<>
struct is_manifold<Pose3> : public boost::true_type {
};
template<>
struct dimension<Pose3> : public boost::integral_constant<int, 6> {
};
}
} // namespace gtsam } // namespace gtsam

26
gtsam/geometry/Rot2.h
View File

@ -31,7 +31,7 @@ namespace gtsam {
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT Rot2 : public DerivedValue<Rot2> { class GTSAM_EXPORT Rot2 {
public: public:
/** get the dimension by the type */ /** get the dimension by the type */
@ -230,23 +230,31 @@ namespace gtsam {
/** return 2*2 transpose (inverse) rotation matrix */ /** return 2*2 transpose (inverse) rotation matrix */
Matrix transpose() const; Matrix transpose() const;
/// @}
/// @name Advanced Interface
/// @{
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Rot2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(c_); ar & BOOST_SERIALIZATION_NVP(c_);
ar & BOOST_SERIALIZATION_NVP(s_); ar & BOOST_SERIALIZATION_NVP(s_);
} }
/// @}
}; };
// Define GTSAM traits
namespace traits {
template<>
struct is_group<Rot2> : public boost::true_type {
};
template<>
struct is_manifold<Rot2> : public boost::true_type {
};
template<>
struct dimension<Rot2> : public boost::integral_constant<int, 1> {
};
}
} // gtsam } // gtsam

40
gtsam/geometry/Rot3.cpp
View File

@ -97,13 +97,33 @@ Unit3 Rot3::operator*(const Unit3& p) const {
return rotate(p); return rotate(p);
} }
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const {
const Matrix3& Rt = transpose();
Point3 q(Rt * p.vector()); // q = Rt*p
const double wx = q.x(), wy = q.y(), wz = q.z();
if (H1)
*H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
if (H2)
*H2 = Rt;
return q;
}
/* ************************************************************************* */ /* ************************************************************************* */
// see doc/math.lyx, SO(3) section // see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p, Point3 Rot3::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
Point3 q(transpose()*p.vector()); // q = Rt*p const Matrix3& Rt = transpose();
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z()); Point3 q(Rt * p.vector()); // q = Rt*p
if (H2) *H2 = transpose(); const double wx = q.x(), wy = q.y(), wz = q.z();
if (H1) {
H1->resize(3,3);
*H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
}
if (H2)
*H2 = Rt;
return q; return q;
} }
@ -235,6 +255,20 @@ ostream &operator<<(ostream &os, const Rot3& R) {
return os; return os;
} }
/* ************************************************************************* */
Point3 Rot3::unrotate(const Point3& p) const {
// Eigen expression
return Point3(transpose()*p.vector()); // q = Rt*p
}
/* ************************************************************************* */
Rot3 Rot3::slerp(double t, const Rot3& other) const {
// amazingly simple in GTSAM :-)
assert(t>=0 && t<=1);
Vector3 omega = localCoordinates(other, Rot3::EXPMAP);
return retract(t * omega, Rot3::EXPMAP);
}
/* ************************************************************************* */ /* ************************************************************************* */
} // namespace gtsam } // namespace gtsam

72
gtsam/geometry/Rot3.h
View File

@ -58,11 +58,10 @@ namespace gtsam {
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT Rot3 : public DerivedValue<Rot3> { class GTSAM_EXPORT Rot3 {
public:
static const size_t dimension = 3;
private: private:
#ifdef GTSAM_USE_QUATERNIONS #ifdef GTSAM_USE_QUATERNIONS
/** Internal Eigen Quaternion */ /** Internal Eigen Quaternion */
Quaternion quaternion_; Quaternion quaternion_;
@ -219,8 +218,15 @@ namespace gtsam {
Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const; Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const;
/// Compose two rotations i.e., R= (*this) * R2 /// Compose two rotations i.e., R= (*this) * R2
Rot3 compose(const Rot3& R2, Rot3 compose(const Rot3& R2) const;
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H2=boost::none) const;
/// Compose two rotations i.e., R= (*this) * R2
Rot3 compose(const Rot3& R2, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const;
/// Compose two rotations i.e., R= (*this) * R2
Rot3 compose(const Rot3& R2, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const;
/** compose two rotations */ /** compose two rotations */
Rot3 operator*(const Rot3& R2) const; Rot3 operator*(const Rot3& R2) const;
@ -247,10 +253,10 @@ namespace gtsam {
/// @{ /// @{
/// dimension of the variable - used to autodetect sizes /// dimension of the variable - used to autodetect sizes
static size_t Dim() { return dimension; } static size_t Dim() { return 3; }
/// return dimensionality of tangent space, DOF = 3 /// return dimensionality of tangent space, DOF = 3
size_t dim() const { return dimension; } size_t dim() const { return 3; }
/** /**
* The method retract() is used to map from the tangent space back to the manifold. * The method retract() is used to map from the tangent space back to the manifold.
@ -328,11 +334,16 @@ namespace gtsam {
/// rotate point from rotated coordinate frame to world = R*p /// rotate point from rotated coordinate frame to world = R*p
Point3 operator*(const Point3& p) const; Point3 operator*(const Point3& p) const;
/** /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
* rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$ Point3 unrotate(const Point3& p) const;
*/
Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1 = boost::none, /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
boost::optional<Matrix&> H2 = boost::none) const; Point3 unrotate(const Point3& p, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const;
/// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const;
/// @} /// @}
/// @name Group Action on Unit3 /// @name Group Action on Unit3
@ -356,8 +367,11 @@ namespace gtsam {
/** return 3*3 rotation matrix */ /** return 3*3 rotation matrix */
Matrix3 matrix() const; Matrix3 matrix() const;
/** return 3*3 transpose (inverse) rotation matrix */ /**
* Return 3*3 transpose (inverse) rotation matrix
*/
Matrix3 transpose() const; Matrix3 transpose() const;
// TODO: const Eigen::Transpose<const Matrix3> transpose() const;
/// @deprecated, this is base 1, and was just confusing /// @deprecated, this is base 1, and was just confusing
Point3 column(int index) const; Point3 column(int index) const;
@ -423,17 +437,25 @@ namespace gtsam {
*/ */
Vector quaternion() const; Vector quaternion() const;
/**
* @brief Spherical Linear intERPolation between *this and other
* @param s a value between 0 and 1
* @param other final point of iterpolation geodesic on manifold
*/
Rot3 slerp(double t, const Rot3& other) const;
/// Output stream operator /// Output stream operator
GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p); GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p);
/// @}
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) void serialize(ARCHIVE & ar, const unsigned int version)
{ {
ar & boost::serialization::make_nvp("Rot3",
boost::serialization::base_object<Value>(*this));
#ifndef GTSAM_USE_QUATERNIONS #ifndef GTSAM_USE_QUATERNIONS
ar & boost::serialization::make_nvp("rot11", rot_(0,0)); ar & boost::serialization::make_nvp("rot11", rot_(0,0));
ar & boost::serialization::make_nvp("rot12", rot_(0,1)); ar & boost::serialization::make_nvp("rot12", rot_(0,1));
@ -451,9 +473,8 @@ namespace gtsam {
ar & boost::serialization::make_nvp("z", quaternion_.z()); ar & boost::serialization::make_nvp("z", quaternion_.z());
#endif #endif
} }
};
/// @} };
/** /**
* [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R * [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
@ -466,4 +487,21 @@ namespace gtsam {
* @return a vector [thetax, thetay, thetaz] in radians. * @return a vector [thetax, thetay, thetaz] in radians.
*/ */
GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A); GTSAM_EXPORT std::pair<Matrix3,Vector3> RQ(const Matrix3& A);
// Define GTSAM traits
namespace traits {
template<>
struct is_group<Rot3> : public boost::true_type {
};
template<>
struct is_manifold<Rot3> : public boost::true_type {
};
template<>
struct dimension<Rot3> : public boost::integral_constant<int, 3> {
};
}
} }

51
gtsam/geometry/Rot3M.cpp
View File

@ -63,11 +63,9 @@ Rot3::Rot3(const Matrix& R) {
rot_ = R; rot_ = R;
} }
///* ************************************************************************* */
//Rot3::Rot3(const Matrix3& R) : rot_(R) {}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {} Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Rx(double t) { Rot3 Rot3::Rx(double t) {
@ -143,6 +141,19 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) {
-swy + C02, swx + C12, c + C22); -swy + C02, swx + C12, c + C22);
} }
/* ************************************************************************* */
Rot3 Rot3::compose (const Rot3& R2) const {
return *this * R2;
}
/* ************************************************************************* */
Rot3 Rot3::compose (const Rot3& R2,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return *this * R2;
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::compose (const Rot3& R2, Rot3 Rot3::compose (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@ -156,10 +167,16 @@ Rot3 Rot3::operator*(const Rot3& R2) const {
return Rot3(Matrix3(rot_*R2.rot_)); return Rot3(Matrix3(rot_*R2.rot_));
} }
/* ************************************************************************* */
// TODO const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
Matrix3 Rot3::transpose() const {
return rot_.transpose();
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const { Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
if (H1) *H1 = -rot_; if (H1) *H1 = -rot_;
return Rot3(Matrix3(rot_.transpose())); return Rot3(Matrix3(transpose()));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -167,7 +184,8 @@ Rot3 Rot3::between (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
if (H1) *H1 = -(R2.transpose()*rot_); if (H1) *H1 = -(R2.transpose()*rot_);
if (H2) *H2 = I3; if (H2) *H2 = I3;
return Rot3(Matrix3(rot_.transpose()*R2.rot_)); Matrix3 R12 = transpose()*R2.rot_;
return Rot3(R12);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -253,23 +271,23 @@ Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const
return Logmap(between(T)); return Logmap(between(T));
} else if(mode == Rot3::CAYLEY) { } else if(mode == Rot3::CAYLEY) {
// Create a fixed-size matrix // Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix()); Matrix3 A = rot_.transpose() * T.matrix();
// Mathematica closed form optimization (procrastination?) gone wild: // Mathematica closed form optimization (procrastination?) gone wild:
const double a=A(0,0),b=A(0,1),c=A(0,2); const double a=A(0,0),b=A(0,1),c=A(0,2);
const double d=A(1,0),e=A(1,1),f=A(1,2); const double d=A(1,0),e=A(1,1),f=A(1,2);
const double g=A(2,0),h=A(2,1),i=A(2,2); const double g=A(2,0),h=A(2,1),i=A(2,2);
const double di = d*i, ce = c*e, cd = c*d, fg=f*g; const double di = d*i, ce = c*e, cd = c*d, fg=f*g;
const double M = 1 + e - f*h + i + e*i; const double M = 1 + e - f*h + i + e*i;
const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); const double K = - 4.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg));
const double x = (a * f - cd + f) * K; const double x = a * f - cd + f;
const double y = (b * f - ce - c) * K; const double y = b * f - ce - c;
const double z = (fg - di - d) * K; const double z = fg - di - d;
return -2 * Vector3(x, y, z); return K * Vector3(x, y, z);
} else if(mode == Rot3::SLOW_CAYLEY) { } else if(mode == Rot3::SLOW_CAYLEY) {
// Create a fixed-size matrix // Create a fixed-size matrix
Eigen::Matrix3d A(between(T).matrix()); Matrix3 A(between(T).matrix());
// using templated version of Cayley // using templated version of Cayley
Eigen::Matrix3d Omega = CayleyFixed<3>(A); Matrix3 Omega = CayleyFixed<3>(A);
return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
} else { } else {
assert(false); assert(false);
@ -282,11 +300,6 @@ Matrix3 Rot3::matrix() const {
return rot_; return rot_;
} }
/* ************************************************************************* */
Matrix3 Rot3::transpose() const {
return rot_.transpose();
}
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::r1() const { return Point3(rot_.col(0)); } Point3 Rot3::r1() const { return Point3(rot_.col(0)); }

46
gtsam/geometry/Rot3Q.cpp
View File

@ -56,21 +56,25 @@ namespace gtsam {
Rot3::Rot3(const Matrix& R) : Rot3::Rot3(const Matrix& R) :
quaternion_(Matrix3(R)) {} quaternion_(Matrix3(R)) {}
// /* ************************************************************************* */ /* ************************************************************************* */
// Rot3::Rot3(const Matrix3& R) : Rot3::Rot3(const Quaternion& q) :
// quaternion_(R) {} quaternion_(q) {
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3::Rot3(const Quaternion& q) : quaternion_(q) {} Rot3 Rot3::Rx(double t) {
return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX()));
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); } Rot3 Rot3::Ry(double t) {
return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY()));
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::Ry(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); } Rot3 Rot3::Rz(double t) {
return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ()));
/* ************************************************************************* */ }
Rot3 Rot3::Rz(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); }
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3( Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3(
@ -83,6 +87,19 @@ namespace gtsam {
Rot3 Rot3::rodriguez(const Vector& w, double theta) { Rot3 Rot3::rodriguez(const Vector& w, double theta) {
return Quaternion(Eigen::AngleAxisd(theta, w)); } return Quaternion(Eigen::AngleAxisd(theta, w)); }
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2) const {
return Rot3(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2,
boost::optional<Matrix3&> H1, boost::optional<Matrix3&> H2) const {
if (H1) *H1 = R2.transpose();
if (H2) *H2 = I3;
return Rot3(quaternion_ * R2.quaternion_);
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::compose(const Rot3& R2, Rot3 Rot3::compose(const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@ -102,6 +119,14 @@ namespace gtsam {
return Rot3(quaternion_.inverse()); return Rot3(quaternion_.inverse());
} }
/* ************************************************************************* */
// TODO: Could we do this? It works in Rot3M but not here, probably because
// here we create an intermediate value by calling matrix()
// const Eigen::Transpose<const Matrix3> Rot3::transpose() const {
Matrix3 Rot3::transpose() const {
return matrix().transpose();
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3 Rot3::between(const Rot3& R2, Rot3 Rot3::between(const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const { boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@ -161,9 +186,6 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();} Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();}
/* ************************************************************************* */
Matrix3 Rot3::transpose() const {return quaternion_.toRotationMatrix().transpose();}
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); } Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); }

22
gtsam/geometry/StereoCamera.h
View File

@ -36,7 +36,7 @@ public:
* A stereo camera class, parameterize by left camera pose and stereo calibration * A stereo camera class, parameterize by left camera pose and stereo calibration
* @addtogroup geometry * @addtogroup geometry
*/ */
class GTSAM_EXPORT StereoCamera : public DerivedValue<StereoCamera> { class GTSAM_EXPORT StereoCamera {
private: private:
Pose3 leftCamPose_; Pose3 leftCamPose_;
@ -147,12 +147,28 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class Archive> template<class Archive>
void serialize(Archive & ar, const unsigned int version) { void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("StereoCamera",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(leftCamPose_); ar & BOOST_SERIALIZATION_NVP(leftCamPose_);
ar & BOOST_SERIALIZATION_NVP(K_); ar & BOOST_SERIALIZATION_NVP(K_);
} }
}; };
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<StereoCamera> : public boost::true_type {
};
template<>
struct dimension<StereoCamera> : public boost::integral_constant<int, 6> {
};
template<>
struct zero<StereoCamera> {
static StereoCamera value() { return StereoCamera();}
};
}
} }

20
gtsam/geometry/StereoPoint2.h
View File

@ -28,7 +28,7 @@ namespace gtsam {
* @addtogroup geometry * @addtogroup geometry
* \nosubgrouping * \nosubgrouping
*/ */
class GTSAM_EXPORT StereoPoint2 : public DerivedValue<StereoPoint2> { class GTSAM_EXPORT StereoPoint2 {
public: public:
static const size_t dimension = 3; static const size_t dimension = 3;
private: private:
@ -162,8 +162,6 @@ namespace gtsam {
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("StereoPoint2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(uL_); ar & BOOST_SERIALIZATION_NVP(uL_);
ar & BOOST_SERIALIZATION_NVP(uR_); ar & BOOST_SERIALIZATION_NVP(uR_);
ar & BOOST_SERIALIZATION_NVP(v_); ar & BOOST_SERIALIZATION_NVP(v_);
@ -173,4 +171,20 @@ namespace gtsam {
}; };
// Define GTSAM traits
namespace traits {
template<>
struct is_group<StereoPoint2> : public boost::true_type {
};
template<>
struct is_manifold<StereoPoint2> : public boost::true_type {
};
template<>
struct dimension<StereoPoint2> : public boost::integral_constant<int, 3> {
};
}
} }

4
gtsam/geometry/TriangulationFactor.h
View File

@ -115,7 +115,7 @@ public:
Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 = Vector evaluateError(const Point3& point, boost::optional<Matrix&> H2 =
boost::none) const { boost::none) const {
try { try {
Point2 error(camera_.project(point, boost::none, H2) - measured_); Point2 error(camera_.project(point, boost::none, H2, boost::none) - measured_);
return error.vector(); return error.vector();
} catch (CheiralityException& e) { } catch (CheiralityException& e) {
if (H2) if (H2)
@ -155,7 +155,7 @@ public:
// Would be even better if we could pass blocks to project // Would be even better if we could pass blocks to project
const Point3& point = x.at<Point3>(key()); const Point3& point = x.at<Point3>(key());
b = -(camera_.project(point, boost::none, A) - measured_).vector(); b = -(camera_.project(point, boost::none, A, boost::none) - measured_).vector();
if (noiseModel_) if (noiseModel_)
this->noiseModel_->WhitenSystem(A, b); this->noiseModel_->WhitenSystem(A, b);

24
gtsam/geometry/Unit3.h
View File

@ -28,7 +28,7 @@
namespace gtsam { namespace gtsam {
/// Represents a 3D point on a unit sphere. /// Represents a 3D point on a unit sphere.
class GTSAM_EXPORT Unit3: public DerivedValue<Unit3> { class GTSAM_EXPORT Unit3{
private: private:
@ -149,8 +149,6 @@ private:
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) { void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Unit3",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(p_); ar & BOOST_SERIALIZATION_NVP(p_);
ar & BOOST_SERIALIZATION_NVP(B_); ar & BOOST_SERIALIZATION_NVP(B_);
} }
@ -159,5 +157,25 @@ private:
}; };
// Define GTSAM traits
namespace traits {
template<>
struct is_manifold<Unit3> : public boost::true_type {
};
template<>
struct dimension<Unit3> : public boost::integral_constant<int, 2> {
};
template<>
struct zero<Unit3> {
static Unit3 value() {
return Unit3();
}
};
}
} // namespace gtsam } // namespace gtsam

12
gtsam/geometry/tests/testCal3Bundler.cpp
View File

@ -14,7 +14,6 @@
* @brief Unit tests for transform derivatives * @brief Unit tests for transform derivatives
*/ */
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
@ -28,6 +27,15 @@ GTSAM_CONCEPT_MANIFOLD_INST(Cal3Bundler)
static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000); static Cal3Bundler K(500, 1e-3, 1e-3, 1000, 2000);
static Point2 p(2,3); static Point2 p(2,3);
/* ************************************************************************* */
TEST( Cal3Bundler, vector)
{
Cal3Bundler K;
Vector expected(3);
expected << 1, 0, 0;
CHECK(assert_equal(expected,K.vector()));
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Cal3Bundler, uncalibrate) TEST( Cal3Bundler, uncalibrate)
{ {
@ -36,7 +44,7 @@ TEST( Cal3Bundler, uncalibrate)
double g = v[0]*(1+v[1]*r+v[2]*r*r) ; double g = v[0]*(1+v[1]*r+v[2]*r*r) ;
Point2 expected (1000+g*p.x(), 2000+g*p.y()) ; Point2 expected (1000+g*p.x(), 2000+g*p.y()) ;
Point2 actual = K.uncalibrate(p); Point2 actual = K.uncalibrate(p);
CHECK(assert_equal(actual,expected)); CHECK(assert_equal(expected,actual));
} }
TEST( Cal3Bundler, calibrate ) TEST( Cal3Bundler, calibrate )

8
gtsam/geometry/tests/testPoint2.cpp
View File

@ -84,8 +84,8 @@ namespace {
Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3); Point2 l1(1, 0), l2(1, 1), l3(2, 2), l4(1, 3);
/* ************************************************************************* */ /* ************************************************************************* */
LieVector norm_proxy(const Point2& point) { double norm_proxy(const Point2& point) {
return LieVector(point.norm()); return point.norm();
} }
} }
TEST( Point2, norm ) { TEST( Point2, norm ) {
@ -112,8 +112,8 @@ TEST( Point2, norm ) {
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
LieVector distance_proxy(const Point2& location, const Point2& point) { double distance_proxy(const Point2& location, const Point2& point) {
return LieVector(location.distance(point)); return location.distance(point);
} }
} }
TEST( Point2, distance ) { TEST( Point2, distance ) {

14
gtsam/geometry/tests/testPoint3.cpp
View File

@ -88,6 +88,20 @@ TEST (Point3, normalize) {
EXPECT(assert_equal(expectedH, actualH, 1e-8)); EXPECT(assert_equal(expectedH, actualH, 1e-8));
} }
//*************************************************************************
double norm_proxy(const Point3& point) {
return double(point.norm());
}
TEST (Point3, norm) {
Matrix actualH;
Point3 point(3,4,5); // arbitrary point
double expected = sqrt(50);
EXPECT_DOUBLES_EQUAL(expected, point.norm(actualH), 1e-8);
Matrix expectedH = numericalDerivative11<double, Point3>(norm_proxy, point);
EXPECT(assert_equal(expectedH, actualH, 1e-8));
}
/* ************************************************************************* */ /* ************************************************************************* */
double testFunc(const Point3& P, const Point3& Q) { double testFunc(const Point3& P, const Point3& Q) {
return P.distance(Q); return P.distance(Q);

8
gtsam/geometry/tests/testPose2.cpp
View File

@ -574,8 +574,8 @@ TEST( Pose2, bearing_pose )
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
LieVector range_proxy(const Pose2& pose, const Point2& point) { double range_proxy(const Pose2& pose, const Point2& point) {
return LieVector(pose.range(point)); return pose.range(point);
} }
} }
TEST( Pose2, range ) TEST( Pose2, range )
@ -611,8 +611,8 @@ TEST( Pose2, range )
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
LieVector range_pose_proxy(const Pose2& pose, const Pose2& point) { double range_pose_proxy(const Pose2& pose, const Pose2& point) {
return LieVector(pose.range(point)); return pose.range(point);
} }
} }
TEST( Pose2, range_pose ) TEST( Pose2, range_pose )

46
gtsam/geometry/tests/testPose3.cpp
View File

@ -547,8 +547,8 @@ Pose3
xl4(Rot3::ypr(0.0, 0.0, 1.0), Point3(1, 4,-4)); xl4(Rot3::ypr(0.0, 0.0, 1.0), Point3(1, 4,-4));
/* ************************************************************************* */ /* ************************************************************************* */
LieVector range_proxy(const Pose3& pose, const Point3& point) { double range_proxy(const Pose3& pose, const Point3& point) {
return LieVector(pose.range(point)); return pose.range(point);
} }
TEST( Pose3, range ) TEST( Pose3, range )
{ {
@ -582,8 +582,8 @@ TEST( Pose3, range )
} }
/* ************************************************************************* */ /* ************************************************************************* */
LieVector range_pose_proxy(const Pose3& pose, const Pose3& point) { double range_pose_proxy(const Pose3& pose, const Pose3& point) {
return LieVector(pose.range(point)); return pose.range(point);
} }
TEST( Pose3, range_pose ) TEST( Pose3, range_pose )
{ {
@ -674,30 +674,24 @@ TEST(Pose3, align_2) {
/* ************************************************************************* */ /* ************************************************************************* */
/// exp(xi) exp(y) = exp(xi + x) /// exp(xi) exp(y) = exp(xi + x)
/// Hence, y = log (exp(-xi)*exp(xi+x)) /// Hence, y = log (exp(-xi)*exp(xi+x))
Vector xi = (Vector(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0); Vector xi = (Vector(6) << 0.1, 0.2, 0.3, 1.0, 2.0, 3.0);
Vector testDerivExpmapInv(const LieVector& dxi) { Vector testDerivExpmapInv(const Vector6& dxi) {
Vector y = Pose3::Logmap(Pose3::Expmap(-xi)*Pose3::Expmap(xi+dxi)); return Pose3::Logmap(Pose3::Expmap(-xi) * Pose3::Expmap(xi + dxi));
return y;
} }
TEST( Pose3, dExpInv_TLN) { TEST( Pose3, dExpInv_TLN) {
Matrix res = Pose3::dExpInv_exp(xi); Matrix res = Pose3::dExpInv_exp(xi);
Matrix numericalDerivExpmapInv = numericalDerivative11( Matrix numericalDerivExpmapInv = numericalDerivative11<Vector, Vector6>(
boost::function<Vector(const LieVector&)>( testDerivExpmapInv, Vector6::Zero(), 1e-5);
boost::bind(testDerivExpmapInv, _1)
),
LieVector(Vector::Zero(6)), 1e-5
);
EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1)); EXPECT(assert_equal(numericalDerivExpmapInv,res,3e-1));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector testDerivAdjoint(const LieVector& xi, const LieVector& v) { Vector testDerivAdjoint(const Vector6& xi, const Vector6& v) {
return Pose3::adjointMap(xi)*v; return Pose3::adjointMap(xi) * v;
} }
TEST( Pose3, adjoint) { TEST( Pose3, adjoint) {
@ -706,20 +700,16 @@ TEST( Pose3, adjoint) {
Matrix actualH; Matrix actualH;
Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH); Vector actual = Pose3::adjoint(screw::xi, screw::xi, actualH);
Matrix numericalH = numericalDerivative21( Matrix numericalH = numericalDerivative21<Vector, Vector6, Vector6>(
boost::function<Vector(const LieVector&, const LieVector&)>( testDerivAdjoint, screw::xi, screw::xi, 1e-5);
boost::bind(testDerivAdjoint, _1, _2)
),
LieVector(screw::xi), LieVector(screw::xi), 1e-5
);
EXPECT(assert_equal(expected,actual,1e-5)); EXPECT(assert_equal(expected,actual,1e-5));
EXPECT(assert_equal(numericalH,actualH,1e-5)); EXPECT(assert_equal(numericalH,actualH,1e-5));
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector testDerivAdjointTranspose(const LieVector& xi, const LieVector& v) { Vector testDerivAdjointTranspose(const Vector6& xi, const Vector6& v) {
return Pose3::adjointMap(xi).transpose()*v; return Pose3::adjointMap(xi).transpose() * v;
} }
TEST( Pose3, adjointTranspose) { TEST( Pose3, adjointTranspose) {
@ -730,12 +720,8 @@ TEST( Pose3, adjointTranspose) {
Matrix actualH; Matrix actualH;
Vector actual = Pose3::adjointTranspose(xi, v, actualH); Vector actual = Pose3::adjointTranspose(xi, v, actualH);
Matrix numericalH = numericalDerivative21( Matrix numericalH = numericalDerivative21<Vector, Vector6, Vector6>(
boost::function<Vector(const LieVector&, const LieVector&)>( testDerivAdjointTranspose, xi, v, 1e-5);
boost::bind(testDerivAdjointTranspose, _1, _2)
),
LieVector(xi), LieVector(v), 1e-5
);
EXPECT(assert_equal(expected,actual,1e-15)); EXPECT(assert_equal(expected,actual,1e-15));
EXPECT(assert_equal(numericalH,actualH,1e-5)); EXPECT(assert_equal(numericalH,actualH,1e-5));

47
gtsam/geometry/tests/testRot3.cpp
View File

@ -130,15 +130,20 @@ TEST( Rot3, rodriguez4)
Rot3 expected(c,-s, 0, Rot3 expected(c,-s, 0,
s, c, 0, s, c, 0,
0, 0, 1); 0, 0, 1);
CHECK(assert_equal(expected,actual,1e-5)); CHECK(assert_equal(expected,actual));
CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual,1e-5)); CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Rot3, retract) TEST( Rot3, retract)
{ {
Vector v = zero(3); Vector v = zero(3);
CHECK(assert_equal(R.retract(v), R)); CHECK(assert_equal(R, R.retract(v)));
// test Canonical coordinates
Canonical<Rot3> chart;
Vector v2 = chart.local(R);
CHECK(assert_equal(R, chart.retract(v2)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -209,9 +214,9 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){
TEST( Rot3, rightJacobianExpMapSO3 ) TEST( Rot3, rightJacobianExpMapSO3 )
{ {
// Linearization point // Linearization point
Vector thetahat = (Vector(3) << 0.1, 0, 0); Vector3 thetahat; thetahat << 0.1, 0, 0;
Matrix expectedJacobian = numericalDerivative11<Rot3, LieVector>( Matrix expectedJacobian = numericalDerivative11<Rot3, Vector3>(
boost::bind(&Rot3::Expmap, _1), thetahat); boost::bind(&Rot3::Expmap, _1), thetahat);
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat); Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat);
CHECK(assert_equal(expectedJacobian, actualJacobian)); CHECK(assert_equal(expectedJacobian, actualJacobian));
@ -221,10 +226,10 @@ TEST( Rot3, rightJacobianExpMapSO3 )
TEST( Rot3, rightJacobianExpMapSO3inverse ) TEST( Rot3, rightJacobianExpMapSO3inverse )
{ {
// Linearization point // Linearization point
Vector thetahat = (Vector(3) << 0.1,0.1,0); ///< Current estimate of rotation rate bias Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias
Vector deltatheta = (Vector(3) << 0, 0, 0); Vector3 deltatheta; deltatheta << 0, 0, 0;
Matrix expectedJacobian = numericalDerivative11<LieVector>( Matrix expectedJacobian = numericalDerivative11<Vector3,Vector3>(
boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta); boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta);
Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat); Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat);
EXPECT(assert_equal(expectedJacobian, actualJacobian)); EXPECT(assert_equal(expectedJacobian, actualJacobian));
@ -381,10 +386,10 @@ TEST( Rot3, between )
EXPECT(assert_equal(expected,actual)); EXPECT(assert_equal(expected,actual));
Matrix numericalH1 = numericalDerivative21(testing::between<Rot3> , R1, R2); Matrix numericalH1 = numericalDerivative21(testing::between<Rot3> , R1, R2);
CHECK(assert_equal(numericalH1,actualH1, 1e-4)); CHECK(assert_equal(numericalH1,actualH1));
Matrix numericalH2 = numericalDerivative22(testing::between<Rot3> , R1, R2); Matrix numericalH2 = numericalDerivative22(testing::between<Rot3> , R1, R2);
CHECK(assert_equal(numericalH2,actualH2, 1e-4)); CHECK(assert_equal(numericalH2,actualH2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -400,12 +405,12 @@ Vector3 testDexpL(const Vector3& dw) {
TEST( Rot3, dexpL) { TEST( Rot3, dexpL) {
Matrix actualDexpL = Rot3::dexpL(w); Matrix actualDexpL = Rot3::dexpL(w);
Matrix expectedDexpL = numericalDerivative11<LieVector>(testDexpL, Matrix expectedDexpL = numericalDerivative11<Vector3, Vector3>(testDexpL,
LieVector(zero(3)), 1e-2); Vector3::Zero(), 1e-2);
EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); EXPECT(assert_equal(expectedDexpL, actualDexpL,1e-7));
Matrix actualDexpInvL = Rot3::dexpInvL(w); Matrix actualDexpInvL = Rot3::dexpInvL(w);
EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL,1e-7));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -518,7 +523,7 @@ TEST( Rot3, logmapStability ) {
// std::cout << "trace: " << tr << std::endl; // std::cout << "trace: " << tr << std::endl;
// R.print("R = "); // R.print("R = ");
Vector actualw = Rot3::Logmap(R); Vector actualw = Rot3::Logmap(R);
CHECK(assert_equal(w, actualw, 1e-15)); // this should be fixed for Quaternions!!! CHECK(assert_equal(w, actualw, 1e-15));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -577,6 +582,18 @@ TEST( Rot3, stream)
EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n"); EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n");
} }
/* ************************************************************************* */
TEST( Rot3, slerp)
{
// A first simple test
Rot3 R1 = Rot3::Rz(1), R2 = Rot3::Rz(2), R3 = Rot3::Rz(1.5);
EXPECT(assert_equal(R1, R1.slerp(0.0,R2)));
EXPECT(assert_equal(R2, R1.slerp(1.0,R2)));
EXPECT(assert_equal(R3, R1.slerp(0.5,R2)));
// Make sure other can be *this
EXPECT(assert_equal(R1, R1.slerp(0.5,R1)));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;

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

@ -32,6 +32,9 @@
using namespace std; using namespace std;
using namespace gtsam; 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 Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
static Point3 P(0.2, 0.7, -2.0); static Point3 P(0.2, 0.7, -2.0);
static const Matrix I3 = eye(3); static const Matrix I3 = eye(3);

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

@ -125,7 +125,7 @@ static Point2 project2(const Pose3& pose, const Point3& point) {
TEST( SimpleCamera, Dproject_point_pose) TEST( SimpleCamera, Dproject_point_pose)
{ {
Matrix Dpose, Dpoint; Matrix Dpose, Dpoint;
Point2 result = camera.project(point1, Dpose, Dpoint); Point2 result = camera.project(point1, Dpose, Dpoint, boost::none);
Matrix numerical_pose = numericalDerivative21(project2, pose1, point1); Matrix numerical_pose = numericalDerivative21(project2, pose1, point1);
Matrix numerical_point = numericalDerivative22(project2, pose1, point1); Matrix numerical_point = numericalDerivative22(project2, pose1, point1);
CHECK(assert_equal(result, Point2(-100, 100) )); CHECK(assert_equal(result, Point2(-100, 100) ));

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

@ -274,11 +274,7 @@ TEST( triangulation, TriangulationFactor ) {
Matrix HActual; Matrix HActual;
factor.evaluateError(landmark, HActual); factor.evaluateError(landmark, HActual);
// Matrix expectedH1 = numericalDerivative11<Pose3>( Matrix HExpected = numericalDerivative11<Vector,Point3>(
// boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
// boost::none, boost::none), pose1);
// The expected Jacobian
Matrix HExpected = numericalDerivative11<Point3>(
boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark); boost::bind(&Factor::evaluateError, &factor, _1, boost::none), landmark);
// Verify the Jacobians are correct // Verify the Jacobians are correct

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

@ -115,13 +115,13 @@ TEST(Unit3, error) {
Matrix actual, expected; Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
{ {
expected = numericalDerivative11<Unit3>( expected = numericalDerivative11<Vector,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), q); boost::bind(&Unit3::error, &p, _1, boost::none), q);
p.error(q, actual); p.error(q, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); EXPECT(assert_equal(expected.transpose(), actual, 1e-9));
} }
{ {
expected = numericalDerivative11<Unit3>( expected = numericalDerivative11<Vector,Unit3>(
boost::bind(&Unit3::error, &p, _1, boost::none), r); boost::bind(&Unit3::error, &p, _1, boost::none), r);
p.error(r, actual); p.error(r, actual);
EXPECT(assert_equal(expected.transpose(), actual, 1e-9)); EXPECT(assert_equal(expected.transpose(), actual, 1e-9));

27
gtsam/linear/JacobianFactor.h
View File

@ -82,20 +82,22 @@ namespace gtsam {
class GTSAM_EXPORT JacobianFactor : public GaussianFactor class GTSAM_EXPORT JacobianFactor : public GaussianFactor
{ {
public: public:
typedef JacobianFactor This; ///< Typedef to this class typedef JacobianFactor This; ///< Typedef to this class
typedef GaussianFactor Base; ///< Typedef to base class typedef GaussianFactor Base; ///< Typedef to base class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
protected:
VerticalBlockMatrix Ab_; // the block view of the full matrix
noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
public:
typedef VerticalBlockMatrix::Block ABlock; typedef VerticalBlockMatrix::Block ABlock;
typedef VerticalBlockMatrix::constBlock constABlock; typedef VerticalBlockMatrix::constBlock constABlock;
typedef ABlock::ColXpr BVector; typedef ABlock::ColXpr BVector;
typedef constABlock::ConstColXpr constBVector; typedef constABlock::ConstColXpr constBVector;
protected:
VerticalBlockMatrix Ab_; // the block view of the full matrix
noiseModel::Diagonal::shared_ptr model_; // Gaussian noise model with diagonal covariance matrix
public:
/** Convert from other GaussianFactor */ /** Convert from other GaussianFactor */
explicit JacobianFactor(const GaussianFactor& gf); explicit JacobianFactor(const GaussianFactor& gf);
@ -328,6 +330,21 @@ namespace gtsam {
private: private:
/** Unsafe Constructor that creates an uninitialized Jacobian of right size
* @param keys in some order
* @param diemnsions of the variables in same order
* @param m output dimension
* @param model noise model (default NULL)
*/
template<class KEYS, class DIMENSIONS>
JacobianFactor(const KEYS& keys, const DIMENSIONS& dims, DenseIndex m,
const SharedDiagonal& model = SharedDiagonal()) :
Base(keys), Ab_(dims.begin(), dims.end(), m, true), model_(model) {
}
// be very selective on who can access these private methods:
template<typename T> friend class ExpressionFactor;
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;
template<class ARCHIVE> template<class ARCHIVE>

10
gtsam/linear/NoiseModel.h
View File

@ -61,6 +61,11 @@ namespace gtsam {
Base(size_t dim = 1):dim_(dim) {} Base(size_t dim = 1):dim_(dim) {}
virtual ~Base() {} virtual ~Base() {}
/** true if a constrained noise mode, saves slow/clumsy dynamic casting */
virtual bool is_constrained() const {
return false;
}
/// Dimensionality /// Dimensionality
inline size_t dim() const { return dim_;} inline size_t dim() const { return dim_;}
@ -385,6 +390,11 @@ namespace gtsam {
virtual ~Constrained() {} virtual ~Constrained() {}
/** true if a constrained noise mode, saves slow/clumsy dynamic casting */
virtual bool is_constrained() const {
return true;
}
/// Access mu as a vector /// Access mu as a vector
const Vector& mu() const { return mu_; } const Vector& mu() const { return mu_; }

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

@ -19,7 +19,6 @@
#include <gtsam/linear/JacobianFactor.h> #include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/GaussianFactorGraph.h> #include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <boost/assign/list_of.hpp> #include <boost/assign/list_of.hpp>
@ -149,8 +148,9 @@ TEST( GaussianBayesNet, DeterminantTest )
} }
/* ************************************************************************* */ /* ************************************************************************* */
typedef Eigen::Matrix<double,10,1> Vector10;
namespace { namespace {
double computeError(const GaussianBayesNet& gbn, const LieVector& values) double computeError(const GaussianBayesNet& gbn, const Vector10& values)
{ {
pair<Matrix,Vector> Rd = GaussianFactorGraph(gbn).jacobian(); pair<Matrix,Vector> Rd = GaussianFactorGraph(gbn).jacobian();
return 0.5 * (Rd.first * values - Rd.second).squaredNorm(); return 0.5 * (Rd.first * values - Rd.second).squaredNorm();
@ -180,14 +180,12 @@ TEST(GaussianBayesNet, ComputeSteepestDescentPoint) {
4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0))); 4, (Vector(2) << 49.0,50.0), (Matrix(2, 2) << 51.0,52.0,0.0,54.0)));
// Compute the Hessian numerically // Compute the Hessian numerically
Matrix hessian = numericalHessian( Matrix hessian = numericalHessian<Vector10>(
boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)), boost::bind(&computeError, gbn, _1), Vector10::Zero());
LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())));
// Compute the gradient numerically // Compute the gradient numerically
Vector gradient = numericalGradient( Vector gradient = numericalGradient<Vector10>(
boost::function<double(const LieVector&)>(boost::bind(&computeError, gbn, _1)), boost::bind(&computeError, gbn, _1), Vector10::Zero());
LieVector(Vector::Zero(GaussianFactorGraph(gbn).jacobian().first.cols())));
// Compute the gradient using dense matrices // Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian(); Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();

23
gtsam/linear/tests/testGaussianBayesTree.cpp
View File

@ -24,7 +24,6 @@
using namespace boost::assign; using namespace boost::assign;
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/geometry/Rot2.h> #include <gtsam/geometry/Rot2.h>
#include <gtsam/linear/GaussianJunctionTree.h> #include <gtsam/linear/GaussianJunctionTree.h>
@ -175,13 +174,13 @@ TEST(GaussianBayesTree, complicatedMarginal) {
EXPECT(assert_equal(expectedCov, actualCov, 1e1)); EXPECT(assert_equal(expectedCov, actualCov, 1e1));
} }
/* ************************************************************************* */
typedef Eigen::Matrix<double, 10, 1> Vector10;
namespace { namespace {
/* ************************************************************************* */ double computeError(const GaussianBayesTree& gbt, const Vector10& values) {
double computeError(const GaussianBayesTree& gbt, const LieVector& values) pair<Matrix, Vector> Rd = GaussianFactorGraph(gbt).jacobian();
{ return 0.5 * (Rd.first * values - Rd.second).squaredNorm();
pair<Matrix,Vector> Rd = GaussianFactorGraph(gbt).jacobian(); }
return 0.5 * (Rd.first * values - Rd.second).squaredNorm();
}
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -243,14 +242,12 @@ TEST(GaussianBayesTree, ComputeSteepestDescentPointBT) {
2, (Vector(4) << 1.0,2.0,15.0,16.0)))))); 2, (Vector(4) << 1.0,2.0,15.0,16.0))))));
// Compute the Hessian numerically // Compute the Hessian numerically
Matrix hessian = numericalHessian( Matrix hessian = numericalHessian<Vector10>(
boost::function<double(const LieVector&)>(boost::bind(&computeError, bt, _1)), boost::bind(&computeError, bt, _1), Vector10::Zero());
LieVector(Vector::Zero(GaussianFactorGraph(bt).jacobian().first.cols())));
// Compute the gradient numerically // Compute the gradient numerically
Vector gradient = numericalGradient( Vector gradient = numericalGradient<Vector10>(
boost::function<double(const LieVector&)>(boost::bind(&computeError, bt, _1)), boost::bind(&computeError, bt, _1), Vector10::Zero());
LieVector(Vector::Zero(GaussianFactorGraph(bt).jacobian().first.cols())));
// Compute the gradient using dense matrices // Compute the gradient using dense matrices
Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian(); Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian();

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

@ -276,8 +276,8 @@ TEST(HessianFactor, CombineAndEliminate)
1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 1.0); 0.0, 0.0, 1.0);
Vector b0 = (Vector(3) << 1.5, 1.5, 1.5); Vector3 b0(1.5, 1.5, 1.5);
Vector s0 = (Vector(3) << 1.6, 1.6, 1.6); Vector3 s0(1.6, 1.6, 1.6);
Matrix A10 = (Matrix(3,3) << Matrix A10 = (Matrix(3,3) <<
2.0, 0.0, 0.0, 2.0, 0.0, 0.0,
@ -287,15 +287,15 @@ TEST(HessianFactor, CombineAndEliminate)
-2.0, 0.0, 0.0, -2.0, 0.0, 0.0,
0.0, -2.0, 0.0, 0.0, -2.0, 0.0,
0.0, 0.0, -2.0); 0.0, 0.0, -2.0);
Vector b1 = (Vector(3) << 2.5, 2.5, 2.5); Vector3 b1(2.5, 2.5, 2.5);
Vector s1 = (Vector(3) << 2.6, 2.6, 2.6); Vector3 s1(2.6, 2.6, 2.6);
Matrix A21 = (Matrix(3,3) << Matrix A21 = (Matrix(3,3) <<
3.0, 0.0, 0.0, 3.0, 0.0, 0.0,
0.0, 3.0, 0.0, 0.0, 3.0, 0.0,
0.0, 0.0, 3.0); 0.0, 0.0, 3.0);
Vector b2 = (Vector(3) << 3.5, 3.5, 3.5); Vector3 b2(3.5, 3.5, 3.5);
Vector s2 = (Vector(3) << 3.6, 3.6, 3.6); Vector3 s2(3.6, 3.6, 3.6);
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
@ -305,8 +305,8 @@ TEST(HessianFactor, CombineAndEliminate)
Matrix zero3x3 = zeros(3,3); Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector b = gtsam::concatVectors(3, &b1, &b0, &b2); Vector9 b; b << b1, b0, b2;
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); Vector9 sigmas; sigmas << s1, s0, s2;
// create a full, uneliminated version of the factor // create a full, uneliminated version of the factor
JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); JacobianFactor expectedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));

16
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -369,8 +369,8 @@ TEST(JacobianFactor, eliminate)
1.0, 0.0, 0.0, 1.0, 0.0, 0.0,
0.0, 1.0, 0.0, 0.0, 1.0, 0.0,
0.0, 0.0, 1.0); 0.0, 0.0, 1.0);
Vector b0 = (Vector(3) << 1.5, 1.5, 1.5); Vector3 b0(1.5, 1.5, 1.5);
Vector s0 = (Vector(3) << 1.6, 1.6, 1.6); Vector3 s0(1.6, 1.6, 1.6);
Matrix A10 = (Matrix(3, 3) << Matrix A10 = (Matrix(3, 3) <<
2.0, 0.0, 0.0, 2.0, 0.0, 0.0,
@ -380,15 +380,15 @@ TEST(JacobianFactor, eliminate)
-2.0, 0.0, 0.0, -2.0, 0.0, 0.0,
0.0, -2.0, 0.0, 0.0, -2.0, 0.0,
0.0, 0.0, -2.0); 0.0, 0.0, -2.0);
Vector b1 = (Vector(3) << 2.5, 2.5, 2.5); Vector3 b1(2.5, 2.5, 2.5);
Vector s1 = (Vector(3) << 2.6, 2.6, 2.6); Vector3 s1(2.6, 2.6, 2.6);
Matrix A21 = (Matrix(3, 3) << Matrix A21 = (Matrix(3, 3) <<
3.0, 0.0, 0.0, 3.0, 0.0, 0.0,
0.0, 3.0, 0.0, 0.0, 3.0, 0.0,
0.0, 0.0, 3.0); 0.0, 0.0, 3.0);
Vector b2 = (Vector(3) << 3.5, 3.5, 3.5); Vector3 b2(3.5, 3.5, 3.5);
Vector s2 = (Vector(3) << 3.6, 3.6, 3.6); Vector3 s2(3.6, 3.6, 3.6);
GaussianFactorGraph gfg; GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true)); gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
@ -398,8 +398,8 @@ TEST(JacobianFactor, eliminate)
Matrix zero3x3 = zeros(3,3); Matrix zero3x3 = zeros(3,3);
Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3); Matrix A0 = gtsam::stack(3, &A10, &zero3x3, &zero3x3);
Matrix A1 = gtsam::stack(3, &A11, &A01, &A21); Matrix A1 = gtsam::stack(3, &A11, &A01, &A21);
Vector b = gtsam::concatVectors(3, &b1, &b0, &b2); Vector9 b; b << b1, b0, b2;
Vector sigmas = gtsam::concatVectors(3, &s1, &s0, &s2); Vector9 sigmas; sigmas << s1, s0, s2;
JacobianFactor combinedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true)); JacobianFactor combinedFactor(0, A0, 1, A1, b, noiseModel::Diagonal::Sigmas(sigmas, true));
GaussianFactorGraph::EliminationResult expected = combinedFactor.eliminate(list_of(0)); GaussianFactorGraph::EliminationResult expected = combinedFactor.eliminate(list_of(0));

17
gtsam/navigation/CombinedImuFactor.h
View File

@ -17,11 +17,10 @@
#pragma once #pragma once
/* GTSAM includes */ /* GTSAM includes */
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/navigation/ImuBias.h> #include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/GaussianFactor.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
/* External or standard includes */ /* External or standard includes */
@ -46,7 +45,7 @@ namespace gtsam {
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
*/ */
class CombinedImuFactor: public NoiseModelFactor6<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias,imuBias::ConstantBias> { class CombinedImuFactor: public NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> {
public: public:
@ -222,7 +221,7 @@ namespace gtsam {
Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; Matrix3 H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT; Matrix3 H_angles_biasomega =- Jrinv_theta_j * Jr_theta_incr * deltaT;
// analytic expression corresponding to the following numerical derivative // analytic expression corresponding to the following numerical derivative
// Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); // Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
// overall Jacobian wrt preintegrated measurements (df/dx) // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(15,15); Matrix F(15,15);
@ -339,7 +338,7 @@ namespace gtsam {
private: private:
typedef CombinedImuFactor This; typedef CombinedImuFactor This;
typedef NoiseModelFactor6<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias,imuBias::ConstantBias> Base; typedef NoiseModelFactor6<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias,imuBias::ConstantBias> Base;
CombinedPreintegratedMeasurements preintegratedMeasurements_; CombinedPreintegratedMeasurements preintegratedMeasurements_;
Vector3 gravity_; Vector3 gravity_;
@ -429,7 +428,7 @@ namespace gtsam {
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/** vector of errors */ /** vector of errors */
Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j, const imuBias::ConstantBias& bias_i, const imuBias::ConstantBias& bias_j,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
@ -643,7 +642,7 @@ namespace gtsam {
/** predicted states from IMU */ /** predicted states from IMU */
static void predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j, const imuBias::ConstantBias& bias_i, imuBias::ConstantBias& bias_j,
const CombinedPreintegratedMeasurements& preintegratedMeasurements, const CombinedPreintegratedMeasurements& preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none, const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
@ -666,7 +665,7 @@ namespace gtsam {
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij; + 0.5 * gravity * deltaTij*deltaTij;
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term

24
gtsam/navigation/ImuBias.h
View File

@ -39,7 +39,7 @@ namespace gtsam {
/// All bias models live in the imuBias namespace /// All bias models live in the imuBias namespace
namespace imuBias { namespace imuBias {
class ConstantBias : public DerivedValue<ConstantBias> { class ConstantBias {
private: private:
Vector3 biasAcc_; Vector3 biasAcc_;
Vector3 biasGyro_; Vector3 biasGyro_;
@ -144,7 +144,7 @@ namespace imuBias {
/// return dimensionality of tangent space /// return dimensionality of tangent space
inline size_t dim() const { return dimension; } inline size_t dim() const { return dimension; }
/** Update the LieVector with a tangent space update */ /** Update the bias with a tangent space update */
inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); } inline ConstantBias retract(const Vector& v) const { return ConstantBias(biasAcc_ + v.head(3), biasGyro_ + v.tail(3)); }
/** @return the local coordinates of another object */ /** @return the local coordinates of another object */
@ -205,8 +205,7 @@ namespace imuBias {
template<class ARCHIVE> template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) void serialize(ARCHIVE & ar, const unsigned int version)
{ {
ar & boost::serialization::make_nvp("imuBias::ConstantBias", ar & boost::serialization::make_nvp("imuBias::ConstantBias",*this);
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(biasAcc_); ar & BOOST_SERIALIZATION_NVP(biasAcc_);
ar & BOOST_SERIALIZATION_NVP(biasGyro_); ar & BOOST_SERIALIZATION_NVP(biasGyro_);
} }
@ -218,6 +217,23 @@ namespace imuBias {
} // namespace ImuBias } // namespace ImuBias
// Define GTSAM traits
namespace traits {
template<>
struct is_group<imuBias::ConstantBias> : public boost::true_type {
};
template<>
struct is_manifold<imuBias::ConstantBias> : public boost::true_type {
};
template<>
struct dimension<imuBias::ConstantBias> : public boost::integral_constant<int, 6> {
};
}
} // namespace gtsam } // namespace gtsam

15
gtsam/navigation/ImuFactor.h
View File

@ -21,7 +21,6 @@
#include <gtsam/linear/GaussianFactor.h> #include <gtsam/linear/GaussianFactor.h>
#include <gtsam/navigation/ImuBias.h> #include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/debug.h> #include <gtsam/base/debug.h>
/* External or standard includes */ /* External or standard includes */
@ -46,7 +45,7 @@ namespace gtsam {
* [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013. * [3] L. Carlone, S. Williams, R. Roberts, "Preintegrated IMU factor: Computation of the Jacobian Matrices", Tech. Report, 2013.
*/ */
class ImuFactor: public NoiseModelFactor5<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias> { class ImuFactor: public NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> {
public: public:
@ -223,13 +222,13 @@ namespace gtsam {
Matrix H_vel_vel = I_3x3; Matrix H_vel_vel = I_3x3;
Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT; Matrix H_vel_angles = - deltaRij_.matrix() * skewSymmetric(correctedAcc) * Jr_theta_i * deltaT;
// analytic expression corresponding to the following numerical derivative // analytic expression corresponding to the following numerical derivative
// Matrix H_vel_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i); // Matrix H_vel_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_vel, correctedOmega, correctedAcc, deltaT, _1, deltaVij), theta_i);
Matrix H_angles_pos = Z_3x3; Matrix H_angles_pos = Z_3x3;
Matrix H_angles_vel = Z_3x3; Matrix H_angles_vel = Z_3x3;
Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i; Matrix H_angles_angles = Jrinv_theta_j * Rincr.inverse().matrix() * Jr_theta_i;
// analytic expression corresponding to the following numerical derivative // analytic expression corresponding to the following numerical derivative
// Matrix H_angles_angles = numericalDerivative11<LieVector, LieVector>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij); // Matrix H_angles_angles = numericalDerivative11<Vector3, Vector3>(boost::bind(&PreIntegrateIMUObservations_delta_angles, correctedOmega, deltaT, _1), thetaij);
// overall Jacobian wrt preintegrated measurements (df/dx) // overall Jacobian wrt preintegrated measurements (df/dx)
Matrix F(9,9); Matrix F(9,9);
@ -315,7 +314,7 @@ namespace gtsam {
private: private:
typedef ImuFactor This; typedef ImuFactor This;
typedef NoiseModelFactor5<Pose3,LieVector,Pose3,LieVector,imuBias::ConstantBias> Base; typedef NoiseModelFactor5<Pose3,Vector3,Pose3,Vector3,imuBias::ConstantBias> Base;
PreintegratedMeasurements preintegratedMeasurements_; PreintegratedMeasurements preintegratedMeasurements_;
Vector3 gravity_; Vector3 gravity_;
@ -403,7 +402,7 @@ namespace gtsam {
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/** vector of errors */ /** vector of errors */
Vector evaluateError(const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, Vector evaluateError(const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias, const imuBias::ConstantBias& bias,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
@ -555,7 +554,7 @@ namespace gtsam {
/** predicted states from IMU */ /** predicted states from IMU */
static void predict(const Pose3& pose_i, const LieVector& vel_i, Pose3& pose_j, LieVector& vel_j, static void Predict(const Pose3& pose_i, const Vector3& vel_i, Pose3& pose_j, Vector3& vel_j,
const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements, const imuBias::ConstantBias& bias, const PreintegratedMeasurements preintegratedMeasurements,
const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none, const Vector3& gravity, const Vector3& omegaCoriolis, boost::optional<const Pose3&> body_P_sensor = boost::none,
const bool use2ndOrderCoriolis = false) const bool use2ndOrderCoriolis = false)
@ -577,7 +576,7 @@ namespace gtsam {
- skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper - skewSymmetric(omegaCoriolis) * vel_i * deltaTij*deltaTij // Coriolis term - we got rid of the 2 wrt ins paper
+ 0.5 * gravity * deltaTij*deltaTij; + 0.5 * gravity * deltaTij*deltaTij;
vel_j = LieVector(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_ vel_j = Vector3(vel_i + Rot_i.matrix() * (preintegratedMeasurements.deltaVij_
+ preintegratedMeasurements.delVdelBiasAcc * biasAccIncr + preintegratedMeasurements.delVdelBiasAcc * biasAccIncr
+ preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr) + preintegratedMeasurements.delVdelBiasOmega * biasOmegaIncr)
- 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term - 2 * skewSymmetric(omegaCoriolis) * vel_i * deltaTij // Coriolis term

11
gtsam/navigation/MagFactor.h
View File

@ -19,7 +19,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/Rot2.h> #include <gtsam/geometry/Rot2.h>
#include <gtsam/geometry/Rot3.h> #include <gtsam/geometry/Rot3.h>
#include <gtsam/base/LieScalar.h>
namespace gtsam { namespace gtsam {
@ -61,7 +60,7 @@ public:
static Point3 unrotate(const Rot2& R, const Point3& p, static Point3 unrotate(const Rot2& R, const Point3& p,
boost::optional<Matrix&> HR = boost::none) { boost::optional<Matrix&> HR = boost::none) {
Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR); Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR, boost::none);
if (HR) { if (HR) {
// assign to temporary first to avoid error in Win-Debug mode // assign to temporary first to avoid error in Win-Debug mode
Matrix H = HR->col(2); Matrix H = HR->col(2);
@ -114,7 +113,7 @@ public:
Vector evaluateError(const Rot3& nRb, Vector evaluateError(const Rot3& nRb,
boost::optional<Matrix&> H = boost::none) const { boost::optional<Matrix&> H = boost::none) const {
// measured bM = nRbÕ * nM + b // measured bM = nRbÕ * nM + b
Point3 hx = nRb.unrotate(nM_, H) + bias_; Point3 hx = nRb.unrotate(nM_, H, boost::none) + bias_;
return (hx - measured_).vector(); return (hx - measured_).vector();
} }
}; };
@ -165,7 +164,7 @@ public:
* This version uses model measured bM = scale * bRn * direction + bias * This version uses model measured bM = scale * bRn * direction + bias
* and optimizes for both scale, direction, and the bias. * and optimizes for both scale, direction, and the bias.
*/ */
class MagFactor3: public NoiseModelFactor3<LieScalar, Unit3, Point3> { class MagFactor3: public NoiseModelFactor3<double, Unit3, Point3> {
const Point3 measured_; ///< The measured magnetometer values const Point3 measured_; ///< The measured magnetometer values
const Rot3 bRn_; ///< The assumed known rotation from nav to body const Rot3 bRn_; ///< The assumed known rotation from nav to body
@ -175,7 +174,7 @@ public:
/** Constructor */ /** Constructor */
MagFactor3(Key key1, Key key2, Key key3, const Point3& measured, MagFactor3(Key key1, Key key2, Key key3, const Point3& measured,
const Rot3& nRb, const SharedNoiseModel& model) : const Rot3& nRb, const SharedNoiseModel& model) :
NoiseModelFactor3<LieScalar, Unit3, Point3>(model, key1, key2, key3), // NoiseModelFactor3<double, Unit3, Point3>(model, key1, key2, key3), //
measured_(measured), bRn_(nRb.inverse()) { measured_(measured), bRn_(nRb.inverse()) {
} }
@ -190,7 +189,7 @@ public:
* @param nM (unknown) local earth magnetic field vector, in nav frame * @param nM (unknown) local earth magnetic field vector, in nav frame
* @param bias (unknown) 3D bias * @param bias (unknown) 3D bias
*/ */
Vector evaluateError(const LieScalar& scale, const Unit3& direction, Vector evaluateError(double scale, const Unit3& direction,
const Point3& bias, boost::optional<Matrix&> H1 = boost::none, const Point3& bias, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 = boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H3 =
boost::none) const { boost::none) const {

4
gtsam/navigation/tests/testAttitudeFactor.cpp
View File

@ -45,7 +45,7 @@ TEST( Rot3AttitudeFactor, Constructor ) {
EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5)); EXPECT(assert_equal(zero(2),factor.evaluateError(nRb),1e-5));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Rot3>( Matrix expectedH = numericalDerivative11<Vector,Rot3>(
boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none), boost::bind(&Rot3AttitudeFactor::evaluateError, &factor, _1, boost::none),
nRb); nRb);
@ -78,7 +78,7 @@ TEST( Pose3AttitudeFactor, Constructor ) {
EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5)); EXPECT(assert_equal(zero(2),factor.evaluateError(T),1e-5));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Pose3>( Matrix expectedH = numericalDerivative11<Vector,Pose3>(
boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1, boost::bind(&Pose3AttitudeFactor::evaluateError, &factor, _1,
boost::none), T); boost::none), T);

9
gtsam/navigation/tests/testCombinedImuFactor.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/navigation/CombinedImuFactor.h> #include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/ImuBias.h> #include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -143,9 +142,9 @@ TEST( CombinedImuFactor, ErrorWithBiases )
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias2(Vector3(0.2, 0.2, 0), Vector3(1, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Vector3 v1(0.5, 0.0, 0.0);
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); Vector3 v2(0.5, 0.0, 0.0);
// Measurements // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity; gravity << 0, 0, 9.81;
@ -239,12 +238,12 @@ TEST( CombinedImuFactor, FirstOrderPreIntegratedMeasurements )
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelPdelBias = numericalDerivative11<imuBias::ConstantBias>( Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
Matrix expectedDelVdelBias = numericalDerivative11<imuBias::ConstantBias>( Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);

2
gtsam/navigation/tests/testGPSFactor.cpp
View File

@ -56,7 +56,7 @@ TEST( GPSFactor, Constructors ) {
EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5)); EXPECT(assert_equal(zero(3),factor.evaluateError(T),1e-5));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH = numericalDerivative11<Pose3>( Matrix expectedH = numericalDerivative11<Vector,Pose3>(
boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T); boost::bind(&GPSFactor::evaluateError, &factor, _1, boost::none), T);
// Use the factor to calculate the derivative // Use the factor to calculate the derivative

67
gtsam/navigation/tests/testImuFactor.cpp
View File

@ -20,7 +20,6 @@
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/navigation/ImuBias.h> #include <gtsam/navigation/ImuBias.h>
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -39,14 +38,14 @@ using symbol_shorthand::B;
/* ************************************************************************* */ /* ************************************************************************* */
namespace { namespace {
Vector callEvaluateError(const ImuFactor& factor, Vector callEvaluateError(const ImuFactor& factor,
const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias) const imuBias::ConstantBias& bias)
{ {
return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias); return factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias);
} }
Rot3 evaluateRotationError(const ImuFactor& factor, Rot3 evaluateRotationError(const ImuFactor& factor,
const Pose3& pose_i, const LieVector& vel_i, const Pose3& pose_j, const LieVector& vel_j, const Pose3& pose_i, const Vector3& vel_i, const Pose3& pose_j, const Vector3& vel_j,
const imuBias::ConstantBias& bias) const imuBias::ConstantBias& bias)
{ {
return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ; return Rot3::Expmap(factor.evaluateError(pose_i, vel_i, pose_j, vel_j, bias).tail(3) ) ;
@ -168,9 +167,9 @@ TEST( ImuFactor, Error )
// Linearization point // Linearization point
imuBias::ConstantBias bias; // Bias imuBias::ConstantBias bias; // Bias
Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity; gravity << 0, 0, 9.81;
@ -192,15 +191,15 @@ TEST( ImuFactor, Error )
EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians // Expected Jacobians
Matrix H1e = numericalDerivative11<Pose3>( Matrix H1e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<LieVector>( Matrix H2e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Pose3>( Matrix H3e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<LieVector>( Matrix H4e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<imuBias::ConstantBias>( Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians // Check rotation Jacobians
@ -240,16 +239,16 @@ TEST( ImuFactor, ErrorWithBiases )
// Linearization point // Linearization point
// Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot) // Vector bias(6); bias << 0.2, 0, 0, 0.1, 0, 0; // Biases (acc, rot)
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); // Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/10.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity; gravity << 0, 0, 9.81;
@ -276,15 +275,15 @@ TEST( ImuFactor, ErrorWithBiases )
// EXPECT(assert_equal(errorExpected, errorActual, 1e-6)); // EXPECT(assert_equal(errorExpected, errorActual, 1e-6));
// Expected Jacobians // Expected Jacobians
Matrix H1e = numericalDerivative11<Pose3>( Matrix H1e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<LieVector>( Matrix H2e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Pose3>( Matrix H3e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<LieVector>( Matrix H4e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<imuBias::ConstantBias>( Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians // Check rotation Jacobians
@ -318,8 +317,8 @@ TEST( ImuFactor, PartialDerivativeExpmap )
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, LieVector>(boost::bind( Matrix expectedDelRdelBiasOmega = numericalDerivative11<Rot3, Vector3>(boost::bind(
&evaluateRotation, measuredOmega, _1, deltaT), LieVector(biasOmega)); &evaluateRotation, measuredOmega, _1, deltaT), Vector3(biasOmega));
const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT); const Matrix3 Jr = Rot3::rightJacobianExpMapSO3((measuredOmega - biasOmega) * deltaT);
@ -341,8 +340,8 @@ TEST( ImuFactor, PartialDerivativeLogmap )
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelFdeltheta = numericalDerivative11<LieVector>(boost::bind( Matrix expectedDelFdeltheta = numericalDerivative11<Vector,Vector3>(boost::bind(
&evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); &evaluateLogRotation, thetahat, _1), Vector3(deltatheta));
const Vector3 x = thetahat; // parametrization of so(3) const Vector3 x = thetahat; // parametrization of so(3)
const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^ const Matrix3 X = skewSymmetric(x); // element of Lie algebra so(3): X = x^
@ -417,12 +416,12 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)); evaluatePreintegratedMeasurements(bias, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0));
// Compute numerical derivatives // Compute numerical derivatives
Matrix expectedDelPdelBias = numericalDerivative11<imuBias::ConstantBias>( Matrix expectedDelPdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); boost::bind(&evaluatePreintegratedMeasurementsPosition, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3); Matrix expectedDelPdelBiasAcc = expectedDelPdelBias.leftCols(3);
Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3); Matrix expectedDelPdelBiasOmega = expectedDelPdelBias.rightCols(3);
Matrix expectedDelVdelBias = numericalDerivative11<imuBias::ConstantBias>( Matrix expectedDelVdelBias = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias); boost::bind(&evaluatePreintegratedMeasurementsVelocity, _1, measuredAccs, measuredOmegas, deltaTs, Vector3(M_PI/100.0, 0.0, 0.0)), bias);
Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3); Matrix expectedDelVdelBiasAcc = expectedDelVdelBias.leftCols(3);
Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3); Matrix expectedDelVdelBiasOmega = expectedDelVdelBias.rightCols(3);
@ -447,9 +446,9 @@ TEST( ImuFactor, FirstOrderPreIntegratedMeasurements )
//{ //{
// // Linearization point // // Linearization point
// Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0)); // Pose3 x1(Rot3::RzRyRx(M_PI/12.0, M_PI/6.0, M_PI/4.0), Point3(5.0, 1.0, -50.0));
// LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); // Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
// Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0)); // Pose3 x2(Rot3::RzRyRx(M_PI/12.0 + M_PI/100.0, M_PI/6.0, M_PI/4.0), Point3(5.5, 1.0, -50.0));
// LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); // Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
// imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012)); // imuBias::ConstantBias bias(Vector3(0.001, 0.002, 0.008), Vector3(0.002, 0.004, 0.012));
// //
// // Pre-integrator // // Pre-integrator
@ -503,9 +502,9 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot) imuBias::ConstantBias bias(Vector3(0.2, 0, 0), Vector3(0, 0, 0.3)); // Biases (acc, rot)
Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0)); Pose3 x1(Rot3::Expmap(Vector3(0, 0, M_PI/4.0)), Point3(5.0, 1.0, -50.0));
LieVector v1((Vector(3) << 0.5, 0.0, 0.0)); Vector3 v1((Vector(3) << 0.5, 0.0, 0.0));
Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0)); Pose3 x2(Rot3::Expmap(Vector3(0, 0, M_PI/4.0 + M_PI/10.0)), Point3(5.5, 1.0, -50.0));
LieVector v2((Vector(3) << 0.5, 0.0, 0.0)); Vector3 v2((Vector(3) << 0.5, 0.0, 0.0));
// Measurements // Measurements
Vector3 gravity; gravity << 0, 0, 9.81; Vector3 gravity; gravity << 0, 0, 9.81;
@ -531,15 +530,15 @@ TEST( ImuFactor, ErrorWithBiasesAndSensorBodyDisplacement )
ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis); ImuFactor factor(X(1), V(1), X(2), V(2), B(1), pre_int_data, gravity, omegaCoriolis);
// Expected Jacobians // Expected Jacobians
Matrix H1e = numericalDerivative11<Pose3>( Matrix H1e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1); boost::bind(&callEvaluateError, factor, _1, v1, x2, v2, bias), x1);
Matrix H2e = numericalDerivative11<LieVector>( Matrix H2e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1); boost::bind(&callEvaluateError, factor, x1, _1, x2, v2, bias), v1);
Matrix H3e = numericalDerivative11<Pose3>( Matrix H3e = numericalDerivative11<Vector,Pose3>(
boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2); boost::bind(&callEvaluateError, factor, x1, v1, _1, v2, bias), x2);
Matrix H4e = numericalDerivative11<LieVector>( Matrix H4e = numericalDerivative11<Vector,Vector3>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2); boost::bind(&callEvaluateError, factor, x1, v1, x2, _1, bias), v2);
Matrix H5e = numericalDerivative11<imuBias::ConstantBias>( Matrix H5e = numericalDerivative11<Vector,imuBias::ConstantBias>(
boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias); boost::bind(&callEvaluateError, factor, x1, v1, x2, v2, _1), bias);
// Check rotation Jacobians // Check rotation Jacobians

14
gtsam/navigation/tests/testMagFactor.cpp
View File

@ -72,35 +72,35 @@ TEST( MagFactor, Factors ) {
// MagFactor // MagFactor
MagFactor f(1, measured, s, dir, bias, model); MagFactor f(1, measured, s, dir, bias, model);
EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5)); EXPECT( assert_equal(zero(3),f.evaluateError(theta,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Rot2> // EXPECT( assert_equal(numericalDerivative11<Vector,Rot2> //
(boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7)); (boost::bind(&MagFactor::evaluateError, &f, _1, none), theta), H1, 1e-7));
// MagFactor1 // MagFactor1
MagFactor1 f1(1, measured, s, dir, bias, model); MagFactor1 f1(1, measured, s, dir, bias, model);
EXPECT( assert_equal(zero(3),f1.evaluateError(nRb,H1),1e-5)); EXPECT( assert_equal(zero(3),f1.evaluateError(nRb,H1),1e-5));
EXPECT( assert_equal(numericalDerivative11<Rot3> // EXPECT( assert_equal(numericalDerivative11<Vector,Rot3> //
(boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7)); (boost::bind(&MagFactor1::evaluateError, &f1, _1, none), nRb), H1, 1e-7));
// MagFactor2 // MagFactor2
MagFactor2 f2(1, 2, measured, nRb, model); MagFactor2 f2(1, 2, measured, nRb, model);
EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5)); EXPECT( assert_equal(zero(3),f2.evaluateError(scaled,bias,H1,H2),1e-5));
EXPECT( assert_equal(numericalDerivative11<Point3> // EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),// (boost::bind(&MagFactor2::evaluateError, &f2, _1, bias, none, none), scaled),//
H1, 1e-7)); H1, 1e-7));
EXPECT( assert_equal(numericalDerivative11<Point3> // EXPECT( assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),// (boost::bind(&MagFactor2::evaluateError, &f2, scaled, _1, none, none), bias),//
H2, 1e-7)); H2, 1e-7));
// MagFactor2 // MagFactor2
MagFactor3 f3(1, 2, 3, measured, nRb, model); MagFactor3 f3(1, 2, 3, measured, nRb, model);
EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5)); EXPECT(assert_equal(zero(3),f3.evaluateError(s,dir,bias,H1,H2,H3),1e-5));
EXPECT(assert_equal(numericalDerivative11<LieScalar> // EXPECT(assert_equal(numericalDerivative11<Vector,LieScalar> //
(boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),// (boost::bind(&MagFactor3::evaluateError, &f3, _1, dir, bias, none, none, none), s),//
H1, 1e-7)); H1, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Unit3> // EXPECT(assert_equal(numericalDerivative11<Vector,Unit3> //
(boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),// (boost::bind(&MagFactor3::evaluateError, &f3, s, _1, bias, none, none, none), dir),//
H2, 1e-7)); H2, 1e-7));
EXPECT(assert_equal(numericalDerivative11<Point3> // EXPECT(assert_equal(numericalDerivative11<Vector,Point3> //
(boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),// (boost::bind(&MagFactor3::evaluateError, &f3, s, dir, _1, none, none, none), bias),//
H3, 1e-7)); H3, 1e-7));
} }

145
gtsam/nonlinear/NonlinearFactor.cpp Normal file
View File

@ -0,0 +1,145 @@
/* ----------------------------------------------------------------------------
* 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 NonlinearFactor.cpp
* @brief Nonlinear Factor base classes
* @author Frank Dellaert
* @author Richard Roberts
*/
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <boost/make_shared.hpp>
#include <boost/format.hpp>
namespace gtsam {
/* ************************************************************************* */
void NonlinearFactor::print(const std::string& s,
const KeyFormatter& keyFormatter) const {
std::cout << s << " keys = { ";
BOOST_FOREACH(Key key, keys()) {
std::cout << keyFormatter(key) << " ";
}
std::cout << "}" << std::endl;
}
/* ************************************************************************* */
bool NonlinearFactor::equals(const NonlinearFactor& f, double tol) const {
return Base::equals(f);
}
/* ************************************************************************* */
NonlinearFactor::shared_ptr NonlinearFactor::rekey(
const std::map<Key, Key>& rekey_mapping) const {
shared_ptr new_factor = clone();
for (size_t i = 0; i < new_factor->size(); ++i) {
Key& cur_key = new_factor->keys()[i];
std::map<Key, Key>::const_iterator mapping = rekey_mapping.find(cur_key);
if (mapping != rekey_mapping.end())
cur_key = mapping->second;
}
return new_factor;
}
/* ************************************************************************* */
NonlinearFactor::shared_ptr NonlinearFactor::rekey(
const std::vector<Key>& new_keys) const {
assert(new_keys.size() == keys().size());
shared_ptr new_factor = clone();
new_factor->keys() = new_keys;
return new_factor;
}
/* ************************************************************************* */
void NoiseModelFactor::print(const std::string& s,
const KeyFormatter& keyFormatter) const {
Base::print(s, keyFormatter);
if (noiseModel_)
noiseModel_->print(" noise model: ");
}
/* ************************************************************************* */
bool NoiseModelFactor::equals(const NonlinearFactor& f, double tol) const {
const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
return e && Base::equals(f, tol)
&& ((!noiseModel_ && !e->noiseModel_)
|| (noiseModel_ && e->noiseModel_
&& noiseModel_->equals(*e->noiseModel_, tol)));
}
/* ************************************************************************* */
static void check(const SharedNoiseModel& noiseModel, size_t m) {
if (noiseModel && m != noiseModel->dim())
throw std::invalid_argument(
boost::str(
boost::format(
"NoiseModelFactor: NoiseModel has dimension %1% instead of %2%.")
% noiseModel->dim() % m));
}
/* ************************************************************************* */
Vector NoiseModelFactor::whitenedError(const Values& c) const {
const Vector b = unwhitenedError(c);
check(noiseModel_, b.size());
return noiseModel_ ? noiseModel_->whiten(b) : b;
}
/* ************************************************************************* */
double NoiseModelFactor::error(const Values& c) const {
if (active(c)) {
const Vector b = unwhitenedError(c);
check(noiseModel_, b.size());
if (noiseModel_)
return 0.5 * noiseModel_->distance(b);
else
return 0.5 * b.squaredNorm();
} else {
return 0.0;
}
}
/* ************************************************************************* */
boost::shared_ptr<GaussianFactor> NoiseModelFactor::linearize(
const Values& x) const {
// Only linearize if the factor is active
if (!active(x))
return boost::shared_ptr<JacobianFactor>();
// Call evaluate error to get Jacobians and RHS vector b
std::vector<Matrix> A(size());
Vector b = -unwhitenedError(x, A);
check(noiseModel_, b.size());
// Whiten the corresponding system now
if (noiseModel_)
noiseModel_->WhitenSystem(A, b);
// Fill in terms, needed to create JacobianFactor below
std::vector<std::pair<Key, Matrix> > terms(size());
for (size_t j = 0; j < size(); ++j) {
terms[j].first = keys()[j];
terms[j].second.swap(A[j]);
}
// TODO pass unwhitened + noise model to Gaussian factor
if (noiseModel_ && noiseModel_->is_constrained())
return GaussianFactor::shared_ptr(
new JacobianFactor(terms, b,
boost::static_pointer_cast<noiseModel::Constrained>(noiseModel_)->unit()));
else
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
}
/* ************************************************************************* */
} // \namespace gtsam

134
gtsam/nonlinear/NonlinearFactor.h
View File

@ -43,12 +43,10 @@ namespace gtsam {
using boost::assign::cref_list_of; using boost::assign::cref_list_of;
/* ************************************************************************* */ /* ************************************************************************* */
/** /**
* Nonlinear factor base class * Nonlinear factor base class
* *
* Templated on a values structure type. The values structures are typically
* more general than just vectors, e.g., Rot3 or Pose3,
* which are objects in non-linear manifolds (Lie groups).
* \nosubgrouping * \nosubgrouping
*/ */
class NonlinearFactor: public Factor { class NonlinearFactor: public Factor {
@ -82,18 +80,10 @@ public:
/** print */ /** print */
virtual void print(const std::string& s = "", virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
{
std::cout << s << " keys = { ";
BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; }
std::cout << "}" << std::endl;
}
/** Check if two factors are equal */ /** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
return Base::equals(f);
}
/// @} /// @}
/// @name Standard Interface /// @name Standard Interface
/// @{ /// @{
@ -128,17 +118,6 @@ public:
virtual boost::shared_ptr<GaussianFactor> virtual boost::shared_ptr<GaussianFactor>
linearize(const Values& c) const = 0; linearize(const Values& c) const = 0;
/**
* Create a symbolic factor using the given ordering to determine the
* variable indices.
*/
//virtual IndexFactor::shared_ptr symbolic(const Ordering& ordering) const {
// std::vector<Index> indices(this->size());
// for(size_t j=0; j<this->size(); ++j)
// indices[j] = ordering[this->keys()[j]];
// return IndexFactor::shared_ptr(new IndexFactor(indices));
//}
/** /**
* Creates a shared_ptr clone of the factor - needs to be specialized to allow * Creates a shared_ptr clone of the factor - needs to be specialized to allow
* for subclasses * for subclasses
@ -155,28 +134,13 @@ public:
* Creates a shared_ptr clone of the factor with different keys using * Creates a shared_ptr clone of the factor with different keys using
* a map from old->new keys * a map from old->new keys
*/ */
shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const { shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const;
shared_ptr new_factor = clone();
for (size_t i=0; i<new_factor->size(); ++i) {
Key& cur_key = new_factor->keys()[i];
std::map<Key,Key>::const_iterator mapping = rekey_mapping.find(cur_key);
if (mapping != rekey_mapping.end())
cur_key = mapping->second;
}
return new_factor;
}
/** /**
* Clones a factor and fully replaces its keys * Clones a factor and fully replaces its keys
* @param new_keys is the full replacement set of keys * @param new_keys is the full replacement set of keys
*/ */
shared_ptr rekey(const std::vector<Key>& new_keys) const { shared_ptr rekey(const std::vector<Key>& new_keys) const;
assert(new_keys.size() == this->keys().size());
shared_ptr new_factor = clone();
new_factor->keys() = new_keys;
return new_factor;
}
}; // \class NonlinearFactor }; // \class NonlinearFactor
@ -229,19 +193,10 @@ public:
/** Print */ /** Print */
virtual void print(const std::string& s = "", virtual void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
{
Base::print(s, keyFormatter);
this->noiseModel_->print(" noise model: ");
}
/** Check if two factors are equal */ /** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const { virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const;
const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
return e && Base::equals(f, tol) &&
((!noiseModel_ && !e->noiseModel_) ||
(noiseModel_ && e->noiseModel_ && noiseModel_->equals(*e->noiseModel_, tol)));
}
/** get the dimension of the factor (number of rows on linearization) */ /** get the dimension of the factor (number of rows on linearization) */
virtual size_t dim() const { virtual size_t dim() const {
@ -256,21 +211,17 @@ public:
/** /**
* Error function *without* the NoiseModel, \f$ z-h(x) \f$. * Error function *without* the NoiseModel, \f$ z-h(x) \f$.
* Override this method to finish implementing an N-way factor. * Override this method to finish implementing an N-way factor.
* If any of the optional Matrix reference arguments are specified, it should compute * If the optional arguments is specified, it should compute
* both the function evaluation and its derivative(s) in X1 (and/or X2, X3...). * both the function evaluation and its derivative(s) in H.
*/ */
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0; virtual Vector unwhitenedError(const Values& x,
boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
/** /**
* Vector of errors, whitened * Vector of errors, whitened
* This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian * This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian
*/ */
Vector whitenedError(const Values& c) const { Vector whitenedError(const Values& c) const;
const Vector unwhitenedErrorVec = unwhitenedError(c);
if((size_t) unwhitenedErrorVec.size() != noiseModel_->dim())
throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension.");
return noiseModel_->whiten(unwhitenedErrorVec);
}
/** /**
* Calculate the error of the factor. * Calculate the error of the factor.
@ -278,60 +229,14 @@ public:
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model * In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5. * to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
*/ */
virtual double error(const Values& c) const { virtual double error(const Values& c) const;
if (this->active(c)) {
const Vector unwhitenedErrorVec = unwhitenedError(c);
if((size_t) unwhitenedErrorVec.size() != noiseModel_->dim())
throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension.");
return 0.5 * noiseModel_->distance(unwhitenedErrorVec);
} else {
return 0.0;
}
}
/** /**
* Linearize a non-linearFactorN to get a GaussianFactor, * Linearize a non-linearFactorN to get a GaussianFactor,
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$ * \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$ * Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/ */
boost::shared_ptr<GaussianFactor> linearize(const Values& x) const { boost::shared_ptr<GaussianFactor> linearize(const Values& x) const;
// Only linearize if the factor is active
if (!this->active(x))
return boost::shared_ptr<JacobianFactor>();
// Create the set of terms - Jacobians for each index
Vector b;
// Call evaluate error to get Jacobians and b vector
std::vector<Matrix> A(this->size());
b = -unwhitenedError(x, A);
if(noiseModel_)
{
if((size_t) b.size() != noiseModel_->dim())
throw std::invalid_argument("This factor was created with a NoiseModel of incorrect dimension.");
this->noiseModel_->WhitenSystem(A,b);
}
std::vector<std::pair<Key, Matrix> > terms(this->size());
// Fill in terms
for(size_t j=0; j<this->size(); ++j) {
terms[j].first = this->keys()[j];
terms[j].second.swap(A[j]);
}
if(noiseModel_)
{
// TODO pass unwhitened + noise model to Gaussian factor
noiseModel::Constrained::shared_ptr constrained =
boost::dynamic_pointer_cast<noiseModel::Constrained>(this->noiseModel_);
if(constrained)
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b, constrained->unit()));
else
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
}
else
return GaussianFactor::shared_ptr(new JacobianFactor(terms, b));
}
private: private:
@ -348,8 +253,15 @@ private:
/* ************************************************************************* */ /* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 1
* variable. To derive from this class, implement evaluateError(). */ /**
* A convenient base class for creating your own NoiseModelFactor with 1
* variable. To derive from this class, implement evaluateError().
*
* Templated on a values structure type. The values structures are typically
* more general than just vectors, e.g., Rot3 or Pose3,
* which are objects in non-linear manifolds (Lie groups).
*/
template<class VALUE> template<class VALUE>
class NoiseModelFactor1: public NoiseModelFactor { class NoiseModelFactor1: public NoiseModelFactor {

107
gtsam/nonlinear/Values-inl.h
View File

@ -33,6 +33,7 @@
namespace gtsam { namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
template<class ValueType> template<class ValueType>
struct _ValuesKeyValuePair { struct _ValuesKeyValuePair {
@ -52,6 +53,36 @@ namespace gtsam {
_ValuesConstKeyValuePair(const _ValuesKeyValuePair<ValueType>& rhs) : key(rhs.key), value(rhs.value) {} _ValuesConstKeyValuePair(const _ValuesKeyValuePair<ValueType>& rhs) : key(rhs.key), value(rhs.value) {}
}; };
/* ************************************************************************* */
// Cast helpers for making _Values[Const]KeyValuePair's from Values::[Const]KeyValuePair
// need to use a struct here for later partial specialization
template<class ValueType, class CastedKeyValuePairType, class KeyValuePairType>
struct ValuesCastHelper {
static CastedKeyValuePairType cast(KeyValuePairType key_value) {
// Static cast because we already checked the type during filtering
return CastedKeyValuePairType(key_value.key, const_cast<GenericValue<ValueType>&>(static_cast<const GenericValue<ValueType>&>(key_value.value)).value());
}
};
// partial specialized version for ValueType == Value
template<class CastedKeyValuePairType, class KeyValuePairType>
struct ValuesCastHelper<Value, CastedKeyValuePairType, KeyValuePairType> {
static CastedKeyValuePairType cast(KeyValuePairType key_value) {
// Static cast because we already checked the type during filtering
// in this case the casted and keyvalue pair are essentially the same type (key, Value&) so perhaps this could be done with just a cast of the key_value?
return CastedKeyValuePairType(key_value.key, key_value.value);
}
};
// partial specialized version for ValueType == Value
template<class CastedKeyValuePairType, class KeyValuePairType>
struct ValuesCastHelper<const Value, CastedKeyValuePairType, KeyValuePairType> {
static CastedKeyValuePairType cast(KeyValuePairType key_value) {
// Static cast because we already checked the type during filtering
// in this case the casted and keyvalue pair are essentially the same type (key, Value&) so perhaps this could be done with just a cast of the key_value?
return CastedKeyValuePairType(key_value.key, key_value.value);
}
};
/* ************************************************************************* */ /* ************************************************************************* */
template<class ValueType> template<class ValueType>
class Values::Filtered { class Values::Filtered {
@ -99,19 +130,19 @@ namespace gtsam {
begin_(boost::make_transform_iterator( begin_(boost::make_transform_iterator(
boost::make_filter_iterator( boost::make_filter_iterator(
filter, values.begin(), values.end()), filter, values.begin(), values.end()),
&castHelper<ValueType, KeyValuePair, Values::KeyValuePair>)), &ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)),
end_(boost::make_transform_iterator( end_(boost::make_transform_iterator(
boost::make_filter_iterator( boost::make_filter_iterator(
filter, values.end(), values.end()), filter, values.end(), values.end()),
&castHelper<ValueType, KeyValuePair, Values::KeyValuePair>)), &ValuesCastHelper<ValueType, KeyValuePair, Values::KeyValuePair>::cast)),
constBegin_(boost::make_transform_iterator( constBegin_(boost::make_transform_iterator(
boost::make_filter_iterator( boost::make_filter_iterator(
filter, ((const Values&)values).begin(), ((const Values&)values).end()), filter, ((const Values&)values).begin(), ((const Values&)values).end()),
&castHelper<const ValueType, ConstKeyValuePair, Values::ConstKeyValuePair>)), &ValuesCastHelper<const ValueType, ConstKeyValuePair, Values::ConstKeyValuePair>::cast)),
constEnd_(boost::make_transform_iterator( constEnd_(boost::make_transform_iterator(
boost::make_filter_iterator( boost::make_filter_iterator(
filter, ((const Values&)values).end(), ((const Values&)values).end()), filter, ((const Values&)values).end(), ((const Values&)values).end()),
&castHelper<const ValueType, ConstKeyValuePair, Values::ConstKeyValuePair>)) {} &ValuesCastHelper<const ValueType, ConstKeyValuePair, Values::ConstKeyValuePair>::cast)) {}
friend class Values; friend class Values;
iterator begin_; iterator begin_;
@ -175,7 +206,7 @@ namespace gtsam {
Values::Values(const Values::Filtered<ValueType>& view) { Values::Values(const Values::Filtered<ValueType>& view) {
BOOST_FOREACH(const typename Filtered<ValueType>::KeyValuePair& key_value, view) { BOOST_FOREACH(const typename Filtered<ValueType>::KeyValuePair& key_value, view) {
Key key = key_value.key; Key key = key_value.key;
insert(key, key_value.value); insert(key, static_cast<const ValueType&>(key_value.value));
} }
} }
@ -184,7 +215,7 @@ namespace gtsam {
Values::Values(const Values::ConstFiltered<ValueType>& view) { Values::Values(const Values::ConstFiltered<ValueType>& view) {
BOOST_FOREACH(const typename ConstFiltered<ValueType>::KeyValuePair& key_value, view) { BOOST_FOREACH(const typename ConstFiltered<ValueType>::KeyValuePair& key_value, view) {
Key key = key_value.key; Key key = key_value.key;
insert(key, key_value.value); insert<ValueType>(key, key_value.value);
} }
} }
@ -214,6 +245,13 @@ namespace gtsam {
return ConstFiltered<ValueType>(boost::bind(&filterHelper<ValueType>, filterFcn, _1), *this); return ConstFiltered<ValueType>(boost::bind(&filterHelper<ValueType>, filterFcn, _1), *this);
} }
/* ************************************************************************* */
template<>
inline bool Values::filterHelper<Value>(const boost::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
// Filter and check the type
return filter(key_value.key);
}
/* ************************************************************************* */ /* ************************************************************************* */
template<typename ValueType> template<typename ValueType>
const ValueType& Values::at(Key j) const { const ValueType& Values::at(Key j) const {
@ -225,11 +263,11 @@ namespace gtsam {
throw ValuesKeyDoesNotExist("retrieve", j); throw ValuesKeyDoesNotExist("retrieve", j);
// Check the type and throw exception if incorrect // Check the type and throw exception if incorrect
if(typeid(*item->second) != typeid(ValueType)) try {
return dynamic_cast<const GenericValue<ValueType>&>(*item->second).value();
} catch (std::bad_cast &) {
throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
}
// We have already checked the type, so do a "blind" static_cast, not dynamic_cast
return static_cast<const ValueType&>(*item->second);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -239,15 +277,52 @@ namespace gtsam {
KeyValueMap::const_iterator item = values_.find(j); KeyValueMap::const_iterator item = values_.find(j);
if(item != values_.end()) { if(item != values_.end()) {
// Check the type and throw exception if incorrect // dynamic cast the type and throw exception if incorrect
if(typeid(*item->second) != typeid(ValueType)) try {
return dynamic_cast<const GenericValue<ValueType>&>(*item->second).value();
} catch (std::bad_cast &) {
throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType)); throw ValuesIncorrectType(j, typeid(*item->second), typeid(ValueType));
}
// We have already checked the type, so do a "blind" static_cast, not dynamic_cast } else {
return static_cast<const ValueType&>(*item->second);
} else {
return boost::none; return boost::none;
} }
} }
/* ************************************************************************* */
// insert a plain value using the default chart
template<typename ValueType>
void Values::insert(Key j, const ValueType& val) {
insert(j, static_cast<const Value&>(ChartValue<ValueType, DefaultChart<ValueType> >(val)));
}
// insert with custom chart type
template<typename ValueType, typename Chart>
void Values::insert(Key j, const ValueType& val) {
insert(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val)));
}
// overloaded insert with chart initializer
template<typename ValueType, typename Chart, typename ChartInit>
void Values::insert(Key j, const ValueType& val, ChartInit chart) {
insert(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, chart)));
}
// update with default chart
template <typename ValueType>
void Values::update(Key j, const ValueType& val) {
update(j, static_cast<const Value&>(ChartValue<ValueType, DefaultChart<ValueType> >(val)));
}
// update with custom chart
template <typename ValueType, typename Chart>
void Values::update(Key j, const ValueType& val) {
update(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val)));
}
// update with chart initializer, /todo: perhaps there is a way to init chart from old value...
template<typename ValueType, typename Chart, typename ChartInit>
void Values::update(Key j, const ValueType& val, ChartInit chart) {
update(j, static_cast<const Value&>(ChartValue<ValueType, Chart>(val, chart)));
}
} }

43
gtsam/nonlinear/Values.h
View File

@ -44,7 +44,7 @@
#include <string> #include <string>
#include <utility> #include <utility>
#include <gtsam/base/Value.h> #include <gtsam/base/ChartValue.h>
#include <gtsam/base/FastMap.h> #include <gtsam/base/FastMap.h>
#include <gtsam/inference/Key.h> #include <gtsam/inference/Key.h>
@ -251,6 +251,22 @@ namespace gtsam {
/** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */ /** Add a set of variables, throws KeyAlreadyExists<J> if a key is already present */
void insert(const Values& values); void insert(const Values& values);
/** Templated version to add a variable with the given j,
* throws KeyAlreadyExists<J> if j is already present
* if no chart is specified, the DefaultChart<ValueType> is used
*/
template <typename ValueType>
void insert(Key j, const ValueType& val);
/// overloaded insert version that also specifies a chart
template <typename ValueType, typename Chart>
void insert(Key j, const ValueType& val);
/// overloaded insert version that also specifies a chart initializer
template <typename ValueType, typename Chart, typename ChartInit>
void insert(Key j, const ValueType& val, ChartInit chart);
/** insert that mimics the STL map insert - if the value already exists, the map is not modified /** insert that mimics the STL map insert - if the value already exists, the map is not modified
* and an iterator to the existing value is returned, along with 'false'. If the value did not * and an iterator to the existing value is returned, along with 'false'. If the value did not
* exist, it is inserted and an iterator pointing to the new element, along with 'true', is * exist, it is inserted and an iterator pointing to the new element, along with 'true', is
@ -260,6 +276,21 @@ namespace gtsam {
/** single element change of existing element */ /** single element change of existing element */
void update(Key j, const Value& val); void update(Key j, const Value& val);
/** Templated version to update a variable with the given j,
* throws KeyAlreadyExists<J> if j is already present
* if no chart is specified, the DefaultChart<ValueType> is used
*/
template <typename T>
void update(Key j, const T& val);
/// overloaded insert version that also specifies a chart
template <typename T, typename Chart>
void update(Key j, const T& val);
/// overloaded insert version that also specifies a chart initializer
template <typename T, typename Chart, typename ChartInit>
void update(Key j, const T& val, ChartInit chart);
/** update the current available values without adding new ones */ /** update the current available values without adding new ones */
void update(const Values& values); void update(const Values& values);
@ -369,15 +400,9 @@ namespace gtsam {
// supplied \c filter function. // supplied \c filter function.
template<class ValueType> template<class ValueType>
static bool filterHelper(const boost::function<bool(Key)> filter, const ConstKeyValuePair& key_value) { static bool filterHelper(const boost::function<bool(Key)> filter, const ConstKeyValuePair& key_value) {
BOOST_STATIC_ASSERT((!boost::is_same<ValueType, Value>::value));
// Filter and check the type // Filter and check the type
return filter(key_value.key) && (typeid(ValueType) == typeid(key_value.value) || typeid(ValueType) == typeid(Value)); return filter(key_value.key) && (dynamic_cast<const GenericValue<ValueType>*>(&key_value.value));
}
// Cast to the derived ValueType
template<class ValueType, class CastedKeyValuePairType, class KeyValuePairType>
static CastedKeyValuePairType castHelper(KeyValuePairType key_value) {
// Static cast because we already checked the type during filtering
return CastedKeyValuePairType(key_value.key, static_cast<ValueType&>(key_value.value));
} }
/** Serialization function */ /** Serialization function */

7
gtsam/nonlinear/WhiteNoiseFactor.h
View File

@ -19,7 +19,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/linear/HessianFactor.h> #include <gtsam/linear/HessianFactor.h>
#include <gtsam/base/LieScalar.h>
#include <cmath> #include <cmath>
namespace gtsam { namespace gtsam {
@ -126,7 +125,7 @@ namespace gtsam {
/// Calculate the error of the factor, typically equal to log-likelihood /// Calculate the error of the factor, typically equal to log-likelihood
inline double error(const Values& x) const { inline double error(const Values& x) const {
return f(z_, x.at<LieScalar>(meanKey_), x.at<LieScalar>(precisionKey_)); return f(z_, x.at<double>(meanKey_), x.at<double>(precisionKey_));
} }
/** /**
@ -155,8 +154,8 @@ namespace gtsam {
/// linearize returns a Hessianfactor that is an approximation of error(p) /// linearize returns a Hessianfactor that is an approximation of error(p)
virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const { virtual boost::shared_ptr<GaussianFactor> linearize(const Values& x) const {
double u = x.at<LieScalar>(meanKey_); double u = x.at<double>(meanKey_);
double p = x.at<LieScalar>(precisionKey_); double p = x.at<double>(precisionKey_);
Key j1 = meanKey_; Key j1 = meanKey_;
Key j2 = precisionKey_; Key j2 = precisionKey_;
return linearize(z_, u, p, j1, j2); return linearize(z_, u, p, j1, j2);

48
gtsam/nonlinear/tests/testSerializationNonlinear.cpp
View File

@ -32,20 +32,37 @@ using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */ /* ************************************************************************* */
// Export all classes derived from Value // Export all classes derived from Value
BOOST_CLASS_EXPORT(gtsam::Cal3_S2) BOOST_CLASS_EXPORT(gtsam::Cal3_S2);
BOOST_CLASS_EXPORT(gtsam::Cal3Bundler) BOOST_CLASS_EXPORT(gtsam::Cal3Bundler);
BOOST_CLASS_EXPORT(gtsam::Point3) BOOST_CLASS_EXPORT(gtsam::Point3);
BOOST_CLASS_EXPORT(gtsam::Pose3) BOOST_CLASS_EXPORT(gtsam::Pose3);
BOOST_CLASS_EXPORT(gtsam::Rot3) BOOST_CLASS_EXPORT(gtsam::Rot3);
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3_S2>) BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3_S2>);
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>) BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>);
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>) BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>);
namespace detail {
template<class T> struct pack {
typedef T type;
};
}
#define CHART_VALUE_EXPORT(UNIQUE_NAME, TYPE) \
typedef gtsam::ChartValue<TYPE,gtsam::DefaultChart<TYPE> > UNIQUE_NAME; \
BOOST_CLASS_EXPORT( UNIQUE_NAME );
/* ************************************************************************* */ /* ************************************************************************* */
typedef PinholeCamera<Cal3_S2> PinholeCal3S2; typedef PinholeCamera<Cal3_S2> PinholeCal3S2;
typedef PinholeCamera<Cal3DS2> PinholeCal3DS2; typedef PinholeCamera<Cal3DS2> PinholeCal3DS2;
typedef PinholeCamera<Cal3Bundler> PinholeCal3Bundler; typedef PinholeCamera<Cal3Bundler> PinholeCal3Bundler;
CHART_VALUE_EXPORT(gtsamPoint3Chart, gtsam::Point3);
CHART_VALUE_EXPORT(Cal3S2Chart, PinholeCal3S2);
CHART_VALUE_EXPORT(Cal3DS2Chart, PinholeCal3DS2);
CHART_VALUE_EXPORT(Cal3BundlerChart, PinholeCal3Bundler);
/* ************************************************************************* */ /* ************************************************************************* */
static Point3 pt3(1.0, 2.0, 3.0); static Point3 pt3(1.0, 2.0, 3.0);
static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0); static Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
@ -56,12 +73,27 @@ static Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
static Cal3Bundler cal3(1.0, 2.0, 3.0); static Cal3Bundler cal3(1.0, 2.0, 3.0);
TEST (Serialization, TemplatedValues) { TEST (Serialization, TemplatedValues) {
std::cout << __LINE__ << std::endl;
EXPECT(equalsObj(pt3));
std::cout << __LINE__ << std::endl;
ChartValue<Point3> chv1(pt3);
std::cout << __LINE__ << std::endl;
EXPECT(equalsObj(chv1));
std::cout << __LINE__ << std::endl;
PinholeCal3S2 pc(pose3,cal1);
EXPECT(equalsObj(pc));
std::cout << __LINE__ << std::endl;
Values values; Values values;
values.insert(1,pt3);
std::cout << __LINE__ << std::endl;
EXPECT(equalsObj(values));
std::cout << __LINE__ << std::endl;
values.insert(Symbol('a',0), PinholeCal3S2(pose3, cal1)); values.insert(Symbol('a',0), PinholeCal3S2(pose3, cal1));
values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2)); values.insert(Symbol('s',5), PinholeCal3DS2(pose3, cal2));
values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3)); values.insert(Symbol('d',47), PinholeCal3Bundler(pose3, cal3));
values.insert(Symbol('a',5), PinholeCal3S2(pose3, cal1)); values.insert(Symbol('a',5), PinholeCal3S2(pose3, cal1));
EXPECT(equalsObj(values)); EXPECT(equalsObj(values));
std::cout << __LINE__ << std::endl;
EXPECT(equalsXML(values)); EXPECT(equalsXML(values));
EXPECT(equalsBinary(values)); EXPECT(equalsBinary(values));
} }

158
gtsam/nonlinear/tests/testValues.cpp
View File

@ -21,7 +21,6 @@
#include <gtsam/geometry/Pose3.h> #include <gtsam/geometry/Pose3.h>
#include <gtsam/base/Testable.h> #include <gtsam/base/Testable.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/base/LieVector.h>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/list.hpp> // for operator += #include <boost/assign/std/list.hpp> // for operator +=
@ -38,7 +37,7 @@ static double inf = std::numeric_limits<double>::infinity();
using symbol_shorthand::X; using symbol_shorthand::X;
using symbol_shorthand::L; using symbol_shorthand::L;
const Symbol key1('v',1), key2('v',2), key3('v',3), key4('v',4); const Symbol key1('v', 1), key2('v', 2), key3('v', 3), key4('v', 4);
class TestValueData { class TestValueData {
@ -51,34 +50,43 @@ public:
}; };
int TestValueData::ConstructorCount = 0; int TestValueData::ConstructorCount = 0;
int TestValueData::DestructorCount = 0; int TestValueData::DestructorCount = 0;
class TestValue : public DerivedValue<TestValue> { class TestValue {
TestValueData data_; TestValueData data_;
public: public:
virtual void print(const std::string& str = "") const {} void print(const std::string& str = "") const {}
bool equals(const TestValue& other, double tol = 1e-9) const { return true; } bool equals(const TestValue& other, double tol = 1e-9) const { return true; }
virtual size_t dim() const { return 0; } size_t dim() const { return 0; }
TestValue retract(const Vector&) const { return TestValue(); } TestValue retract(const Vector&) const { return TestValue(); }
Vector localCoordinates(const TestValue&) const { return Vector(); } Vector localCoordinates(const TestValue&) const { return Vector(); }
}; };
namespace gtsam {
namespace traits {
template <>
struct is_manifold<TestValue> : public boost::true_type {};
template <>
struct dimension<TestValue> : public boost::integral_constant<int, 0> {};
}
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Values, equals1 ) TEST( Values, equals1 )
{ {
Values expected; Values expected;
LieVector v((Vector(3) << 5.0, 6.0, 7.0)); Vector3 v(5.0, 6.0, 7.0);
expected.insert(key1,v); expected.insert(key1, v);
Values actual; Values actual;
actual.insert(key1,v); actual.insert(key1, v);
CHECK(assert_equal(expected,actual)); CHECK(assert_equal(expected, actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Values, equals2 ) TEST( Values, equals2 )
{ {
Values cfg1, cfg2; Values cfg1, cfg2;
LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); Vector3 v1(5.0, 6.0, 7.0);
LieVector v2((Vector(3) << 5.0, 6.0, 8.0)); Vector3 v2(5.0, 6.0, 8.0);
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
cfg2.insert(key1, v2); cfg2.insert(key1, v2);
@ -90,8 +98,8 @@ TEST( Values, equals2 )
TEST( Values, equals_nan ) TEST( Values, equals_nan )
{ {
Values cfg1, cfg2; Values cfg1, cfg2;
LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); Vector3 v1(5.0, 6.0, 7.0);
LieVector v2((Vector(3) << inf, inf, inf)); Vector3 v2(inf, inf, inf);
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
cfg2.insert(key1, v2); cfg2.insert(key1, v2);
@ -103,10 +111,10 @@ TEST( Values, equals_nan )
TEST( Values, insert_good ) TEST( Values, insert_good )
{ {
Values cfg1, cfg2, expected; Values cfg1, cfg2, expected;
LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); Vector3 v1(5.0, 6.0, 7.0);
LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); Vector3 v2(8.0, 9.0, 1.0);
LieVector v3((Vector(3) << 2.0, 4.0, 3.0)); Vector3 v3(2.0, 4.0, 3.0);
LieVector v4((Vector(3) << 8.0, 3.0, 7.0)); Vector3 v4(8.0, 3.0, 7.0);
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
cfg1.insert(key2, v2); cfg1.insert(key2, v2);
@ -125,10 +133,10 @@ TEST( Values, insert_good )
TEST( Values, insert_bad ) TEST( Values, insert_bad )
{ {
Values cfg1, cfg2; Values cfg1, cfg2;
LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); Vector3 v1(5.0, 6.0, 7.0);
LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); Vector3 v2(8.0, 9.0, 1.0);
LieVector v3((Vector(3) << 2.0, 4.0, 3.0)); Vector3 v3(2.0, 4.0, 3.0);
LieVector v4((Vector(3) << 8.0, 3.0, 7.0)); Vector3 v4(8.0, 3.0, 7.0);
cfg1.insert(key1, v1); cfg1.insert(key1, v1);
cfg1.insert(key2, v2); cfg1.insert(key2, v2);
@ -142,16 +150,16 @@ TEST( Values, insert_bad )
TEST( Values, update_element ) TEST( Values, update_element )
{ {
Values cfg; Values cfg;
LieVector v1((Vector(3) << 5.0, 6.0, 7.0)); Vector3 v1(5.0, 6.0, 7.0);
LieVector v2((Vector(3) << 8.0, 9.0, 1.0)); Vector3 v2(8.0, 9.0, 1.0);
cfg.insert(key1, v1); cfg.insert(key1, v1);
CHECK(cfg.size() == 1); CHECK(cfg.size() == 1);
CHECK(assert_equal(v1, cfg.at<LieVector>(key1))); CHECK(assert_equal((Vector)v1, cfg.at<Vector3>(key1)));
cfg.update(key1, v2); cfg.update(key1, v2);
CHECK(cfg.size() == 1); CHECK(cfg.size() == 1);
CHECK(assert_equal(v2, cfg.at<LieVector>(key1))); CHECK(assert_equal((Vector)v2, cfg.at<Vector3>(key1)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -159,10 +167,10 @@ TEST(Values, basic_functions)
{ {
Values values; Values values;
const Values& values_c = values; const Values& values_c = values;
values.insert(2, LieVector()); values.insert(2, Vector3());
values.insert(4, LieVector()); values.insert(4, Vector3());
values.insert(6, LieVector()); values.insert(6, Vector3());
values.insert(8, LieVector()); values.insert(8, Vector3());
// find // find
EXPECT_LONGS_EQUAL(4, values.find(4)->key); EXPECT_LONGS_EQUAL(4, values.find(4)->key);
@ -186,8 +194,8 @@ TEST(Values, basic_functions)
//TEST(Values, dim_zero) //TEST(Values, dim_zero)
//{ //{
// Values config0; // Values config0;
// config0.insert(key1, LieVector((Vector(2) << 2.0, 3.0)); // config0.insert(key1, Vector2((Vector(2) << 2.0, 3.0));
// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0)); // config0.insert(key2, Vector3(5.0, 6.0, 7.0));
// LONGS_EQUAL(5, config0.dim()); // LONGS_EQUAL(5, config0.dim());
// //
// VectorValues expected; // VectorValues expected;
@ -200,16 +208,16 @@ TEST(Values, basic_functions)
TEST(Values, expmap_a) TEST(Values, expmap_a)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); config0.insert(key1, Vector3(1.0, 2.0, 3.0));
config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); config0.insert(key2, Vector3(5.0, 6.0, 7.0));
VectorValues increment = pair_list_of<Key, Vector> VectorValues increment = pair_list_of<Key, Vector>
(key1, (Vector(3) << 1.0, 1.1, 1.2)) (key1, (Vector(3) << 1.0, 1.1, 1.2))
(key2, (Vector(3) << 1.3, 1.4, 1.5)); (key2, (Vector(3) << 1.3, 1.4, 1.5));
Values expected; Values expected;
expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2))); expected.insert(key1, Vector3(2.0, 3.1, 4.2));
expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); expected.insert(key2, Vector3(6.3, 7.4, 8.5));
CHECK(assert_equal(expected, config0.retract(increment))); CHECK(assert_equal(expected, config0.retract(increment)));
} }
@ -218,15 +226,15 @@ TEST(Values, expmap_a)
TEST(Values, expmap_b) TEST(Values, expmap_b)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); config0.insert(key1, Vector3(1.0, 2.0, 3.0));
config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); config0.insert(key2, Vector3(5.0, 6.0, 7.0));
VectorValues increment = pair_list_of<Key, Vector> VectorValues increment = pair_list_of<Key, Vector>
(key2, (Vector(3) << 1.3, 1.4, 1.5)); (key2, (Vector(3) << 1.3, 1.4, 1.5));
Values expected; Values expected;
expected.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); expected.insert(key1, Vector3(1.0, 2.0, 3.0));
expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); expected.insert(key2, Vector3(6.3, 7.4, 8.5));
CHECK(assert_equal(expected, config0.retract(increment))); CHECK(assert_equal(expected, config0.retract(increment)));
} }
@ -235,16 +243,16 @@ TEST(Values, expmap_b)
//TEST(Values, expmap_c) //TEST(Values, expmap_c)
//{ //{
// Values config0; // Values config0;
// config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); // config0.insert(key1, Vector3(1.0, 2.0, 3.0));
// config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); // config0.insert(key2, Vector3(5.0, 6.0, 7.0));
// //
// Vector increment = LieVector(6, // Vector increment = Vector6(
// 1.0, 1.1, 1.2, // 1.0, 1.1, 1.2,
// 1.3, 1.4, 1.5); // 1.3, 1.4, 1.5);
// //
// Values expected; // Values expected;
// expected.insert(key1, LieVector((Vector(3) << 2.0, 3.1, 4.2))); // expected.insert(key1, Vector3(2.0, 3.1, 4.2));
// expected.insert(key2, LieVector((Vector(3) << 6.3, 7.4, 8.5))); // expected.insert(key2, Vector3(6.3, 7.4, 8.5));
// //
// CHECK(assert_equal(expected, config0.retract(increment))); // CHECK(assert_equal(expected, config0.retract(increment)));
//} //}
@ -253,14 +261,14 @@ TEST(Values, expmap_b)
TEST(Values, expmap_d) TEST(Values, expmap_d)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); config0.insert(key1, Vector3(1.0, 2.0, 3.0));
config0.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); config0.insert(key2, Vector3(5.0, 6.0, 7.0));
//config0.print("config0"); //config0.print("config0");
CHECK(equal(config0, config0)); CHECK(equal(config0, config0));
CHECK(config0.equals(config0)); CHECK(config0.equals(config0));
Values poseconfig; Values poseconfig;
poseconfig.insert(key1, Pose2(1,2,3)); poseconfig.insert(key1, Pose2(1, 2, 3));
poseconfig.insert(key2, Pose2(0.3, 0.4, 0.5)); poseconfig.insert(key2, Pose2(0.3, 0.4, 0.5));
CHECK(equal(config0, config0)); CHECK(equal(config0, config0));
@ -271,8 +279,8 @@ TEST(Values, expmap_d)
TEST(Values, localCoordinates) TEST(Values, localCoordinates)
{ {
Values valuesA; Values valuesA;
valuesA.insert(key1, LieVector((Vector(3) << 1.0, 2.0, 3.0))); valuesA.insert(key1, Vector3(1.0, 2.0, 3.0));
valuesA.insert(key2, LieVector((Vector(3) << 5.0, 6.0, 7.0))); valuesA.insert(key2, Vector3(5.0, 6.0, 7.0));
VectorValues expDelta = pair_list_of<Key, Vector> VectorValues expDelta = pair_list_of<Key, Vector>
(key1, (Vector(3) << 0.1, 0.2, 0.3)) (key1, (Vector(3) << 0.1, 0.2, 0.3))
@ -308,29 +316,29 @@ TEST(Values, extract_keys)
TEST(Values, exists_) TEST(Values, exists_)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector((Vector(1) << 1.))); config0.insert(key1, 1.0);
config0.insert(key2, LieVector((Vector(1) << 2.))); config0.insert(key2, 2.0);
boost::optional<const LieVector&> v = config0.exists<LieVector>(key1); boost::optional<const double&> v = config0.exists<double>(key1);
CHECK(assert_equal((Vector(1) << 1.),*v)); DOUBLES_EQUAL(1.0,*v,1e-9);
} }
/* ************************************************************************* */ /* ************************************************************************* */
TEST(Values, update) TEST(Values, update)
{ {
Values config0; Values config0;
config0.insert(key1, LieVector((Vector(1) << 1.))); config0.insert(key1, 1.0);
config0.insert(key2, LieVector((Vector(1) << 2.))); config0.insert(key2, 2.0);
Values superset; Values superset;
superset.insert(key1, LieVector((Vector(1) << -1.))); superset.insert(key1, -1.0);
superset.insert(key2, LieVector((Vector(1) << -2.))); superset.insert(key2, -2.0);
config0.update(superset); config0.update(superset);
Values expected; Values expected;
expected.insert(key1, LieVector((Vector(1) << -1.))); expected.insert(key1, -1.0);
expected.insert(key2, LieVector((Vector(1) << -2.))); expected.insert(key2, -2.0);
CHECK(assert_equal(expected,config0)); CHECK(assert_equal(expected, config0));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -353,12 +361,14 @@ TEST(Values, filter) {
BOOST_FOREACH(const Values::Filtered<>::KeyValuePair& key_value, filtered) { BOOST_FOREACH(const Values::Filtered<>::KeyValuePair& key_value, filtered) {
if(i == 0) { if(i == 0) {
LONGS_EQUAL(2, (long)key_value.key); LONGS_EQUAL(2, (long)key_value.key);
EXPECT(typeid(Pose2) == typeid(key_value.value)); try {key_value.value.cast<Pose2>();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose2");}
EXPECT(assert_equal(pose2, dynamic_cast<const Pose2&>(key_value.value))); THROWS_EXCEPTION(key_value.value.cast<Pose3>());
EXPECT(assert_equal(pose2, key_value.value.cast<Pose2>()));
} else if(i == 1) { } else if(i == 1) {
LONGS_EQUAL(3, (long)key_value.key); LONGS_EQUAL(3, (long)key_value.key);
EXPECT(typeid(Pose3) == typeid(key_value.value)); try {key_value.value.cast<Pose3>();} catch (const std::bad_cast& e) { FAIL("can't cast Value to Pose3");}
EXPECT(assert_equal(pose3, dynamic_cast<const Pose3&>(key_value.value))); THROWS_EXCEPTION(key_value.value.cast<Pose2>());
EXPECT(assert_equal(pose3, key_value.value.cast<Pose3>()));
} else { } else {
EXPECT(false); EXPECT(false);
} }
@ -408,18 +418,18 @@ TEST(Values, Symbol_filter) {
Values values; Values values;
values.insert(X(0), pose0); values.insert(X(0), pose0);
values.insert(Symbol('y',1), pose1); values.insert(Symbol('y', 1), pose1);
values.insert(X(2), pose2); values.insert(X(2), pose2);
values.insert(Symbol('y',3), pose3); values.insert(Symbol('y', 3), pose3);
int i = 0; int i = 0;
BOOST_FOREACH(const Values::Filtered<Value>::KeyValuePair& key_value, values.filter(Symbol::ChrTest('y'))) { BOOST_FOREACH(const Values::Filtered<Value>::KeyValuePair& key_value, values.filter(Symbol::ChrTest('y'))) {
if(i == 0) { if(i == 0) {
LONGS_EQUAL(Symbol('y', 1), (long)key_value.key); LONGS_EQUAL(Symbol('y', 1), (long)key_value.key);
EXPECT(assert_equal(pose1, dynamic_cast<const Pose3&>(key_value.value))); EXPECT(assert_equal(pose1, key_value.value.cast<Pose3>()));
} else if(i == 1) { } else if(i == 1) {
LONGS_EQUAL(Symbol('y', 3), (long)key_value.key); LONGS_EQUAL(Symbol('y', 3), (long)key_value.key);
EXPECT(assert_equal(pose3, dynamic_cast<const Pose3&>(key_value.value))); EXPECT(assert_equal(pose3, key_value.value.cast<Pose3>()));
} else { } else {
EXPECT(false); EXPECT(false);
} }
@ -441,11 +451,15 @@ TEST(Values, Destructors) {
values.insert(0, value1); values.insert(0, value1);
values.insert(1, value2); values.insert(1, value2);
} }
LONGS_EQUAL(4, (long)TestValueData::ConstructorCount); // additional 2 con/destructor counts for the temporary
LONGS_EQUAL(2, (long)TestValueData::DestructorCount); // GenericValue<TestValue> in insert()
// but I'm sure some advanced programmer can figure out
// a way to avoid the temporary, or optimize it out
LONGS_EQUAL(4+2, (long)TestValueData::ConstructorCount);
LONGS_EQUAL(2+2, (long)TestValueData::DestructorCount);
} }
LONGS_EQUAL(4, (long)TestValueData::ConstructorCount); LONGS_EQUAL(4+2, (long)TestValueData::ConstructorCount);
LONGS_EQUAL(4, (long)TestValueData::DestructorCount); LONGS_EQUAL(4+2, (long)TestValueData::DestructorCount);
} }
/* ************************************************************************* */ /* ************************************************************************* */

19
gtsam/slam/EssentialMatrixFactor.h
View File

@ -10,7 +10,6 @@
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/geometry/EssentialMatrix.h> #include <gtsam/geometry/EssentialMatrix.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/base/LieScalar.h>
#include <iostream> #include <iostream>
namespace gtsam { namespace gtsam {
@ -87,14 +86,13 @@ public:
* Binary factor that optimizes for E and inverse depth d: assumes measurement * Binary factor that optimizes for E and inverse depth d: assumes measurement
* in image 2 is perfect, and returns re-projection error in image 1 * in image 2 is perfect, and returns re-projection error in image 1
*/ */
class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix, class EssentialMatrixFactor2: public NoiseModelFactor2<EssentialMatrix, double> {
LieScalar> {
Point3 dP1_; ///< 3D point corresponding to measurement in image 1 Point3 dP1_; ///< 3D point corresponding to measurement in image 1
Point2 pn_; ///< Measurement in image 2, in ideal coordinates Point2 pn_; ///< Measurement in image 2, in ideal coordinates
double f_; ///< approximate conversion factor for error scaling double f_; ///< approximate conversion factor for error scaling
typedef NoiseModelFactor2<EssentialMatrix, LieScalar> Base; typedef NoiseModelFactor2<EssentialMatrix, double> Base;
typedef EssentialMatrixFactor2 This; typedef EssentialMatrixFactor2 This;
public: public:
@ -155,7 +153,7 @@ public:
* @param E essential matrix * @param E essential matrix
* @param d inverse depth d * @param d inverse depth d
*/ */
Vector evaluateError(const EssentialMatrix& E, const LieScalar& d, Vector evaluateError(const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd = boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
boost::none) const { boost::none) const {
@ -180,12 +178,14 @@ public:
} else { } else {
// Calculate derivatives. TODO if slow: optimize with Mathematica // Calculate derivatives. TODO if slow: optimize with Mathematica
// 3*2 3*3 3*3 2*3 // 3*2 3*3 3*3
Matrix D_1T2_dir, DdP2_rot, DP2_point, Dpn_dP2; Matrix D_1T2_dir, DdP2_rot, DP2_point;
Point3 _1T2 = E.direction().point3(D_1T2_dir); Point3 _1T2 = E.direction().point3(D_1T2_dir);
Point3 d1T2 = d * _1T2; Point3 d1T2 = d * _1T2;
Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point); Point3 dP2 = E.rotation().unrotate(dP1_ - d1T2, DdP2_rot, DP2_point);
Matrix23 Dpn_dP2;
pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2); pn = SimpleCamera::project_to_camera(dP2, Dpn_dP2);
if (DE) { if (DE) {
@ -245,7 +245,8 @@ public:
*/ */
template<class CALIBRATION> template<class CALIBRATION>
EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB, EssentialMatrixFactor3(Key key1, Key key2, const Point2& pA, const Point2& pB,
const Rot3& cRb, const SharedNoiseModel& model, boost::shared_ptr<CALIBRATION> K) : const Rot3& cRb, const SharedNoiseModel& model,
boost::shared_ptr<CALIBRATION> K) :
EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) { EssentialMatrixFactor2(key1, key2, pA, pB, model, K), cRb_(cRb) {
} }
@ -267,7 +268,7 @@ public:
* @param E essential matrix * @param E essential matrix
* @param d inverse depth d * @param d inverse depth d
*/ */
Vector evaluateError(const EssentialMatrix& E, const LieScalar& d, Vector evaluateError(const EssentialMatrix& E, const double& d,
boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd = boost::optional<Matrix&> DE = boost::none, boost::optional<Matrix&> Dd =
boost::none) const { boost::none) const {
if (!DE) { if (!DE) {

6
gtsam/slam/ProjectionFactor.h
View File

@ -132,17 +132,17 @@ namespace gtsam {
if(H1) { if(H1) {
gtsam::Matrix H0; gtsam::Matrix H0;
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_); PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_, H0), *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_); Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
*H1 = *H1 * H0; *H1 = *H1 * H0;
return reprojectionError.vector(); return reprojectionError.vector();
} else { } else {
PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_); PinholeCamera<CALIBRATION> camera(pose.compose(*body_P_sensor_), *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_); Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
return reprojectionError.vector(); return reprojectionError.vector();
} }
} else { } else {
PinholeCamera<CALIBRATION> camera(pose, *K_); PinholeCamera<CALIBRATION> camera(pose, *K_);
Point2 reprojectionError(camera.project(point, H1, H2) - measured_); Point2 reprojectionError(camera.project(point, H1, H2, boost::none) - measured_);
return reprojectionError.vector(); return reprojectionError.vector();
} }
} catch( CheiralityException& e) { } catch( CheiralityException& e) {

3
gtsam/slam/RegularImplicitSchurFactor.h
View File

@ -29,7 +29,6 @@ public:
protected: protected:
typedef Eigen::Matrix<double, 2, D> Matrix2D; ///< type of an F block typedef Eigen::Matrix<double, 2, D> Matrix2D; ///< type of an F block
typedef Eigen::Matrix<double, 2, 3> Matrix23;
typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian typedef Eigen::Matrix<double, D, D> MatrixDD; ///< camera hessian
typedef std::pair<Key, Matrix2D> KeyMatrix2D; ///< named F block typedef std::pair<Key, Matrix2D> KeyMatrix2D; ///< named F block
@ -203,7 +202,7 @@ public:
// F'*(I - E*P*E')*F, TODO: this should work, but it does not :-( // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
// static const Eigen::Matrix<double, 2, 2> I2 = eye(2); // static const Eigen::Matrix<double, 2, 2> I2 = eye(2);
// Eigen::Matrix<double, 2, 2> Q = // // Matrix2 Q = //
// I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); // I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose();
// blocks[j] = Fj.transpose() * Q * Fj; // blocks[j] = Fj.transpose() * Q * Fj;
} }

25
gtsam/slam/dataset.cpp
View File

@ -1,5 +1,4 @@
/* ---------------------------------------------------------------------------- /* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation, * GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415 * Atlanta, Georgia 30332-0415
* All Rights Reserved * All Rights Reserved
@ -349,8 +348,9 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
fstream stream(filename.c_str(), fstream::out); fstream stream(filename.c_str(), fstream::out);
// save poses // save poses
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) { BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) {
const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value); const Pose2& pose = key_value.value.cast<Pose2>();
stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y() stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y()
<< " " << pose.theta() << endl; << " " << pose.theta() << endl;
} }
@ -392,25 +392,22 @@ GraphAndValues readG2o(const string& g2oFile, const bool is3D,
/* ************************************************************************* */ /* ************************************************************************* */
void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate, void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
const string& filename) { const string& filename) {
fstream stream(filename.c_str(), fstream::out); fstream stream(filename.c_str(), fstream::out);
// save 2D & 3D poses // save 2D & 3D poses
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) { Values::ConstFiltered<Pose2> viewPose2 = estimate.filter<Pose2>();
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, viewPose2) {
stream << "VERTEX_SE2 " << key_value.key << " " << key_value.value.x() << " "
<< key_value.value.y() << " " << key_value.value.theta() << endl;
}
const Pose2* pose2D = dynamic_cast<const Pose2*>(&key_value.value); Values::ConstFiltered<Pose3> viewPose3 = estimate.filter<Pose3>();
if(pose2D){ BOOST_FOREACH(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value, viewPose3) {
stream << "VERTEX_SE2 " << key_value.key << " " << pose2D->x() << " " Point3 p = key_value.value.translation();
<< pose2D->y() << " " << pose2D->theta() << endl; Rot3 R = key_value.value.rotation();
}
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() stream << "VERTEX_SE3:QUAT " << key_value.key << " " << p.x() << " " << p.y() << " " << p.z()
<< " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z() << " " << R.toQuaternion().x() << " " << R.toQuaternion().y() << " " << R.toQuaternion().z()
<< " " << R.toQuaternion().w() << endl; << " " << R.toQuaternion().w() << endl;
}
} }
// save edges (2D or 3D) // save edges (2D or 3D)

4
gtsam/slam/tests/testEssentialMatrixConstraint.cpp
View File

@ -50,10 +50,10 @@ TEST( EssentialMatrixConstraint, test ) {
CHECK(assert_equal(expected, actual, 1e-8)); CHECK(assert_equal(expected, actual, 1e-8));
// Calculate numerical derivatives // Calculate numerical derivatives
Matrix expectedH1 = numericalDerivative11<Pose3>( Matrix expectedH1 = numericalDerivative11<Vector,Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2, boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, _1, pose2,
boost::none, boost::none), pose1); boost::none, boost::none), pose1);
Matrix expectedH2 = numericalDerivative11<Pose3>( Matrix expectedH2 = numericalDerivative11<Vector,Pose3>(
boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1, boost::bind(&EssentialMatrixConstraint::evaluateError, &factor, pose1, _1,
boost::none, boost::none), pose2); boost::none, boost::none), pose2);

66
gtsam/slam/tests/testEssentialMatrixFactor.cpp
View File

@ -36,7 +36,7 @@ SfM_data data;
bool readOK = readBAL(filename, data); bool readOK = readBAL(filename, data);
Rot3 c1Rc2 = data.cameras[1].pose().rotation(); Rot3 c1Rc2 = data.cameras[1].pose().rotation();
Point3 c1Tc2 = data.cameras[1].pose().translation(); Point3 c1Tc2 = data.cameras[1].pose().translation();
PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(),Cal3_S2()); PinholeCamera<Cal3_S2> camera2(data.cameras[1].pose(), Cal3_S2());
EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2)); EssentialMatrix trueE(c1Rc2, Unit3(c1Tc2));
double baseline = 0.1; // actual baseline of the camera double baseline = 0.1; // actual baseline of the camera
@ -96,7 +96,7 @@ TEST (EssentialMatrixFactor, factor) {
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected; Matrix Hexpected;
Hexpected = numericalDerivative11<EssentialMatrix>( Hexpected = numericalDerivative11<Vector, EssentialMatrix>(
boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1, boost::bind(&EssentialMatrixFactor::evaluateError, &factor, _1,
boost::none), trueE); boost::none), trueE);
@ -164,17 +164,17 @@ TEST (EssentialMatrixFactor2, factor) {
Vector expected = reprojectionError.vector(); Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2; Matrix Hactual1, Hactual2;
LieScalar d(baseline / P1.z()); double d(baseline / P1.z());
Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
EXPECT(assert_equal(expected, actual, 1e-7)); EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2; Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f = boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none,
boost::none, boost::none); boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, trueE, d); Hexpected1 = numericalDerivative21<Vector, EssentialMatrix, double>(f, trueE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, trueE, d); Hexpected2 = numericalDerivative22<Vector, EssentialMatrix, double>(f, trueE, d);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@ -197,7 +197,7 @@ TEST (EssentialMatrixFactor2, minimization) {
truth.insert(100, trueE); truth.insert(100, trueE);
for (size_t i = 0; i < 5; i++) { for (size_t i = 0; i < 5; i++) {
Point3 P1 = data.tracks[i].p; Point3 P1 = data.tracks[i].p;
truth.insert(i, LieScalar(baseline / P1.z())); truth.insert(i, double(baseline / P1.z()));
} }
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
@ -211,7 +211,7 @@ TEST (EssentialMatrixFactor2, minimization) {
EssentialMatrix actual = result.at<EssentialMatrix>(100); EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(trueE, actual, 1e-1)); EXPECT(assert_equal(trueE, actual, 1e-1));
for (size_t i = 0; i < 5; i++) for (size_t i = 0; i < 5; i++)
EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1)); EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result // Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
@ -238,17 +238,17 @@ TEST (EssentialMatrixFactor3, factor) {
Vector expected = reprojectionError.vector(); Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2; Matrix Hactual1, Hactual2;
LieScalar d(baseline / P1.z()); double d(baseline / P1.z());
Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
EXPECT(assert_equal(expected, actual, 1e-7)); EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2; Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f = boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none,
boost::none, boost::none); boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, bodyE, d); Hexpected1 = numericalDerivative21<Vector, EssentialMatrix, double>(f, bodyE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, bodyE, d); Hexpected2 = numericalDerivative22<Vector, EssentialMatrix, double>(f, bodyE, d);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8)); EXPECT(assert_equal(Hexpected1, Hactual1, 1e-8));
@ -270,7 +270,7 @@ TEST (EssentialMatrixFactor3, minimization) {
truth.insert(100, bodyE); truth.insert(100, bodyE);
for (size_t i = 0; i < 5; i++) { for (size_t i = 0; i < 5; i++) {
Point3 P1 = data.tracks[i].p; Point3 P1 = data.tracks[i].p;
truth.insert(i, LieScalar(baseline / P1.z())); truth.insert(i, double(baseline / P1.z()));
} }
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
@ -284,7 +284,7 @@ TEST (EssentialMatrixFactor3, minimization) {
EssentialMatrix actual = result.at<EssentialMatrix>(100); EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(bodyE, actual, 1e-1)); EXPECT(assert_equal(bodyE, actual, 1e-1));
for (size_t i = 0; i < 5; i++) for (size_t i = 0; i < 5; i++)
EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1)); EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result // Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
@ -314,7 +314,7 @@ Point2 pB(size_t i) {
boost::shared_ptr<Cal3Bundler> // boost::shared_ptr<Cal3Bundler> //
K = boost::make_shared<Cal3Bundler>(500, 0, 0); K = boost::make_shared<Cal3Bundler>(500, 0, 0);
PinholeCamera<Cal3Bundler> camera2(data.cameras[1].pose(),*K); PinholeCamera<Cal3Bundler> camera2(data.cameras[1].pose(), *K);
Vector vA(size_t i) { Vector vA(size_t i) {
Point2 xy = K->calibrate(pA(i)); Point2 xy = K->calibrate(pA(i));
@ -380,17 +380,17 @@ TEST (EssentialMatrixFactor2, extraTest) {
Vector expected = reprojectionError.vector(); Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2; Matrix Hactual1, Hactual2;
LieScalar d(baseline / P1.z()); double d(baseline / P1.z());
Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2); Vector actual = factor.evaluateError(trueE, d, Hactual1, Hactual2);
EXPECT(assert_equal(expected, actual, 1e-7)); EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2; Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f = boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
boost::bind(&EssentialMatrixFactor2::evaluateError, &factor, _1, _2, &EssentialMatrixFactor2::evaluateError, &factor, _1, _2, boost::none,
boost::none, boost::none); boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, trueE, d); Hexpected1 = numericalDerivative21<Vector, EssentialMatrix, double>(f, trueE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, trueE, d); Hexpected2 = numericalDerivative22<Vector, EssentialMatrix, double>(f, trueE, d);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));
@ -413,7 +413,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) {
truth.insert(100, trueE); truth.insert(100, trueE);
for (size_t i = 0; i < data.number_tracks(); i++) { for (size_t i = 0; i < data.number_tracks(); i++) {
Point3 P1 = data.tracks[i].p; Point3 P1 = data.tracks[i].p;
truth.insert(i, LieScalar(baseline / P1.z())); truth.insert(i, double(baseline / P1.z()));
} }
EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8); EXPECT_DOUBLES_EQUAL(0, graph.error(truth), 1e-8);
@ -427,7 +427,7 @@ TEST (EssentialMatrixFactor2, extraMinimization) {
EssentialMatrix actual = result.at<EssentialMatrix>(100); EssentialMatrix actual = result.at<EssentialMatrix>(100);
EXPECT(assert_equal(trueE, actual, 1e-1)); EXPECT(assert_equal(trueE, actual, 1e-1));
for (size_t i = 0; i < data.number_tracks(); i++) for (size_t i = 0; i < data.number_tracks(); i++)
EXPECT(assert_equal(truth.at<LieScalar>(i), result.at<LieScalar>(i), 1e-1)); EXPECT_DOUBLES_EQUAL(truth.at<double>(i), result.at<double>(i), 1e-1);
// Check error at result // Check error at result
EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4); EXPECT_DOUBLES_EQUAL(0, graph.error(result), 1e-4);
@ -449,17 +449,17 @@ TEST (EssentialMatrixFactor3, extraTest) {
Vector expected = reprojectionError.vector(); Vector expected = reprojectionError.vector();
Matrix Hactual1, Hactual2; Matrix Hactual1, Hactual2;
LieScalar d(baseline / P1.z()); double d(baseline / P1.z());
Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2); Vector actual = factor.evaluateError(bodyE, d, Hactual1, Hactual2);
EXPECT(assert_equal(expected, actual, 1e-7)); EXPECT(assert_equal(expected, actual, 1e-7));
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
Matrix Hexpected1, Hexpected2; Matrix Hexpected1, Hexpected2;
boost::function<Vector(const EssentialMatrix&, const LieScalar&)> f = boost::function<Vector(const EssentialMatrix&, double)> f = boost::bind(
boost::bind(&EssentialMatrixFactor3::evaluateError, &factor, _1, _2, &EssentialMatrixFactor3::evaluateError, &factor, _1, _2, boost::none,
boost::none, boost::none); boost::none);
Hexpected1 = numericalDerivative21<EssentialMatrix>(f, bodyE, d); Hexpected1 = numericalDerivative21<Vector, EssentialMatrix, double>(f, bodyE, d);
Hexpected2 = numericalDerivative22<EssentialMatrix>(f, bodyE, d); Hexpected2 = numericalDerivative22<Vector, EssentialMatrix, double>(f, bodyE, d);
// Verify the Jacobian is correct // Verify the Jacobian is correct
EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6)); EXPECT(assert_equal(Hexpected1, Hactual1, 1e-6));

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

@ -38,13 +38,13 @@ 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); const Rot2 rot2C = Rot2::fromAngle(M_PI-0.01), rot2D = Rot2::fromAngle(M_PI+0.01);
/* ************************************************************************* */ /* ************************************************************************* */
LieVector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) { Vector evalFactorError3(const Pose3RotationPrior& factor, const Pose3& x) {
return LieVector(factor.evaluateError(x)); return factor.evaluateError(x);
} }
/* ************************************************************************* */ /* ************************************************************************* */
LieVector evalFactorError2(const Pose2RotationPrior& factor, const Pose2& x) { Vector evalFactorError2(const Pose2RotationPrior& factor, const Pose2& x) {
return LieVector(factor.evaluateError(x)); return factor.evaluateError(x);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -53,8 +53,7 @@ TEST( testPoseRotationFactor, level3_zero_error ) {
Pose3RotationPrior factor(poseKey, rot3A, model3); Pose3RotationPrior factor(poseKey, rot3A, model3);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>( Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -68,13 +67,10 @@ TEST( testPoseRotationFactor, level3_error ) {
#else #else
EXPECT(assert_equal((Vector(3) << -0.1, -0.2, -0.3), factor.evaluateError(pose1, actH1),1e-2)); EXPECT(assert_equal((Vector(3) << -0.1, -0.2, -0.3), factor.evaluateError(pose1, actH1),1e-2));
#endif #endif
Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
// the derivative is more complex, but is close to the identity for Rot3 around the origin // 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-2);
EXPECT(assert_equal(expH1, actH1, tol));*/
// If not using true expmap will be close, but not exact around the origin // If not using true expmap will be close, but not exact around the origin
// EXPECT(assert_equal(expH1, actH1, tol));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -83,8 +79,7 @@ TEST( testPoseRotationFactor, level2_zero_error ) {
Pose2RotationPrior factor(poseKey, rot2A, model1); Pose2RotationPrior factor(poseKey, rot2A, model1);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal(zero(1), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>( Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -94,8 +89,7 @@ TEST( testPoseRotationFactor, level2_error ) {
Pose2RotationPrior factor(poseKey, rot2B, model1); Pose2RotationPrior factor(poseKey, rot2B, model1);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal((Vector(1) << -M_PI_2), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal((Vector(1) << -M_PI_2), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>( Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -105,8 +99,7 @@ TEST( testPoseRotationFactor, level2_error_wrap ) {
Pose2RotationPrior factor(poseKey, rot2D, model1); Pose2RotationPrior factor(poseKey, rot2D, model1);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal((Vector(1) << -0.02), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal((Vector(1) << -0.02), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>( Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }

32
gtsam/slam/tests/testPoseTranslationPrior.cpp
View File

@ -34,13 +34,13 @@ const Point2 point2A(1.0, 2.0), point2B(4.0, 6.0);
const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2); const Rot2 rot2A, rot2B = Rot2::fromAngle(M_PI_2);
/* ************************************************************************* */ /* ************************************************************************* */
LieVector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) { Vector evalFactorError3(const Pose3TranslationPrior& factor, const Pose3& x) {
return LieVector(factor.evaluateError(x)); return factor.evaluateError(x);
} }
/* ************************************************************************* */ /* ************************************************************************* */
LieVector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) { Vector evalFactorError2(const Pose2TranslationPrior& factor, const Pose2& x) {
return LieVector(factor.evaluateError(x)); return factor.evaluateError(x);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -49,8 +49,7 @@ TEST( testPoseTranslationFactor, level3_zero_error ) {
Pose3TranslationPrior factor(poseKey, point3A, model3); Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>( Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -60,8 +59,7 @@ TEST( testPoseTranslationFactor, level3_error ) {
Pose3TranslationPrior factor(poseKey, point3B, model3); Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>( Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -71,8 +69,7 @@ TEST( testPoseTranslationFactor, pitched3_zero_error ) {
Pose3TranslationPrior factor(poseKey, point3A, model3); Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>( Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -82,8 +79,7 @@ TEST( testPoseTranslationFactor, pitched3_error ) {
Pose3TranslationPrior factor(poseKey, point3B, model3); Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>( Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -93,8 +89,7 @@ TEST( testPoseTranslationFactor, smallrot3_zero_error ) {
Pose3TranslationPrior factor(poseKey, point3A, model3); Pose3TranslationPrior factor(poseKey, point3A, model3);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal(zero(3), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>( Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -104,8 +99,7 @@ TEST( testPoseTranslationFactor, smallrot3_error ) {
Pose3TranslationPrior factor(poseKey, point3B, model3); Pose3TranslationPrior factor(poseKey, point3B, model3);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal((Vector(3) << -3.0,-4.0,-5.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose3>( Matrix expH1 = numericalDerivative22(evalFactorError3, factor, pose1, 1e-5);
boost::bind(evalFactorError3, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -115,8 +109,7 @@ TEST( testPoseTranslationFactor, level2_zero_error ) {
Pose2TranslationPrior factor(poseKey, point2A, model2); Pose2TranslationPrior factor(poseKey, point2A, model2);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal(zero(2), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>( Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }
@ -126,8 +119,7 @@ TEST( testPoseTranslationFactor, level2_error ) {
Pose2TranslationPrior factor(poseKey, point2B, model2); Pose2TranslationPrior factor(poseKey, point2B, model2);
Matrix actH1; Matrix actH1;
EXPECT(assert_equal((Vector(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1))); EXPECT(assert_equal((Vector(2) << -3.0,-4.0), factor.evaluateError(pose1, actH1)));
Matrix expH1 = numericalDerivative11<LieVector,Pose2>( Matrix expH1 = numericalDerivative22(evalFactorError2, factor, pose1, 1e-5);
boost::bind(evalFactorError2, factor, _1), pose1, 1e-5);
EXPECT(assert_equal(expH1, actH1, tol)); EXPECT(assert_equal(expH1, actH1, tol));
} }

28
gtsam/slam/tests/testPriorFactor.cpp Normal file
View File

@ -0,0 +1,28 @@
/**
* @file testPriorFactor.cpp
* @brief Test PriorFactor
* @author Frank Dellaert
* @date Nov 4, 2014
*/
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/base/LieScalar.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
/* ************************************************************************* */
// Constructor
TEST(PriorFactor, Constructor) {
SharedNoiseModel model;
PriorFactor<LieScalar> factor(1, LieScalar(1.0), model);
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

21
gtsam/slam/tests/testRangeFactor.cpp
View File

@ -23,7 +23,6 @@
#include <gtsam/geometry/Pose2.h> #include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h> #include <gtsam/geometry/Point2.h>
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <boost/bind.hpp> #include <boost/bind.hpp>
@ -37,12 +36,12 @@ typedef RangeFactor<Pose2, Point2> RangeFactor2D;
typedef RangeFactor<Pose3, Point3> RangeFactor3D; typedef RangeFactor<Pose3, Point3> RangeFactor3D;
/* ************************************************************************* */ /* ************************************************************************* */
LieVector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) { Vector factorError2D(const Pose2& pose, const Point2& point, const RangeFactor2D& factor) {
return factor.evaluateError(pose, point); return factor.evaluateError(pose, point);
} }
/* ************************************************************************* */ /* ************************************************************************* */
LieVector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) { Vector factorError3D(const Pose3& pose, const Point3& point, const RangeFactor3D& factor) {
return factor.evaluateError(pose, point); return factor.evaluateError(pose, point);
} }
@ -214,8 +213,8 @@ TEST( RangeFactor, Jacobian2D ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose); H1Expected = numericalDerivative11<Vector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point); H2Expected = numericalDerivative11<Vector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
// Verify the Jacobians are correct // Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -243,8 +242,8 @@ TEST( RangeFactor, Jacobian2DWithTransform ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose); H1Expected = numericalDerivative11<Vector, Pose2>(boost::bind(&factorError2D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point); H2Expected = numericalDerivative11<Vector, Point2>(boost::bind(&factorError2D, pose, _1, factor), point);
// Verify the Jacobians are correct // Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -269,8 +268,8 @@ TEST( RangeFactor, Jacobian3D ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose); H1Expected = numericalDerivative11<Vector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point); H2Expected = numericalDerivative11<Vector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
// Verify the Jacobians are correct // Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); CHECK(assert_equal(H1Expected, H1Actual, 1e-9));
@ -298,8 +297,8 @@ TEST( RangeFactor, Jacobian3DWithTransform ) {
// Use numerical derivatives to calculate the Jacobians // Use numerical derivatives to calculate the Jacobians
Matrix H1Expected, H2Expected; Matrix H1Expected, H2Expected;
H1Expected = numericalDerivative11<LieVector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose); H1Expected = numericalDerivative11<Vector, Pose3>(boost::bind(&factorError3D, _1, point, factor), pose);
H2Expected = numericalDerivative11<LieVector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point); H2Expected = numericalDerivative11<Vector, Point3>(boost::bind(&factorError3D, pose, _1, factor), point);
// Verify the Jacobians are correct // Verify the Jacobians are correct
CHECK(assert_equal(H1Expected, H1Actual, 1e-9)); CHECK(assert_equal(H1Expected, H1Actual, 1e-9));

16
gtsam/slam/tests/testReferenceFrameFactor.cpp
View File

@ -53,9 +53,9 @@ TEST( ReferenceFrameFactor, equals ) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
LieVector evaluateError_(const PointReferenceFrameFactor& c, Vector evaluateError_(const PointReferenceFrameFactor& c,
const Point2& global, const Pose2& trans, const Point2& local) { const Point2& global, const Pose2& trans, const Point2& local) {
return LieVector(c.evaluateError(global, trans, local)); return Vector(c.evaluateError(global, trans, local));
} }
TEST( ReferenceFrameFactor, jacobians ) { TEST( ReferenceFrameFactor, jacobians ) {
@ -68,13 +68,13 @@ TEST( ReferenceFrameFactor, jacobians ) {
tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
Matrix numericalDT, numericalDL, numericalDF; Matrix numericalDT, numericalDL, numericalDF;
numericalDF = numericalDerivative31<LieVector,Point2,Pose2,Point2>( numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3), boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5); global, trans, local, 1e-5);
numericalDT = numericalDerivative32<LieVector,Point2,Pose2,Point2>( numericalDT = numericalDerivative32<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3), boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5); global, trans, local, 1e-5);
numericalDL = numericalDerivative33<LieVector,Point2,Pose2,Point2>( numericalDL = numericalDerivative33<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3), boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5); global, trans, local, 1e-5);
@ -100,13 +100,13 @@ TEST( ReferenceFrameFactor, jacobians_zero ) {
tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL); tc.evaluateError(global, trans, local, actualDF, actualDT, actualDL);
Matrix numericalDT, numericalDL, numericalDF; Matrix numericalDT, numericalDL, numericalDF;
numericalDF = numericalDerivative31<LieVector,Point2,Pose2,Point2>( numericalDF = numericalDerivative31<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3), boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5); global, trans, local, 1e-5);
numericalDT = numericalDerivative32<LieVector,Point2,Pose2,Point2>( numericalDT = numericalDerivative32<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3), boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5); global, trans, local, 1e-5);
numericalDL = numericalDerivative33<LieVector,Point2,Pose2,Point2>( numericalDL = numericalDerivative33<Vector,Point2,Pose2,Point2>(
boost::bind(evaluateError_, tc, _1, _2, _3), boost::bind(evaluateError_, tc, _1, _2, _3),
global, trans, local, 1e-5); global, trans, local, 1e-5);

8
gtsam/slam/tests/testRotateFactor.cpp
View File

@ -69,13 +69,13 @@ TEST (RotateFactor, test) {
Matrix actual, expected; Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
{ {
expected = numericalDerivative11<Rot3>( expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc); boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), iRc);
f.evaluateError(iRc, actual); f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-9));
} }
{ {
expected = numericalDerivative11<Rot3>( expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R); boost::bind(&RotateFactor::evaluateError, &f, _1, boost::none), R);
f.evaluateError(R, actual); f.evaluateError(R, actual);
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-9));
@ -141,14 +141,14 @@ TEST (RotateDirectionsFactor, test) {
Matrix actual, expected; Matrix actual, expected;
// Use numerical derivatives to calculate the expected Jacobian // Use numerical derivatives to calculate the expected Jacobian
{ {
expected = numericalDerivative11<Rot3>( expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
boost::none), iRc); boost::none), iRc);
f.evaluateError(iRc, actual); f.evaluateError(iRc, actual);
EXPECT(assert_equal(expected, actual, 1e-9)); EXPECT(assert_equal(expected, actual, 1e-9));
} }
{ {
expected = numericalDerivative11<Rot3>( expected = numericalDerivative11<Vector,Rot3>(
boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1, boost::bind(&RotateDirectionsFactor::evaluateError, &f, _1,
boost::none), R); boost::none), R);
f.evaluateError(R, actual); f.evaluateError(R, actual);

29
gtsam_unstable/dynamics/FullIMUFactor.h
View File

@ -7,7 +7,6 @@
#pragma once #pragma once
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/dynamics/PoseRTV.h> #include <gtsam_unstable/dynamics/PoseRTV.h>
@ -29,20 +28,20 @@ public:
protected: protected:
/** measurements from the IMU */ /** measurements from the IMU */
Vector accel_, gyro_; Vector3 accel_, gyro_;
double dt_; /// time between measurements double dt_; /// time between measurements
public: public:
/** Standard constructor */ /** Standard constructor */
FullIMUFactor(const Vector& accel, const Vector& gyro, FullIMUFactor(const Vector3& accel, const Vector3& gyro,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
: Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) { : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {
assert(model->dim() == 9); assert(model->dim() == 9);
} }
/** Single IMU vector - imu = [accel, gyro] */ /** Single IMU vector - imu = [accel, gyro] */
FullIMUFactor(const Vector& imu, FullIMUFactor(const Vector6& imu,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
: Base(model, key1, key2), accel_(imu.head(3)), gyro_(imu.tail(3)), dt_(dt) { : Base(model, key1, key2), accel_(imu.head(3)), gyro_(imu.tail(3)), dt_(dt) {
assert(imu.size() == 6); assert(imu.size() == 6);
@ -68,15 +67,15 @@ public:
void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const {
std::string a = "FullIMUFactor: " + s; std::string a = "FullIMUFactor: " + s;
Base::print(a, formatter); Base::print(a, formatter);
gtsam::print(accel_, "accel"); gtsam::print((Vector)accel_, "accel");
gtsam::print(gyro_, "gyro"); gtsam::print((Vector)gyro_, "gyro");
std::cout << "dt: " << dt_ << std::endl; std::cout << "dt: " << dt_ << std::endl;
} }
// access // access
const Vector& gyro() const { return gyro_; } const Vector3& gyro() const { return gyro_; }
const Vector& accel() const { return accel_; } const Vector3& accel() const { return accel_; }
Vector z() const { return concatVectors(2, &accel_, &gyro_); } Vector6 z() const { return (Vector6() << accel_, gyro_); }
/** /**
* Error evaluation with optional derivatives - calculates * Error evaluation with optional derivatives - calculates
@ -85,13 +84,13 @@ public:
virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const {
Vector z(9); Vector9 z;
z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang z.head(3).operator=(accel_); // Strange syntax to work around ambiguous operator error with clang
z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang z.segment(3, 3).operator=(gyro_); // Strange syntax to work around ambiguous operator error with clang
z.tail(3).operator=(x2.t().vector()); // Strange syntax to work around ambiguous operator error with clang z.tail(3).operator=(x2.t().vector()); // Strange syntax to work around ambiguous operator error with clang
if (H1) *H1 = numericalDerivative21<LieVector, PoseRTV, PoseRTV>( if (H1) *H1 = numericalDerivative21<Vector9, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
if (H2) *H2 = numericalDerivative22<LieVector, PoseRTV, PoseRTV>( if (H2) *H2 = numericalDerivative22<Vector9, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5); boost::bind(This::predict_proxy, _1, _2, dt_), x1, x2, 1e-5);
return z - predict_proxy(x1, x2, dt_); return z - predict_proxy(x1, x2, dt_);
} }
@ -107,11 +106,11 @@ public:
private: private:
/** copy of the measurement function formulated for numerical derivatives */ /** copy of the measurement function formulated for numerical derivatives */
static LieVector predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) { static Vector9 predict_proxy(const PoseRTV& x1, const PoseRTV& x2, double dt) {
Vector hx(9); Vector9 hx;
hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang hx.head(6).operator=(x1.imuPrediction(x2, dt)); // Strange syntax to work around ambiguous operator error with clang
hx.tail(3).operator=(x1.translationIntegration(x2, dt).vector()); // Strange syntax to work around ambiguous operator error with clang hx.tail(3).operator=(x1.translationIntegration(x2, dt).vector()); // Strange syntax to work around ambiguous operator error with clang
return LieVector(hx); return hx;
} }
}; };

31
gtsam_unstable/dynamics/IMUFactor.h
View File

@ -7,7 +7,6 @@
#pragma once #pragma once
#include <gtsam/base/numericalDerivative.h> #include <gtsam/base/numericalDerivative.h>
#include <gtsam/base/LieVector.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam_unstable/dynamics/PoseRTV.h> #include <gtsam_unstable/dynamics/PoseRTV.h>
@ -27,18 +26,18 @@ public:
protected: protected:
/** measurements from the IMU */ /** measurements from the IMU */
Vector accel_, gyro_; Vector3 accel_, gyro_;
double dt_; /// time between measurements double dt_; /// time between measurements
public: public:
/** Standard constructor */ /** Standard constructor */
IMUFactor(const Vector& accel, const Vector& gyro, IMUFactor(const Vector3& accel, const Vector3& gyro,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
: Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {} : Base(model, key1, key2), accel_(accel), gyro_(gyro), dt_(dt) {}
/** Full IMU vector specification */ /** Full IMU vector specification */
IMUFactor(const Vector& imu_vector, IMUFactor(const Vector6& imu_vector,
double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model) double dt, const Key& key1, const Key& key2, const SharedNoiseModel& model)
: Base(model, key1, key2), accel_(imu_vector.head(3)), gyro_(imu_vector.tail(3)), dt_(dt) {} : Base(model, key1, key2), accel_(imu_vector.head(3)), gyro_(imu_vector.tail(3)), dt_(dt) {}
@ -61,15 +60,15 @@ public:
void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const { void print(const std::string& s="", const gtsam::KeyFormatter& formatter = gtsam::DefaultKeyFormatter) const {
std::string a = "IMUFactor: " + s; std::string a = "IMUFactor: " + s;
Base::print(a, formatter); Base::print(a, formatter);
gtsam::print(accel_, "accel"); gtsam::print((Vector)accel_, "accel");
gtsam::print(gyro_, "gyro"); gtsam::print((Vector)gyro_, "gyro");
std::cout << "dt: " << dt_ << std::endl; std::cout << "dt: " << dt_ << std::endl;
} }
// access // access
const Vector& gyro() const { return gyro_; } const Vector3& gyro() const { return gyro_; }
const Vector& accel() const { return accel_; } const Vector3& accel() const { return accel_; }
Vector z() const { return concatVectors(2, &accel_, &gyro_); } Vector6 z() const { return (Vector6() << accel_, gyro_);}
/** /**
* Error evaluation with optional derivatives - calculates * Error evaluation with optional derivatives - calculates
@ -78,10 +77,10 @@ public:
virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2, virtual Vector evaluateError(const PoseRTV& x1, const PoseRTV& x2,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const { boost::optional<Matrix&> H2 = boost::none) const {
const Vector meas = z(); const Vector6 meas = z();
if (H1) *H1 = numericalDerivative21<LieVector, PoseRTV, PoseRTV>( if (H1) *H1 = numericalDerivative21<Vector6, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
if (H2) *H2 = numericalDerivative22<LieVector, PoseRTV, PoseRTV>( if (H2) *H2 = numericalDerivative22<Vector6, PoseRTV, PoseRTV>(
boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5); boost::bind(This::predict_proxy, _1, _2, dt_, meas), x1, x2, 1e-5);
return predict_proxy(x1, x2, dt_, meas); return predict_proxy(x1, x2, dt_, meas);
} }
@ -96,10 +95,10 @@ public:
private: private:
/** copy of the measurement function formulated for numerical derivatives */ /** copy of the measurement function formulated for numerical derivatives */
static LieVector predict_proxy(const PoseRTV& x1, const PoseRTV& x2, static Vector6 predict_proxy(const PoseRTV& x1, const PoseRTV& x2,
double dt, const Vector& meas) { double dt, const Vector6& meas) {
Vector hx = x1.imuPrediction(x2, dt); Vector6 hx = x1.imuPrediction(x2, dt);
return LieVector(meas - hx); return meas - hx;
} }
}; };

51
gtsam_unstable/dynamics/Pendulum.h
View File

@ -9,7 +9,6 @@
#pragma once #pragma once
#include <gtsam/base/LieScalar.h>
#include <gtsam/nonlinear/NonlinearFactor.h> #include <gtsam/nonlinear/NonlinearFactor.h>
namespace gtsam { namespace gtsam {
@ -21,11 +20,11 @@ namespace gtsam {
* - For implicit Euler method: q_{k+1} = q_k + h*v_{k+1} * - For implicit Euler method: q_{k+1} = q_k + h*v_{k+1}
* - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1} * - For sympletic Euler method: q_{k+1} = q_k + h*v_{k+1}
*/ */
class PendulumFactor1: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> { class PendulumFactor1: public NoiseModelFactor3<double, double, double> {
public: public:
protected: protected:
typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base; typedef NoiseModelFactor3<double, double, double> Base;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
PendulumFactor1() {} PendulumFactor1() {}
@ -38,7 +37,7 @@ public:
///Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step ///Constructor. k1: q_{k+1}, k: q_k, velKey: velocity variable depending on the chosen method, h: time step
PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0) PendulumFactor1(Key k1, Key k, Key velKey, double h, double mu = 1000.0)
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), k1, k, velKey), h_(h) {} : Base(noiseModel::Constrained::All(1, fabs(mu)), k1, k, velKey), h_(h) {}
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const { virtual gtsam::NonlinearFactor::shared_ptr clone() const {
@ -46,15 +45,15 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactor1(*this))); }
/** q_k + h*v - q_k1 = 0, with optional derivatives */ /** q_k + h*v - q_k1 = 0, with optional derivatives */
Vector evaluateError(const LieScalar& qk1, const LieScalar& qk, const LieScalar& v, Vector evaluateError(const double& qk1, const double& qk, const double& v,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = LieScalar::Dim(); const size_t p = 1;
if (H1) *H1 = -eye(p); if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p); if (H2) *H2 = eye(p);
if (H3) *H3 = eye(p)*h_; if (H3) *H3 = eye(p)*h_;
return qk1.localCoordinates(qk.compose(LieScalar(v*h_))); return (Vector(1) << qk+v*h_-qk1);
} }
}; // \PendulumFactor1 }; // \PendulumFactor1
@ -67,11 +66,11 @@ public:
* - For implicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_{k+1}) * - For implicit Euler method: v_{k+1} = v_k - h*g/L*sin(q_{k+1})
* - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k) * - For sympletic Euler method: v_{k+1} = v_k - h*g/L*sin(q_k)
*/ */
class PendulumFactor2: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> { class PendulumFactor2: public NoiseModelFactor3<double, double, double> {
public: public:
protected: protected:
typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base; typedef NoiseModelFactor3<double, double, double> Base;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
PendulumFactor2() {} PendulumFactor2() {}
@ -86,7 +85,7 @@ public:
///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step ///Constructor. vk1: v_{k+1}, vk: v_k, qkey: q's key depending on the chosen method, h: time step
PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0) PendulumFactor2(Key vk1, Key vk, Key qkey, double h, double r = 1.0, double g = 9.81, double mu = 1000.0)
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {} : Base(noiseModel::Constrained::All(1, fabs(mu)), vk1, vk, qkey), h_(h), g_(g), r_(r) {}
/// @return a deep copy of this factor /// @return a deep copy of this factor
virtual gtsam::NonlinearFactor::shared_ptr clone() const { virtual gtsam::NonlinearFactor::shared_ptr clone() const {
@ -94,15 +93,15 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactor2(*this))); }
/** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */ /** v_k - h*g/L*sin(q) - v_k1 = 0, with optional derivatives */
Vector evaluateError(const LieScalar& vk1, const LieScalar& vk, const LieScalar& q, Vector evaluateError(const double & vk1, const double & vk, const double & q,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = LieScalar::Dim(); const size_t p = 1;
if (H1) *H1 = -eye(p); if (H1) *H1 = -eye(p);
if (H2) *H2 = eye(p); if (H2) *H2 = eye(p);
if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q.value()); if (H3) *H3 = -eye(p)*h_*g_/r_*cos(q);
return vk1.localCoordinates(LieScalar(vk - h_*g_/r_*sin(q))); return (Vector(1) << vk - h_ * g_ / r_ * sin(q) - vk1);
} }
}; // \PendulumFactor2 }; // \PendulumFactor2
@ -114,11 +113,11 @@ public:
* p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) * p_k = -D_1 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)+mgrh(1-\alpha)\,\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right)
* = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1}) * = (1/h)mr^2 (q_{k+1}-q_k) + mgrh(1-alpha) sin ((1-alpha)q_k+\alpha q_{k+1})
*/ */
class PendulumFactorPk: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> { class PendulumFactorPk: public NoiseModelFactor3<double, double, double> {
public: public:
protected: protected:
typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base; typedef NoiseModelFactor3<double, double, double> Base;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
PendulumFactorPk() {} PendulumFactorPk() {}
@ -136,7 +135,7 @@ public:
///Constructor ///Constructor
PendulumFactorPk(Key pKey, Key qKey, Key qKey1, PendulumFactorPk(Key pKey, Key qKey, Key qKey1,
double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0) double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), pKey, qKey, qKey1), : Base(noiseModel::Constrained::All(1, fabs(mu)), pKey, qKey, qKey1),
h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
/// @return a deep copy of this factor /// @return a deep copy of this factor
@ -145,11 +144,11 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk(*this))); }
/** 1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */ /** 1/h mr^2 (qk1-qk)+mgrh (1-a) sin((1-a)pk + a*pk1) - pk = 0, with optional derivatives */
Vector evaluateError(const LieScalar& pk, const LieScalar& qk, const LieScalar& qk1, Vector evaluateError(const double & pk, const double & qk, const double & qk1,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = LieScalar::Dim(); const size_t p = 1;
double qmid = (1-alpha_)*qk + alpha_*qk1; double qmid = (1-alpha_)*qk + alpha_*qk1;
double mr2_h = 1/h_*m_*r_*r_; double mr2_h = 1/h_*m_*r_*r_;
@ -159,7 +158,7 @@ public:
if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid)); if (H2) *H2 = eye(p)*(-mr2_h + mgrh*(1-alpha_)*(1-alpha_)*cos(qmid));
if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid)); if (H3) *H3 = eye(p)*( mr2_h + mgrh*(1-alpha_)*(alpha_)*cos(qmid));
return pk.localCoordinates(LieScalar(mr2_h*(qk1-qk) + mgrh*(1-alpha_)*sin(qmid))); return (Vector(1) << mr2_h * (qk1 - qk) + mgrh * (1 - alpha_) * sin(qmid) - pk);
} }
}; // \PendulumFactorPk }; // \PendulumFactorPk
@ -170,11 +169,11 @@ public:
* p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right) * p_k1 = D_2 L_d(q_k,q_{k+1},h) = \frac{1}{h}mr^{2}\left(q_{k+1}-q_{k}\right)-mgrh\alpha\sin\left((1-\alpha)q_{k}+\alpha q_{k+1}\right)
* = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1}) * = (1/h)mr^2 (q_{k+1}-q_k) - mgrh alpha sin ((1-alpha)q_k+\alpha q_{k+1})
*/ */
class PendulumFactorPk1: public NoiseModelFactor3<LieScalar, LieScalar, LieScalar> { class PendulumFactorPk1: public NoiseModelFactor3<double, double, double> {
public: public:
protected: protected:
typedef NoiseModelFactor3<LieScalar, LieScalar, LieScalar> Base; typedef NoiseModelFactor3<double, double, double> Base;
/** default constructor to allow for serialization */ /** default constructor to allow for serialization */
PendulumFactorPk1() {} PendulumFactorPk1() {}
@ -192,7 +191,7 @@ public:
///Constructor ///Constructor
PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1, PendulumFactorPk1(Key pKey1, Key qKey, Key qKey1,
double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0) double h, double m = 1.0, double r = 1.0, double g = 9.81, double alpha = 0.0, double mu = 1000.0)
: Base(noiseModel::Constrained::All(LieScalar::Dim(), fabs(mu)), pKey1, qKey, qKey1), : Base(noiseModel::Constrained::All(1, fabs(mu)), pKey1, qKey, qKey1),
h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {} h_(h), m_(m), r_(r), g_(g), alpha_(alpha) {}
/// @return a deep copy of this factor /// @return a deep copy of this factor
@ -201,11 +200,11 @@ public:
gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); } gtsam::NonlinearFactor::shared_ptr(new PendulumFactorPk1(*this))); }
/** 1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */ /** 1/h mr^2 (qk1-qk) - mgrh a sin((1-a)pk + a*pk1) - pk1 = 0, with optional derivatives */
Vector evaluateError(const LieScalar& pk1, const LieScalar& qk, const LieScalar& qk1, Vector evaluateError(const double & pk1, const double & qk, const double & qk1,
boost::optional<Matrix&> H1 = boost::none, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none, boost::optional<Matrix&> H2 = boost::none,
boost::optional<Matrix&> H3 = boost::none) const { boost::optional<Matrix&> H3 = boost::none) const {
const size_t p = LieScalar::Dim(); const size_t p = 1;
double qmid = (1-alpha_)*qk + alpha_*qk1; double qmid = (1-alpha_)*qk + alpha_*qk1;
double mr2_h = 1/h_*m_*r_*r_; double mr2_h = 1/h_*m_*r_*r_;
@ -215,7 +214,7 @@ public:
if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid)); if (H2) *H2 = eye(p)*(-mr2_h - mgrh*(1-alpha_)*alpha_*cos(qmid));
if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid)); if (H3) *H3 = eye(p)*( mr2_h - mgrh*alpha_*alpha_*cos(qmid));
return pk1.localCoordinates(LieScalar(mr2_h*(qk1-qk) - mgrh*alpha_*sin(qmid))); return (Vector(1) << mr2_h * (qk1 - qk) - mgrh * alpha_ * sin(qmid) - pk1);
} }
}; // \PendulumFactorPk1 }; // \PendulumFactorPk1

29
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -57,18 +57,17 @@ void PoseRTV::print(const string& s) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
PoseRTV PoseRTV::Expmap(const Vector& v) { PoseRTV PoseRTV::Expmap(const Vector9& v) {
assert(v.size() == 9); Pose3 newPose = Pose3::Expmap(v.head<6>());
Pose3 newPose = Pose3::Expmap(sub(v, 0, 6)); Velocity3 newVel = Velocity3::Expmap(v.tail<3>());
Velocity3 newVel = Velocity3::Expmap(sub(v, 6, 9));
return PoseRTV(newPose, newVel); return PoseRTV(newPose, newVel);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector PoseRTV::Logmap(const PoseRTV& p) { Vector9 PoseRTV::Logmap(const PoseRTV& p) {
Vector Lx = Pose3::Logmap(p.Rt_); Vector6 Lx = Pose3::Logmap(p.Rt_);
Vector Lv = Velocity3::Logmap(p.v_); Vector3 Lv = Velocity3::Logmap(p.v_);
return concatVectors(2, &Lx, &Lv); return (Vector9() << Lx, Lv);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -84,9 +83,9 @@ PoseRTV PoseRTV::retract(const Vector& v) const {
Vector PoseRTV::localCoordinates(const PoseRTV& p1) const { Vector PoseRTV::localCoordinates(const PoseRTV& p1) const {
const Pose3& x0 = pose(), &x1 = p1.pose(); const Pose3& x0 = pose(), &x1 = p1.pose();
// First order approximation // First order approximation
Vector poseLogmap = x0.localCoordinates(x1); Vector6 poseLogmap = x0.localCoordinates(x1);
Vector lv = rotation().unrotate(p1.velocity() - v_).vector(); Vector3 lv = rotation().unrotate(p1.velocity() - v_).vector();
return concatVectors(2, &poseLogmap, &lv); return (Vector(9) << poseLogmap, lv);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -190,16 +189,16 @@ PoseRTV PoseRTV::generalDynamics(
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const { Vector6 PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
// split out states // split out states
const Rot3 &r1 = R(), &r2 = x2.R(); const Rot3 &r1 = R(), &r2 = x2.R();
const Velocity3 &v1 = v(), &v2 = x2.v(); const Velocity3 &v1 = v(), &v2 = x2.v();
Vector imu(6); Vector6 imu;
// acceleration // acceleration
Vector accel = v1.localCoordinates(v2) / dt; Vector accel = v1.localCoordinates(v2) / dt;
imu.head(3) = r2.transpose() * (accel - g); imu.head<3>() = r2.transpose() * (accel - g);
// rotation rates // rotation rates
// just using euler angles based on matlab code // just using euler angles based on matlab code
@ -211,7 +210,7 @@ Vector PoseRTV::imuPrediction(const PoseRTV& x2, double dt) const {
// normalize yaw in difference (as per Mitch's code) // normalize yaw in difference (as per Mitch's code)
dR(2) = Rot2::fromAngle(dR(2)).theta(); dR(2) = Rot2::fromAngle(dR(2)).theta();
dR /= dt; dR /= dt;
imu.tail(3) = Enb * dR; imu.tail<3>() = Enb * dR;
// imu.tail(3) = r1.transpose() * dR; // imu.tail(3) = r1.transpose() * dR;
return imu; return imu;

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