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

Fixed significant bug in Pose2Factor, where we forgot to multiply the error with the square root inverse covariance.

release/4.3a0
Frank Dellaert 2010-01-10 13:55:55 +00:00
parent 8061f29ec5
commit 794ffd3b2f
6 changed files with 238 additions and 138 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
.cproject
View File

@ -15,35 +15,35 @@
</extensions> </extensions>
</storageModule> </storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> <storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration artifactName="gtsam" buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" name="MacOSX GCC" parent="org.eclipse.cdt.build.core.emptycfg"> <configuration artifactName="gtsam" buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" name="MacOSX GCC" parent="org.eclipse.cdt.build.core.emptycfg">
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath=""> <folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.2031210194" name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base"> <toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.677243255" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.MachO" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/> <targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.MachO" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/> <builder id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/> <tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.457360678" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base"> <tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input"> <inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">
<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/> <additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
<additionalInput kind="additionalinput" paths="$(LIBS)"/> <additionalInput kind="additionalinput" paths="$(LIBS)"/>
</inputType> </inputType>
</tool> </tool>
<tool id="cdt.managedbuild.tool.gnu.assembler.macosx.base.85834184" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.macosx.base"> <tool id="cdt.managedbuild.tool.gnu.assembler.macosx.base.85834184" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.macosx.base">
<inputType id="cdt.managedbuild.tool.gnu.assembler.input.1787895621" superClass="cdt.managedbuild.tool.gnu.assembler.input"/> <inputType id="cdt.managedbuild.tool.gnu.assembler.input.1787895621" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
</tool> </tool>
<tool id="cdt.managedbuild.tool.gnu.archiver.macosx.base.217086069" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.macosx.base"/> <tool id="cdt.managedbuild.tool.gnu.archiver.macosx.base.217086069" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.macosx.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base.1629258328" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base"> <tool id="cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base.1629258328" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base">
<option id="gnu.cpp.compiler.option.include.paths.1552452888" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" valueType="includePath"> <option id="gnu.cpp.compiler.option.include.paths.1552452888" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" valueType="includePath">
<listOptionValue builtIn="false" value="&quot;${HOME}/include&quot;"/> <listOptionValue builtIn="false" value="&quot;${HOME}/include&quot;"/>
</option> </option>
<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.1833545667" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/> <inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.1833545667" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
</tool> </tool>
<tool id="cdt.managedbuild.tool.gnu.c.compiler.macosx.base.1347102680" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.macosx.base"> <tool id="cdt.managedbuild.tool.gnu.c.compiler.macosx.base.1347102680" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.macosx.base">
<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.1718446861" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/> <inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.1718446861" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
</tool> </tool>
</toolChain> </toolChain>
</folderInfo> </folderInfo>
</configuration> </configuration>
</storageModule> </storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/> <storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/> <storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/> <storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
@ -300,7 +300,7 @@
<buildTargets> <buildTargets>
<target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="install" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>install</buildTarget> <buildTarget>install</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -308,7 +308,7 @@
</target> </target>
<target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget> <buildTarget>check</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -324,7 +324,7 @@
</target> </target>
<target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSimpleCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testSimpleCamera.run</buildTarget> <buildTarget>testSimpleCamera.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -347,7 +347,7 @@
</target> </target>
<target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testCalibratedCamera.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testCalibratedCamera.run</buildTarget> <buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -362,7 +362,7 @@
</target> </target>
<target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testPose2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testPose2.run</buildTarget> <buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -370,7 +370,7 @@
</target> </target>
<target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testRot3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testRot3.run</buildTarget> <buildTarget>testRot3.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -385,7 +385,7 @@
</target> </target>
<target name="testGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget> <buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -393,7 +393,7 @@
</target> </target>
<target name="testGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactorGraph.run</buildTarget> <buildTarget>testGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -408,7 +408,7 @@
</target> </target>
<target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testPose3.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testPose3.run</buildTarget> <buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -430,7 +430,7 @@
</target> </target>
<target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testNonlinearFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testNonlinearFactor.run</buildTarget> <buildTarget>testNonlinearFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -438,7 +438,7 @@
</target> </target>
<target name="timeGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="timeGaussianFactor.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>timeGaussianFactor.run</buildTarget> <buildTarget>timeGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -446,7 +446,7 @@
</target> </target>
<target name="timeGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="timeGaussianFactorGraph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>timeGaussianFactorGraph.run</buildTarget> <buildTarget>timeGaussianFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -454,7 +454,7 @@
</target> </target>
<target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGaussianBayesNet.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testGaussianBayesNet.run</buildTarget> <buildTarget>testGaussianBayesNet.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -484,7 +484,7 @@
</target> </target>
<target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testVector.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testVector.run</buildTarget> <buildTarget>testVector.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -492,7 +492,7 @@
</target> </target>
<target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testMatrix.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testMatrix.run</buildTarget> <buildTarget>testMatrix.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -514,7 +514,7 @@
</target> </target>
<target name="testSQP.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSQP.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testSQP.run</buildTarget> <buildTarget>testSQP.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -529,7 +529,7 @@
</target> </target>
<target name="testSQPOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSQPOptimizer.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testSQPOptimizer.run</buildTarget> <buildTarget>testSQPOptimizer.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -537,7 +537,7 @@
</target> </target>
<target name="testVSLAMConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testVSLAMConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testVSLAMConfig.run</buildTarget> <buildTarget>testVSLAMConfig.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -545,7 +545,7 @@
</target> </target>
<target name="testControlConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testControlConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testControlConfig.run</buildTarget> <buildTarget>testControlConfig.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -553,7 +553,7 @@
</target> </target>
<target name="testControlPoint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testControlPoint.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testControlPoint.run</buildTarget> <buildTarget>testControlPoint.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -568,7 +568,7 @@
</target> </target>
<target name="testOrdering.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testOrdering.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testOrdering.run</buildTarget> <buildTarget>testOrdering.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -583,7 +583,7 @@
</target> </target>
<target name="testRot2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testRot2.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testRot2.run</buildTarget> <buildTarget>testRot2.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -591,7 +591,7 @@
</target> </target>
<target name="testGaussianBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testGaussianBayesTree.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testGaussianBayesTree.run</buildTarget> <buildTarget>testGaussianBayesTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -627,7 +627,7 @@
</target> </target>
<target name="testUrbanOdometry.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testUrbanOdometry.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testUrbanOdometry.run</buildTarget> <buildTarget>testUrbanOdometry.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -649,7 +649,7 @@
</target> </target>
<target name="testSubgraphPreconditioner.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testSubgraphPreconditioner.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testSubgraphPreconditioner.run</buildTarget> <buildTarget>testSubgraphPreconditioner.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -657,7 +657,7 @@
</target> </target>
<target name="testBayesNetPreconditioner.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testBayesNetPreconditioner.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments/> <buildArguments>-j2</buildArguments>
<buildTarget>testBayesNetPreconditioner.run</buildTarget> <buildTarget>testBayesNetPreconditioner.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
@ -684,10 +684,10 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="testLieConfig.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="testPose2Graph.run" path="cpp" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments/>
<buildTarget>testLieConfig.run</buildTarget> <buildTarget>testPose2Graph.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
@ -718,6 +718,6 @@
</cconfiguration> </cconfiguration>
</storageModule> </storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0"> <storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="gtsam.null.1344331286" name="gtsam"/> <project id="gtsam.null.1344331286" name="gtsam"/>
</storageModule> </storageModule>
</cproject> </cproject>

24
cpp/BetweenFactor.h
View File

@ -63,10 +63,12 @@ namespace gtsam {
/** implement functions needed to derive from Factor */ /** implement functions needed to derive from Factor */
/** vector of errors */ /** vector of errors */
Vector error_vector(const Config& config) const { Vector error_vector(const Config& x) const {
//z-h //z-h
T p1 = config.get(key1_), p2 = config.get(key2_); T p1 = x.get(key1_), p2 = x.get(key2_);
return -logmap(between(p1, p2), measured_); T hx = between(p1,p2);
// manifold equivalent of z-h(x) -> log(h(x),z)
return square_root_inverse_covariance_ * logmap(hx,measured_);
} }
/** keys as a list */ /** keys as a list */
@ -76,14 +78,14 @@ namespace gtsam {
inline std::size_t size() const { return 2;} inline std::size_t size() const { return 2;}
/** linearize */ /** linearize */
boost::shared_ptr<GaussianFactor> linearize(const Config& config) const { boost::shared_ptr<GaussianFactor> linearize(const Config& x0) const {
T p1 = config.get(key1_), p2 = config.get(key2_); T p1 = x0.get(key1_), p2 = x0.get(key2_);
Vector b = -logmap(between(p1, p2), measured_); Matrix A1 = Dbetween1(p1, p2);
Matrix H1 = Dbetween1(p1, p2); Matrix A2 = Dbetween2(p1, p2);
Matrix H2 = Dbetween2(p1, p2); Vector b = error_vector(x0); // already has sigmas in !
boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(key1_, boost::shared_ptr<GaussianFactor> linearized(new GaussianFactor(
square_root_inverse_covariance_ * H1, key2_, key1_, square_root_inverse_covariance_ * A1,
square_root_inverse_covariance_ * H2, b, 1.0)); key2_, square_root_inverse_covariance_ * A2, b, 1.0));
return linearized; return linearized;
} }
}; };

8
cpp/Pose3Factor.h
View File

@ -11,6 +11,9 @@
namespace gtsam { namespace gtsam {
/**
* A config specifically for 3D poses
*/
class Pose3Config: public std::map<std::string, Pose3> { class Pose3Config: public std::map<std::string, Pose3> {
public: public:
@ -23,7 +26,10 @@ namespace gtsam {
}; };
/** This is just a typedef now */ /**
* A Factor for 3D pose measurements
* This is just a typedef now
*/
typedef BetweenFactor<Pose3, Pose3Config> Pose3Factor; typedef BetweenFactor<Pose3, Pose3Config> Pose3Factor;
} /// namespace gtsam } /// namespace gtsam

148
cpp/testPose2Factor.cpp
View File

@ -1,10 +1,9 @@
/** /**
* @file testPose2Constraint.cpp * @file testPose2Factor.cpp
* @brief Unit tests for Pose2Factor Class * @brief Unit tests for Pose2Factor Class
* @authors Frank Dellaert, Viorela Ila * @authors Frank Dellaert, Viorela Ila
**/ **/
/*STL/C++*/
#include <iostream> #include <iostream>
#include <boost/shared_ptr.hpp> #include <boost/shared_ptr.hpp>
#include <boost/assign/std/list.hpp> #include <boost/assign/std/list.hpp>
@ -12,6 +11,8 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "Vector.h"
#include "numericalDerivative.h"
#include "NonlinearOptimizer-inl.h" #include "NonlinearOptimizer-inl.h"
#include "NonlinearEquality.h" #include "NonlinearEquality.h"
#include "Pose2Factor.h" #include "Pose2Factor.h"
@ -20,51 +21,124 @@ using namespace boost::assign;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// Common measurement covariance
static double sx=0.5, sy=0.5,st=0.1;
static Matrix covariance = Matrix_(3,3,
sx*sx, 0.0, 0.0,
0.0, sy*sy, 0.0,
0.0, 0.0, st*st
);
/* ************************************************************************* */
// Very simple test establishing Ax-b \approx h(x)-z
TEST( Pose2Factor, error )
{
// Choose a linearization point
Pose2 p1; // robot at origin
Pose2 p2(1, 0, 0); // robot at (1,0)
Pose2Config x0;
x0.insert("p1", p1);
x0.insert("p2", p2);
// Create factor
Pose2 z = between(p1,p2);
Pose2Factor factor("p1", "p2", z, covariance);
// Actual linearization
boost::shared_ptr<GaussianFactor> linear = factor.linearize(x0);
// Check error at x0 = zero !
VectorConfig delta;
delta.insert("p1", zero(3));
delta.insert("p2", zero(3));
Vector error_at_zero = Vector_(3,0.0,0.0,0.0);
CHECK(assert_equal(error_at_zero,factor.error_vector(x0)));
CHECK(assert_equal(-error_at_zero,linear->error_vector(delta)));
// Check error after increasing p2
VectorConfig plus = delta + VectorConfig("p2", Vector_(3, 0.1, 0.0, 0.0));
Pose2Config x1 = expmap(x0, plus);
Vector error_at_plus = Vector_(3,-0.1/sx,0.0,0.0);
CHECK(assert_equal(error_at_plus,factor.error_vector(x1)));
CHECK(assert_equal(-error_at_plus,linear->error_vector(plus)));
}
/* ************************************************************************* */
// common Pose2Factor for tests below
static Pose2 measured(2,2,M_PI_2);
static Pose2Factor factor("p1","p2",measured, covariance);
/* ************************************************************************* */
TEST( Pose2Factor, rhs )
{
// Choose a linearization point
Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at -1,4)
Pose2Config x0;
x0.insert("p1",p1);
x0.insert("p2",p2);
// Actual linearization
boost::shared_ptr<GaussianFactor> linear = factor.linearize(x0);
// Check RHS
Pose2 hx0 = between(p1,p2);
CHECK(assert_equal(Pose2(2.1, 2.1, M_PI_2),hx0));
Vector expected_b = Vector_(3, -0.1/sx, 0.1/sy, 0.0);
CHECK(assert_equal(expected_b,factor.error_vector(x0)));
CHECK(assert_equal(expected_b,linear->get_b()));
}
/* ************************************************************************* */
// The error |A*dx-b| approximates (h(x0+dx)-z) = -error_vector
// Hence i.e., b = approximates z-h(x0) = error_vector(x0)
Vector h(const Pose2& p1,const Pose2& p2) {
Pose2Config x;
x.insert("p1",p1);
x.insert("p2",p2);
return -factor.error_vector(x);
}
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose2Factor, linearize ) TEST( Pose2Factor, linearize )
{ {
// create a factor between unknown poses p1 and p2 // Choose a linearization point at ground truth
Pose2 measured(2,2,M_PI_2); Pose2 p1(1,2,M_PI_2);
Matrix measurement_covariance = Matrix_(3,3, Pose2 p2(-1,4,M_PI);
0.25, 0.0, 0.0, Pose2Config x0;
0.0, 0.25, 0.0, x0.insert("p1",p1);
0.0, 0.0, 0.01 x0.insert("p2",p2);
);
Pose2Factor constraint("p1","p2",measured, measurement_covariance);
// Choose a linearization point
Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
Pose2 p2(-1,4.1,M_PI); // robot at (-1,4.1) looking at negative (ground truth is at 4.1,2)
Pose2Config config;
config.insert("p1",p1);
config.insert("p2",p2);
// expected linearization // expected linearization
// we need the minus signs below as "inverse_square_root"
// uses SVD and the sign is simply arbitrary (but still a square root!)
Matrix square_root_inverse_covariance = Matrix_(3,3, Matrix square_root_inverse_covariance = Matrix_(3,3,
-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, -10.0 0.0, 0.0, 10.0
); );
Matrix expectedH1 = Matrix_(3,3, Matrix expectedH1 = square_root_inverse_covariance*Matrix_(3,3,
0.0,-1.0,-2.1, 0.0,-1.0,-2.0,
1.0, 0.0,-2.1, 1.0, 0.0,-2.0,
0.0, 0.0,-1.0 0.0, 0.0,-1.0
); );
Matrix expectedH2 = Matrix_(3,3, Matrix expectedH2 = square_root_inverse_covariance*Matrix_(3,3,
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
); );
GaussianFactor expected( Vector expected_b = Vector_(3, 0.0, 0.0, 0.0);
"p1", square_root_inverse_covariance*expectedH1,
"p2", square_root_inverse_covariance*expectedH2, // expected linear factor
Vector_(3, 0.1, -0.1, 0.0), 1.0); GaussianFactor expected("p1", expectedH1, "p2", expectedH2, expected_b, 1.0);
// Actual linearization // Actual linearization
boost::shared_ptr<GaussianFactor> actual = constraint.linearize(config); boost::shared_ptr<GaussianFactor> actual = factor.linearize(x0);
CHECK(assert_equal(expected,*actual)); CHECK(assert_equal(expected,*actual));
// Numerical do not work out because BetweenFactor is approximate ?
Matrix numericalH1 = numericalDerivative21(h, p1, p2, 1e-5);
CHECK(assert_equal(expectedH1,numericalH1));
Matrix numericalH2 = numericalDerivative22(h, p1, p2, 1e-5);
CHECK(assert_equal(expectedH2,numericalH2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -84,10 +158,7 @@ TEST(Pose2Factor, optimize) {
fg.push_back(Pose2Graph::sharedFactor( fg.push_back(Pose2Graph::sharedFactor(
new NonlinearEquality<Pose2Config>("p0", feasible, dim(Pose2()), poseCompare))); new NonlinearEquality<Pose2Config>("p0", feasible, dim(Pose2()), poseCompare)));
fg.push_back(Pose2Graph::sharedFactor( fg.push_back(Pose2Graph::sharedFactor(
new Pose2Factor("p0", "p1", Pose2(1,2,M_PI_2), Matrix_(3,3, new Pose2Factor("p0", "p1", Pose2(1,2,M_PI_2), covariance)));
0.5, 0.0, 0.0,
0.0, 0.5, 0.0,
0.0, 0.0, 0.5))));
// Create initial config // Create initial config
boost::shared_ptr<Pose2Config> initial = boost::shared_ptr<Pose2Config> initial =
@ -98,8 +169,11 @@ TEST(Pose2Factor, optimize) {
// Choose an ordering and optimize // Choose an ordering and optimize
Ordering ordering; Ordering ordering;
ordering += "p0","p1"; ordering += "p0","p1";
NonlinearOptimizer<Pose2Graph, Pose2Config> optimizer(fg, ordering, initial); typedef NonlinearOptimizer<Pose2Graph, Pose2Config> Optimizer;
optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15); Optimizer optimizer(fg, ordering, initial);
Optimizer::verbosityLevel verbosity = Optimizer::SILENT;
//Optimizer::verbosityLevel verbosity = Optimizer::ERROR;
optimizer = optimizer.levenbergMarquardt(1e-15, 1e-15, verbosity);
// Check with expected config // Check with expected config
Pose2Config expected; Pose2Config expected;

51
cpp/testPose2Graph.cpp
View File

@ -1,5 +1,8 @@
/**
* @file testPose2Graph.cpp
* @authors Frank Dellaert, Viorela Ila
**/
/*STL/C++*/
#include <iostream> #include <iostream>
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
@ -10,19 +13,22 @@
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
// common measurement covariance
static double sx=0.5, sy=0.5,st=0.1;
static Matrix covariance = Matrix_(3,3,
sx*sx, 0.0, 0.0,
0.0, sy*sy, 0.0,
0.0, 0.0, st*st
);
/* ************************************************************************* */
TEST( Pose2Graph, constructor ) TEST( Pose2Graph, constructor )
{ {
// create a factor between unknown poses p1 and p2 // create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2); Pose2 measured(2,2,M_PI_2);
Matrix measurement_covariance = Matrix_(3,3, Pose2Factor constraint("x1","x2",measured, covariance);
0.25, 0.0, 0.0,
0.0, 0.25, 0.0,
0.0, 0.0, 0.01
);
Pose2Factor constraint("x1","x2",measured, measurement_covariance);
Pose2Graph graph; Pose2Graph graph;
graph.push_back(Pose2Factor::shared_ptr(new Pose2Factor("x1","x2",measured, measurement_covariance))); graph.push_back(Pose2Factor::shared_ptr(new Pose2Factor("x1","x2",measured, covariance)));
// get the size of the graph // get the size of the graph
size_t actual = graph.size(); size_t actual = graph.size();
// verify // verify
@ -30,18 +36,15 @@ TEST( Pose2Graph, constructor )
CHECK(actual == expected); CHECK(actual == expected);
} }
/* ************************************************************************* */
TEST( Pose2Graph, linerization ) TEST( Pose2Graph, linerization )
{ {
// create a factor between unknown poses p1 and p2 // create a factor between unknown poses p1 and p2
Pose2 measured(2,2,M_PI_2); Pose2 measured(2,2,M_PI_2);
Matrix measurement_covariance = Matrix_(3,3, Pose2Factor constraint("x1","x2",measured, covariance);
0.25, 0.0, 0.0,
0.0, 0.25, 0.0,
0.0, 0.0, 0.01
);
Pose2Factor constraint("x1","x2",measured, measurement_covariance);
Pose2Graph graph; Pose2Graph graph;
graph.push_back(Pose2Factor::shared_ptr(new Pose2Factor("x1","x2",measured, measurement_covariance))); graph.push_back(Pose2Factor::shared_ptr(new Pose2Factor("x1","x2",measured, covariance)));
// Choose a linearization point // Choose a linearization point
Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2) Pose2 p1(1.1,2,M_PI_2); // robot at (1.1,2) looking towards y (ground truth is at 1,2, see testPose2)
@ -56,24 +59,22 @@ TEST( Pose2Graph, linerization )
// the expected linear factor // the expected linear factor
GaussianFactorGraph lfg_expected; GaussianFactorGraph lfg_expected;
Matrix A1 = Matrix_(3,3, Matrix A1 = Matrix_(3,3,
0.0, 2.0, 4.2, 0.0,-2.0, -4.2,
-2.0, 0.0, 4.2, 2.0, 0.0, -4.2,
0.0, 0.0, 10.0); 0.0, 0.0,-10.0);
Matrix A2 = Matrix_(3,3, Matrix A2 = Matrix_(3,3,
-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, -10.0); 0.0, 0.0, 10.0);
double sigma = 1; double sigma = 1;
Vector b = Vector_(3,0.1,-0.1,0.0); Vector b = Vector_(3,-0.1/sx,0.1/sy,0.0);
lfg_expected.add("x1", A1, "x2", A2, b, sigma); lfg_expected.add("x1", A1, "x2", A2, b, sigma);
CHECK(assert_equal(lfg_expected, lfg_linearized)); CHECK(assert_equal(lfg_expected, lfg_linearized));
} }
/* ************************************************************************* */ /* ************************************************************************* */
int main() { int main() {
TestResult tr; TestResult tr;

25
cpp/testPose3Factor.cpp
View File

@ -5,6 +5,8 @@
**/ **/
#include <iostream> #include <iostream>
#include <boost/assign/std/list.hpp>
using namespace boost::assign;
#include <CppUnitLite/TestHarness.h> #include <CppUnitLite/TestHarness.h>
#include "Pose3Factor.h" #include "Pose3Factor.h"
@ -12,11 +14,26 @@ using namespace std;
using namespace gtsam; using namespace gtsam;
/* ************************************************************************* */ /* ************************************************************************* */
TEST( Pose3Factor, constructor ) TEST( Pose3Factor, error )
{ {
Pose3 measured; // Create example
Matrix measurement_covariance; Pose3 t1; // origin
Pose3Factor("x1", "x2", measured, measurement_covariance); Pose3 t2(rodriguez(0.1,0.2,0.3),Point3(0,1,0));
Pose3 z(rodriguez(0.2,0.2,0.3),Point3(0,1.1,0));;
// Create factor
Matrix measurement_covariance = eye(6);
Pose3Factor factor("t1", "t2", z, measurement_covariance);
// Create config
Pose3Config x;
x.insert(make_pair("t1",t1));
x.insert(make_pair("t2",t2));
// Get error z-h(x) -> logmap(h(x),z) = logmap(between(t1,t2),z)
Vector actual = factor.error_vector(x);
Vector expected = logmap(between(t1,t2),z);
CHECK(assert_equal(expected,actual));
} }
/* ************************************************************************* */ /* ************************************************************************* */