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

Merge branch 'develop'

release/4.3a0
cbeall3 2016-06-20 13:30:37 -04:00
parent 3c1d2746a8 f3fce5cb1c
commit c21186c621
1376 changed files with 485897 additions and 28797 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

738
.cproject
View File

@ -19,7 +19,7 @@
<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">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.752782918" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder arguments="" buildPath="${ProjDirPath}/build-3_0_0" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<builder buildPath="${ProjDirPath}/build" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="4" 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.cpp.linker.macosx.base.1011140787" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<option id="macosx.cpp.link.option.paths.451252615" name="Library search path (-L)" superClass="macosx.cpp.link.option.paths"/>
@ -130,7 +130,7 @@
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216." name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.1052998998" name="cdt.managedbuild.toolchain.gnu.macosx.base" superClass="cdt.managedbuild.toolchain.gnu.macosx.base">
<targetPlatform archList="all" binaryParser="org.eclipse.cdt.core.ELF;org.eclipse.cdt.core.MachO64" id="cdt.managedbuild.target.gnu.platform.macosx.base.1735323246" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder arguments="" buildPath="${workspace_loc:/gtsam/build-fast}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.1127775962" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<builder arguments="" buildPath="${workspace_loc:/gtsam/build-fast}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.1127775962" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="optimal" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.1922513433" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.1097733515" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<option id="macosx.cpp.link.option.paths.2077171343" name="Library search path (-L)" superClass="macosx.cpp.link.option.paths" valueType="libPaths">
@ -176,16 +176,8 @@
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="gtsam.null.1344331286" name="gtsam"/>
</storageModule>
<storageModule moduleId="refreshScope" versionNumber="2">
<configuration configurationName="Timing">
<resource resourceType="PROJECT" workspacePath="/gtsam"/>
</configuration>
<configuration configurationName="fast">
<resource resourceType="PROJECT" workspacePath="/gtsam"/>
</configuration>
<configuration configurationName="MacOSX GCC">
<resource resourceType="PROJECT" workspacePath="/gtsam"/>
</configuration>
<storageModule moduleId="refreshScope" versionNumber="1">
<resource resourceType="PROJECT" workspacePath="/gtsam"/>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
<storageModule moduleId="org.eclipse.cdt.core.LanguageSettingsProviders"/>
@ -516,22 +508,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthCamera3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInvDepthCamera3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testTriangulation.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testTriangulation.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -556,6 +532,38 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSimilarity3.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSimilarity3.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">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInvDepthCamera3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testTriangulation.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testTriangulation.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEvent.run" path="build/gtsam_unstable/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testEvent.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -724,14 +732,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -788,7 +788,87 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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="testSmartStereoProjectionPoseFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSmartStereoProjectionPoseFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testTOAFactor.run" path="build/gtsam_unstable/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testTOAFactor.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>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianFactorGraphUnordered.run</buildTarget>
@ -884,6 +964,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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="testCombinedImuFactor.run" path="build/gtsam/navigation/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -900,6 +988,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testAHRSFactor.run" path="build/gtsam/navigation/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testAHRSFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testAttitudeFactor.run" path="build/gtsam/navigation/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j8</buildArguments>
<buildTarget>testAttitudeFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build/wrap/gtsam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1236,46 +1340,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1417,6 +1481,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBTree.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testBTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSF.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSF.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFMap.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFMap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDSFVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDSFVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testFixedVector.run" path="build/gtsam_unstable/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testFixedVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1457,6 +1561,14 @@
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLabeledSymbol.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testLabeledSymbol.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1872,6 +1984,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.navigation" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2 VERBOSE=1</buildArguments>
<buildTarget>check.navigation</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.sam" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>check.sam</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2031,6 +2159,86 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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="testSO3.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSO3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testQuaternion.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testQuaternion.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testOrientedPlane3.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testOrientedPlane3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPinholePose.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testPinholePose.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCyclic.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testCyclic.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testUnit3.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testUnit3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBearingRange.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testBearingRange.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2111,22 +2319,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testIMUSystem.run" path="build/gtsam_unstable/dynamics/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testIMUSystem.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoseRTV.run" path="build/gtsam_unstable/dynamics/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPoseRTV.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">
<buildCommand>make</buildCommand>
<buildArguments>-j1</buildArguments>
@ -2167,6 +2359,38 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testIMUSystem.run" path="build/gtsam_unstable/dynamics/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testIMUSystem.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoseRTV.run" path="build/gtsam_unstable/dynamics/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPoseRTV.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2207,6 +2431,78 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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="testType.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testType.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testArgument.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testArgument.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testReturnValue.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testReturnValue.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testTemplate.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testTemplate.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGlobalFunction.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testGlobalFunction.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">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2287,6 +2583,38 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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="testOptionalJacobian.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testOptionalJacobian.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGroup.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testGroup.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">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2503,6 +2831,30 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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="testLie.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testLie.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSerializationSLAM.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSerializationSLAM.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">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2575,10 +2927,10 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j5</buildArguments>
<buildTarget>testBetweenFactor.run</buildTarget>
<buildTarget>testPriorFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -2631,18 +2983,10 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</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>
<buildArguments>-j5</buildArguments>
<buildTarget>testGPSFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussMarkov1stOrderFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussMarkov1stOrderFactor.run</buildTarget>
<buildTarget>testPoseRotationPrior.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -2655,6 +2999,30 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testOrientedPlane3Factor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testOrientedPlane3Factor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSmartProjectionPoseFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSmartProjectionPoseFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInitializePose3.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testInitializePose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2831,6 +3199,30 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="Pose2SLAMExampleExpressions.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>Pose2SLAMExampleExpressions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SFMExampleExpressions.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>SFMExampleExpressions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SFMExampleExpressions_bal.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>SFMExampleExpressions_bal.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLago.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2871,6 +3263,54 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testExpression.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testExpression.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testAdaptAutoDiff.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testAdaptAutoDiff.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCallRecord.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testCallRecord.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testExpressionFactor.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testExpressionFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testExecutionTrace.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testExecutionTrace.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSerializationNonlinear.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testSerializationNonlinear.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
@ -2879,6 +3319,21 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeCalibratedCamera.run" path="build/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2919,19 +3374,60 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="timeAdaptAutoDiff.run" path="build/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>check</buildTarget>
<buildArguments>-j4</buildArguments>
<buildTarget>timeAdaptAutoDiff.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="timeCameraExpression.run" path="build/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<buildArguments>-j4</buildArguments>
<buildTarget>timeCameraExpression.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeOneCameraExpression.run" path="build/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>timeOneCameraExpression.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeSFMExpressions.run" path="build/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>timeSFMExpressions.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeIncremental.run" path="build/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>timeIncremental.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeSchurFactors.run" path="build/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>timeSchurFactors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeRot2.run" path="build/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>timeRot2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
@ -3030,18 +3526,26 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testBearingFactor.run" path="build/gtsam/sam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSpirit.run</buildTarget>
<buildArguments>-j4</buildArguments>
<buildTarget>testBearingFactor.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">
<target name="testRangeFactor.run" path="build/gtsam/sam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.wrap</buildTarget>
<buildArguments>-j4</buildArguments>
<buildTarget>testRangeFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testBearingRangeFactor.run" path="build/gtsam/sam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testBearingRangeFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>

6
.gitignore vendored
View File

@ -1,7 +1,11 @@
/build*
/doc*
.idea
*.pyc
*.DS_Store
/examples/Data/dubrovnik-3-7-pre-rewritten.txt
/examples/Data/pose2example-rewritten.txt
/examples/Data/pose3example-rewritten.txt
*.txt.user
*.txt.user.6d59f0c
/python-build/
*.pydevproject

4
.project
View File

@ -23,7 +23,7 @@
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildArguments</key>
<value>-j5</value>
<value>-j4</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildCommand</key>
@ -63,7 +63,7 @@
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.useDefaultBuildCmd</key>
<value>false</value>
<value>true</value>
</dictionary>
</arguments>
</buildCommand>

1
.settings/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
/org.eclipse.cdt.codan.core.prefs

6
.settings/org.eclipse.cdt.core.prefs Normal file
View File

@ -0,0 +1,6 @@
eclipse.preferences.version=1
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/delimiter=\:
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/operation=append
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/PATH/value=$PATH\:/opt/local/bin
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/append=true
environment/project/cdt.managedbuild.toolchain.gnu.macosx.base.1359703544/appendContributed=true

142
CMakeLists.txt
View File

@ -9,9 +9,9 @@ if(NOT DEFINED CMAKE_MACOSX_RPATH)
endif()
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 3)
set (GTSAM_VERSION_MINOR 2)
set (GTSAM_VERSION_PATCH 1)
set (GTSAM_VERSION_MAJOR 4)
set (GTSAM_VERSION_MINOR 0)
set (GTSAM_VERSION_PATCH 0)
math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
@ -59,11 +59,16 @@ option(GTSAM_BUILD_STATIC_LIBRARY "Build a static gtsam library, instead
option(GTSAM_USE_QUATERNIONS "Enable/Disable using an internal Quaternion representation for rotations instead of rotation matrices. If enable, Rot3::EXPMAP is enforced by default." OFF)
option(GTSAM_POSE3_EXPMAP "Enable/Disable using Pose3::EXPMAP as the default mode. If disabled, Pose3::FIRST_ORDER will be used." OFF)
option(GTSAM_ROT3_EXPMAP "Ignore if GTSAM_USE_QUATERNIONS is OFF (Rot3::EXPMAP by default). Otherwise, enable Rot3::EXPMAP, or if disabled, use Rot3::CAYLEY." OFF)
option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF)
option(GTSAM_ENABLE_CONSISTENCY_CHECKS "Enable/Disable expensive consistency checks" OFF)
option(GTSAM_WITH_TBB "Use Intel Threaded Building Blocks (TBB) if available" ON)
option(GTSAM_WITH_EIGEN_MKL "Eigen will use Intel MKL if available" ON)
option(GTSAM_WITH_EIGEN_MKL_OPENMP "Eigen, when using Intel MKL, will also use OpenMP for multithreading if available" ON)
option(GTSAM_THROW_CHEIRALITY_EXCEPTION "Throw exception when a triangulated point is behind a camera" ON)
option(GTSAM_BUILD_PYTHON "Enable/Disable building & installation of Python module" OFF)
option(GTSAM_ALLOW_DEPRECATED_SINCE_V4 "Allow use of methods/functions deprecated in GTSAM 4" ON)
option(GTSAM_TYPEDEF_POINTS_TO_VECTORS "Typdef Point2 and Point3 to Eigen::Vector equivalents" OFF)
option(GTSAM_SUPPORT_NESTED_DISSECTION "Support Metis-based nested dissection" ON)
option(GTSAM_TANGENT_PREINTEGRATION "Use new ImuFactor with integration on tangent space" ON)
# Options relating to MATLAB wrapper
# TODO: Check for matlab mex binary before handling building of binaries
@ -82,6 +87,14 @@ if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_BUILD_STATIC_LIBRARY)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_BUILD_STATIC_LIBRARY are both enabled. The MATLAB wrapper cannot be compiled with a static GTSAM library because mex modules are themselves shared libraries. If you want a self-contained mex module, enable GTSAM_MEX_BUILD_STATIC_MODULE instead of GTSAM_BUILD_STATIC_LIBRARY.")
endif()
if(GTSAM_BUILD_PYTHON AND GTSAM_ALLOW_DEPRECATED_SINCE_V4)
message(FATAL_ERROR "GTSAM_BUILD_PYTHON and GTSAM_ALLOW_DEPRECATED_SINCE_V4 are both enabled. The python module cannot be compiled with deprecated functions turned on. Turn one of the two options off.")
endif()
if(GTSAM_INSTALL_MATLAB_TOOLBOX AND GTSAM_TYPEDEF_POINTS_TO_VECTORS)
message(FATAL_ERROR "GTSAM_INSTALL_MATLAB_TOOLBOX and GTSAM_TYPEDEF_POINTS_TO_VECTORS are both enabled. For now, the MATLAB toolbox cannot deal with this yet. Please turn one of the two options off.")
endif()
# Flags for choosing default packaging tools
set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator")
set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator")
@ -131,7 +144,7 @@ endif()
if(NOT (${Boost_VERSION} LESS 105600))
message("Ignoring Boost restriction on optional lvalue assignment from rvalues")
add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES)
add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES -DBOOST_OPTIONAL_CONFIG_ALLOW_BINDING_TO_RVALUES)
endif()
###############################################################################
@ -158,6 +171,12 @@ else()
set(GTSAM_USE_TBB 0) # This will go into config.h
endif()
###############################################################################
# Prohibit Timing build mode in combination with TBB
if(GTSAM_USE_TBB AND (CMAKE_BUILD_TYPE STREQUAL "Timing"))
message(FATAL_ERROR "Timing build mode cannot be used together with TBB. Use a sampling profiler such as Instruments or Intel VTune Amplifier instead.")
endif()
###############################################################################
# Find Google perftools
@ -173,6 +192,11 @@ if(MKL_FOUND AND GTSAM_WITH_EIGEN_MKL)
set(EIGEN_USE_MKL_ALL 1) # This will go into config.h - it makes Eigen use MKL
include_directories(${MKL_INCLUDE_DIR})
list(APPEND GTSAM_ADDITIONAL_LIBRARIES ${MKL_LIBRARIES})
# --no-as-needed is required with gcc according to the MKL link advisor
if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,--no-as-needed")
endif()
else()
set(GTSAM_USE_EIGEN_MKL 0)
set(EIGEN_USE_MKL_ALL 0)
@ -192,37 +216,41 @@ endif()
###############################################################################
# Option for using system Eigen or GTSAM-bundled Eigen
### Disabled until our patches are included in Eigen ###
### These patches only affect usage of MKL. If you want to enable MKL, you *must*
### use our patched version of Eigen
### See: http://eigen.tuxfamily.org/bz/show_bug.cgi?id=704 (Householder QR MKL selection)
### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=705 (Fix MKL LLT return code)
### http://eigen.tuxfamily.org/bz/show_bug.cgi?id=716 (Improved comma initialization)
# option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
set(GTSAM_USE_SYSTEM_EIGEN OFF)
option(GTSAM_USE_SYSTEM_EIGEN "Find and use system-installed Eigen. If 'off', use the one bundled with GTSAM" OFF)
# Switch for using system Eigen or GTSAM-bundled Eigen
if(GTSAM_USE_SYSTEM_EIGEN)
# Use generic Eigen include paths e.g. <Eigen/Core>
set(GTSAM_EIGEN_INCLUDE_PREFIX "")
find_package(Eigen3 REQUIRED)
include_directories(AFTER "${EIGEN3_INCLUDE_DIR}")
# Use generic Eigen include paths e.g. <Eigen/Core>
set(GTSAM_EIGEN_INCLUDE_PREFIX "${EIGEN3_INCLUDE_DIR}")
# check if MKL is also enabled - can have one or the other, but not both!
# Note: Eigen >= v3.2.5 includes our patches
if(EIGEN_USE_MKL_ALL AND (EIGEN3_VERSION VERSION_LESS 3.2.5))
message(FATAL_ERROR "MKL requires at least Eigen 3.2.5, and your system appears to have an older version. Disable GTSAM_USE_SYSTEM_EIGEN to use GTSAM's copy of Eigen, or disable GTSAM_WITH_EIGEN_MKL")
endif()
else()
# Use bundled Eigen include paths e.g. <gtsam/3rdparty/Eigen/Eigen/Core>
set(GTSAM_EIGEN_INCLUDE_PREFIX "gtsam/3rdparty/Eigen/")
# Use bundled Eigen include path.
# Clear any variables set by FindEigen3
if(EIGEN3_INCLUDE_DIR)
set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
endif()
# Add the bundled version of eigen to the include path so that it can still be included
# with #include <Eigen/Core>
include_directories(BEFORE "gtsam/3rdparty/Eigen/")
# set full path to be used by external projects
# this will be added to GTSAM_INCLUDE_DIR by gtsam_extra.cmake.in
set(GTSAM_EIGEN_INCLUDE_PREFIX "${CMAKE_INSTALL_PREFIX}/include/gtsam/3rdparty/Eigen/")
endif()
# Write Eigen include file with the paths for either the system Eigen or the GTSAM-bundled Eigen
configure_file(gtsam/3rdparty/gtsam_eigen_includes.h.in gtsam/3rdparty/gtsam_eigen_includes.h)
# Install the configuration file for Eigen
install(FILES ${PROJECT_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTINATION include/gtsam/3rdparty)
###############################################################################
# Global compile options
@ -263,21 +291,31 @@ endif()
# Include boost - use 'BEFORE' so that a specific boost specified to CMake
# takes precedence over a system-installed one.
include_directories(BEFORE ${Boost_INCLUDE_DIR})
include_directories(BEFORE SYSTEM ${Boost_INCLUDE_DIR})
if(GTSAM_SUPPORT_NESTED_DISSECTION)
set(METIS_INCLUDE_DIRECTORIES
gtsam/3rdparty/metis/include
gtsam/3rdparty/metis/libmetis
gtsam/3rdparty/metis/GKlib)
else()
set(METIS_INCLUDE_DIRECTORIES)
endif()
# Add includes for source directories 'BEFORE' boost and any system include
# paths so that the compiler uses GTSAM headers in our source directory instead
# of any previously installed GTSAM headers.
include_directories(BEFORE
gtsam/3rdparty/UFconfig
gtsam/3rdparty/SuiteSparse_config
gtsam/3rdparty/CCOLAMD/Include
${METIS_INCLUDE_DIRECTORIES}
${PROJECT_SOURCE_DIR}
${PROJECT_BINARY_DIR} # So we can include generated config header files
CppUnitLite)
if(MSVC)
add_definitions(-D_CRT_SECURE_NO_WARNINGS -D_SCL_SECURE_NO_WARNINGS)
add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344) # Disable non-DLL-exported base class and other warnings
add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344 /wd4503) # Disable non-DLL-exported base class and other warnings
endif()
# GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
@ -287,6 +325,13 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
endif()
endif()
# As of XCode 7, clang also complains about this
if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang")
if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 7.0)
add_definitions(-Wno-unused-local-typedefs)
endif()
endif()
if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS)
endif()
@ -319,6 +364,20 @@ if (GTSAM_INSTALL_MATLAB_TOOLBOX)
add_subdirectory(matlab)
endif()
# Python wrap
if (GTSAM_BUILD_PYTHON)
include(GtsamPythonWrap)
# NOTE: The automatic generation of python wrapper from the gtsampy.h interface is
# not working yet, so we're using a handwritten wrapper files on python/handwritten.
# Once the python wrapping from the interface file is working, you can _swap_ the
# comments on the next lines
# wrap_and_install_python(gtsampy.h "${GTSAM_ADDITIONAL_LIBRARIES}" "")
add_subdirectory(python)
endif()
# Build gtsam_unstable
if (GTSAM_BUILD_UNSTABLE)
add_subdirectory(gtsam_unstable)
@ -328,6 +387,7 @@ endif(GTSAM_BUILD_UNSTABLE)
GtsamMakeConfigFile(GTSAM "${CMAKE_CURRENT_SOURCE_DIR}/gtsam_extra.cmake.in")
export(TARGETS ${GTSAM_EXPORTED_TARGETS} FILE GTSAM-exports.cmake)
# Check for doxygen availability - optional dependency
find_package(Doxygen)
@ -368,6 +428,8 @@ set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>=
# Print configuration variables
message(STATUS "===============================================================")
message(STATUS "================ Configuration Options ======================")
message(STATUS " CMAKE_CXX_COMPILER_ID type : ${CMAKE_CXX_COMPILER_ID}")
message(STATUS " CMAKE_CXX_COMPILER_VERSION : ${CMAKE_CXX_COMPILER_VERSION}")
message(STATUS "Build flags ")
print_config_flag(${GTSAM_BUILD_TESTS} "Build Tests ")
print_config_flag(${GTSAM_BUILD_EXAMPLES_ALWAYS} "Build examples with 'make all' ")
@ -386,6 +448,11 @@ if(NOT MSVC AND NOT XCODE_VERSION)
message(STATUS " C compilation flags : ${CMAKE_C_FLAGS} ${CMAKE_C_FLAGS_${cmake_build_type_toupper}}")
message(STATUS " C++ compilation flags : ${CMAKE_CXX_FLAGS} ${CMAKE_CXX_FLAGS_${cmake_build_type_toupper}}")
endif()
if(GTSAM_USE_SYSTEM_EIGEN)
message(STATUS " Use System Eigen : Yes")
else()
message(STATUS " Use System Eigen : No")
endif()
if(GTSAM_USE_TBB)
message(STATUS " Use Intel TBB : Yes")
elseif(TBB_FOUND)
@ -419,13 +486,29 @@ message(STATUS " CPack Source Generator : ${CPACK_SOURCE_GENERATOR}")
message(STATUS " CPack Generator : ${CPACK_GENERATOR}")
message(STATUS "GTSAM flags ")
print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_config_flag(${GTSAM_USE_QUATERNIONS} "Quaternions as default Rot3 ")
print_config_flag(${GTSAM_ENABLE_CONSISTENCY_CHECKS} "Runtime consistency checking ")
print_config_flag(${GTSAM_ROT3_EXPMAP} "Rot3 retract is full ExpMap ")
print_config_flag(${GTSAM_POSE3_EXPMAP} "Pose3 retract is full ExpMap ")
print_config_flag(${GTSAM_ALLOW_DEPRECATED_SINCE_V4} "Deprecated in GTSAM 4 allowed ")
print_config_flag(${GTSAM_TYPEDEF_POINTS_TO_VECTORS} "Point3 is typedef to Vector3 ")
print_config_flag(${GTSAM_SUPPORT_NESTED_DISSECTION} "Metis-based Nested Dissection ")
print_config_flag(${GTSAM_TANGENT_PREINTEGRATION} "Use tangent-space preintegration")
message(STATUS "MATLAB toolbox flags ")
print_config_flag(${GTSAM_INSTALL_MATLAB_TOOLBOX} "Install matlab toolbox ")
print_config_flag(${GTSAM_BUILD_WRAP} "Build Wrap ")
message(STATUS "Python module flags ")
if(GTSAM_PYTHON_WARNINGS)
message(STATUS " Build python module : No - dependencies missing")
else()
print_config_flag(${GTSAM_BUILD_PYTHON} "Build python module ")
endif()
if(GTSAM_BUILD_PYTHON)
message(STATUS " Python version : ${GTSAM_PYTHON_VERSION}")
endif()
message(STATUS "===============================================================")
# Print warnings at the end
@ -438,6 +521,9 @@ endif()
if(GTSAM_WITH_EIGEN_MKL_OPENMP AND NOT OPENMP_FOUND AND MKL_FOUND)
message(WARNING "Your compiler does not support OpenMP - this is ok, but performance may be improved with OpenMP. Set GTSAM_WITH_EIGEN_MKL_OPENMP to 'Off' to avoid this warning.")
endif()
if(GTSAM_BUILD_PYTHON AND GTSAM_PYTHON_WARNINGS)
message(WARNING "${GTSAM_PYTHON_WARNINGS}")
endif()
# Include CPack *after* all flags
include(CPack)

12
CppUnitLite/Test.h
View File

@ -68,6 +68,12 @@ protected:
testGroup##testName##Instance; \
void testGroup##testName##Test::run (TestResult& result_)
/**
* Declare friend in a class to test its private methods
*/
#define FRIEND_TEST(testGroup, testName) \
friend class testGroup##testName##Test;
/**
* For debugging only: use TEST_UNSAFE to allow debuggers to have access to exceptions, as this
* will not wrap execution with a try/catch block
@ -80,6 +86,12 @@ protected:
testGroup##testName##Instance; \
void testGroup##testName##Test::run (TestResult& result_)
/**
* Use this to disable unwanted tests without commenting them out.
*/
#define TEST_DISABLED(testGroup, testName)\
void testGroup##testName##Test(TestResult& result_, const std::string& name_)
/*
* Convention for tests:
* - "EXPECT" is a test that will not end execution of the series of tests

2
CppUnitLite/TestResult.h
View File

@ -18,8 +18,6 @@
//
///////////////////////////////////////////////////////////////////////////////
#ifndef TESTRESULT_H
#define TESTRESULT_H

206
GTSAM-Concepts.md Normal file
View File

@ -0,0 +1,206 @@
GTSAM Concepts
==============
As discussed in [Generic Programming Techniques](http://www.boost.org/community/generic_programming.html), concepts define
* associated types
* valid expressions, like functions and values
* invariants
* complexity guarantees
Below we discuss the most important concepts use in GTSAM, and after that we discuss how they are implemented/used/enforced.
Manifold
--------
To optimize over continuous types, we assume they are manifolds. This is central to GTSAM and hence discussed in some more detail below.
[Manifolds](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) and [charts](http://en.wikipedia.org/wiki/Manifold#Charts.2C_atlases.2C_and_transition_maps) are intimately linked concepts. We are only interested here in [differentiable manifolds](http://en.wikipedia.org/wiki/Differentiable_manifold#Definition), continuous spaces that can be locally approximated *at any point* using a local vector space, called the [tangent space](http://en.wikipedia.org/wiki/Tangent_space). A *chart* is an invertible map from the manifold to that tangent space.
In GTSAM, all properties and operations needed to use a type must be defined through template specialization of the struct `gtsam::traits`. Concept checks are used to check that all required functions are implemented.
In detail, we ask that the following items are defined in the traits object (although, not all are needed for optimization):
* values:
* `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimensionality is only defined at runtime, by specifying the value -1.
* types:
* `TangentVector`, type that lives in tangent space. This will almost always be an `Eigen::Matrix<double,n,1>`.
* `ChartJacobian`, a typedef for `OptionalJacobian<dimension, dimension>`.
* `ManifoldType`, a pointer back to the type.
* `structure_category`, a tag type that defines what requirements the type fulfills, and therefore what requirements this traits class must fulfill. It should be defined to be one of the following:
* `gtsam::traits::manifold_tag` -- Everything in this list is expected
* `gtsam::traits::group_tag` -- The functions defined under **Groups** below.
* `gtsam::traits::lie_group_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below.
* `gtsam::traits::vector_space_tag` -- Everything in this list is expected, plus the functions defined under **Groups**, and **Lie Groups** below.
* valid expressions:
* `size_t dim = traits<T>::GetDimension(p);` static function should be defined. This is mostly useful if the size is not known at compile time.
* `v = traits<T>::Local(p,q)`, the chart, from manifold to tangent space, think of it as *q (-) p*, where *p* and *q* are elements of the manifold and the result, *v* is an element of the vector space.
* `p = traits<T>::Retract(p,v)`, the inverse chart, from tangent space to manifold, think of it as *p (+) v*, where *p* is an element of the manifold and the result, *v* is an element of the vector space.
* invariants
* `Retract(p, Local(p,q)) == q`
* `Local(p, Retract(p, v)) == v`
Group
-----
A [group]("http://en.wikipedia.org/wiki/Group_(mathematics)"") should be well known from grade school :-), and provides a type with a composition operation that is closed, associative, has an identity element, and an inverse for each element. The following should be added to the traits class for a group:
* valid expressions:
* `r = traits<T>::Compose(p,q)`, where *p*, *q*, and *r* are elements of the manifold.
* `q = traits<T>::Inverse(p)`, where *p* and*q* are elements of the manifold.
* `r = traits<T>::Between(p,q)`, where *p*, *q*, and *r* are elements of the manifold.
* static members:
* `traits<T>::Identity`, a static const member that represents the group's identity element.
* invariants:
* `Compose(p,Inverse(p)) == Identity`
* `Compose(p,Between(p,q)) == q`
* `Between(p,q) == Compose(Inverse(p),q)`
The `gtsam::group::traits` namespace defines the following:
* values:
* `traits<T>::Identity` -- The identity element for this group stored as a static const.
* `traits<T>::group_flavor` -- the flavor of this group's `compose()` operator, either:
* `gtsam::traits::group_multiplicative_tag` for multiplicative operator syntax ,or
* `gtsam::traits::group_additive_tag` for additive operator syntax.
We do *not* at this time support more than one composition operator per type. Although mathematically possible, it is hardly ever needed, and the machinery to support it would be burdensome and counter-intuitive.
Also, a type should provide either multiplication or addition operators depending on the flavor of the operation. To distinguish between the two, we will use a tag (see below).
Lie Group
---------
A Lie group is both a manifold *and* a group. Hence, a LIE_GROUP type should implements both MANIFOLD and GROUP concepts.
However, we now also need to be able to evaluate the derivatives of compose and inverse.
Hence, we have the following extra valid static functions defined in the struct `gtsam::traits<T>`:
* `r = traits<T>::Compose(p,q,Hq,Hp)`
* `q = traits<T>::Inverse(p,Hp)`
* `r = traits<T>::Between(p,q,Hq,H2p)`
where above the *H* arguments stand for optional Jacobian arguments.
That makes it possible to create factors implementing priors (PriorFactor) or relations between two instances of a Lie group type (BetweenFactor).
In addition, a Lie group has a Lie algebra, which affords two extra valid expressions:
* `v = traits<T>::Logmap(p,Hp)`, the log map, with optional Jacobian
* `p = traits<T>::Expmap(v,Hv)`, the exponential map, with optional Jacobian
Note that in the Lie group case, the usual valid expressions for Retract and Local can be generated automatically, e.g.
```
T Retract(p,v,Hp,Hv) {
T q = Expmap(v,Hqv);
T r = Compose(p,q,Hrp,Hrq);
Hv = Hrq * Hqv; // chain rule
return r;
}
```
For Lie groups, the `exponential map` above is the most obvious mapping: it
associates straight lines in the tangent space with geodesics on the manifold
(and it's inverse, the log map). However, there are two cases in which we deviate from this:
However, the exponential map is unnecessarily expensive for use in optimization. Hence, in GTSAM there is the option to provide a cheaper chart by means of the `ChartAtOrigin` struct in a class. This is done for *SE(2)*, *SO(3)* and *SE(3)* (see `Pose2`, `Rot3`, `Pose3`)
Most Lie groups we care about are *Matrix groups*, continuous sub-groups of *GL(n)*, the group of *n x n* invertible matrices. In this case, a lot of the derivatives calculations needed can be standardized, and this is done by the `LieGroup` superclass. You only need to provide an `AdjointMap` method.
Vector Space
------------
While vector spaces are in principle also manifolds, it is overkill to think about charts etc. Really, we should simply think about vector addition and subtraction. I.e.where
* `Identity == 0`
* `Inverse(p) == -p`
* `Compose(p,q) == p+q`
* `Between(p,q) == q-p`
* `Local(q) == p-q`
* `Retract(v) == p+v`
This considerably simplifies certain operations. A `VectorSpace` superclass is available to implement the traits. Types that are vector space models include `Matrix`, `Vector`, any fixed or dynamic Eigen Matrix, `Point2`, and `Point3`.
Testable
--------
Unit tests heavily depend on the following two functions being defined for all types that need to be tested:
* valid expressions:
* `Print(p,s)` where s is an optional string
* `Equals(p,q,tol)` where tol is an optional (double) tolerance
Implementation
==============
GTSAM Types start with Uppercase, e.g., `gtsam::Point2`, and are models of the
TESTABLE, MANIFOLD, GROUP, LIE_GROUP, and VECTOR_SPACE concepts.
`gtsam::traits` is our way to associate these concepts with types,
and we also define a limited number of `gtsam::tags` to select the correct implementation
of certain functions at compile time (tag dispatching).
Traits
------
However, a base class is not a good way to implement/check the other concepts, as we would like these
to apply equally well to types that are outside GTSAM control, e.g., `Eigen::VectorXd`. This is where
[traits](http://www.boost.org/doc/libs/1_57_0/libs/type_traits/doc/html/boost_typetraits/background.html) come in.
We use Eigen-style or STL-style traits, that define *many* properties at once.
Note that not everything that makes a concept is defined by traits. Valid expressions such as traits<T>::Compose are
defined simply as static functions within the traits class.
Finally, for GTSAM types, it is perfectly acceptable (and even desired) to define associated types as internal types,
rather than having to use traits internally.
Concept Checks
--------------
Boost provides a nice way to check whether a given type satisfies a concept. For example, the following
BOOST_CONCEPT_ASSERT(IsVectorSpace<Point2>)
asserts that Point2 indeed is a model for the VectorSpace concept.
Future Concepts
===============
Group Action
------------
Group actions are concepts in and of themselves that can be concept checked (see below).
In particular, a group can *act* on another space.
For example, the [cyclic group of order 6](http://en.wikipedia.org/wiki/Cyclic_group) can rotate 2D vectors around the origin:
q = R(i)*p
where R(i) = R(60)^i, where R(60) rotates by 60 degrees
Hence, we formalize by the following extension of the concept:
* valid expressions:
* `q = traits<T>::Act(g,p)`, for some instance, *p*, of a space *S*, that can be acted upon by the group element *g* to produce *q* in *S*.
* `q = traits<T>::Act(g,p,Hp)`, if the space acted upon is a continuous differentiable manifold. *
In the latter case, if *S* is an n-dimensional manifold, *Hp* is an output argument that should be
filled with the *nxn* Jacobian matrix of the action with respect to a change in *p*. It typically depends
on the group element *g*, but in most common example will *not* depend on the value of *p*. For example, in
the cyclic group example above, we simply have
Hp = R(i)
Note there is no derivative of the action with respect to a change in g. That will only be defined
for Lie groups, which we introduce now.
Lie Group Action
----------------
When a Lie group acts on a space, we have two derivatives to care about:
* `gtasm::manifold::traits<T>::act(g,p,Hg,Hp)`, if the space acted upon is a continuous differentiable manifold.
An example is a *similarity transform* in 3D, which can act on 3D space, like
q = s*R*p + t
Note that again the derivative in *p*, *Hp* is simply *s R*, which depends on *g* but not on *p*.
The derivative in *g*, *Hg*, is in general more complex.
For now, we won't care about Lie groups acting on non-manifolds.

37
README.md
View File

@ -1,6 +1,5 @@
README - Georgia Tech Smoothing and Mapping library
===================================================
Version: Pre-Release 3.2.0
What is GTSAM?
--------------
@ -32,17 +31,47 @@ Prerequisites:
- [Boost](http://www.boost.org/users/download/) >= 1.43 (Ubuntu: `sudo apt-get install libboost-all-dev`)
- [CMake](http://www.cmake.org/cmake/resources/software.html) >= 2.6 (Ubuntu: `sudo apt-get install cmake`)
- A modern compiler, i.e., at least gcc 4.7.3 on Linux.
Optional prerequisites - used automatically if findable by CMake:
- [Intel Threaded Building Blocks (TBB)](http://www.threadingbuildingblocks.org/) (Ubuntu: `sudo apt-get install libtbb-dev`)
- [Intel Math Kernel Library (MKL)](http://software.intel.com/en-us/intel-mkl)
GTSAM 4 Compatibility
---------------------
GTSAM 4 will introduce several new features, most notably Expressions and a python toolbox. We will also deprecate some legacy functionality and wrongly named methods, but by default the flag GTSAM_ALLOW_DEPRECATED_SINCE_V4 is enabled, allowing anyone to just pull V4 and compile. To build the python toolbox, however, you will have to explicitly disable that flag.
Also, GTSAM 4 introduces traits, a C++ technique that allows optimizing with non-GTSAM types. That opens the door to retiring geometric types such as Point2 and Point3 to pure Eigen types, which we will also do. A significant change which will not trigger a compile error is that zero-initializing of Point2 and Point3 will be deprecated, so please be aware that this might render functions using their default constructor incorrect.
The Preintegrated IMU Factor
----------------------------
GTSAM includes a state of the art IMU handling scheme based on
- Todd Lupton and Salah Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.
Our implementation improves on this using integration on the manifold, as detailed in
- Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, "Eliminating conditionally independent sets in factor graphs: a unifying perspective based on smart factors", Int. Conf. on Robotics and Automation (ICRA), 2014.
- Christian Forster, Luca Carlone, Frank Dellaert, and Davide Scaramuzza, "IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-Posteriori Estimation", Robotics: Science and Systems (RSS), 2015.
If you are using the factor in academic work, please cite the publications above.
In GTSAM 4 a new and more efficient implementation, based on integrating on the NavState tangent space and detailed in docs/ImuFactor.pdf, is enabled by default. To switch to the RSS 2015 version, set the flag **GTSAM_TANGENT_PREINTEGRATION** to OFF.
Additional Information
----------------------
See the [`INSTALL`](https://bitbucket.org/gtborg/gtsam/src/develop/INSTALL) file for more detailed installation instructions.
Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here.
GTSAM is open source under the BSD license, see the [`LICENSE`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE) and [`LICENSE.BSD`](https://bitbucket.org/gtborg/gtsam/src/develop/LICENSE.BSD) files.
See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
Please see the [`examples/`](https://bitbucket.org/gtborg/gtsam/src/develop/examples) directory and the [`USAGE`](https://bitbucket.org/gtborg/gtsam/src/develop/USAGE) file for examples on how to use GTSAM.
GTSAM is open source under the BSD license, see the [`LICENSE`](LICENSE) and [`LICENSE.BSD`](LICENSE.BSD) files.
Please see the [`examples/`](examples) directory and the [`USAGE`](USAGE.md) file for examples on how to use GTSAM.
GTSAM was developed in the lab of [Frank Dellaert](http://www.cc.gatech.edu/~dellaert) at the [Georgia Institute of Technology](http://www.gatech.edu), with the help of many contributors over the years, see [THANKS](THANKS).

59
THANKS
View File

@ -1,20 +1,47 @@
GTSAM was made possible by the efforts of many collaborators at Georgia Tech
GTSAM was made possible by the efforts of many collaborators at Georgia Tech, listed below with their current afffiliation, if they left Tech:
Doru Balcan
Chris Beall
Alex Cunningham
Alireza Fathi
Eohan George
Viorela Ila
Yong-Dian Jian
Michael Kaess
Kai Ni
Carlos Nieto
Duy-Nguyen
Manohar Paluri
Christian Potthast
Richard Roberts
Grant Schindler
* Sungtae An
* Doru Balcan, Bank of America
* Chris Beall
* Luca Carlone
* Alex Cunningham, U Michigan
* Jing Dong
* Alireza Fathi, Stanford
* Eohan George
* Alex Hagiopol
* Viorela Ila, Czeck Republic
* Vadim Indelman, the Technion
* David Jensen, GTRI
* Yong-Dian Jian, Baidu
* Michael Kaess, Carnegie Mellon
* Zhaoyang Lv
* Andrew Melim, Oculus Rift
* Kai Ni, Baidu
* Carlos Nieto
* Duy-Nguyen Ta
* Manohar Paluri, Facebook
* Christian Potthast, USC
* Richard Roberts, Google X
* Grant Schindler, Consultant
* Natesh Srinivasan
* Alex Trevor
* Stephen Williams, BossaNova
at ETH, Zurich
* Paul Furgale
* Mike Bosse
* Hannes Sommer
* Thomas Schneider
at Uni Zurich:
* Christian Forster
at LAAS-CNRS
* Ellon Paiva
Many thanks for your hard work!!!!
Frank Dellaert

41
USAGE → USAGE.md
View File

@ -1,6 +1,5 @@
USAGE - Georgia Tech Smoothing and Mapping library
---------------------------------------------------
===================================
What is this file?
This file explains how to make use of the library for common SLAM tasks,
@ -34,18 +33,12 @@ The GTSAM library has three primary components necessary for the construction
of factor graph representation and optimization which users will need to
adapt to their particular problem.
FactorGraph:
A factor graph contains a set of variables to solve for (i.e., robot poses,
landmark poses, etc.) and a set of constraints between these variables, which
make up factors.
Values:
Values is a single object containing labeled values for all of the
variables. Currently, all variables are labeled with strings, but the type
or organization of the variables can change
Factors:
A nonlinear factor expresses a constraint between variables, which in the
SLAM example, is a measurement such as a visual reading on a landmark or
odometry.
* FactorGraph:
A factor graph contains a set of variables to solve for (i.e., robot poses, landmark poses, etc.) and a set of constraints between these variables, which make up factors.
* Values:
Values is a single object containing labeled values for all of the variables. Currently, all variables are labeled with strings, but the type or organization of the variables can change
* Factors:
A nonlinear factor expresses a constraint between variables, which in the SLAM example, is a measurement such as a visual reading on a landmark or odometry.
The library is organized according to the following directory structure:
@ -59,23 +52,3 @@ The library is organized according to the following directory structure:
VSLAM Example
---------------------------------------------------
The visual slam example shows a full implementation of a slam system. The example contains
derived versions of NonlinearFactor, NonlinearFactorGraph, in classes visualSLAM::ProjectionFactor,
visualSLAM::Graph, respectively. The values for the system are stored in the generic
Values structure. For definitions and interface, see gtsam/slam/visualSLAM.h.
The clearest example of the use of the graph to find a solution is in
testVSLAM. The basic process for using graphs is as follows (and can be seen in
the test):
- Create a NonlinearFactorGraph object (visualSLAM::Graph)
- Add factors to the graph (note the use of Boost.shared_ptr here) (visualSLAM::ProjectionFactor)
- Create an initial configuration (Values)
- Create an elimination ordering of variables (this must include all variables)
- Create and initialize a NonlinearOptimizer object (Note that this is a generic
algorithm that does not need to be derived for a particular problem)
- Call optimization functions with the optimizer to optimize the graph
- Extract an updated values from the optimizer

1
cmake/CMakeLists.txt
View File

@ -19,7 +19,6 @@ install(FILES
GtsamMatlabWrap.cmake
GtsamPythonWrap.cmake
GtsamTesting.cmake
GtsamTestingObsolete.cmake
README.html
DESTINATION "${SCRIPT_INSTALL_DIR}/GTSAMCMakeTools")

102
cmake/FindNumPy.cmake Normal file
View File

@ -0,0 +1,102 @@
# - Find the NumPy libraries
# This module finds if NumPy is installed, and sets the following variables
# indicating where it is.
#
# TODO: Update to provide the libraries and paths for linking npymath lib.
#
# NUMPY_FOUND - was NumPy found
# NUMPY_VERSION - the version of NumPy found as a string
# NUMPY_VERSION_MAJOR - the major version number of NumPy
# NUMPY_VERSION_MINOR - the minor version number of NumPy
# NUMPY_VERSION_PATCH - the patch version number of NumPy
# NUMPY_VERSION_DECIMAL - e.g. version 1.6.1 is 10601
# NUMPY_INCLUDE_DIRS - path to the NumPy include files
#============================================================================
# Copyright 2012 Continuum Analytics, Inc.
#
# MIT License
#
# Permission is hereby granted, free of charge, to any person obtaining
# a copy of this software and associated documentation files
# (the "Software"), to deal in the Software without restriction, including
# without limitation the rights to use, copy, modify, merge, publish,
# distribute, sublicense, and/or sell copies of the Software, and to permit
# persons to whom the Software is furnished to do so, subject to
# the following conditions:
#
# The above copyright notice and this permission notice shall be included
# in all copies or substantial portions of the Software.
#
# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
# OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
# THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
# OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
# ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
# OTHER DEALINGS IN THE SOFTWARE.
#
#============================================================================
# Finding NumPy involves calling the Python interpreter
if(NumPy_FIND_REQUIRED)
find_package(PythonInterp REQUIRED)
else()
find_package(PythonInterp)
endif()
if(NOT PYTHONINTERP_FOUND)
set(NUMPY_FOUND FALSE)
return()
endif()
execute_process(COMMAND "${PYTHON_EXECUTABLE}" "-c"
"import numpy as n; print(n.__version__); print(n.get_include());"
RESULT_VARIABLE _NUMPY_SEARCH_SUCCESS
OUTPUT_VARIABLE _NUMPY_VALUES_OUTPUT
ERROR_VARIABLE _NUMPY_ERROR_VALUE
OUTPUT_STRIP_TRAILING_WHITESPACE)
if(NOT _NUMPY_SEARCH_SUCCESS MATCHES 0)
if(NumPy_FIND_REQUIRED)
message(FATAL_ERROR
"NumPy import failure:\n${_NUMPY_ERROR_VALUE}")
endif()
set(NUMPY_FOUND FALSE)
return()
endif()
# Convert the process output into a list
string(REGEX REPLACE ";" "\\\\;" _NUMPY_VALUES ${_NUMPY_VALUES_OUTPUT})
string(REGEX REPLACE "\n" ";" _NUMPY_VALUES ${_NUMPY_VALUES})
# Just in case there is unexpected output from the Python command.
list(GET _NUMPY_VALUES -2 NUMPY_VERSION)
list(GET _NUMPY_VALUES -1 NUMPY_INCLUDE_DIRS)
string(REGEX MATCH "^[0-9]+\\.[0-9]+\\.[0-9]+" _VER_CHECK "${NUMPY_VERSION}")
if("${_VER_CHECK}" STREQUAL "")
# The output from Python was unexpected. Raise an error always
# here, because we found NumPy, but it appears to be corrupted somehow.
message(FATAL_ERROR
"Requested version and include path from NumPy, got instead:\n${_NUMPY_VALUES_OUTPUT}\n")
return()
endif()
# Make sure all directory separators are '/'
string(REGEX REPLACE "\\\\" "/" NUMPY_INCLUDE_DIRS ${NUMPY_INCLUDE_DIRS})
# Get the major and minor version numbers
string(REGEX REPLACE "\\." ";" _NUMPY_VERSION_LIST ${NUMPY_VERSION})
list(GET _NUMPY_VERSION_LIST 0 NUMPY_VERSION_MAJOR)
list(GET _NUMPY_VERSION_LIST 1 NUMPY_VERSION_MINOR)
list(GET _NUMPY_VERSION_LIST 2 NUMPY_VERSION_PATCH)
string(REGEX MATCH "[0-9]*" NUMPY_VERSION_PATCH ${NUMPY_VERSION_PATCH})
math(EXPR NUMPY_VERSION_DECIMAL
"(${NUMPY_VERSION_MAJOR} * 10000) + (${NUMPY_VERSION_MINOR} * 100) + ${NUMPY_VERSION_PATCH}")
find_package_message(NUMPY
"Found NumPy: version \"${NUMPY_VERSION}\" ${NUMPY_INCLUDE_DIRS}"
"${NUMPY_INCLUDE_DIRS}${NUMPY_VERSION}")
set(NUMPY_FOUND TRUE)

158
cmake/GtsamBuildTypes.cmake
View File

@ -3,69 +3,99 @@
list(APPEND CMAKE_PREFIX_PATH "${CMAKE_INSTALL_PREFIX}")
# Default to Release mode
if(NOT FIRST_PASS_DONE AND NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION)
set(CMAKE_BUILD_TYPE "Release" CACHE STRING
"Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo."
FORCE)
if(NOT CMAKE_BUILD_TYPE AND NOT MSVC AND NOT XCODE_VERSION)
set(GTSAM_CMAKE_BUILD_TYPE "Release" CACHE STRING
"Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo.")
set(CMAKE_BUILD_TYPE ${GTSAM_CMAKE_BUILD_TYPE} CACHE STRING
"Choose the type of build, options are: None Debug Release Timing Profiling RelWithDebInfo." FORCE)
endif()
# Add option for using build type postfixes to allow installing multiple build modes
option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build type to the name of compiled libraries" ON)
# Add debugging flags but only on the first pass
if(NOT FIRST_PASS_DONE)
if(MSVC)
set(CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /d2Zi+ /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
set(CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE)
set(CMAKE_CXX_FLAGS_RELEASE "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during release builds." FORCE)
set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE)
set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE)
set(CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
set(CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
set(CMAKE_MODULE_LINKER_FLAGS_TIMING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
mark_as_advanced(CMAKE_C_FLAGS_TIMING CMAKE_CXX_FLAGS_TIMING CMAKE_EXE_LINKER_FLAGS_TIMING CMAKE_SHARED_LINKER_FLAGS_TIMING CMAKE_MODULE_LINKER_FLAGS_TIMING)
set(CMAKE_C_FLAGS_PROFILING "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during profiling builds." FORCE)
set(CMAKE_CXX_FLAGS_PROFILING "/MD /O2 /DNDEBUG /W3 /GR /EHsc /MP /Zi /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler during profiling builds." FORCE)
set(CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
set(CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING CMAKE_MODULE_LINKER_FLAGS_PROFILING)
else()
set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_C_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during relwithdebinfo builds." FORCE)
set(CMAKE_C_FLAGS_RELEASE "-O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE)
set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -DNDEBUG -Wall" CACHE STRING "Flags used by the compiler during release builds." FORCE)
set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE)
set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Flags used by the compiler during timing builds." FORCE)
set(CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
set(CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds." FORCE)
mark_as_advanced(CMAKE_C_FLAGS_TIMING CMAKE_CXX_FLAGS_TIMING CMAKE_EXE_LINKER_FLAGS_TIMING CMAKE_SHARED_LINKER_FLAGS_TIMING)
set(CMAKE_C_FLAGS_PROFILING "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE)
set(CMAKE_CXX_FLAGS_PROFILING "-g -O3 -Wall -DNDEBUG" CACHE STRING "Flags used by the compiler during profiling builds." FORCE)
set(CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
set(CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE__LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds." FORCE)
mark_as_advanced(CMAKE_C_FLAGS_PROFILING CMAKE_CXX_FLAGS_PROFILING CMAKE_EXE_LINKER_FLAGS_PROFILING CMAKE_SHARED_LINKER_FLAGS_PROFILING)
endif()
# Set custom compilation flags.
# NOTE: We set all the CACHE variables with a GTSAM prefix, and then set a normal local variable below
# so that we don't "pollute" the global variable namespace in the cmake cache.
# Set all CMAKE_BUILD_TYPE flags:
# (see https://cmake.org/Wiki/CMake_Useful_Variables#Compilers_and_Tools)
if(MSVC)
set(GTSAM_CMAKE_C_FLAGS "/W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler for all builds.")
set(GTSAM_CMAKE_CXX_FLAGS "/W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN" CACHE STRING "Flags used by the compiler for all builds.")
set(GTSAM_CMAKE_C_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /Zi /d2Zi+" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "/MD /O2 /DNDEBUG /Zi /d2Zi+" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
set(GTSAM_CMAKE_C_FLAGS_RELEASE "/MD /O2 /DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
set(GTSAM_CMAKE_CXX_FLAGS_RELEASE "/MD /O2 /DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /Zi" CACHE STRING "Extra flags used by the compiler during profiling builds.")
set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /Zi" CACHE STRING "Extra flags used by the compiler during profiling builds.")
set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} /DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
else()
set(GTSAM_CMAKE_C_FLAGS "-std=c11 -Wall" CACHE STRING "Flags used by the compiler for all builds.")
set(GTSAM_CMAKE_CXX_FLAGS "-std=c++11 -Wall" CACHE STRING "Flags used by the compiler for all builds.")
set(GTSAM_CMAKE_C_FLAGS_DEBUG "-g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
set(GTSAM_CMAKE_CXX_FLAGS_DEBUG "-g -fno-inline -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Extra flags used by the compiler during debug builds.")
set(GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO "-g -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
set(GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO "-g -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during relwithdebinfo builds.")
set(GTSAM_CMAKE_C_FLAGS_RELEASE " -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
set(GTSAM_CMAKE_CXX_FLAGS_RELEASE " -O3 -DNDEBUG" CACHE STRING "Extra flags used by the compiler during release builds.")
set(GTSAM_CMAKE_C_FLAGS_PROFILING "${GTSAM_CMAKE_C_FLAGS_RELEASE}" CACHE STRING "Extra flags used by the compiler during profiling builds.")
set(GTSAM_CMAKE_CXX_FLAGS_PROFILING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE}" CACHE STRING "Extra flags used by the compiler during profiling builds.")
set(GTSAM_CMAKE_C_FLAGS_TIMING "${GTSAM_CMAKE_C_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
set(GTSAM_CMAKE_CXX_FLAGS_TIMING "${GTSAM_CMAKE_CXX_FLAGS_RELEASE} -DENABLE_TIMING" CACHE STRING "Extra flags used by the compiler during timing builds.")
endif()
# Clang on Mac uses a template depth that is less than standard and is too small
if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024")
endif()
set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
set(GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during timing builds.")
set(GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING "${CMAKE_SHARED_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.")
set(GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING "${CMAKE_MODULE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.")
set(GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING "${CMAKE_EXE_LINKER_FLAGS_RELEASE}" CACHE STRING "Linker flags during profiling builds.")
mark_as_advanced(GTSAM_CMAKE_C_FLAGS_TIMING GTSAM_CMAKE_CXX_FLAGS_TIMING GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING
GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING
GTSAM_CMAKE_C_FLAGS_PROFILING GTSAM_CMAKE_CXX_FLAGS_PROFILING GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING
GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING)
# Apply the gtsam specific build flags as normal variables. This makes it so that they only
# apply to the gtsam part of the build if gtsam is built as a subproject
set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${GTSAM_CMAKE_C_FLAGS}")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GTSAM_CMAKE_CXX_FLAGS}")
set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS_DEBUG} ${GTSAM_CMAKE_C_FLAGS_DEBUG}")
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} ${GTSAM_CMAKE_CXX_FLAGS_DEBUG}")
set(CMAKE_C_FLAGS_RELWITHDEBINFO "${CMAKE_C_FLAGS_RELWITHDEBINFO} ${GTSAM_CMAKE_C_FLAGS_RELWITHDEBINFO}")
set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "${CMAKE_CXX_FLAGS_RELWITHDEBINFO} ${GTSAM_CMAKE_CXX_FLAGS_RELWITHDEBINFO}")
set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} ${GTSAM_CMAKE_C_FLAGS_RELEASE}")
set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} ${GTSAM_CMAKE_CXX_FLAGS_RELEASE}")
set(CMAKE_C_FLAGS_PROFILING "${CMAKE_C_FLAGS_PROFILING} ${GTSAM_CMAKE_C_FLAGS_PROFILING}")
set(CMAKE_CXX_FLAGS_PROFILING "${CMAKE_CXX_FLAGS_PROFILING} ${GTSAM_CMAKE_CXX_FLAGS_PROFILING}")
set(CMAKE_C_FLAGS_TIMING "${CMAKE_C_FLAGS_TIMING} ${GTSAM_CMAKE_C_FLAGS_TIMING}")
set(CMAKE_CXX_FLAGS_TIMING "${CMAKE_CXX_FLAGS_TIMING} ${GTSAM_CMAKE_CXX_FLAGS_TIMING}")
set(CMAKE_SHARED_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_TIMING})
set(CMAKE_MODULE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_TIMING})
set(CMAKE_EXE_LINKER_FLAGS_TIMING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_TIMING})
set(CMAKE_SHARED_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_SHARED_LINKER_FLAGS_PROFILING})
set(CMAKE_MODULE_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_MODULE_LINKER_FLAGS_PROFILING})
set(CMAKE_EXE_LINKER_FLAGS_PROFILING ${GTSAM_CMAKE_EXE_LINKER_FLAGS_PROFILING})
# Clang uses a template depth that is less than standard and is too small
if(${CMAKE_CXX_COMPILER_ID} STREQUAL "Clang")
# Apple Clang before 5.0 does not support -ftemplate-depth.
if(NOT (APPLE AND "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0"))
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024")
endif()
endif()
# Set up build type library postfixes
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type Debug Timing Profiling RelWithDebInfo MinSizeRel)
string(TOUPPER "${build_type}" build_type_toupper)
set(CMAKE_${build_type_toupper}_POSTFIX ${build_type})
endforeach()
foreach(build_type Debug Timing Profiling RelWithDebInfo MinSizeRel)
string(TOUPPER "${build_type}" build_type_toupper)
set(CMAKE_${build_type_toupper}_POSTFIX ${build_type})
endforeach()
endif()
# Make common binary output directory when on Windows
@ -77,17 +107,16 @@ endif()
# Set up build type list for cmake-gui
if(NOT "${CMAKE_BUILD_TYPE}" STREQUAL "")
if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_GREATER 2.8 OR ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_EQUAL 2.8)
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS None Debug Release Timing Profiling RelWithDebInfo MinSizeRel)
endif()
if(${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_GREATER 2.8 OR ${CMAKE_MAJOR_VERSION}.${CMAKE_MINOR_VERSION} VERSION_EQUAL 2.8)
set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS None Debug Release Timing Profiling RelWithDebInfo MinSizeRel)
endif()
endif()
# Set up build types for MSVC and XCode
if(NOT FIRST_PASS_DONE)
set(CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel
CACHE STRING "Build types available to MSVC and XCode" FORCE)
mark_as_advanced(FORCE CMAKE_CONFIGURATION_TYPES)
endif()
set(GTSAM_CMAKE_CONFIGURATION_TYPES Debug Release Timing Profiling RelWithDebInfo MinSizeRel
CACHE STRING "Build types available to MSVC and XCode")
mark_as_advanced(FORCE GTSAM_CMAKE_CONFIGURATION_TYPES)
set(CMAKE_CONFIGURATION_TYPES ${GTSAM_CMAKE_CONFIGURATION_TYPES})
# Check build types
string(TOLOWER "${CMAKE_BUILD_TYPE}" cmake_build_type_tolower)
@ -97,14 +126,11 @@ if( NOT cmake_build_type_tolower STREQUAL ""
AND NOT cmake_build_type_tolower STREQUAL "release"
AND NOT cmake_build_type_tolower STREQUAL "timing"
AND NOT cmake_build_type_tolower STREQUAL "profiling"
AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo")
message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo (case-insensitive).")
AND NOT cmake_build_type_tolower STREQUAL "relwithdebinfo"
AND NOT cmake_build_type_tolower STREQUAL "minsizerel")
message(FATAL_ERROR "Unknown build type \"${CMAKE_BUILD_TYPE}\". Allowed values are None, Debug, Release, Timing, Profiling, RelWithDebInfo, MinSizeRel (case-insensitive).")
endif()
# Mark that first pass is done
set(FIRST_PASS_DONE TRUE CACHE INTERNAL "Internally used to mark whether cmake has been run multiple times")
mark_as_advanced(FIRST_PASS_DONE)
# Enable Visual Studio solution folders
set_property(GLOBAL PROPERTY USE_FOLDERS On)

13
cmake/GtsamMatlabWrap.cmake
View File

@ -270,7 +270,7 @@ function(install_wrapped_library_internal interfaceHeader)
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
if("${build_type_upper}" STREQUAL "RELEASE")
if(${build_type_upper} STREQUAL "RELEASE")
set(build_type_tag "") # Don't create release mode tag on installed directory
else()
set(build_type_tag "${build_type}")
@ -367,13 +367,18 @@ endfunction()
# should be installed to all build type toolboxes
function(install_matlab_scripts source_directory patterns)
set(patterns_args "")
set(exclude_patterns "")
if(NOT GTSAM_WRAP_SERIALIZATION)
set(exclude_patterns "testSerialization.m")
endif()
foreach(pattern ${patterns})
list(APPEND patterns_args PATTERN "${pattern}")
endforeach()
if(GTSAM_BUILD_TYPE_POSTFIXES)
foreach(build_type ${CMAKE_CONFIGURATION_TYPES})
string(TOUPPER "${build_type}" build_type_upper)
if("${build_type_upper}" STREQUAL "RELEASE")
if(${build_type_upper} STREQUAL "RELEASE")
set(build_type_tag "") # Don't create release mode tag on installed directory
else()
set(build_type_tag "${build_type}")
@ -381,10 +386,10 @@ function(install_matlab_scripts source_directory patterns)
# Split up filename to strip trailing '/' in GTSAM_TOOLBOX_INSTALL_PATH if there is one
get_filename_component(location "${GTSAM_TOOLBOX_INSTALL_PATH}" PATH)
get_filename_component(name "${GTSAM_TOOLBOX_INSTALL_PATH}" NAME)
install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN ".svn" EXCLUDE)
install(DIRECTORY "${source_directory}" DESTINATION "${location}/${name}${build_type_tag}" CONFIGURATIONS "${build_type}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
endforeach()
else()
install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN ".svn" EXCLUDE)
install(DIRECTORY "${source_directory}" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING ${patterns_args} PATTERN "${exclude_patterns}" EXCLUDE)
endif()
endfunction()

77
cmake/GtsamPythonWrap.cmake
View File

@ -1,5 +1,5 @@
#Setup cache options
option(GTSAM_BUILD_PYTHON "Build Python wrapper statically (increases build time)" OFF)
set(GTSAM_PYTHON_VERSION "Default" CACHE STRING "Target python version for GTSAM python module. Use 'Default' to chose the default version")
set(GTSAM_BUILD_PYTHON_FLAGS "" CACHE STRING "Extra flags for running Matlab PYTHON compilation")
set(GTSAM_PYTHON_INSTALL_PATH "" CACHE PATH "Python toolbox destination, blank defaults to CMAKE_INSTALL_PREFIX/borg/python")
if(NOT GTSAM_PYTHON_INSTALL_PATH)
@ -8,13 +8,13 @@ endif()
#Author: Paul Furgale Modified by Andrew Melim
function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY)
# Boost
find_package(Boost COMPONENTS python filesystem system REQUIRED)
include_directories(${Boost_INCLUDE_DIRS})
# # Boost
# find_package(Boost COMPONENTS python filesystem system REQUIRED)
# include_directories(${Boost_INCLUDE_DIRS})
# Find Python
FIND_PACKAGE(PythonLibs 2.7 REQUIRED)
INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
# # Find Python
# FIND_PACKAGE(PythonLibs 2.7 REQUIRED)
# INCLUDE_DIRECTORIES(${PYTHON_INCLUDE_DIRS})
IF(APPLE)
# The apple framework headers don't include the numpy headers for some reason.
@ -36,23 +36,46 @@ function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY)
ENDIF()
ENDIF(APPLE)
if(MSVC)
add_library(${moduleName}_python MODULE ${ARGN})
set_target_properties(${moduleName}_python PROPERTIES
OUTPUT_NAME ${moduleName}_python
CLEAN_DIRECT_OUTPUT 1
VERSION 1
SOVERSION 0
SUFFIX ".pyd")
target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp
# Create a static library version
add_library(${TARGET_NAME} SHARED ${ARGN})
set(PYLIB_OUTPUT_FILE $<TARGET_FILE:${moduleName}_python>)
message(${PYLIB_OUTPUT_FILE})
get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.pyd)
target_link_libraries(${TARGET_NAME} ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} gtsam-shared)
set_target_properties(${TARGET_NAME} PROPERTIES
OUTPUT_NAME ${TARGET_NAME}
CLEAN_DIRECT_OUTPUT 1
VERSION 1
SOVERSION 0)
ELSE()
# Create a shared library
add_library(${moduleName}_python SHARED ${generated_cpp_file})
set_target_properties(${moduleName}_python PROPERTIES
OUTPUT_NAME ${moduleName}_python
CLEAN_DIRECT_OUTPUT 1)
target_link_libraries(${moduleName}_python ${Boost_PYTHON_LIBRARY} ${PYTHON_LIBRARY} ${gtsamLib}) #temp
# On OSX and Linux, the python library must end in the extension .so. Build this
# filename here.
get_property(PYLIB_OUTPUT_FILE TARGET ${moduleName}_python PROPERTY LOCATION)
set(PYLIB_OUTPUT_FILE $<TARGET_FILE:${moduleName}_python>)
message(${PYLIB_OUTPUT_FILE})
get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
set(PYLIB_SO_NAME lib${moduleName}_python.so)
ENDIF(MSVC)
# On OSX and Linux, the python library must end in the extension .so. Build this
# filename here.
get_property(PYLIB_OUTPUT_FILE TARGET ${TARGET_NAME} PROPERTY LOCATION)
get_filename_component(PYLIB_OUTPUT_NAME ${PYLIB_OUTPUT_FILE} NAME_WE)
set(PYLIB_SO_NAME ${PYLIB_OUTPUT_NAME}.so)
# Installs the library in the gtsam folder, which is used by setup.py to create the gtsam package
set(PYTHON_MODULE_DIRECTORY ${CMAKE_SOURCE_DIR}/python/gtsam)
# Cause the library to be output in the correct directory.
add_custom_command(TARGET ${moduleName}_python
POST_BUILD
COMMAND cp -v ${PYLIB_OUTPUT_FILE} ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME}
WORKING_DIRECTORY ${PROJECT_SOURCE_DIR}
COMMENT "Copying library files to python directory" )
# Cause the library to be output in the correct directory.
add_custom_command(TARGET ${TARGET_NAME}
@ -64,4 +87,16 @@ function(wrap_python TARGET_NAME PYTHON_MODULE_DIRECTORY)
get_directory_property(AMCF ADDITIONAL_MAKE_CLEAN_FILES)
list(APPEND AMCF ${PYTHON_MODULE_DIRECTORY}/${PYLIB_SO_NAME})
set_directory_properties(PROPERTIES ADDITIONAL_MAKE_CLEAN_FILES "${AMCF}")
endfunction(wrap_python)
endfunction(wrap_python)
# Macro to get list of subdirectories
macro(SUBDIRLIST result curdir)
file(GLOB children RELATIVE ${curdir} ${curdir}/*)
set(dirlist "")
foreach(child ${children})
if(IS_DIRECTORY ${curdir}/${child})
list(APPEND dirlist ${child})
endif()
endforeach()
set(${result} ${dirlist})
endmacro()

21
cmake/GtsamTesting.cmake
View File

@ -109,9 +109,8 @@ add_custom_target(examples)
# Add timing target
add_custom_target(timing)
# Include obsolete macros - will be removed in the near future
include(GtsamTestingObsolete)
# Add target to build tests without running
add_custom_target(all.tests)
# Implementations of this file's macros:
@ -165,8 +164,9 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
add_test(NAME ${script_name} COMMAND ${script_name})
add_dependencies(check.${groupName} ${script_name})
add_dependencies(check ${script_name})
add_dependencies(all.tests ${script_name})
if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name})
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name})
endif()
# Add TOPSRCDIR
@ -179,11 +179,17 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
set_property(TARGET ${script_name} PROPERTY FOLDER "Unit tests/${groupName}")
endforeach()
else()
#skip folders which don't have any tests
if(NOT script_srcs)
return()
endif()
# Default on MSVC and XCode - combine test group into a single exectuable
set(target_name check_${groupName}_program)
# Add executable
add_executable(${target_name} ${script_srcs} ${script_headers})
add_executable(${target_name} "${script_srcs}" ${script_headers})
target_link_libraries(${target_name} CppUnitLite ${linkLibraries})
# Only have a main function in one script - use preprocessor
@ -195,6 +201,9 @@ macro(gtsamAddTestsGlob_impl groupName globPatterns excludedFiles linkLibraries)
add_test(NAME ${target_name} COMMAND ${target_name})
add_dependencies(check.${groupName} ${target_name})
add_dependencies(check ${target_name})
if(NOT XCODE_VERSION)
add_dependencies(all.tests ${target_name})
endif()
# Add TOPSRCDIR
set_property(SOURCE ${script_srcs} APPEND PROPERTY COMPILE_DEFINITIONS "TOPSRCDIR=\"${PROJECT_SOURCE_DIR}\"")
@ -251,7 +260,7 @@ macro(gtsamAddExesGlob_impl globPatterns excludedFiles linkLibraries groupName b
# Add target dependencies
add_dependencies(${groupName} ${script_name})
if(NOT MSVC AND NOT XCODE_VERSION)
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name})
add_custom_target(${script_name}.run ${EXECUTABLE_OUTPUT_PATH}${script_name} DEPENDS ${script_name})
endif()
# Add TOPSRCDIR

0
cmake/GtsamTestingObsolete.cmake → cmake/obsolete/GtsamTestingObsolete.cmake
View File

3
doc/.gitignore vendored Normal file
View File

@ -0,0 +1,3 @@
/html/
*.lyx~
*.bib~

7
doc/CMakeLists.txt
View File

@ -23,12 +23,17 @@ if (GTSAM_BUILD_DOCS)
# GTSAM core subfolders
set(gtsam_doc_subdirs
gtsam/base
gtsam/discrete
gtsam/geometry
gtsam/inference
gtsam/discrete
gtsam/linear
gtsam/navigation
gtsam/nonlinear
gtsam/sam
gtsam/sfm
gtsam/slam
gtsam/smart
gtsam/symbolic
gtsam
)

1399
doc/ImuFactor.lyx Normal file
View File

File diff suppressed because it is too large Load Diff

BIN
doc/ImuFactor.pdf Normal file
View File

Binary file not shown.

592
doc/LieGroups.lyx
View File

@ -1,5 +1,5 @@
#LyX 2.0 created this file. For more info see http://www.lyx.org/
\lyxformat 413
#LyX 2.1 created this file. For more info see http://www.lyx.org/
\lyxformat 474
\begin_document
\begin_header
\textclass article
@ -15,13 +15,13 @@ theorems-std
\font_roman times
\font_sans default
\font_typewriter default
\font_math auto
\font_default_family rmdefault
\use_non_tex_fonts false
\font_sc false
\font_osf false
\font_sf_scale 100
\font_tt_scale 100
\graphics default
\default_output_format default
\output_sync 0
@ -32,15 +32,24 @@ theorems-std
\use_hyperref false
\papersize default
\use_geometry true
\use_amsmath 1
\use_esint 0
\use_mhchem 1
\use_mathdots 1
\use_package amsmath 1
\use_package amssymb 1
\use_package cancel 1
\use_package esint 0
\use_package mathdots 1
\use_package mathtools 1
\use_package mhchem 1
\use_package stackrel 1
\use_package stmaryrd 1
\use_package undertilde 1
\cite_engine basic
\cite_engine_type default
\biblio_style plain
\use_bibtopic false
\use_indices false
\paperorientation portrait
\suppress_date false
\justification true
\use_refstyle 0
\index Index
\shortcut idx
@ -76,335 +85,10 @@ Frank Dellaert
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_inset CommandInset include
LatexCommand include
filename "macros.lyx"
\begin_layout Plain Layout
Derivatives
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\deriv}[2]{\frac{\partial#1}{\partial#2}}
{\frac{\partial#1}{\partial#2}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\at}[2]{#1\biggr\rvert_{#2}}
{#1\biggr\rvert_{#2}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} }
{\at{\deriv{#1}{#2}}{#3}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
Lie Groups
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\xhat}{\hat{x}}
{\hat{x}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\yhat}{\hat{y}}
{\hat{y}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Ad}[1]{Ad_{#1}}
{Ad_{#1}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\AAdd}[1]{\mathbf{\mathop{Ad}}{}_{#1}}
{\mathbf{\mathop{Ad}}{}_{#1}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\define}{\stackrel{\Delta}{=}}
{\stackrel{\Delta}{=}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\gg}{\mathfrak{g}}
{\mathfrak{g}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Rn}{\mathbb{R}^{n}}
{\mathbb{R}^{n}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
SO(2), 1
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rtwo}{\mathfrak{\mathbb{R}^{2}}}
{\mathfrak{\mathbb{R}^{2}}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\SOtwo}{SO(2)}
{SO(2)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\sotwo}{\mathfrak{so(2)}}
{\mathfrak{so(2)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\that}{\hat{\theta}}
{\hat{\theta}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\skew}[1]{[#1]_{+}}
{[#1]_{+}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
SE(2), 3
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\SEtwo}{SE(2)}
{SE(2)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\setwo}{\mathfrak{se(2)}}
{\mathfrak{se(2)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Skew}[1]{[#1]_{\times}}
{[#1]_{\times}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
SO(3), 3
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rthree}{\mathfrak{\mathbb{R}^{3}}}
{\mathfrak{\mathbb{R}^{3}}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\SOthree}{SO(3)}
{SO(3)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\sothree}{\mathfrak{so(3)}}
{\mathfrak{so(3)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\what}{\hat{\omega}}
{\hat{\omega}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
SE(3),6
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rsix}{\mathfrak{\mathbb{R}^{6}}}
{\mathfrak{\mathbb{R}^{6}}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\SEthree}{SE(3)}
{SE(3)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\sethree}{\mathfrak{se(3)}}
{\mathfrak{se(3)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\xihat}{\hat{\xi}}
{\hat{\xi}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
Aff(2),6
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Afftwo}{Aff(2)}
{Aff(2)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\afftwo}{\mathfrak{aff(2)}}
{\mathfrak{aff(2)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\aa}{a}
{a}
\end_inset
\begin_inset FormulaMacro
\newcommand{\ahat}{\hat{a}}
{\hat{a}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status collapsed
\begin_layout Plain Layout
SL(3),8
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\SLthree}{SL(3)}
{SL(3)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\slthree}{\mathfrak{sl(3)}}
{\mathfrak{sl(3)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\hh}{h}
{h}
\end_inset
\begin_inset FormulaMacro
\newcommand{\hhat}{\hat{h}}
{\hat{h}}
\end_inset
@ -421,7 +105,7 @@ We will start with a small example of a robot moving in a plane, parameterized
2D pose
\emph default
\begin_inset Formula $(x,\, y,\,\theta)$
\begin_inset Formula $(x,\,y,\,\theta)$
\end_inset
.
@ -460,7 +144,7 @@ A similar story holds for translation in the
direction, and in fact for translations in general:
\begin_inset Formula
\[
(x_{t},\, y_{t},\,\theta_{t})=(x_{0}+v_{x}t,\, y_{0}+v_{y}t,\,\theta_{0})
(x_{t},\,y_{t},\,\theta_{t})=(x_{0}+v_{x}t,\,y_{0}+v_{y}t,\,\theta_{0})
\]
\end_inset
@ -468,7 +152,7 @@ A similar story holds for translation in the
Similarly for rotation we have
\begin_inset Formula
\[
(x_{t},\, y_{t},\,\theta_{t})=(x_{0},\, y_{0},\,\theta_{0}+\omega t)
(x_{t},\,y_{t},\,\theta_{t})=(x_{0},\,y_{0},\,\theta_{0}+\omega t)
\]
\end_inset
@ -500,7 +184,7 @@ status collapsed
\end_inset
\begin_inset Caption
\begin_inset Caption Standard
\begin_layout Plain Layout
Robot moving along a circular trajectory.
@ -521,20 +205,20 @@ However, if we combine translation and rotation, the story breaks down!
We cannot write
\begin_inset Formula
\[
(x_{t},\, y_{t},\,\theta_{t})=(x_{0}+v_{x}t,\, y_{0}+v_{y}t,\,\theta_{0}+\omega t)
(x_{t},\,y_{t},\,\theta_{t})=(x_{0}+v_{x}t,\,y_{0}+v_{y}t,\,\theta_{0}+\omega t)
\]
\end_inset
The reason is that, if we move the robot a tiny bit according to the velocity
vector
\begin_inset Formula $(v_{x},\, v_{y},\,\omega)$
\begin_inset Formula $(v_{x},\,v_{y},\,\omega)$
\end_inset
, we have (to first order)
\begin_inset Formula
\[
(x_{\delta},\, y_{\delta},\,\theta_{\delta})=(x_{0}+v_{x}\delta,\, y_{0}+v_{y}\delta,\,\theta_{0}+\omega\delta)
(x_{\delta},\,y_{\delta},\,\theta_{\delta})=(x_{0}+v_{x}\delta,\,y_{0}+v_{y}\delta,\,\theta_{0}+\omega\delta)
\]
\end_inset
@ -580,7 +264,7 @@ status open
\end_inset
\begin_inset Caption
\begin_inset Caption Standard
\begin_layout Plain Layout
\begin_inset CommandInset label
@ -615,7 +299,7 @@ To make progress, we have to be more precise about how the robot behaves.
as
\begin_inset Formula
\[
T_{1}T_{2}=(x_{1},\, y_{1},\,\theta_{1})(x_{2},\, y_{2},\,\theta_{2})=(x_{1}+\cos\theta_{1}x_{2}-\sin\theta y_{2},\, y_{1}+\sin\theta_{1}x_{2}+\cos\theta_{1}y_{2},\,\theta_{1}+\theta_{2})
T_{1}T_{2}=(x_{1},\,y_{1},\,\theta_{1})(x_{2},\,y_{2},\,\theta_{2})=(x_{1}+\cos\theta_{1}x_{2}-\sin\theta y_{2},\,y_{1}+\sin\theta_{1}x_{2}+\cos\theta_{1}y_{2},\,\theta_{1}+\theta_{2})
\]
\end_inset
@ -1925,13 +1609,13 @@ Hence, an alternative way of writing down elements of
\end_inset
is as the ordered pair
\begin_inset Formula $(R,\, t)$
\begin_inset Formula $(R,\,t)$
\end_inset
, with composition defined a
\begin_inset Formula
\[
(R_{1},\, t_{1})(R_{2},\, t_{2})=(R_{1}R_{2},\, R{}_{1}t_{2}+t_{1})
(R_{1},\,t_{1})(R_{2},\,t_{2})=(R_{1}R_{2},\,R{}_{1}t_{2}+t_{1})
\]
\end_inset
@ -2894,13 +2578,13 @@ where
\end_inset
is as the ordered pair
\begin_inset Formula $(R,\, t)$
\begin_inset Formula $(R,\,t)$
\end_inset
, with composition defined as
\begin_inset Formula
\[
(R_{1},\, t_{1})(R_{2},\, t_{2})=(R_{1}R_{2},\, R{}_{1}t_{2}+t_{1})
(R_{1},\,t_{1})(R_{2},\,t_{2})=(R_{1}R_{2},\,R{}_{1}t_{2}+t_{1})
\]
\end_inset
@ -3269,6 +2953,218 @@ p\\
\end_inset
\end_layout
\begin_layout Section
3D Similarity Transformations
\end_layout
\begin_layout Standard
The group of 3D similarity transformations
\begin_inset Formula $Sim(3)$
\end_inset
is the set of
\begin_inset Formula $4\times4$
\end_inset
invertible matrices of the form
\begin_inset Formula
\[
T\define\left[\begin{array}{cc}
R & t\\
0 & s^{-1}
\end{array}\right]
\]
\end_inset
where
\begin_inset Formula $s$
\end_inset
is a scalar.
There are several different conventions in use for the Lie algebra generators,
but we use
\begin_inset Formula
\[
G^{1}=\left(\begin{array}{cccc}
0 & 0 & 0 & 0\\
0 & 0 & -1 & 0\\
0 & 1 & 0 & 0\\
0 & 0 & 0 & 0
\end{array}\right)\mbox{}G^{2}=\left(\begin{array}{cccc}
0 & 0 & 1 & 0\\
0 & 0 & 0 & 0\\
-1 & 0 & 0 & 0\\
0 & 0 & 0 & 0
\end{array}\right)\mbox{ }G^{3}=\left(\begin{array}{cccc}
0 & -1 & 0 & 0\\
1 & 0 & 0 & 0\\
0 & 0 & 0 & 0\\
0 & 0 & 0 & 0
\end{array}\right)
\]
\end_inset
\begin_inset Formula
\[
G^{4}=\left(\begin{array}{cccc}
0 & 0 & 0 & 1\\
0 & 0 & 0 & 0\\
0 & 0 & 0 & 0\\
0 & 0 & 0 & 0
\end{array}\right)\mbox{}G^{5}=\left(\begin{array}{cccc}
0 & 0 & 0 & 0\\
0 & 0 & 0 & 1\\
0 & 0 & 0 & 0\\
0 & 0 & 0 & 0
\end{array}\right)\mbox{ }G^{6}=\left(\begin{array}{cccc}
0 & 0 & 0 & 0\\
0 & 0 & 0 & 0\\
0 & 0 & 0 & 1\\
0 & 0 & 0 & 0
\end{array}\right)\mbox{ }G^{7}=\left(\begin{array}{cccc}
0 & 0 & 0 & 0\\
0 & 0 & 0 & 0\\
0 & 0 & 0 & 0\\
0 & 0 & 0 & -1
\end{array}\right)
\]
\end_inset
\end_layout
\begin_layout Subsection
Actions
\end_layout
\begin_layout Standard
The action of
\begin_inset Formula $\SEthree$
\end_inset
on 3D points is done by embedding the points in
\begin_inset Formula $\mathbb{R}^{4}$
\end_inset
by using homogeneous coordinates
\begin_inset Formula
\[
\hat{q}=\left[\begin{array}{c}
q\\
s^{-1}
\end{array}\right]=\left[\begin{array}{c}
Rp+t\\
s^{-1}
\end{array}\right]=\left[\begin{array}{cc}
R & t\\
0 & s^{-1}
\end{array}\right]\left[\begin{array}{c}
p\\
1
\end{array}\right]=T\hat{p}
\]
\end_inset
The derivative
\begin_inset Formula $D_{1}f(\xi)$
\end_inset
in an incremental change
\begin_inset Formula $\xi$
\end_inset
to
\begin_inset Formula $T$
\end_inset
is given by
\begin_inset Formula $TH(p)$
\end_inset
where
\begin_inset Formula
\[
H(p)=G_{jk}^{i}p^{j}=\left(\begin{array}{ccccccc}
0 & z & -y & 1 & 0 & 0 & 0\\
-z & 0 & x & 0 & 1 & 0 & 0\\
y & -x & 0 & 0 & 0 & 1 & 0\\
0 & 0 & 0 & 0 & 0 & 0 & -1
\end{array}\right)
\]
\end_inset
In other words
\begin_inset Formula
\[
D_{1}f(\xi)=\left[\begin{array}{cc}
R & t\\
0 & s^{-1}
\end{array}\right]\left[\begin{array}{ccc}
-\left[p\right]_{x} & I_{3} & 0\\
0 & 0 & -1
\end{array}\right]=\left[\begin{array}{ccc}
-R\left[p\right]_{x} & R & -t\\
0 & 0 & -s^{-1}
\end{array}\right]
\]
\end_inset
This is the derivative for the action on homogeneous coordinates.
Switching back to non-homogeneous coordinates is done by
\begin_inset Formula
\[
\left[\begin{array}{c}
q\\
a
\end{array}\right]\rightarrow q/a
\]
\end_inset
with derivative
\begin_inset Formula
\[
\left[\begin{array}{cc}
a^{-1}I_{3} & -qa^{-2}\end{array}\right]
\]
\end_inset
For
\begin_inset Formula $a=s^{-1}$
\end_inset
we obtain
\begin_inset Formula
\[
D_{1}f(\xi)=\left[\begin{array}{cc}
sI_{3} & -qs^{2}\end{array}\right]\left[\begin{array}{ccc}
-R\left[p\right]_{x} & R & -t\\
0 & 0 & -s^{-1}
\end{array}\right]=\left[\begin{array}{ccc}
-sR\left[p\right]_{x} & sR & -st+qs\end{array}\right]=\left[\begin{array}{ccc}
-sR\left[p\right]_{x} & sR & sRp\end{array}\right]
\]
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Newpage pagebreak
\end_inset
\end_layout
\begin_layout Section

BIN
doc/LieGroups.pdf
View File

Binary file not shown.

226
doc/Mathematica/Quaternion-Logmap.nb Normal file
View File

@ -0,0 +1,226 @@
(* Content-type: application/vnd.wolfram.mathematica *)
(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)
(* CreatedBy='Mathematica 10.0' *)
(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[ 158, 7]
NotebookDataLength[ 6004, 217]
NotebookOptionsPosition[ 5104, 179]
NotebookOutlinePosition[ 5456, 195]
CellTagsIndexPosition[ 5413, 192]
WindowFrame->Normal*)
(* Beginning of Notebook Content *)
Notebook[{
Cell["\<\
In Quaternion.h we have Logmap, but we have to be careful when qw approaches \
-1 (from above) or 1 (from below). The Taylor expansions below are the basis \
for the code.\
\>", "Text",
CellChangeTimes->{{3.632651837171029*^9, 3.6326518973274307`*^9}}],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{"angle", "=",
RowBox[{"2",
RowBox[{"ArcCos", "[", "qw", "]"}]}]}]], "Input",
CellChangeTimes->{{3.6326509558588057`*^9, 3.632650976842943*^9}}],
Cell[BoxData[
RowBox[{"2", " ",
RowBox[{"ArcCos", "[", "qw", "]"}]}]], "Output",
CellChangeTimes->{{3.6326509669341784`*^9, 3.6326509795921097`*^9}}]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{"s", "=",
RowBox[{"Sqrt", "[",
RowBox[{"1", "-",
RowBox[{"qw", "*", "qw"}]}], "]"}]}]], "Input",
CellChangeTimes->{{3.632650983796185*^9, 3.632650994132272*^9}}],
Cell[BoxData[
SqrtBox[
RowBox[{"1", "-",
SuperscriptBox["qw", "2"]}]]], "Output",
CellChangeTimes->{3.63265099440246*^9}]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{"factor", " ", "=", " ",
RowBox[{"angle", "/", "s"}]}]], "Input",
CellChangeTimes->{{3.632650999925654*^9, 3.632651001339293*^9}, {
3.632651070297429*^9, 3.632651071527272*^9}}],
Cell[BoxData[
FractionBox[
RowBox[{"2", " ",
RowBox[{"ArcCos", "[", "qw", "]"}]}],
SqrtBox[
RowBox[{"1", "-",
SuperscriptBox["qw", "2"]}]]]], "Output",
CellChangeTimes->{3.632651001671771*^9, 3.632651072007021*^9}]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{"Expand", "[",
RowBox[{"Series", "[",
RowBox[{
FractionBox[
RowBox[{"2", " ",
RowBox[{"ArcCos", "[", "qw", "]"}]}],
SqrtBox[
RowBox[{"1", "-",
SuperscriptBox["qw", "2"]}]]], ",",
RowBox[{"{",
RowBox[{"qw", ",", "1", ",", "1"}], "}"}], ",",
RowBox[{"Assumptions", "->",
RowBox[{"(",
RowBox[{"qw", "<", "1"}], ")"}]}]}], "]"}], "]"}]], "Input",
CellChangeTimes->{{3.6326510739355927`*^9, 3.632651117949705*^9}, {
3.6326511716876993`*^9, 3.632651189491748*^9}, {3.632651248821335*^9,
3.632651267905816*^9}}],
Cell[BoxData[
InterpretationBox[
RowBox[{"2", "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"(",
RowBox[{"qw", "-", "1"}], ")"}]}], "3"], "+",
InterpretationBox[
SuperscriptBox[
RowBox[{"O", "[",
RowBox[{"qw", "-", "1"}], "]"}],
RowBox[{"3", "/", "2"}]],
SeriesData[$CellContext`qw, 1, {}, 0, 3, 2],
Editable->False]}],
SeriesData[$CellContext`qw, 1, {2, 0,
Rational[-2, 3]}, 0, 3, 2],
Editable->False]], "Output",
CellChangeTimes->{{3.632651102947558*^9, 3.632651118218814*^9}, {
3.632651179610784*^9, 3.6326511898522263`*^9}, {3.632651249719887*^9,
3.632651268312502*^9}}]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{"ArcCos", "[",
RowBox[{"-", "1"}], "]"}]], "Input",
CellChangeTimes->{{3.632651352754121*^9, 3.63265135286866*^9}}],
Cell[BoxData["\[Pi]"], "Output",
CellChangeTimes->{3.632651353300222*^9}]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{"Expand", "[",
RowBox[{"Series", "[",
RowBox[{
FractionBox[
RowBox[{
RowBox[{"-", "2"}],
RowBox[{"ArcCos", "[",
RowBox[{"-", "qw"}], "]"}]}],
SqrtBox[
RowBox[{"1", "-",
SuperscriptBox["qw", "2"]}]]], ",",
RowBox[{"{",
RowBox[{"qw", ",",
RowBox[{"-", "1"}], ",", "1"}], "}"}], ",",
RowBox[{"Assumptions", "->",
RowBox[{"(",
RowBox[{"qw", ">",
RowBox[{"-", "1"}]}], ")"}]}]}], "]"}], "]"}]], "Input",
CellChangeTimes->{{3.6326510739355927`*^9, 3.632651117949705*^9}, {
3.6326511716876993`*^9, 3.6326512088422937`*^9}, {3.632651301817163*^9,
3.6326513406015453`*^9}, {3.63265150259446*^9, 3.632651505055284*^9}, {
3.632651744223112*^9, 3.632651772717318*^9}}],
Cell[BoxData[
InterpretationBox[
RowBox[{
RowBox[{"-", "2"}], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"(",
RowBox[{"qw", "+", "1"}], ")"}]}], "3"], "+",
InterpretationBox[
SuperscriptBox[
RowBox[{"O", "[",
RowBox[{"qw", "+", "1"}], "]"}],
RowBox[{"3", "/", "2"}]],
SeriesData[$CellContext`qw, -1, {}, 0, 3, 2],
Editable->False]}],
SeriesData[$CellContext`qw, -1, {-2, 0,
Rational[-2, 3]}, 0, 3, 2],
Editable->False]], "Output",
CellChangeTimes->{
3.632651209181905*^9, 3.6326513025091133`*^9, {3.632651332608609*^9,
3.632651341031022*^9}, 3.632651506516138*^9, {3.632651746679185*^9,
3.632651773032124*^9}}]
}, Open ]]
},
WindowSize->{808, 751},
WindowMargins->{{4, Automatic}, {Automatic, 4}},
FrontEndVersion->"10.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (June 27, \
2014)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)
(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[558, 20, 263, 5, 49, "Text"],
Cell[CellGroupData[{
Cell[846, 29, 174, 4, 28, "Input"],
Cell[1023, 35, 154, 3, 28, "Output"]
}, Open ]],
Cell[CellGroupData[{
Cell[1214, 43, 197, 5, 28, "Input"],
Cell[1414, 50, 129, 4, 40, "Output"]
}, Open ]],
Cell[CellGroupData[{
Cell[1580, 59, 206, 4, 28, "Input"],
Cell[1789, 65, 233, 7, 59, "Output"]
}, Open ]],
Cell[CellGroupData[{
Cell[2059, 77, 605, 17, 61, "Input"],
Cell[2667, 96, 645, 19, 48, "Output"]
}, Open ]],
Cell[CellGroupData[{
Cell[3349, 120, 142, 3, 28, "Input"],
Cell[3494, 125, 74, 1, 28, "Output"]
}, Open ]],
Cell[CellGroupData[{
Cell[3605, 131, 788, 22, 61, "Input"],
Cell[4396, 155, 692, 21, 48, "Output"]
}, Open ]]
}
]
*)
(* End of internal cache information *)

607
doc/Mathematica/dexpInvL_SE2.nb Normal file
View File

@ -0,0 +1,607 @@
(* Content-type: application/vnd.wolfram.mathematica *)
(*** Wolfram Notebook File ***)
(* http://www.wolfram.com/nb *)
(* CreatedBy='Mathematica 8.0' *)
(*CacheID: 234*)
(* Internal cache information:
NotebookFileLineBreakTest
NotebookFileLineBreakTest
NotebookDataPosition[ 157, 7]
NotebookDataLength[ 18933, 598]
NotebookOptionsPosition[ 18110, 565]
NotebookOutlinePosition[ 18464, 581]
CellTagsIndexPosition[ 18421, 578]
WindowFrame->Normal*)
(* Beginning of Notebook Content *)
Notebook[{
Cell[TextData[{
"The \[OpenCurlyQuote]right trivialised\[CloseCurlyQuote] tangent of the \
exponential map, ",
Cell[BoxData[
FormBox["dexpR", TraditionalForm]],
FormatType->"TraditionalForm"],
", according to Iserles05an, formula 2.42, pg. 32 can be written as\n\t",
Cell[BoxData[
FormBox[GridBox[{
{"\t"},
{
RowBox[{
RowBox[{
RowBox[{"g", "'"}],
SuperscriptBox["g",
RowBox[{"-", "1"}]]}], "=",
RowBox[{
SubscriptBox["dexpR", "\[Omega]"], "(",
RowBox[{"\[Omega]", "'"}], ")"}]}]}
}], TraditionalForm]],
FormatType->"TraditionalForm"],
"\nwhere ",
Cell[BoxData[
FormBox[
RowBox[{"g", "=",
RowBox[{"exp", "(", "\[Omega]", ")"}]}], TraditionalForm]],
FormatType->"TraditionalForm"],
", and ",
Cell[BoxData[
FormBox[
RowBox[{
RowBox[{"g", "'"}], "=",
RowBox[{
RowBox[{"exp", "'"}],
RowBox[{"(", "\[Omega]", ")"}]}]}], TraditionalForm]],
FormatType->"TraditionalForm"],
".\nCompare this to the left Jacobian matrix ",
Cell[BoxData[
FormBox[
SubscriptBox["J", "l"], TraditionalForm]],
FormatType->"TraditionalForm"],
" in Chirikjian11book2, pg. 26, we see that ",
Cell[BoxData[
FormBox["dexpR", TraditionalForm]],
FormatType->"TraditionalForm"],
" and ",
Cell[BoxData[
FormBox[
SubscriptBox["J", "l"], TraditionalForm]],
FormatType->"TraditionalForm"],
" are the same.\n\nHence, be careful, Iserles\[CloseCurlyQuote]s \
\[OpenCurlyQuote]",
StyleBox["right",
FontWeight->"Bold"],
" trivialised\[CloseCurlyQuote] tangent of the exponential map ",
Cell[BoxData[
FormBox["dexpR", TraditionalForm]],
FormatType->"TraditionalForm"],
" is in fact Chirikjian\[CloseCurlyQuote]s ",
StyleBox["left",
FontWeight->"Bold"],
" Jacobian matrix ",
Cell[BoxData[
FormBox[
SubscriptBox["J", "l"], TraditionalForm]],
FormatType->"TraditionalForm"],
"!!!\n\nWe want to compute the s \[OpenCurlyQuote]",
StyleBox["left",
FontWeight->"Bold"],
" trivialised\[CloseCurlyQuote] tangent of the exponential map, ",
Cell[BoxData[
FormBox["dexpL", TraditionalForm]],
FormatType->"TraditionalForm"],
", for SE2, hence, we need to use Chirikjian\[CloseCurlyQuote]s ",
StyleBox["right",
FontWeight->"Bold"],
" Jacobian matrix ",
Cell[BoxData[
FormBox[
SubscriptBox["J", "r"], TraditionalForm]],
FormatType->"TraditionalForm"],
" formula in Chirikjian11book2, pg. 36."
}], "Text",
CellChangeTimes->{{3.6279967389044943`*^9, 3.6279968865058002`*^9}, {
3.6279969695759087`*^9, 3.6279974871811028`*^9}, 3.62799757389325*^9}],
Cell[BoxData[{
RowBox[{"Clear", "[", "J", "]"}], "\[IndentingNewLine]",
RowBox[{
RowBox[{"J", "[", "\[Alpha]_", "]"}], ":=",
RowBox[{"{",
RowBox[{
RowBox[{"{",
RowBox[{
RowBox[{
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "/", "\[Alpha]"}], ",",
RowBox[{
RowBox[{"(",
RowBox[{"1", "-",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}], "/", "\[Alpha]"}],
",", " ",
RowBox[{
RowBox[{"(",
RowBox[{
RowBox[{"\[Alpha]", " ",
SubscriptBox["v", "1"]}], "-",
SubscriptBox["v", "2"], "+",
RowBox[{
SubscriptBox["v", "2"],
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "-",
RowBox[{
SubscriptBox["v", "1"],
RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], "/",
SuperscriptBox["\[Alpha]", "2"]}]}], "}"}], ",",
RowBox[{"{",
RowBox[{
RowBox[{
RowBox[{"-",
RowBox[{"(",
RowBox[{"1", "-",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}], "/", "\[Alpha]"}],
",",
RowBox[{
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "/", "\[Alpha]"}], ",", " ",
RowBox[{
RowBox[{"(",
RowBox[{
SubscriptBox["v", "1"], "+",
RowBox[{"\[Alpha]", " ",
SubscriptBox["v", "2"]}], "-",
RowBox[{
SubscriptBox["v", "1"],
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "-",
RowBox[{
SubscriptBox["v", "2"],
RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], "/",
SuperscriptBox["\[Alpha]", "2"]}]}], "}"}], ",",
RowBox[{"{",
RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}]}]}], "Input",
CellChangeTimes->{{3.627993817228732*^9, 3.6279939547434673`*^9}, {
3.627993986274671*^9, 3.6279940386007967`*^9}, {3.627995391081044*^9,
3.627995412846286*^9}, 3.6279954452391644`*^9, {3.627995531089571*^9,
3.6279955341932592`*^9}, {3.627996429604282*^9, 3.62799643077184*^9}}],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{
RowBox[{
RowBox[{"Jinv", "[", "\[Alpha]_", "]"}], "=",
RowBox[{"Inverse", "[",
RowBox[{"J", "[", "\[Alpha]", "]"}], "]"}]}],
"\[IndentingNewLine]"}]], "Input",
CellChangeTimes->{
3.627995475343099*^9, {3.627995548533964*^9, 3.627995559455151*^9}, {
3.627996438504909*^9, 3.6279964431657553`*^9}}],
Cell[BoxData[
RowBox[{"{",
RowBox[{
RowBox[{"{",
RowBox[{
FractionBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}],
RowBox[{"\[Alpha]", " ",
RowBox[{"(",
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}], ")"}]}]], ",",
FractionBox[
RowBox[{
RowBox[{"-",
FractionBox["1", "\[Alpha]"]}], "+",
FractionBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Alpha]"]}],
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}]], ",",
FractionBox[
RowBox[{
FractionBox[
SubscriptBox["v", "1"],
SuperscriptBox["\[Alpha]", "3"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "1"]}],
SuperscriptBox["\[Alpha]", "3"]], "+",
FractionBox[
RowBox[{
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], " ",
SubscriptBox["v", "1"]}],
SuperscriptBox["\[Alpha]", "3"]], "-",
FractionBox[
RowBox[{
RowBox[{"Sin", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "1"]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
RowBox[{
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], " ",
SubscriptBox["v", "1"]}],
SuperscriptBox["\[Alpha]", "3"]], "+",
FractionBox[
SubscriptBox["v", "2"],
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{
RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "2"]}],
SuperscriptBox["\[Alpha]", "2"]]}],
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}]]}], "}"}], ",",
RowBox[{"{",
RowBox[{
FractionBox[
RowBox[{
FractionBox["1", "\[Alpha]"], "-",
FractionBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "\[Alpha]"]}],
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}]], ",",
FractionBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}],
RowBox[{"\[Alpha]", " ",
RowBox[{"(",
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}], ")"}]}]], ",",
FractionBox[
RowBox[{
RowBox[{"-",
FractionBox[
SubscriptBox["v", "1"],
SuperscriptBox["\[Alpha]", "2"]]}], "+",
FractionBox[
RowBox[{
RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "1"]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SubscriptBox["v", "2"],
SuperscriptBox["\[Alpha]", "3"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "2"]}],
SuperscriptBox["\[Alpha]", "3"]], "+",
FractionBox[
RowBox[{
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"], " ",
SubscriptBox["v", "2"]}],
SuperscriptBox["\[Alpha]", "3"]], "-",
FractionBox[
RowBox[{
RowBox[{"Sin", "[", "\[Alpha]", "]"}], " ",
SubscriptBox["v", "2"]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
RowBox[{
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"], " ",
SubscriptBox["v", "2"]}],
SuperscriptBox["\[Alpha]", "3"]]}],
RowBox[{
FractionBox["1",
SuperscriptBox["\[Alpha]", "2"]], "-",
FractionBox[
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Cos", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]], "+",
FractionBox[
SuperscriptBox[
RowBox[{"Sin", "[", "\[Alpha]", "]"}], "2"],
SuperscriptBox["\[Alpha]", "2"]]}]]}], "}"}], ",",
RowBox[{"{",
RowBox[{"0", ",", "0", ",", "1"}], "}"}]}], "}"}]], "Output",
CellChangeTimes->{
3.627995560030972*^9, {3.627996412919798*^9, 3.627996444306521*^9}}]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{
RowBox[{
RowBox[{"Jinv", "[", "\[Alpha]", "]"}], "//", "Simplify"}], "//",
"MatrixForm"}]], "Input",
CellChangeTimes->{{3.627993835637863*^9, 3.627993839233502*^9}, {
3.627994046108457*^9, 3.627994058781851*^9}, {3.627995546842499*^9,
3.6279955664940767`*^9}}],
Cell[BoxData[
TagBox[
RowBox[{"(", "\[NoBreak]", GridBox[{
{
RowBox[{
FractionBox["1", "2"], " ", "\[Alpha]", " ",
RowBox[{"Cot", "[",
FractionBox["\[Alpha]", "2"], "]"}]}],
RowBox[{"-",
FractionBox["\[Alpha]", "2"]}],
FractionBox[
RowBox[{
RowBox[{
RowBox[{"(",
RowBox[{
RowBox[{"-", "2"}], "+",
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "+",
RowBox[{"\[Alpha]", " ",
RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], " ",
SubscriptBox["v", "1"]}], "+",
RowBox[{"\[Alpha]", " ",
RowBox[{"(",
RowBox[{
RowBox[{"-", "1"}], "+",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}], " ",
SubscriptBox["v", "2"]}]}],
RowBox[{"2", " ", "\[Alpha]", " ",
RowBox[{"(",
RowBox[{
RowBox[{"-", "1"}], "+",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}]]},
{
FractionBox["\[Alpha]", "2"],
RowBox[{
FractionBox["1", "2"], " ", "\[Alpha]", " ",
RowBox[{"Cot", "[",
FractionBox["\[Alpha]", "2"], "]"}]}],
FractionBox[
RowBox[{
RowBox[{
RowBox[{"(",
RowBox[{"\[Alpha]", "-",
RowBox[{"\[Alpha]", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}]}], ")"}], " ",
SubscriptBox["v", "1"]}], "+",
RowBox[{
RowBox[{"(",
RowBox[{
RowBox[{"-", "2"}], "+",
RowBox[{"2", " ",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], "+",
RowBox[{"\[Alpha]", " ",
RowBox[{"Sin", "[", "\[Alpha]", "]"}]}]}], ")"}], " ",
SubscriptBox["v", "2"]}]}],
RowBox[{"2", " ", "\[Alpha]", " ",
RowBox[{"(",
RowBox[{
RowBox[{"-", "1"}], "+",
RowBox[{"Cos", "[", "\[Alpha]", "]"}]}], ")"}]}]]},
{"0", "0", "1"}
},
GridBoxAlignment->{
"Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}},
"RowsIndexed" -> {}},
GridBoxSpacings->{"Columns" -> {
Offset[0.27999999999999997`], {
Offset[0.7]},
Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
Offset[0.2], {
Offset[0.4]},
Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
Function[BoxForm`e$,
MatrixForm[BoxForm`e$]]]], "Output",
CellChangeTimes->{
3.627993840513033*^9, {3.62799404156531*^9, 3.6279940592345743`*^9},
3.627995567356995*^9, 3.627996415136314*^9, 3.6279964490074778`*^9}]
}, Open ]],
Cell[TextData[{
"In case ",
Cell[BoxData[
FormBox[
RowBox[{"\[Alpha]", "=", "0"}], TraditionalForm]],
FormatType->"TraditionalForm"],
", we compute the limits of ",
Cell[BoxData[
FormBox[
SubscriptBox["J", "r"], TraditionalForm]],
FormatType->"TraditionalForm"],
" and ",
Cell[BoxData[
FormBox[
SuperscriptBox[
SubscriptBox["J", "r"],
RowBox[{"-", "1"}]], TraditionalForm]],
FormatType->"TraditionalForm"],
" as follows"
}], "Text",
CellChangeTimes->{{3.627997495449997*^9, 3.627997524522543*^9}}],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{
RowBox[{
RowBox[{"Limit", "[",
RowBox[{
RowBox[{"Jinv", "[", "\[Alpha]", "]"}], ",",
RowBox[{"\[Alpha]", "\[Rule]", "0"}]}], "]"}], "//", "Simplify"}], "//",
"MatrixForm"}]], "Input",
CellChangeTimes->{{3.627995572179821*^9, 3.627995606373824*^9}}],
Cell[BoxData[
TagBox[
RowBox[{"(", "\[NoBreak]", GridBox[{
{"1", "0",
FractionBox[
SubscriptBox["v", "2"], "2"]},
{"0", "1",
RowBox[{"-",
FractionBox[
SubscriptBox["v", "1"], "2"]}]},
{"0", "0", "1"}
},
GridBoxAlignment->{
"Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}},
"RowsIndexed" -> {}},
GridBoxSpacings->{"Columns" -> {
Offset[0.27999999999999997`], {
Offset[0.7]},
Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
Offset[0.2], {
Offset[0.4]},
Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
Function[BoxForm`e$,
MatrixForm[BoxForm`e$]]]], "Output",
CellChangeTimes->{{3.627995585954463*^9, 3.627995606858135*^9},
3.6279964178087473`*^9, 3.6279964634008904`*^9}]
}, Open ]],
Cell[CellGroupData[{
Cell[BoxData[
RowBox[{
RowBox[{
RowBox[{"Limit", "[",
RowBox[{
RowBox[{"J", "[", "\[Alpha]", "]"}], ",",
RowBox[{"\[Alpha]", "\[Rule]", "0"}]}], "]"}], "//", "Simplify"}], "//",
"MatrixForm"}]], "Input",
CellChangeTimes->{{3.6279956527343893`*^9, 3.627995660697241*^9}}],
Cell[BoxData[
TagBox[
RowBox[{"(", "\[NoBreak]", GridBox[{
{"1", "0",
RowBox[{"-",
FractionBox[
SubscriptBox["v", "2"], "2"]}]},
{"0", "1",
FractionBox[
SubscriptBox["v", "1"], "2"]},
{"0", "0", "1"}
},
GridBoxAlignment->{
"Columns" -> {{Center}}, "ColumnsIndexed" -> {}, "Rows" -> {{Baseline}},
"RowsIndexed" -> {}},
GridBoxSpacings->{"Columns" -> {
Offset[0.27999999999999997`], {
Offset[0.7]},
Offset[0.27999999999999997`]}, "ColumnsIndexed" -> {}, "Rows" -> {
Offset[0.2], {
Offset[0.4]},
Offset[0.2]}, "RowsIndexed" -> {}}], "\[NoBreak]", ")"}],
Function[BoxForm`e$,
MatrixForm[BoxForm`e$]]]], "Output",
CellChangeTimes->{{3.627995653969245*^9, 3.627995661346282*^9},
3.627996419383007*^9, 3.627996465705708*^9}]
}, Open ]],
Cell[BoxData[""], "Input",
CellChangeTimes->{{3.627995694633294*^9, 3.627995695945466*^9}}]
},
WindowSize->{654, 852},
WindowMargins->{{Automatic, 27}, {Automatic, 0}},
FrontEndVersion->"8.0 for Mac OS X x86 (32-bit, 64-bit Kernel) (October 5, \
2011)",
StyleDefinitions->"Default.nb"
]
(* End of Notebook Content *)
(* Internal cache information *)
(*CellTagsOutline
CellTagsIndex->{}
*)
(*CellTagsIndex
CellTagsIndex->{}
*)
(*NotebookFileOutline
Notebook[{
Cell[557, 20, 2591, 84, 197, "Text"],
Cell[3151, 106, 2022, 56, 68, "Input"],
Cell[CellGroupData[{
Cell[5198, 166, 343, 9, 43, "Input"],
Cell[5544, 177, 6519, 190, 290, "Output"]
}, Open ]],
Cell[CellGroupData[{
Cell[12100, 372, 298, 7, 27, "Input"],
Cell[12401, 381, 2665, 76, 99, "Output"]
}, Open ]],
Cell[15081, 460, 535, 20, 29, "Text"],
Cell[CellGroupData[{
Cell[15641, 484, 297, 8, 27, "Input"],
Cell[15941, 494, 863, 25, 91, "Output"]
}, Open ]],
Cell[CellGroupData[{
Cell[16841, 524, 296, 8, 27, "Input"],
Cell[17140, 534, 859, 25, 91, "Output"]
}, Open ]],
Cell[18014, 562, 92, 1, 27, "Input"]
}
]
*)
(* End of internal cache information *)

137
doc/macros.lyx
View File

@ -1,42 +1,60 @@
#LyX 1.6.5 created this file. For more info see http://www.lyx.org/
\lyxformat 345
#LyX 2.0 created this file. For more info see http://www.lyx.org/
\lyxformat 413
\begin_document
\begin_header
\textclass article
\use_default_options true
\maintain_unincluded_children false
\language english
\language_package default
\inputencoding auto
\fontencoding global
\font_roman default
\font_sans default
\font_typewriter default
\font_default_family default
\use_non_tex_fonts false
\font_sc false
\font_osf false
\font_sf_scale 100
\font_tt_scale 100
\graphics default
\default_output_format default
\output_sync 0
\bibtex_command default
\index_command default
\paperfontsize default
\use_hyperref false
\papersize default
\use_geometry false
\use_amsmath 1
\use_esint 1
\use_mhchem 1
\use_mathdots 0
\cite_engine basic
\use_bibtopic false
\use_indices false
\paperorientation portrait
\suppress_date false
\use_refstyle 0
\index Index
\shortcut idx
\color #008000
\end_index
\secnumdepth 3
\tocdepth 3
\paragraph_separation indent
\defskip medskip
\paragraph_indentation default
\quotes_language english
\papercolumns 1
\papersides 1
\paperpagestyle default
\tracking_changes false
\output_changes false
\author ""
\author ""
\html_math_output 0
\html_css_as_file 0
\html_be_strict false
\end_header
\begin_body
@ -62,14 +80,14 @@ Derivatives
\begin_inset FormulaMacro
\newcommand{\at}[2]{#1\biggr\rvert_{#2}}
{#1\biggr\rvert_{#2}}
\newcommand{\at}[1]{#1\biggr\vert_{\#2}}
{#1\biggr\vert_{\#2}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Jac}[3]{ \at{\deriv{#1}{#2}} {#3} }
{\at{\deriv{#1}{#2}}{#3}}
{\at{\deriv{#1}{#2}}#3}
\end_inset
@ -107,6 +125,15 @@ Lie Groups
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\AAdd}[1]{\mathbf{\mathop{Ad}}{}_{#1}}
{\mathbf{\mathop{Ad}}{}_{#1}}
\end_inset
\end_layout
\begin_layout Standard
@ -144,6 +171,12 @@ SO(2)
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Rone}{\mathfrak{\mathbb{R}}}
{\mathfrak{\mathbb{R}}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\Rtwo}{\mathfrak{\mathbb{R}^{2}}}
{\mathfrak{\mathbb{R}^{2}}}
@ -202,6 +235,12 @@ SE(2)
\end_inset
\begin_inset FormulaMacro
\newcommand{\Skew}[1]{[#1]_{\times}}
{[#1]_{\times}}
\end_inset
\end_layout
\begin_layout Standard
@ -243,7 +282,7 @@ SO(3)
\begin_inset FormulaMacro
\newcommand{\Skew}[1]{[#1]_{\times}}
\renewcommand{\Skew}[1]{[#1]_{\times}}
{[#1]_{\times}}
\end_inset
@ -288,6 +327,86 @@ SE(3)
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status open
\begin_layout Plain Layout
Aff(2),6
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\Afftwo}{Aff(2)}
{Aff(2)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\afftwo}{\mathfrak{aff(2)}}
{\mathfrak{aff(2)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\aa}{a}
{a}
\end_inset
\begin_inset FormulaMacro
\newcommand{\ahat}{\hat{a}}
{\hat{a}}
\end_inset
\end_layout
\begin_layout Standard
\begin_inset Note Comment
status collapsed
\begin_layout Plain Layout
SL(3),8
\end_layout
\end_inset
\end_layout
\begin_layout Standard
\begin_inset FormulaMacro
\newcommand{\SLthree}{SL(3)}
{SL(3)}
\end_inset
\begin_inset FormulaMacro
\newcommand{\slthree}{\mathfrak{sl(3)}}
{\mathfrak{sl(3)}}
\end_inset
\begin_inset FormulaMacro
\newcommand{\hh}{h}
{h}
\end_inset
\begin_inset FormulaMacro
\newcommand{\hhat}{\hat{h}}
{\hat{h}}
\end_inset
\end_layout
\end_body

437
doc/math.lyx
View File

@ -1,5 +1,5 @@
#LyX 2.0 created this file. For more info see http://www.lyx.org/
\lyxformat 413
#LyX 2.1 created this file. For more info see http://www.lyx.org/
\lyxformat 474
\begin_document
\begin_header
\textclass article
@ -17,13 +17,13 @@ theorems-ams-bytype
\font_roman times
\font_sans default
\font_typewriter default
\font_math auto
\font_default_family rmdefault
\use_non_tex_fonts false
\font_sc false
\font_osf false
\font_sf_scale 100
\font_tt_scale 100
\graphics default
\default_output_format default
\output_sync 0
@ -34,15 +34,24 @@ theorems-ams-bytype
\use_hyperref false
\papersize default
\use_geometry true
\use_amsmath 1
\use_esint 0
\use_mhchem 1
\use_mathdots 1
\use_package amsmath 1
\use_package amssymb 1
\use_package cancel 1
\use_package esint 0
\use_package mathdots 1
\use_package mathtools 1
\use_package mhchem 1
\use_package stackrel 0
\use_package stmaryrd 1
\use_package undertilde 1
\cite_engine basic
\cite_engine_type default
\biblio_style plain
\use_bibtopic false
\use_indices false
\paperorientation portrait
\suppress_date false
\justification true
\use_refstyle 0
\index Index
\shortcut idx
@ -1237,21 +1246,28 @@ reference "eq:ApproximateObjective"
\end_inset
.
In particular, the notion of an exponential map allows us to define an
incremental transformation as tracing out a geodesic curve on the group
manifold along a certain
In particular, the notion of an exponential map allows us to define a mapping
from
\series bold
tangent vector
local coordinates
\series default
\begin_inset Formula $\xi$
\end_inset
back to a neighborhood in
\begin_inset Formula $G$
\end_inset
around
\begin_inset Formula $a$
\end_inset
,
\begin_inset Formula
\[
a\oplus\xi\define a\exp\left(\hat{\xi}\right)
\]
\begin{equation}
a\oplus\xi\define a\exp\left(\hat{\xi}\right)\label{eq:expmap}
\end{equation}
\end_inset
@ -1263,11 +1279,12 @@ with
\begin_inset Formula $n$
\end_inset
-dimensional Lie group,
-dimensional Lie group.
Above,
\begin_inset Formula $\hat{\xi}\in\mathfrak{g}$
\end_inset
the Lie algebra element corresponding to the vector
is the Lie algebra element corresponding to the vector
\begin_inset Formula $\xi$
\end_inset
@ -1305,7 +1322,7 @@ For the Lie group
\end_inset
is denoted as
\begin_inset Formula $\omega$
\begin_inset Formula $\omega t$
\end_inset
and represents an angular displacement.
@ -1314,17 +1331,17 @@ For the Lie group
\end_inset
is a skew symmetric matrix denoted as
\begin_inset Formula $\Skew{\omega}\in\sothree$
\begin_inset Formula $\Skew{\omega t}\in\sothree$
\end_inset
, and is given by
\begin_inset Formula
\[
\Skew{\omega}=\left[\begin{array}{ccc}
\Skew{\omega t}=\left[\begin{array}{ccc}
0 & -\omega_{z} & \omega_{y}\\
\omega_{z} & 0 & -\omega_{x}\\
-\omega_{y} & \omega_{x} & 0
\end{array}\right]
\end{array}\right]t
\]
\end_inset
@ -1334,12 +1351,136 @@ Finally, the increment
\end_inset
corresponds to an incremental rotation
\begin_inset Formula $R\oplus\omega=Re^{\Skew{\omega}}$
\begin_inset Formula $R\oplus\omega t=Re^{\Skew{\omega t}}$
\end_inset
.
\end_layout
\begin_layout Subsection
Local Coordinates vs.
Tangent Vectors
\end_layout
\begin_layout Standard
In differential geometry,
\series bold
tangent vectors
\series default
\begin_inset Formula $v\in T_{a}G$
\end_inset
at
\begin_inset Formula $a$
\end_inset
are elements of the Lie algebra
\begin_inset Formula $\mathfrak{g}$
\end_inset
, and are defined as
\begin_inset Formula
\[
v\define\Jac{\gamma(t)}t{t=0}
\]
\end_inset
where
\begin_inset Formula $\gamma$
\end_inset
is some curve that passes through
\begin_inset Formula $a$
\end_inset
at
\begin_inset Formula $t=0$
\end_inset
, i.e.
\begin_inset Formula $\gamma(0)=a$
\end_inset
.
In particular, for any fixed local coordinate
\begin_inset Formula $\xi$
\end_inset
the map
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:expmap"
\end_inset
can be used to define a
\series bold
geodesic curve
\series default
on the group manifold defined by
\begin_inset Formula $\gamma:t\mapsto ae^{\widehat{t\xi}}$
\end_inset
, and the corresponding tangent vector is given by
\begin_inset Formula
\begin{equation}
\Jac{ae^{\widehat{t\xi}}}t{t=0}=a\xihat\label{eq:tangent-vector}
\end{equation}
\end_inset
This defines the mapping between local coordinates
\begin_inset Formula $\xi\in\Rn$
\end_inset
and actual tangent vectors
\begin_inset Formula $a\xihat\in g$
\end_inset
: the vector
\begin_inset Formula $\xi$
\end_inset
defines a direction of travel on the manifold, but does so in the local
coordinate frame
\begin_inset Formula $a$
\end_inset
.
\end_layout
\begin_layout Example
Assume a rigid body's attitude is described by
\begin_inset Formula $R_{b}^{n}(t)$
\end_inset
, where the indices denote the navigation frame
\begin_inset Formula $N$
\end_inset
and body frame
\begin_inset Formula $B$
\end_inset
, respectively.
An extrinsically calibrated gyroscope measures the angular velocity
\begin_inset Formula $\omega^{b}$
\end_inset
, in the body frame, and the corresponding tangent vector is
\begin_inset Formula
\[
\dot{R}_{b}^{n}(t)=R_{b}^{n}(t)\widehat{\omega^{b}}
\]
\end_inset
\end_layout
\begin_layout Subsection
Derivatives
\end_layout
@ -1352,7 +1493,7 @@ reference "def:differentiable"
\end_inset
to map exponential coordinates
to map local coordinates
\begin_inset Formula $\xi$
\end_inset
@ -1368,7 +1509,7 @@ reference "def:differentiable"
\begin_inset Formula $Df_{a}$
\end_inset
locally approximates a function
approximates the function
\begin_inset Formula $f$
\end_inset
@ -1378,6 +1519,10 @@ reference "def:differentiable"
to
\begin_inset Formula $\Reals m$
\end_inset
in a neighborhood around
\begin_inset Formula $a$
\end_inset
:
@ -1455,41 +1600,6 @@ derivative
.
\end_layout
\begin_layout Standard
Note that the vectors
\begin_inset Formula $\xi$
\end_inset
can be viewed as lying in the tangent space to
\begin_inset Formula $G$
\end_inset
at
\begin_inset Formula $a$
\end_inset
, but defining this rigorously would take us on a longer tour of differential
geometry.
Informally,
\begin_inset Formula $\xi$
\end_inset
is simply the direction, in a local coordinate frame, that is locally tangent
at
\begin_inset Formula $a$
\end_inset
to a geodesic curve
\begin_inset Formula $\gamma:t\mapsto ae^{\widehat{t\xi}}$
\end_inset
traced out by the exponential map, with
\begin_inset Formula $\gamma(0)=a$
\end_inset
.
\end_layout
\begin_layout Subsection
Derivative of an Action
\begin_inset CommandInset label
@ -1502,11 +1612,7 @@ name "sec:Derivatives-of-Actions"
\end_layout
\begin_layout Standard
The (usual) action of an
\begin_inset Formula $n$
\end_inset
-dimensional matrix group
The (usual) action of a matrix group
\begin_inset Formula $G$
\end_inset
@ -1531,7 +1637,7 @@ Since this is a function defined on the product
\end_inset
the derivative is a linear transformation
\begin_inset Formula $Df:\Multi{2n}n$
\begin_inset Formula $Df:\Multi{m+n}n$
\end_inset
with
@ -1542,7 +1648,15 @@ Df_{(T,p)}\left(\xi,\delta p\right)=D_{1}f_{(T,p)}\left(\xi\right)+D_{2}f_{(T,p)
\end_inset
where
\begin_inset Formula $m$
\end_inset
is the dimensionality of the manifold
\begin_inset Formula $G$
\end_inset
.
\end_layout
\begin_layout Theorem
@ -1571,7 +1685,7 @@ H(p) & I_{n}\end{array}\right]
\end_inset
with
\begin_inset Formula $H:\Reals n\rightarrow\Reals{n\times n}$
\begin_inset Formula $H:\Reals m\rightarrow\Reals{n\times m}$
\end_inset
a linear mapping that depends on
@ -1698,7 +1812,7 @@ with
\end_inset
an
\begin_inset Formula $n\times n$
\begin_inset Formula $n\times m$
\end_inset
matrix that depends on
@ -3000,7 +3114,7 @@ f(ge^{\xhat})=f(ge^{\xhat}g^{-1}g)=f(e^{\Ad g\xhat}g)
\end_layout
\begin_layout Subsection
Derivative of the Exponential and Logarithm Map
Derivative of the Exponential Map
\end_layout
\begin_layout Theorem
@ -3064,17 +3178,196 @@ For
\begin_inset Formula $\xi\neq0$
\end_inset
, things are not simple, see .
, things are not simple.
As with pushforwards above, we will be looking for an
\begin_inset Formula $n\times n$
\end_inset
\begin_inset Flex URL
\family roman
\series medium
\shape up
\size normal
\emph off
\bar no
\strikeout off
\uuline off
\uwave off
\noun off
\color none
Jacobian
\begin_inset Formula $f'(\xi)$
\end_inset
such that
\family default
\series default
\shape default
\size default
\emph default
\bar default
\strikeout default
\uuline default
\uwave default
\noun default
\color inherit
\begin_inset Formula
\begin{equation}
f\left(\xi+\delta\right)\approx f\left(\xi\right)\exp\left(\widehat{f'(\xi)\delta}\right)\label{eq:push_exp}
\end{equation}
\end_inset
Differential geometry tells us that for any Lie algebra element
\begin_inset Formula $\xihat\in\mathfrak{g}$
\end_inset
there exists a
\emph on
linear
\emph default
map
\begin_inset Formula $d\exp_{\xihat}:T_{\xihat}\mathfrak{g}\rightarrow T_{\exp(\xihat)}G$
\end_inset
, which is given by
\begin_inset Foot
status collapsed
\begin_layout Plain Layout
See
\begin_inset Flex URL
status open
http://deltaepsilons.wordpress.com/2009/11/06/helgasons-formula-for-the-differenti
al-of-the-exponential/
\begin_layout Plain Layout
http://deltaepsilons.wordpress.com/2009/11/06/
\end_layout
\end_inset
or
\begin_inset Flex URL
status open
\begin_layout Plain Layout
https://en.wikipedia.org/wiki/Derivative_of_the_exponential_map
\end_layout
\end_inset
.
\end_layout
\end_inset
\begin_inset Formula
\begin{equation}
d\exp_{\xihat}\hat{x}=\exp(\xihat)\frac{1-\exp(-ad_{\xihat})}{ad_{\xihat}}\hat{x}\label{eq:dexp}
\end{equation}
\end_inset
with
\begin_inset Formula $\hat{x}\in T_{\xihat}\mathfrak{g}$
\end_inset
and
\begin_inset Formula $ad_{\xihat}$
\end_inset
itself a linear map taking
\begin_inset Formula $\hat{x}$
\end_inset
to
\begin_inset Formula $[\xihat,\hat{x}]$
\end_inset
, the Lie bracket.
The actual formula above is not really as important as the fact that the
linear map exists, although it is expressed directly in terms of tangent
vectors to
\begin_inset Formula $\mathfrak{g}$
\end_inset
and
\begin_inset Formula $G$
\end_inset
.
Equation
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:dexp"
\end_inset
is a tangent vector, and comparing with
\begin_inset CommandInset ref
LatexCommand eqref
reference "eq:tangent-vector"
\end_inset
we see that it maps to local coordinates
\begin_inset Formula $y$
\end_inset
as follows:
\begin_inset Formula
\[
\yhat=\frac{1-\exp(-ad_{\xihat})}{ad_{\xihat}}\hat{x}
\]
\end_inset
which can be used to construct the Jacobian
\begin_inset Formula $f'(\xi)$
\end_inset
.
\end_layout
\begin_layout Example
For
\begin_inset Formula $\SOthree$
\end_inset
, the operator
\begin_inset Formula $ad_{\xihat}$
\end_inset
is simply a matrix multiplication when representing
\begin_inset Formula $\sothree$
\end_inset
using 3-vectors, i.e.,
\begin_inset Formula $ad_{\xihat}x=\xihat x$
\end_inset
, and the
\begin_inset Formula $3\times3$
\end_inset
Jacobian corresponding to
\begin_inset Formula $d\exp$
\end_inset
is
\begin_inset Formula
\[
f'(\xi)=\frac{I_{3\times3}-\exp(-\xihat)}{\xihat}=\sum_{k=0}^{\infty}\frac{(-1)^{k}}{(k+1)!}\xihat^{k}
\]
\end_inset
which, similar to the exponential map, has a simple closed form expression
for
\begin_inset Formula $\SOthree$
\end_inset
.
@ -6797,7 +7090,7 @@ Then
\begin_layout Standard
\begin_inset CommandInset bibtex
LatexCommand bibtex
bibfiles "/Users/dellaert/papers/refs"
bibfiles "refs"
options "plain"
\end_inset

BIN
doc/math.pdf
View File

Binary file not shown.

26
doc/refs.bib Normal file
View File

@ -0,0 +1,26 @@
@article{Iserles00an,
title = {Lie-group methods},
author = {Iserles, Arieh and Munthe-Kaas, Hans Z and
N{\o}rsett, Syvert P and Zanna, Antonella},
journal = {Acta Numerica 2000},
volume = {9},
pages = {215--365},
year = {2000},
publisher = {Cambridge Univ Press}
}
@book{Murray94book,
title = {A mathematical introduction to robotic manipulation},
author = {Murray, Richard M and Li, Zexiang and Sastry, S
Shankar and Sastry, S Shankara},
year = {1994},
publisher = {CRC press}
}
@book{Spivak65book,
title = {Calculus on manifolds},
author = {Spivak, Michael},
volume = {1},
year = {1965},
publisher = {WA Benjamin New York}
}

5
examples/CameraResectioning.cpp
View File

@ -48,8 +48,7 @@ public:
virtual Vector evaluateError(const Pose3& pose, boost::optional<Matrix&> H =
boost::none) const {
SimpleCamera camera(pose, *K_);
Point2 reprojectionError(camera.project(P_, H) - p_);
return reprojectionError.vector();
return camera.project(P_, H, boost::none, boost::none) - p_;
}
};
@ -70,7 +69,7 @@ int main(int argc, char* argv[]) {
/* 2. add factors to the graph */
// add measurement factors
SharedDiagonal measurementNoise = Diagonal::Sigmas((Vector(2) << 0.5, 0.5));
SharedDiagonal measurementNoise = Diagonal::Sigmas(Vector2(0.5, 0.5));
boost::shared_ptr<ResectioningFactor> factor;
graph.push_back(
boost::make_shared<ResectioningFactor>(measurementNoise, X(1), calib,

9
examples/CreateSFMExampleData.cpp
View File

@ -18,7 +18,6 @@
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <boost/foreach.hpp>
#include <boost/assign/std/vector.hpp>
using namespace boost::assign;
@ -35,13 +34,13 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
SfM_data data;
// Create two cameras
Rot3 aRb = Rot3::yaw(M_PI_2);
Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(0.1, 0, 0);
Pose3 identity, aPb(aRb, aTb);
data.cameras.push_back(SfM_Camera(pose1, K));
data.cameras.push_back(SfM_Camera(pose2, K));
BOOST_FOREACH(const Point3& p, P) {
for(const Point3& p: P) {
// Create the track
SfM_Track track;
@ -66,7 +65,7 @@ void createExampleBALFile(const string& filename, const vector<Point3>& P,
void create5PointExample1() {
// Create two cameras poses
Rot3 aRb = Rot3::yaw(M_PI_2);
Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(0.1, 0, 0);
Pose3 pose1, pose2(aRb, aTb);
@ -85,7 +84,7 @@ void create5PointExample1() {
void create5PointExample2() {
// Create two cameras poses
Rot3 aRb = Rot3::yaw(M_PI_2);
Rot3 aRb = Rot3::Yaw(M_PI_2);
Point3 aTb(10, 0, 0);
Pose3 pose1, pose2(aRb, aTb);

1
examples/Data/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
*.txt

21
examples/Data/QPExample.QPS Normal file
View File

@ -0,0 +1,21 @@
NAME QP example
ROWS
N obj
G r1
L r2
COLUMNS
c1 r1 2.0 r2 -1.0
c1 obj 1.5
c2 r1 1.0 r2 2.0
c2 obj -2.0
RHS
rhs1 obj -4.0
rhs1 r1 2.0 r2 6.0
RANGES
BOUNDS
UP BOUNDS c1 20.0
QUADOBJ
c1 c1 8.0
c1 c2 2.0
c2 c2 10.0
ENDATA

20101
examples/Data/imuAndGPSdata.csv Normal file
View File

File diff suppressed because it is too large Load Diff

269
examples/ImuFactorsExample.cpp Normal file
View File

@ -0,0 +1,269 @@
/* ----------------------------------------------------------------------------
* 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 imuFactorsExample
* @brief Test example for using GTSAM ImuFactor and ImuCombinedFactor navigation code.
* @author Garrett (ghemann@gmail.com), Luca Carlone
*/
/**
* Example of use of the imuFactors (imuFactor and combinedImuFactor) in conjunction with GPS
* - you can test imuFactor (resp. combinedImuFactor) by commenting (resp. uncommenting)
* the line #define USE_COMBINED (few lines below)
* - we read IMU and GPS data from a CSV file, with the following format:
* A row starting with "i" is the first initial position formatted with
* N, E, D, qx, qY, qZ, qW, velN, velE, velD
* A row starting with "0" is an imu measurement
* linAccN, linAccE, linAccD, angVelN, angVelE, angVelD
* A row starting with "1" is a gps correction formatted with
* N, E, D, qX, qY, qZ, qW
* Note that for GPS correction, we're only using the position not the rotation. The
* rotation is provided in the file for ground truth comparison.
*/
// GTSAM related includes.
#include <gtsam/navigation/CombinedImuFactor.h>
#include <gtsam/navigation/GPSFactor.h>
#include <gtsam/navigation/ImuFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/inference/Symbol.h>
#include <fstream>
#include <iostream>
// Uncomment line below to use the CombinedIMUFactor as opposed to the standard ImuFactor.
// #define USE_COMBINED
using namespace gtsam;
using namespace std;
using symbol_shorthand::X; // Pose3 (x,y,z,r,p,y)
using symbol_shorthand::V; // Vel (xdot,ydot,zdot)
using symbol_shorthand::B; // Bias (ax,ay,az,gx,gy,gz)
const string output_filename = "imuFactorExampleResults.csv";
// This will either be PreintegratedImuMeasurements (for ImuFactor) or
// PreintegratedCombinedMeasurements (for CombinedImuFactor).
PreintegrationType *imu_preintegrated_;
int main(int argc, char* argv[])
{
string data_filename;
if (argc < 2) {
printf("using default CSV file\n");
data_filename = findExampleDataFile("imuAndGPSdata.csv");
} else {
data_filename = argv[1];
}
// Set up output file for plotting errors
FILE* fp_out = fopen(output_filename.c_str(), "w+");
fprintf(fp_out, "#time(s),x(m),y(m),z(m),qx,qy,qz,qw,gt_x(m),gt_y(m),gt_z(m),gt_qx,gt_qy,gt_qz,gt_qw\n");
// Begin parsing the CSV file. Input the first line for initialization.
// From there, we'll iterate through the file and we'll preintegrate the IMU
// or add in the GPS given the input.
ifstream file(data_filename.c_str());
string value;
// Format is (N,E,D,qX,qY,qZ,qW,velN,velE,velD)
Eigen::Matrix<double,10,1> initial_state = Eigen::Matrix<double,10,1>::Zero();
getline(file, value, ','); // i
for (int i=0; i<9; i++) {
getline(file, value, ',');
initial_state(i) = atof(value.c_str());
}
getline(file, value, '\n');
initial_state(9) = atof(value.c_str());
cout << "initial state:\n" << initial_state.transpose() << "\n\n";
// Assemble initial quaternion through gtsam constructor ::quaternion(w,x,y,z);
Rot3 prior_rotation = Rot3::Quaternion(initial_state(6), initial_state(3),
initial_state(4), initial_state(5));
Point3 prior_point(initial_state.head<3>());
Pose3 prior_pose(prior_rotation, prior_point);
Vector3 prior_velocity(initial_state.tail<3>());
imuBias::ConstantBias prior_imu_bias; // assume zero initial bias
Values initial_values;
int correction_count = 0;
initial_values.insert(X(correction_count), prior_pose);
initial_values.insert(V(correction_count), prior_velocity);
initial_values.insert(B(correction_count), prior_imu_bias);
// Assemble prior noise model and add it the graph.
noiseModel::Diagonal::shared_ptr pose_noise_model = noiseModel::Diagonal::Sigmas((Vector(6) << 0.01, 0.01, 0.01, 0.5, 0.5, 0.5).finished()); // rad,rad,rad,m, m, m
noiseModel::Diagonal::shared_ptr velocity_noise_model = noiseModel::Isotropic::Sigma(3,0.1); // m/s
noiseModel::Diagonal::shared_ptr bias_noise_model = noiseModel::Isotropic::Sigma(6,1e-3);
// Add all prior factors (pose, velocity, bias) to the graph.
NonlinearFactorGraph *graph = new NonlinearFactorGraph();
graph->add(PriorFactor<Pose3>(X(correction_count), prior_pose, pose_noise_model));
graph->add(PriorFactor<Vector3>(V(correction_count), prior_velocity,velocity_noise_model));
graph->add(PriorFactor<imuBias::ConstantBias>(B(correction_count), prior_imu_bias,bias_noise_model));
// We use the sensor specs to build the noise model for the IMU factor.
double accel_noise_sigma = 0.0003924;
double gyro_noise_sigma = 0.000205689024915;
double accel_bias_rw_sigma = 0.004905;
double gyro_bias_rw_sigma = 0.000001454441043;
Matrix33 measured_acc_cov = Matrix33::Identity(3,3) * pow(accel_noise_sigma,2);
Matrix33 measured_omega_cov = Matrix33::Identity(3,3) * pow(gyro_noise_sigma,2);
Matrix33 integration_error_cov = Matrix33::Identity(3,3)*1e-8; // error committed in integrating position from velocities
Matrix33 bias_acc_cov = Matrix33::Identity(3,3) * pow(accel_bias_rw_sigma,2);
Matrix33 bias_omega_cov = Matrix33::Identity(3,3) * pow(gyro_bias_rw_sigma,2);
Matrix66 bias_acc_omega_int = Matrix::Identity(6,6)*1e-5; // error in the bias used for preintegration
boost::shared_ptr<PreintegratedCombinedMeasurements::Params> p = PreintegratedCombinedMeasurements::Params::MakeSharedD(0.0);
// PreintegrationBase params:
p->accelerometerCovariance = measured_acc_cov; // acc white noise in continuous
p->integrationCovariance = integration_error_cov; // integration uncertainty continuous
// should be using 2nd order integration
// PreintegratedRotation params:
p->gyroscopeCovariance = measured_omega_cov; // gyro white noise in continuous
// PreintegrationCombinedMeasurements params:
p->biasAccCovariance = bias_acc_cov; // acc bias in continuous
p->biasOmegaCovariance = bias_omega_cov; // gyro bias in continuous
p->biasAccOmegaInt = bias_acc_omega_int;
#ifdef USE_COMBINED
imu_preintegrated_ = new PreintegratedCombinedMeasurements(p, prior_imu_bias);
#else
imu_preintegrated_ = new PreintegratedImuMeasurements(p, prior_imu_bias);
#endif
// Store previous state for the imu integration and the latest predicted outcome.
NavState prev_state(prior_pose, prior_velocity);
NavState prop_state = prev_state;
imuBias::ConstantBias prev_bias = prior_imu_bias;
// Keep track of the total error over the entire run for a simple performance metric.
double current_position_error = 0.0, current_orientation_error = 0.0;
double output_time = 0.0;
double dt = 0.005; // The real system has noise, but here, results are nearly
// exactly the same, so keeping this for simplicity.
// All priors have been set up, now iterate through the data file.
while (file.good()) {
// Parse out first value
getline(file, value, ',');
int type = atoi(value.c_str());
if (type == 0) { // IMU measurement
Eigen::Matrix<double,6,1> imu = Eigen::Matrix<double,6,1>::Zero();
for (int i=0; i<5; ++i) {
getline(file, value, ',');
imu(i) = atof(value.c_str());
}
getline(file, value, '\n');
imu(5) = atof(value.c_str());
// Adding the IMU preintegration.
imu_preintegrated_->integrateMeasurement(imu.head<3>(), imu.tail<3>(), dt);
} else if (type == 1) { // GPS measurement
Eigen::Matrix<double,7,1> gps = Eigen::Matrix<double,7,1>::Zero();
for (int i=0; i<6; ++i) {
getline(file, value, ',');
gps(i) = atof(value.c_str());
}
getline(file, value, '\n');
gps(6) = atof(value.c_str());
correction_count++;
// Adding IMU factor and GPS factor and optimizing.
#ifdef USE_COMBINED
PreintegratedCombinedMeasurements *preint_imu_combined = dynamic_cast<PreintegratedCombinedMeasurements*>(imu_preintegrated_);
CombinedImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
X(correction_count ), V(correction_count ),
B(correction_count-1), B(correction_count ),
*preint_imu_combined);
graph->add(imu_factor);
#else
PreintegratedImuMeasurements *preint_imu = dynamic_cast<PreintegratedImuMeasurements*>(imu_preintegrated_);
ImuFactor imu_factor(X(correction_count-1), V(correction_count-1),
X(correction_count ), V(correction_count ),
B(correction_count-1),
*preint_imu);
graph->add(imu_factor);
imuBias::ConstantBias zero_bias(Vector3(0, 0, 0), Vector3(0, 0, 0));
graph->add(BetweenFactor<imuBias::ConstantBias>(B(correction_count-1),
B(correction_count ),
zero_bias, bias_noise_model));
#endif
noiseModel::Diagonal::shared_ptr correction_noise = noiseModel::Isotropic::Sigma(3,1.0);
GPSFactor gps_factor(X(correction_count),
Point3(gps(0), // N,
gps(1), // E,
gps(2)), // D,
correction_noise);
graph->add(gps_factor);
// Now optimize and compare results.
prop_state = imu_preintegrated_->predict(prev_state, prev_bias);
initial_values.insert(X(correction_count), prop_state.pose());
initial_values.insert(V(correction_count), prop_state.v());
initial_values.insert(B(correction_count), prev_bias);
LevenbergMarquardtOptimizer optimizer(*graph, initial_values);
Values result = optimizer.optimize();
// Overwrite the beginning of the preintegration for the next step.
prev_state = NavState(result.at<Pose3>(X(correction_count)),
result.at<Vector3>(V(correction_count)));
prev_bias = result.at<imuBias::ConstantBias>(B(correction_count));
// Reset the preintegration object.
imu_preintegrated_->resetIntegrationAndSetBias(prev_bias);
// Print out the position and orientation error for comparison.
Vector3 gtsam_position = prev_state.pose().translation();
Vector3 position_error = gtsam_position - gps.head<3>();
current_position_error = position_error.norm();
Quaternion gtsam_quat = prev_state.pose().rotation().toQuaternion();
Quaternion gps_quat(gps(6), gps(3), gps(4), gps(5));
Quaternion quat_error = gtsam_quat * gps_quat.inverse();
quat_error.normalize();
Vector3 euler_angle_error(quat_error.x()*2,
quat_error.y()*2,
quat_error.z()*2);
current_orientation_error = euler_angle_error.norm();
// display statistics
cout << "Position error:" << current_position_error << "\t " << "Angular error:" << current_orientation_error << "\n";
fprintf(fp_out, "%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f\n",
output_time, gtsam_position(0), gtsam_position(1), gtsam_position(2),
gtsam_quat.x(), gtsam_quat.y(), gtsam_quat.z(), gtsam_quat.w(),
gps(0), gps(1), gps(2),
gps_quat.x(), gps_quat.y(), gps_quat.z(), gps_quat.w());
output_time += 1.0;
} else {
cerr << "ERROR parsing file\n";
return 1;
}
}
fclose(fp_out);
cout << "Complete, results written to " << output_filename << "\n\n";;
return 0;
}

8
examples/LocalizationExample.cpp
View File

@ -93,8 +93,8 @@ public:
// Consequently, the Jacobians are:
// [ derror_x/dx derror_x/dy derror_x/dtheta ] = [1 0 0]
// [ derror_y/dx derror_y/dy derror_y/dtheta ] = [0 1 0]
if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0);
return (Vector(2) << q.x() - mx_, q.y() - my_);
if (H) (*H) = (Matrix(2,3) << 1.0,0.0,0.0, 0.0,1.0,0.0).finished();
return (Vector(2) << q.x() - mx_, q.y() - my_).finished();
}
// The second is a 'clone' function that allows the factor to be copied. Under most
@ -118,14 +118,14 @@ int main(int argc, char** argv) {
// 2a. Add odometry factors
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, 0.0), odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, 0.0), odometryNoise));
// 2b. Add "GPS-like" measurements
// We will use our custom UnaryFactor for this.
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1)); // 10cm std on x,y
noiseModel::Diagonal::shared_ptr unaryNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1)); // 10cm std on x,y
graph.add(boost::make_shared<UnaryFactor>(1, 0.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(2, 2.0, 0.0, unaryNoise));
graph.add(boost::make_shared<UnaryFactor>(3, 4.0, 0.0, unaryNoise));

64
examples/METISOrderingExample.cpp Normal file
View File

@ -0,0 +1,64 @@
/* ----------------------------------------------------------------------------
* 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 METISOrdering.cpp
* @brief Simple robot motion example, with prior and two odometry measurements, using a METIS ordering
* @author Frank Dellaert
* @author Andrew Melim
*/
/**
* Example of a simple 2D localization example optimized using METIS ordering
* - For more details on the full optimization pipeline, see OdometryExample.cpp
*/
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
NonlinearFactorGraph graph;
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
Pose2 odometry(2.0, 0.0, 0.0);
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));
graph.print("\nFactor Graph:\n"); // print
Values initial;
initial.insert(1, Pose2(0.5, 0.0, 0.2));
initial.insert(2, Pose2(2.3, 0.1, -0.2));
initial.insert(3, Pose2(4.1, 0.1, 0.1));
initial.print("\nInitial Estimate:\n"); // print
// optimize using Levenberg-Marquardt optimization
LevenbergMarquardtParams params;
// In order to specify the ordering type, we need to se the NonlinearOptimizerParameter "orderingType"
// By default this parameter is set to OrderingType::COLAMD
params.orderingType = Ordering::METIS;
LevenbergMarquardtOptimizer optimizer(graph, initial, params);
Values result = optimizer.optimize();
result.print("Final Result:\n");
return 0;
}

4
examples/OdometryExample.cpp
View File

@ -64,13 +64,13 @@ int main(int argc, char** argv) {
// Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, priorMean, priorNoise));
// Add odometry factors
Pose2 odometry(2.0, 0.0, 0.0);
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.add(BetweenFactor<Pose2>(1, 2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(2, 3, odometry, odometryNoise));

8
examples/PlanarSLAMExample.cpp
View File

@ -42,7 +42,7 @@
// Also, we will initialize the robot at the origin using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/sam/BearingRangeFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.
@ -80,18 +80,18 @@ int main(int argc, char** argv) {
// Add a prior on pose x1 at the origin. A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior mean is at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1)); // 30cm std on x,y, 0.1 rad on theta
graph.add(PriorFactor<Pose2>(x1, prior, priorNoise)); // add directly to graph
// Add two odometry factors
Pose2 odometry(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1)); // 20cm std on x,y, 0.1 rad on theta
graph.add(BetweenFactor<Pose2>(x1, x2, odometry, odometryNoise));
graph.add(BetweenFactor<Pose2>(x2, x3, odometry, odometryNoise));
// Add Range-Bearing measurements to two different landmarks
// create a noise model for the landmark measurements
noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
noiseModel::Diagonal::shared_ptr measurementNoise = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.2)); // 0.1 rad std on bearing, 20cm on range
// create the measurement values - indices are (pose id, landmark id)
Rot2 bearing11 = Rot2::fromDegrees(45),
bearing21 = Rot2::fromDegrees(90),

4
examples/Pose2SLAMExample.cpp
View File

@ -71,11 +71,11 @@ int main(int argc, char** argv) {
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.add(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
// Create odometry (Between) factors between consecutive poses

86
examples/Pose2SLAMExampleExpressions.cpp Normal file
View File

@ -0,0 +1,86 @@
/* ----------------------------------------------------------------------------
* 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 Pose2SLAMExampleExpressions.cpp
* @brief Expressions version of Pose2SLAMExample.cpp
* @date Oct 2, 2014
* @author Frank Dellaert
*/
// The two new headers that allow using our Automatic Differentiation Expression framework
#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
// For an explanation of headers below, please see Pose2SLAMExample.cpp
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv) {
// 1. Create a factor graph container and add factors to it
ExpressionFactorGraph graph;
// Create Expressions for unknowns
Pose2_ x1(1), x2(2), x3(3), x4(4), x5(5);
// 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.addExpressionFactor(x1, Pose2(0, 0, 0), priorNoise);
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
graph.addExpressionFactor(between(x1,x2), Pose2(2, 0, 0 ), model);
graph.addExpressionFactor(between(x2,x3), Pose2(2, 0, M_PI_2), model);
graph.addExpressionFactor(between(x3,x4), Pose2(2, 0, M_PI_2), model);
graph.addExpressionFactor(between(x4,x5), Pose2(2, 0, M_PI_2), model);
// 2c. Add the loop closure constraint
graph.addExpressionFactor(between(x5,x2), Pose2(2, 0, M_PI_2), model);
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2 ));
initialEstimate.insert(2, Pose2(2.3, 0.1, -0.2 ));
initialEstimate.insert(3, Pose2(4.1, 0.1, M_PI_2));
initialEstimate.insert(4, Pose2(4.0, 2.0, M_PI ));
initialEstimate.insert(5, Pose2(2.1, 2.1, -M_PI_2));
initialEstimate.print("\nInitial Estimate:\n"); // print
// 4. Optimize the initial values using a Gauss-Newton nonlinear optimizer
GaussNewtonParams parameters;
parameters.relativeErrorTol = 1e-5;
parameters.maxIterations = 100;
GaussNewtonOptimizer optimizer(graph, initialEstimate, parameters);
Values result = optimizer.optimize();
result.print("Final Result:\n");
// 5. Calculate and print marginal covariances for all variables
cout.precision(3);
Marginals marginals(graph, result);
cout << "x1 covariance:\n" << marginals.marginalCovariance(1) << endl;
cout << "x2 covariance:\n" << marginals.marginalCovariance(2) << endl;
cout << "x3 covariance:\n" << marginals.marginalCovariance(3) << endl;
cout << "x4 covariance:\n" << marginals.marginalCovariance(4) << endl;
cout << "x5 covariance:\n" << marginals.marginalCovariance(5) << endl;
return 0;
}

3
examples/Pose2SLAMExample_g2o.cpp
View File

@ -20,6 +20,7 @@
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <fstream>
@ -64,7 +65,7 @@ int main(const int argc, const char *argv[]) {
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
std::cout << "Adding prior on pose 0 " << std::endl;

14
examples/Pose2SLAMExample_graph.cpp
View File

@ -16,11 +16,15 @@
* @author Frank Dellaert
*/
#include <gtsam/slam/dataset.h>
// For an explanation of headers below, please see Pose2SLAMExample.cpp
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
// This new header allows us to read examples easily from .graph files
#include <gtsam/slam/dataset.h>
using namespace std;
using namespace gtsam;
@ -31,14 +35,14 @@ int main (int argc, char** argv) {
// we are in build/examples, data is in examples/Data
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0).finished());
string graph_file = findExampleDataFile("w100.graph");
boost::tie(graph, initial) = load2D(graph_file, model);
initial->print("Initial estimate:\n");
// Add a Gaussian prior on first poses
Pose2 priorMean(0.0, 0.0, 0.0); // prior at origin
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.01, 0.01, 0.01));
SharedDiagonal priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.01, 0.01, 0.01));
graph->push_back(PriorFactor<Pose2>(0, priorMean, priorNoise));
// Single Step Optimization using Levenberg-Marquardt

8
examples/Pose2SLAMExample_graphviz.cpp
View File

@ -16,11 +16,11 @@
* @author Frank Dellaert
*/
// For an explanation of headers below, please see Pose2SLAMExample.cpp
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <fstream>
using namespace std;
@ -32,11 +32,11 @@ int main (int argc, char** argv) {
NonlinearFactorGraph graph;
// 2a. Add a prior on the first pose, setting it to the origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, Pose2(0, 0, 0), priorNoise));
// For simplicity, we will use the same noise model for odometry and loop closures
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// 2b. Add odometry factors
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2, 0, 0 ), model));

3
examples/Pose2SLAMExample_lago.cpp
View File

@ -22,6 +22,7 @@
#include <gtsam/slam/lago.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <fstream>
using namespace std;
@ -43,7 +44,7 @@ int main(const int argc, const char *argv[]) {
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
noiseModel::Diagonal::Variances(Vector3(1e-6, 1e-6, 1e-8));
graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
graphWithPrior.print();

75
examples/Pose2SLAMwSPCG.cpp
View File

@ -16,47 +16,15 @@
* @date June 2, 2012
*/
/**
* A simple 2D pose slam example solved using a Conjugate-Gradient method
* - The robot moves in a 2 meter square
* - The robot moves 2 meters each step, turning 90 degrees after each step
* - The robot initially faces along the X axis (horizontal, to the right in 2D)
* - We have full odometry between pose
* - We have a loop closure constraint when the robot returns to the first position
*/
// As this is a planar SLAM example, we will use Pose2 variables (x, y, theta) to represent
// the robot positions
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Point2.h>
// Each variable in the system (poses) must be identified with a unique key.
// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
// Here we will use simple integer keys
#include <gtsam/inference/Key.h>
// In GTSAM, measurement functions are represented as 'factors'. Several common factors
// have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
// Here we will use Between factors for the relative motion described by odometry measurements.
// We will also use a Between Factor to encode the loop closure constraint
// Also, we will initialize the robot at the origin using a Prior factor.
// For an explanation of headers below, please see Pose2SLAMExample.cpp
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
// nonlinear functions around an initial linearization point, then solve the linear system
// to update the linearization point. This happens repeatedly until the solver converges
// to a consistent set of variable values. This requires us to specify an initial guess
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
#include <gtsam/linear/SubgraphSolver.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
// In contrast to that example, however, we will use a PCG solver here
#include <gtsam/linear/SubgraphSolver.h>
using namespace std;
using namespace gtsam;
@ -66,32 +34,24 @@ int main(int argc, char** argv) {
NonlinearFactorGraph graph;
// 2a. Add a prior on the first pose, setting it to the origin
// A prior factor consists of a mean and a noise model (covariance matrix)
Pose2 prior(0.0, 0.0, 0.0); // prior at origin
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.3, 0.3, 0.1));
noiseModel::Diagonal::shared_ptr priorNoise = noiseModel::Diagonal::Sigmas(Vector3(0.3, 0.3, 0.1));
graph.push_back(PriorFactor<Pose2>(1, prior, priorNoise));
// 2b. Add odometry factors
// For simplicity, we will use the same noise model for each odometry factor
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(3, 4, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(4, 5, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
// 2c. Add the loop closure constraint
// This factor encodes the fact that we have returned to the same pose. In real systems,
// these constraints may be identified in many ways, such as appearance-based techniques
// with camera images.
// We will use another Between Factor to enforce this constraint, with the distance set to zero,
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.2, 0.2, 0.1));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
graph.push_back(BetweenFactor<Pose2>(5, 1, Pose2(0.0, 0.0, 0.0), model));
graph.print("\nFactor Graph:\n"); // print
// 3. Create the data structure to hold the initialEstimate estimate to the solution
// For illustrative purposes, these have been deliberately set to incorrect values
Values initialEstimate;
initialEstimate.insert(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(2, Pose2(2.3, 0.1, 1.1));
@ -104,15 +64,18 @@ int main(int argc, char** argv) {
LevenbergMarquardtParams parameters;
parameters.verbosity = NonlinearOptimizerParams::ERROR;
parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
{
parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
Values result = optimizer.optimize();
result.print("Final Result:\n");
cout << "subgraph solver final error = " << graph.error(result) << endl;
}
// LM is still the outer optimization loop, but by specifying "Iterative" below
// We indicate that an iterative linear solver should be used.
// In addition, the *type* of the iterativeParams decides on the type of
// iterative solver, in this case the SPCG (subgraph PCG)
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
Values result = optimizer.optimize();
result.print("Final Result:\n");
cout << "subgraph solver final error = " << graph.error(result) << endl;
return 0;
}

4
examples/Pose3SLAMExample_changeKeys.cpp
View File

@ -56,7 +56,7 @@ int main(const int argc, const char *argv[]) {
std::cout << "Rewriting input to file: " << inputFileRewritten << std::endl;
// Additional: rewrite input with simplified keys 0,1,...
Values simpleInitial;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
for(const Values::ConstKeyValuePair& key_value: *initial) {
Key key;
if(add)
key = key_value.key + firstKey;
@ -66,7 +66,7 @@ int main(const int argc, const char *argv[]) {
simpleInitial.insert(key, initial->at(key_value.key));
}
NonlinearFactorGraph simpleGraph;
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, *graph) {
for(const boost::shared_ptr<NonlinearFactor>& factor: *graph) {
boost::shared_ptr<BetweenFactor<Pose3> > pose3Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose3> >(factor);
if (pose3Between){

4
examples/Pose3SLAMExample_g2o.cpp
View File

@ -43,9 +43,9 @@ int main(const int argc, const char *argv[]) {
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));

4
examples/Pose3SLAMExample_initializePose3Chordal.cpp
View File

@ -43,9 +43,9 @@ int main(const int argc, const char *argv[]) {
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));

4
examples/Pose3SLAMExample_initializePose3Gradient.cpp
View File

@ -43,9 +43,9 @@ int main(const int argc, const char *argv[]) {
// Add prior on the first key
NonlinearFactorGraph graphWithPrior = *graph;
noiseModel::Diagonal::shared_ptr priorModel = //
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4));
noiseModel::Diagonal::Variances((Vector(6) << 1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4).finished());
Key firstKey = 0;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, *initial) {
for(const Values::ConstKeyValuePair& key_value: *initial) {
std::cout << "Adding prior to g2o file " << std::endl;
firstKey = key_value.key;
graphWithPrior.add(PriorFactor<Pose3>(firstKey, Pose3(), priorModel));

37
examples/README
View File

@ -1,37 +0,0 @@
This directory contains a number of examples that illustrate the use of GTSAM:
SimpleRotation: a super-simple example of optimizing a single rotation according to a single prior
Kalman Filter Examples
======================
elaboratePoint2KalmanFilter: simple linear Kalman filter on a moving 2D point, but done using factor graphs
easyPoint2KalmanFilter: uses the cool generic templated Kalman filter class to do the same
fullStateKalmanFilter: simple 1D example with a full-state filter
errorStateKalmanFilter: simple 1D example of a moving target measured by a accelerometer, incl. drift-rate bias
2D Pose SLAM
============
LocalizationExample.cpp: modeling robot motion
LocalizationExample2.cpp: example with GPS like measurements
Pose2SLAMExample: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h
Pose2SLAMExample_advanced: same, but uses an Optimizer object
Pose2SLAMwSPCG: solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
Planar SLAM with landmarks
==========================
PlanarSLAMExample: simple robotics example using the pre-built planar SLAM domain
PlanarSLAMExample_selfcontained: simple robotics example with all typedefs internal to this script.
Visual SLAM
===========
CameraResectioning.cpp: An example of gtsam for solving the camera resectioning problem
The directory vSLAMexample includes 2 simple examples using GTSAM:
- vSFMexample using visualSLAM in for structure-from-motion (SFM), and
- vISAMexample using visualSLAM and ISAM for incremental SLAM updates
See the separate README file there.
Undirected Graphical Models (UGM)
=================================
The best representation for a Markov Random Field is a factor graph :-)
This is illustrated with some discrete examples from the UGM MATLAB toolbox, which
can be found at http://www.di.ens.fr/~mschmidt/Software/UGM

96
examples/README.md Normal file
View File

@ -0,0 +1,96 @@
# GTSAM Examples
This directory contains all GTSAM C++ examples GTSAM pertaining to SFM
## Basic Examples:
* **SimpleRotation**: a simple example of optimizing a single rotation according to a single prior
* **CameraResectioning**: resection camera from some known points
* **SFMExample**: basic structure from motion
* **SFMExample_bal**: same, but read data from read from BAL file
* **SelfCalibrationExample**: Do SFM while also optimizing for calibration
## Stereo Visual Odometry Examples
Visual odometry using a stereo rig:
* **StereoVOExample**: basic example of stereo VO
* **StereoVOExample_large**: larger, with a snippet of Kitti data
## More Advanced Examples
The following examples illustrate some concepts from Georgia Tech's research papers, listed in the references section at the end:
* **VisualISAMExample**: uses iSAM [TRO08]
* **VisualISAM2Example**: uses iSAM2 [IJRR12]
* **SFMExample_SmartFactor**: uses smartFactors [ICRA14]
## Kalman Filter Examples
* **elaboratePoint2KalmanFilter**: simple linear Kalman filter on a moving 2D point, but done using factor graphs
* **easyPoint2KalmanFilter**: uses the generic templated Kalman filter class to do the same
* **fullStateKalmanFilter**: simple 1D example with a full-state filter
* **errorStateKalmanFilter**: simple 1D example of a moving target measured by a accelerometer, incl. drift-rate bias
## 2D Pose SLAM
* **LocalizationExample.cpp**: modeling robot motion
* **LocalizationExample2.cpp**: example with GPS like measurements
* **Pose2SLAMExample**: A 2D Pose SLAM example using the predefined typedefs in gtsam/slam/pose2SLAM.h
* **Pose2SLAMExample_advanced**: same, but uses an Optimizer object
* **Pose2SLAMwSPCG**: solve a simple 3 by 3 grid of Pose2 SLAM problem by using easy SPCG interface
## Planar SLAM with landmarks
* **PlanarSLAMExample**: simple robotics example using the pre-built planar SLAM domain
* **PlanarSLAMExample_selfcontained**: simple robotics example with all typedefs internal to this script.
## Visual SLAM
The directory **vSLAMexample** includes 2 simple examples using GTSAM:
- **vSFMexample** using visual SLAM for structure-from-motion (SFM)
- **vISAMexample** using visual SLAM and ISAM for incremental SLAM updates
See the separate README file there.
##Undirected Graphical Models (UGM)
The best representation for a Markov Random Field is a factor graph :-) This is illustrated with some discrete examples from the UGM MATLAB toolbox, which
can be found at <http://www.di.ens.fr/~mschmidt/Software/UGM>
##Building and Running
To build, cd into the directory and do:
```
mkdir build
cd build
cmake ..
```
For each .cpp file in this directory two make targets are created, one to build the executable, and one to build and run it. For example, the file `CameraResectioning.cpp` contains simple example to resection a camera from 4 known points. You can build it using
```
make CameraResectioning
```
or build and run it immediately with
```
make CameraResectioning.run
```
which should output:
```
Final result:
Values with 1 values:
Value x1: R:
[
1, 0.0, 0.0,
0.0, -1, 0.0,
0.0, 0.0, -1,
];
t: [0, 0, 2]';
```
## References
- [TRO08]: [iSAM: Incremental Smoothing and Mapping, Michael Kaess](http://frank.dellaert.com/pub/Kaess08tro.pdf), Michael Kaess, Ananth Ranganathan, and Frank Dellaert, IEEE Transactions on Robotics, 2008
- [IJRR12]: [iSAM2: Incremental Smoothing and Mapping Using the Bayes Tree](http://www.cc.gatech.edu/~dellaert/pub/Kaess12ijrr.pdf), Michael Kaess, Hordur Johannsson, Richard Roberts, Viorela Ila, John Leonard, and Frank Dellaert, International Journal of Robotics Research, 2012
- [ICRA14]: [Eliminating Conditionally Independent Sets in Factor Graphs: A Unifying Perspective based on Smart Factors](http://frank.dellaert.com/pub/Carlone14icra.pdf), Luca Carlone, Zsolt Kira, Chris Beall, Vadim Indelman, and Frank Dellaert, IEEE International Conference on Robotics and Automation (ICRA), 2014

10
examples/RangeISAMExample_plaza2.cpp
View File

@ -39,11 +39,10 @@
// have been provided with the library for solving robotics SLAM problems.
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/RangeFactor.h>
#include <gtsam/sam/RangeFactor.h>
#include <gtsam/slam/dataset.h>
// Standard headers, added last, so we know headers above work on their own
#include <boost/foreach.hpp>
#include <fstream>
#include <iostream>
@ -82,7 +81,8 @@ vector<RangeTriple> readTriples() {
ifstream is(data_file.c_str());
while (is) {
double t, sender, receiver, range;
double t, sender, range;
size_t receiver;
is >> t >> sender >> receiver >> range;
triples.push_back(RangeTriple(t, receiver, range));
}
@ -150,7 +150,7 @@ int main (int argc, char** argv) {
// Loop over odometry
gttic_(iSAM);
BOOST_FOREACH(const TimedOdometry& timedOdometry, odometry) {
for(const TimedOdometry& timedOdometry: odometry) {
//--------------------------------- odometry loop -----------------------------------------
double t;
Pose2 odometry;
@ -195,7 +195,7 @@ int main (int argc, char** argv) {
}
i += 1;
//--------------------------------- odometry loop -----------------------------------------
} // BOOST_FOREACH
} // end for
gttoc_(iSAM);
// Print timings

18
examples/SFMExample.cpp
View File

@ -15,13 +15,7 @@
* @author Duy-Nguyen Ta
*/
/**
* A structure-from-motion example with landmarks
* - The landmarks form a 10 meter cube
* - The robot rotates around the landmarks, always facing towards the cube
*/
// For loading the data
// For loading the data, see the comments therein for scenario (camera rotates around cube)
#include "SFMdata.h"
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
@ -80,13 +74,13 @@ int main(int argc, char* argv[]) {
NonlinearFactorGraph graph;
// Add a prior on pose x1. This indirectly specifies where the origin is.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) {
SimpleCamera camera(poses[i], *K);
for (size_t j = 0; j < points.size(); ++j) {
SimpleCamera camera(poses[i], *K);
Point2 measurement = camera.project(points[j]);
graph.push_back(GenericProjectionFactor<Pose3, Point3, Cal3_S2>(measurement, measurementNoise, Symbol('x', i), Symbol('l', j), K));
}
@ -103,14 +97,16 @@ int main(int argc, char* argv[]) {
// Intentionally initialize the variables off from the ground truth
Values initialEstimate;
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
initialEstimate.print("Initial Estimates:\n");
/* Optimize the graph and print results */
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
result.print("Final results:\n");
cout << "initial error = " << graph.error(initialEstimate) << endl;
cout << "final error = " << graph.error(result) << endl;
return 0;
}

101
examples/SFMExampleExpressions.cpp Normal file
View File

@ -0,0 +1,101 @@
/* ----------------------------------------------------------------------------
* 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 SFMExampleExpressions.cpp
* @brief A structure-from-motion example done with Expressions
* @author Frank Dellaert
* @author Duy-Nguyen Ta
* @date October 1, 2014
*/
/**
* This is the Expression version of SFMExample
* See detailed description of headers there, this focuses on explaining the AD part
*/
// The two new headers that allow using our Automatic Differentiation Expression framework
#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
// Header order is close to far
#include <examples/SFMdata.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/nonlinear/DoglegOptimizer.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/inference/Symbol.h>
#include <vector>
using namespace std;
using namespace gtsam;
using namespace noiseModel;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
Cal3_S2 K(50.0, 50.0, 0.0, 50.0, 50.0);
Isotropic::shared_ptr measurementNoise = Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks and poses
vector<Point3> points = createPoints();
vector<Pose3> poses = createPoses();
// Create a factor graph
ExpressionFactorGraph graph;
// Specify uncertainty on first pose prior
Vector6 sigmas; sigmas << Vector3(0.3,0.3,0.3), Vector3(0.1,0.1,0.1);
Diagonal::shared_ptr poseNoise = Diagonal::Sigmas(sigmas);
// Here we don't use a PriorFactor but directly the ExpressionFactor class
// x0 is an Expression, and we create a factor wanting it to be equal to poses[0]
Pose3_ x0('x',0);
graph.addExpressionFactor(x0, poses[0], poseNoise);
// We create a constant Expression for the calibration here
Cal3_S2_ cK(K);
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t i = 0; i < poses.size(); ++i) {
Pose3_ x('x', i);
SimpleCamera camera(poses[i], K);
for (size_t j = 0; j < points.size(); ++j) {
Point2 measurement = camera.project(points[j]);
// Below an expression for the prediction of the measurement:
Point3_ p('l', j);
Point2_ prediction = uncalibrate(cK, project(transform_to(x, p)));
// Again, here we use an ExpressionFactor
graph.addExpressionFactor(prediction, measurement, measurementNoise);
}
}
// Add prior on first point to constrain scale, again with ExpressionFactor
Isotropic::shared_ptr pointNoise = Isotropic::Sigma(3, 0.1);
graph.addExpressionFactor(Point3_('l', 0), points[0], pointNoise);
// Create perturbed initial
Values initial;
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
initial.insert(Symbol('x', i), poses[i].compose(delta));
for (size_t j = 0; j < points.size(); ++j)
initial.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
cout << "initial error = " << graph.error(initial) << endl;
/* Optimize the graph and print results */
Values result = DoglegOptimizer(graph, initial).optimize();
cout << "final error = " << graph.error(result) << endl;
return 0;
}
/* ************************************************************************* */

121
examples/SFMExampleExpressions_bal.cpp Normal file
View File

@ -0,0 +1,121 @@
/* ----------------------------------------------------------------------------
* 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 SFMExampleExpressions_bal.cpp
* @brief A structure-from-motion example done with Expressions
* @author Frank Dellaert
* @date January 2015
*/
/**
* This is the Expression version of SFMExample
* See detailed description of headers there, this focuses on explaining the AD part
*/
// The two new headers that allow using our Automatic Differentiation Expression framework
#include <gtsam/slam/expressions.h>
#include <gtsam/nonlinear/ExpressionFactorGraph.h>
// Header order is close to far
#include <gtsam/inference/Symbol.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <vector>
using namespace std;
using namespace gtsam;
using namespace noiseModel;
using symbol_shorthand::C;
using symbol_shorthand::P;
// An SfM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
// and has a total of 9 free parameters
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Find default file, but if an argument is given, try loading a file
string filename = findExampleDataFile("dubrovnik-3-7-pre");
if (argc > 1)
filename = string(argv[1]);
// Load the SfM data from file
SfM_data mydata;
readBAL(filename, mydata);
cout
<< boost::format("read %1% tracks on %2% cameras\n")
% mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph
ExpressionFactorGraph graph;
// Here we don't use a PriorFactor but directly the ExpressionFactor class
// First, we create an expression to the pose from the first camera
Expression<SfM_Camera> camera0_(C(0));
// Then, to get its pose:
Pose3_ pose0_(&SfM_Camera::getPose, camera0_);
// Finally, we say it should be equal to first guess
graph.addExpressionFactor(pose0_, mydata.cameras[0].pose(),
noiseModel::Isotropic::Sigma(6, 0.1));
// similarly, we create a prior on the first point
Point3_ point0_(P(0));
graph.addExpressionFactor(point0_, mydata.tracks[0].p,
noiseModel::Isotropic::Sigma(3, 0.1));
// We share *one* noiseModel between all projection factors
noiseModel::Isotropic::shared_ptr noise = noiseModel::Isotropic::Sigma(2,
1.0); // one pixel in u and v
// Simulated measurements from each camera pose, adding them to the factor graph
size_t j = 0;
for(const SfM_Track& track: mydata.tracks) {
// Leaf expression for j^th point
Point3_ point_('p', j);
for(const SfM_Measurement& m: track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
// Leaf expression for i^th camera
Expression<SfM_Camera> camera_(C(i));
// Below an expression for the prediction of the measurement:
Point2_ predict_ = project2<SfM_Camera>(camera_, point_);
// Again, here we use an ExpressionFactor
graph.addExpressionFactor(predict_, uv, noise);
}
j += 1;
}
// Create initial estimate
Values initial;
size_t i = 0;
j = 0;
for(const SfM_Camera& camera: mydata.cameras)
initial.insert(C(i++), camera);
for(const SfM_Track& track: mydata.tracks)
initial.insert(P(j++), track.p);
/* Optimize the graph and print results */
Values result;
try {
LevenbergMarquardtParams params;
params.setVerbosity("ERROR");
LevenbergMarquardtOptimizer lm(graph, initial, params);
result = lm.optimize();
} catch (exception& e) {
cout << e.what();
}
cout << "final error: " << graph.error(result) << endl;
return 0;
}
/* ************************************************************************* */

78
examples/SFMExample_SmartFactor.cpp
View File

@ -17,52 +17,25 @@
* @author Frank Dellaert
*/
/**
* A structure-from-motion example with landmarks
* - The landmarks form a 10 meter cube
* - The robot rotates around the landmarks, always facing towards the cube
*/
// For loading the data
#include "SFMdata.h"
// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
#include <gtsam/geometry/Point2.h>
// In GTSAM, measurement functions are represented as 'factors'.
// The factor we used here is SmartProjectionPoseFactor. Every smart factor represent a single landmark,
// The SmartProjectionPoseFactor only optimize the pose of camera, not the calibration,
// The calibration should be known.
// The factor we used here is SmartProjectionPoseFactor.
// Every smart factor represent a single landmark, seen from multiple cameras.
// The SmartProjectionPoseFactor only optimizes for the poses of a camera,
// not the calibration, which is assumed known.
#include <gtsam/slam/SmartProjectionPoseFactor.h>
// Also, we will initialize the robot at some location using a Prior factor.
#include <gtsam/slam/PriorFactor.h>
// When the factors are created, we will add them to a Factor Graph. As the factors we are using
// are nonlinear factors, we will need a Nonlinear Factor Graph.
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
// Finally, once all of the factors have been added to our factor graph, we will want to
// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
// trust-region method known as Powell's Degleg
#include <gtsam/nonlinear/DoglegOptimizer.h>
// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
// nonlinear functions around an initial linearization point, then solve the linear system
// to update the linearization point. This happens repeatedly until the solver converges
// to a consistent set of variable values. This requires us to specify an initial guess
// for each variable, held in a Values container.
#include <gtsam/nonlinear/Values.h>
#include <vector>
// For an explanation of these headers, see SFMExample.cpp
#include "SFMdata.h"
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
using namespace std;
using namespace gtsam;
// Make the typename short so it looks much cleaner
typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3,
gtsam::Cal3_S2> SmartFactor;
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
// create a typedef to the camera type
typedef PinholePose<Cal3_S2> Camera;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
@ -85,12 +58,12 @@ int main(int argc, char* argv[]) {
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor());
SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
for (size_t i = 0; i < poses.size(); ++i) {
// generate the 2D measurement
SimpleCamera camera(poses[i], *K);
Camera camera(poses[i], K);
Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we need to add:
@ -98,7 +71,7 @@ int main(int argc, char* argv[]) {
// 2. the corresponding camera's key
// 3. camera noise model
// 4. camera calibration
smartfactor->add(measurement, i, measurementNoise, K);
smartfactor->add(measurement, i);
}
// insert the smart factor in the graph
@ -107,28 +80,29 @@ int main(int argc, char* argv[]) {
// Add a prior on pose x0. This indirectly specifies where the origin is.
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)));
graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Pose3>(0, poses[0], noise));
// Because the structure-from-motion problem has a scale ambiguity, the problem is
// still under-constrained. Here we add a prior on the second pose x1, so this will
// fix the scale by indicating the distance between x0 and x1.
// Because these two are fixed, the rest of the poses will be also be fixed.
graph.push_back(PriorFactor<Pose3>(1, poses[1], poseNoise)); // add directly to graph
graph.push_back(PriorFactor<Pose3>(1, poses[1], noise)); // add directly to graph
graph.print("Factor Graph:\n");
// Create the initial estimate to the solution
// Intentionally initialize the variables off from the ground truth
Values initialEstimate;
Pose3 delta(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(i, poses[i].compose(delta));
initialEstimate.print("Initial Estimates:\n");
// Optimize the graph and print results
Values result = DoglegOptimizer(graph, initialEstimate).optimize();
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate);
Values result = optimizer.optimize();
result.print("Final results:\n");
// A smart factor represent the 3D point inside the factor, not as a variable.
@ -137,20 +111,20 @@ int main(int argc, char* argv[]) {
Values landmark_result;
for (size_t j = 0; j < points.size(); ++j) {
// The output of point() is in boost::optional<gtsam::Point3>, as sometimes
// the triangulation operation inside smart factor will encounter degeneracy.
boost::optional<Point3> point;
// The graph stores Factor shared_ptrs, so we cast back to a SmartFactor first
SmartFactor::shared_ptr smart = boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) {
point = smart->point(result);
// The output of point() is in boost::optional<Point3>, as sometimes
// the triangulation operation inside smart factor will encounter degeneracy.
boost::optional<Point3> point = smart->point(result);
if (point) // ignore if boost::optional return NULL
landmark_result.insert(j, *point);
}
}
landmark_result.print("Landmark results:\n");
cout << "final error: " << graph.error(result) << endl;
cout << "number of iterations: " << optimizer.iterations() << endl;
return 0;
}

129
examples/SFMExample_SmartFactorPCG.cpp Normal file
View File

@ -0,0 +1,129 @@
/* ----------------------------------------------------------------------------
* 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 SFMExample_SmartFactorPCG.cpp
* @brief Version of SFMExample_SmartFactor that uses Preconditioned Conjugate Gradient
* @author Frank Dellaert
*/
// For an explanation of these headers, see SFMExample_SmartFactor.cpp
#include "SFMdata.h"
#include <gtsam/slam/SmartProjectionPoseFactor.h>
// These extra headers allow us a LM outer loop with PCG linear solver (inner loop)
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/linear/Preconditioner.h>
#include <gtsam/linear/PCGSolver.h>
using namespace std;
using namespace gtsam;
// Make the typename short so it looks much cleaner
typedef SmartProjectionPoseFactor<Cal3_S2> SmartFactor;
// create a typedef to the camera type
typedef PinholePose<Cal3_S2> Camera;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
// Define the camera calibration parameters
Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
// Define the camera observation noise model
noiseModel::Isotropic::shared_ptr measurementNoise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Create the set of ground-truth landmarks and poses
vector<Point3> points = createPoints();
vector<Pose3> poses = createPoses();
// Create a factor graph
NonlinearFactorGraph graph;
// Simulated measurements from each camera pose, adding them to the factor graph
for (size_t j = 0; j < points.size(); ++j) {
// every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
SmartFactor::shared_ptr smartfactor(new SmartFactor(measurementNoise, K));
for (size_t i = 0; i < poses.size(); ++i) {
// generate the 2D measurement
Camera camera(poses[i], K);
Point2 measurement = camera.project(points[j]);
// call add() function to add measurement into a single factor, here we need to add:
smartfactor->add(measurement, i);
}
// insert the smart factor in the graph
graph.push_back(smartfactor);
}
// Add a prior on pose x0. This indirectly specifies where the origin is.
// 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr noise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Pose3>(0, poses[0], noise));
// Fix the scale ambiguity by adding a prior
graph.push_back(PriorFactor<Pose3>(1, poses[0], noise));
// Create the initial estimate to the solution
Values initialEstimate;
Pose3 delta(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(i, poses[i].compose(delta));
// We will use LM in the outer optimization loop, but by specifying "Iterative" below
// We indicate that an iterative linear solver should be used.
// In addition, the *type* of the iterativeParams decides on the type of
// iterative solver, in this case the SPCG (subgraph PCG)
LevenbergMarquardtParams parameters;
parameters.linearSolverType = NonlinearOptimizerParams::Iterative;
parameters.absoluteErrorTol = 1e-10;
parameters.relativeErrorTol = 1e-10;
parameters.maxIterations = 500;
PCGSolverParameters::shared_ptr pcg =
boost::make_shared<PCGSolverParameters>();
pcg->preconditioner_ =
boost::make_shared<BlockJacobiPreconditionerParameters>();
// Following is crucial:
pcg->setEpsilon_abs(1e-10);
pcg->setEpsilon_rel(1e-10);
parameters.iterativeParams = pcg;
LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
Values result = optimizer.optimize();
// Display result as in SFMExample_SmartFactor.run
result.print("Final results:\n");
Values landmark_result;
for (size_t j = 0; j < points.size(); ++j) {
SmartFactor::shared_ptr smart = //
boost::dynamic_pointer_cast<SmartFactor>(graph[j]);
if (smart) {
boost::optional<Point3> point = smart->point(result);
if (point) // ignore if boost::optional return NULL
landmark_result.insert(j, *point);
}
}
landmark_result.print("Landmark results:\n");
cout << "final error: " << graph.error(result) << endl;
cout << "number of iterations: " << optimizer.iterations() << endl;
return 0;
}
/* ************************************************************************* */

10
examples/SFMExample_bal.cpp
View File

@ -43,7 +43,7 @@ int main (int argc, char* argv[]) {
// Load the SfM data from file
SfM_data mydata;
assert(readBAL(filename, mydata));
readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph
@ -55,8 +55,8 @@ int main (int argc, char* argv[]) {
// Add measurements to the factor graph
size_t j = 0;
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) {
BOOST_FOREACH(const SfM_Measurement& m, track.measurements) {
for(const SfM_Track& track: mydata.tracks) {
for(const SfM_Measurement& m: track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
@ -72,8 +72,8 @@ int main (int argc, char* argv[]) {
// Create initial estimate
Values initial;
size_t i = 0; j = 0;
BOOST_FOREACH(const SfM_Camera& camera, mydata.cameras) initial.insert(C(i++), camera);
BOOST_FOREACH(const SfM_Track& track, mydata.tracks) initial.insert(P(j++), track.p);
for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera);
for(const SfM_Track& track: mydata.tracks) initial.insert(P(j++), track.p);
/* Optimize the graph and print results */
Values result;

144
examples/SFMExample_bal_COLAMD_METIS.cpp Normal file
View File

@ -0,0 +1,144 @@
/* ----------------------------------------------------------------------------
* 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 SFMExample.cpp
* @brief This file is to compare the ordering performance for COLAMD vs METIS.
* Example problem is to solve a structure-from-motion problem from a "Bundle Adjustment in the Large" file.
* @author Frank Dellaert, Zhaoyang Lv
*/
// For an explanation of headers, see SFMExample.cpp
#include <gtsam/inference/Symbol.h>
#include <gtsam/inference/Ordering.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/GeneralSFMFactor.h>
#include <gtsam/slam/dataset.h> // for loading BAL datasets !
#include <gtsam/base/timing.h>
#include <vector>
using namespace std;
using namespace gtsam;
using symbol_shorthand::C;
using symbol_shorthand::P;
// We will be using a projection factor that ties a SFM_Camera to a 3D point.
// An SFM_Camera is defined in datase.h as a camera with unknown Cal3Bundler calibration
// and has a total of 9 free parameters
typedef GeneralSFMFactor<SfM_Camera,Point3> MyFactor;
/* ************************************************************************* */
int main (int argc, char* argv[]) {
// Find default file, but if an argument is given, try loading a file
string filename = findExampleDataFile("dubrovnik-3-7-pre");
if (argc>1) filename = string(argv[1]);
// Load the SfM data from file
SfM_data mydata;
readBAL(filename, mydata);
cout << boost::format("read %1% tracks on %2% cameras\n") % mydata.number_tracks() % mydata.number_cameras();
// Create a factor graph
NonlinearFactorGraph graph;
// We share *one* noiseModel between all projection factors
noiseModel::Isotropic::shared_ptr noise =
noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
// Add measurements to the factor graph
size_t j = 0;
for(const SfM_Track& track: mydata.tracks) {
for(const SfM_Measurement& m: track.measurements) {
size_t i = m.first;
Point2 uv = m.second;
graph.push_back(MyFactor(uv, noise, C(i), P(j))); // note use of shorthand symbols C and P
}
j += 1;
}
// Add a prior on pose x1. This indirectly specifies where the origin is.
// and a prior on the position of the first landmark to fix the scale
graph.push_back(PriorFactor<SfM_Camera>(C(0), mydata.cameras[0], noiseModel::Isotropic::Sigma(9, 0.1)));
graph.push_back(PriorFactor<Point3> (P(0), mydata.tracks[0].p, noiseModel::Isotropic::Sigma(3, 0.1)));
// Create initial estimate
Values initial;
size_t i = 0; j = 0;
for(const SfM_Camera& camera: mydata.cameras) initial.insert(C(i++), camera);
for(const SfM_Track& track: mydata.tracks) initial.insert(P(j++), track.p);
/** --------------- COMPARISON -----------------------**/
/** ----------------------------------------------------**/
LevenbergMarquardtParams params_using_COLAMD, params_using_METIS;
try {
params_using_METIS.setVerbosity("ERROR");
gttic_(METIS_ORDERING);
params_using_METIS.ordering = Ordering::Create(Ordering::METIS, graph);
gttoc_(METIS_ORDERING);
params_using_COLAMD.setVerbosity("ERROR");
gttic_(COLAMD_ORDERING);
params_using_COLAMD.ordering = Ordering::Create(Ordering::COLAMD, graph);
gttoc_(COLAMD_ORDERING);
} catch (exception& e) {
cout << e.what();
}
// expect they have different ordering results
if(params_using_COLAMD.ordering == params_using_METIS.ordering) {
cout << "COLAMD and METIS produce the same ordering. "
<< "Problem here!!!" << endl;
}
/* Optimize the graph with METIS and COLAMD and time the results */
Values result_METIS, result_COLAMD;
try {
gttic_(OPTIMIZE_WITH_METIS);
LevenbergMarquardtOptimizer lm_METIS(graph, initial, params_using_METIS);
result_METIS = lm_METIS.optimize();
gttoc_(OPTIMIZE_WITH_METIS);
gttic_(OPTIMIZE_WITH_COLAMD);
LevenbergMarquardtOptimizer lm_COLAMD(graph, initial, params_using_COLAMD);
result_COLAMD = lm_COLAMD.optimize();
gttoc_(OPTIMIZE_WITH_COLAMD);
} catch (exception& e) {
cout << e.what();
}
{ // printing the result
cout << "COLAMD final error: " << graph.error(result_COLAMD) << endl;
cout << "METIS final error: " << graph.error(result_METIS) << endl;
cout << endl << endl;
cout << "Time comparison by solving " << filename << " results:" << endl;
cout << boost::format("%1% point tracks and %2% cameras\n") \
% mydata.number_tracks() % mydata.number_cameras() \
<< endl;
tictoc_print_();
}
return 0;
}
/* ************************************************************************* */

8
examples/SelfCalibrationExample.cpp
View File

@ -53,7 +53,7 @@ int main(int argc, char* argv[]) {
NonlinearFactorGraph graph;
// Add a prior on pose x1.
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Simulated measurements from each camera pose, adding them to the factor graph
@ -74,7 +74,7 @@ int main(int argc, char* argv[]) {
graph.push_back(PriorFactor<Point3>(Symbol('l', 0), points[0], pointNoise)); // add directly to graph
// Add a prior on the calibration.
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100));
noiseModel::Diagonal::shared_ptr calNoise = noiseModel::Diagonal::Sigmas((Vector(5) << 500, 500, 0.1, 100, 100).finished());
graph.push_back(PriorFactor<Cal3_S2>(Symbol('K', 0), K, calNoise));
// Create the initial estimate to the solution
@ -82,9 +82,9 @@ int main(int argc, char* argv[]) {
Values initialEstimate;
initialEstimate.insert(Symbol('K', 0), Cal3_S2(60.0, 60.0, 0.0, 45.0, 45.0));
for (size_t i = 0; i < poses.size(); ++i)
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.insert<Point3>(Symbol('l', j), points[j] + Point3(-0.25, 0.20, 0.15));
/* Optimize the graph and print results */
Values result = DoglegOptimizer(graph, initialEstimate).optimize();

76
examples/SolverComparer.cpp
View File

@ -31,28 +31,31 @@
*
*/
#include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/BearingRangeFactor.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/sam/BearingRangeFactor.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/nonlinear/ISAM2.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/linear/GaussianJunctionTree.h>
#include <gtsam/linear/GaussianEliminationTree.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/timing.h>
#include <gtsam/base/treeTraversal-inst.h>
#include <gtsam/config.h> // for GTSAM_USE_TBB
#include <boost/archive/binary_iarchive.hpp>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/program_options.hpp>
#include <boost/range/algorithm/set_algorithm.hpp>
#include <boost/range/adaptor/reversed.hpp>
#include <boost/random.hpp>
#include <boost/serialization/export.hpp>
#include <fstream>
#include <iostream>
#include <boost/archive/binary_oarchive.hpp>
#include <boost/archive/binary_iarchive.hpp>
#include <boost/serialization/export.hpp>
#include <boost/program_options.hpp>
#include <boost/range/algorithm/set_algorithm.hpp>
#include <boost/random.hpp>
#ifdef GTSAM_USE_TBB
#include <tbb/tbb.h>
@ -72,30 +75,13 @@ typedef NoiseModelFactor1<Pose> NM1;
typedef NoiseModelFactor2<Pose,Pose> NM2;
typedef BearingRangeFactor<Pose,Point2> BR;
BOOST_CLASS_EXPORT(Value);
BOOST_CLASS_EXPORT(Pose);
BOOST_CLASS_EXPORT(Rot2);
BOOST_CLASS_EXPORT(Point2);
BOOST_CLASS_EXPORT(NonlinearFactor);
BOOST_CLASS_EXPORT(NoiseModelFactor);
BOOST_CLASS_EXPORT(NM1);
BOOST_CLASS_EXPORT(NM2);
BOOST_CLASS_EXPORT(BetweenFactor<Pose>);
BOOST_CLASS_EXPORT(PriorFactor<Pose>);
BOOST_CLASS_EXPORT(BR);
BOOST_CLASS_EXPORT(noiseModel::Base);
BOOST_CLASS_EXPORT(noiseModel::Isotropic);
BOOST_CLASS_EXPORT(noiseModel::Gaussian);
BOOST_CLASS_EXPORT(noiseModel::Diagonal);
BOOST_CLASS_EXPORT(noiseModel::Unit);
double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
// Compute degrees of freedom (observations - variables)
// In ocaml, +1 was added to the observations to account for the prior, but
// the factor graph already includes a factor for the prior/equality constraint.
// double dof = graph.size() - config.size();
int graph_dim = 0;
BOOST_FOREACH(const boost::shared_ptr<gtsam::NonlinearFactor>& nlf, graph) {
for(const boost::shared_ptr<gtsam::NonlinearFactor>& nlf: graph) {
graph_dim += (int)nlf->dim();
}
double dof = double(graph_dim) - double(config.dim()); // kaess: changed to dim
@ -269,12 +255,12 @@ void runIncremental()
boost::dynamic_pointer_cast<BetweenFactor<Pose> >(datasetMeasurements[nextMeasurement]))
{
Key key1 = measurement->key1(), key2 = measurement->key2();
if((key1 >= firstStep && key1 < key2) || (key2 >= firstStep && key2 < key1)) {
if(((int)key1 >= firstStep && key1 < key2) || ((int)key2 >= firstStep && key2 < key1)) {
// We found an odometry starting at firstStep
firstPose = std::min(key1, key2);
break;
}
if((key2 >= firstStep && key1 < key2) || (key1 >= firstStep && key2 < key1)) {
if(((int)key2 >= firstStep && key1 < key2) || ((int)key1 >= firstStep && key2 < key1)) {
// We found an odometry joining firstStep with a previous pose
havePreviousPose = true;
firstPose = std::max(key1, key2);
@ -295,7 +281,7 @@ void runIncremental()
NonlinearFactorGraph newFactors;
Values newVariables;
newFactors.push_back(boost::make_shared<PriorFactor<Pose> >(firstPose, Pose(), noiseModel::Unit::Create(Pose::Dim())));
newFactors.push_back(boost::make_shared<PriorFactor<Pose> >(firstPose, Pose(), noiseModel::Unit::Create(3)));
newVariables.insert(firstPose, Pose());
isam2.update(newFactors, newVariables);
@ -303,7 +289,9 @@ void runIncremental()
cout << "Playing forward time steps..." << endl;
for(size_t step = firstPose; nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || step <= lastStep); ++step)
for (size_t step = firstPose;
nextMeasurement < datasetMeasurements.size() && (lastStep == -1 || (int)step <= lastStep);
++step)
{
Values newVariables;
NonlinearFactorGraph newFactors;
@ -434,9 +422,9 @@ void runIncremental()
//try {
// Marginals marginals(graph, values);
// int i=0;
// BOOST_REVERSE_FOREACH(Key key1, values.keys()) {
// for (Key key1: boost::adaptors::reverse(values.keys())) {
// int j=0;
// BOOST_REVERSE_FOREACH(Key key2, values.keys()) {
// for (Key key12: boost::adaptors::reverse(values.keys())) {
// if(i != j) {
// gttic_(jointMarginalInformation);
// std::vector<Key> keys(2);
@ -455,7 +443,7 @@ void runIncremental()
// break;
// }
// tictoc_print_();
// BOOST_FOREACH(Key key, values.keys()) {
// for(Key key: values.keys()) {
// gttic_(marginalInformation);
// Matrix info = marginals.marginalInformation(key);
// gttoc_(marginalInformation);
@ -474,7 +462,7 @@ void runBatch()
cout << "Creating batch optimizer..." << endl;
NonlinearFactorGraph measurements = datasetMeasurements;
measurements.push_back(boost::make_shared<PriorFactor<Pose> >(0, Pose(), noiseModel::Unit::Create(Pose::Dim())));
measurements.push_back(boost::make_shared<PriorFactor<Pose> >(0, Pose(), noiseModel::Unit::Create(3)));
gttic_(Create_optimizer);
GaussNewtonParams params;
@ -548,7 +536,7 @@ void runCompare()
vector<Key> commonKeys;
br::set_intersection(soln1.keys(), soln2.keys(), std::back_inserter(commonKeys));
double maxDiff = 0.0;
BOOST_FOREACH(Key j, commonKeys)
for(Key j: commonKeys)
maxDiff = std::max(maxDiff, soln1.at(j).localCoordinates_(soln2.at(j)).norm());
cout << " Maximum solution difference (norm of logmap): " << maxDiff << endl;
}
@ -562,7 +550,7 @@ void runPerturb()
// Perturb values
VectorValues noise;
BOOST_FOREACH(const Values::KeyValuePair& key_val, initial)
for(const Values::KeyValuePair& key_val: initial)
{
Vector noisev(key_val.value.dim());
for(Vector::Index i = 0; i < noisev.size(); ++i)
@ -589,7 +577,7 @@ void runStats()
{
cout << "Gathering statistics..." << endl;
GaussianFactorGraph linear = *datasetMeasurements.linearize(initial);
GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::COLAMD(linear)));
GaussianJunctionTree jt(GaussianEliminationTree(linear, Ordering::Colamd(linear)));
treeTraversal::ForestStatistics statistics = treeTraversal::GatherStatistics(jt);
ofstream file;

2
examples/StereoVOExample.cpp
View File

@ -67,7 +67,7 @@ int main(int argc, char** argv){
initial_estimate.insert(5, Point3(0, -0.5, 5));
//create Levenberg-Marquardt optimizer for resulting factor graph, optimize
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
LevenbergMarquardtOptimizer optimizer(graph, initial_estimate);
Values result = optimizer.optimize();
result.print("Final result:\n");

4
examples/StereoVOExample_large.cpp
View File

@ -103,7 +103,9 @@ int main(int argc, char** argv){
cout << "Optimizing" << endl;
//create Levenberg-Marquardt optimizer to optimize the factor graph
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
LevenbergMarquardtParams params;
params.orderingType = Ordering::METIS;
LevenbergMarquardtOptimizer optimizer(graph, initial_estimate, params);
Values result = optimizer.optimize();
cout << "Final result sample:" << endl;

22
examples/TimeTBB.cpp
View File

@ -17,8 +17,8 @@
#include <gtsam/global_includes.h>
#include <gtsam/base/Matrix.h>
#include <boost/assign/list_of.hpp>
#include <boost/foreach.hpp>
#include <map>
using namespace std;
@ -75,7 +75,7 @@ map<int, double> testWithoutMemoryAllocation()
const vector<size_t> grainSizes = list_of(1)(10)(100)(1000);
map<int, double> timingResults;
BOOST_FOREACH(size_t grainSize, grainSizes)
for(size_t grainSize: grainSizes)
{
tbb::tick_count t0 = tbb::tick_count::now();
tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithoutAllocation(results));
@ -128,13 +128,13 @@ map<int, double> testWithMemoryAllocation()
const vector<size_t> grainSizes = list_of(1)(10)(100)(1000);
map<int, double> timingResults;
BOOST_FOREACH(size_t grainSize, grainSizes)
for(size_t grainSize: grainSizes)
{
tbb::tick_count t0 = tbb::tick_count::now();
tbb::parallel_for(tbb::blocked_range<size_t>(0, numberOfProblems), WorkerWithAllocation(results));
tbb::tick_count t1 = tbb::tick_count::now();
cout << "With memory allocation, grain size = " << grainSize << ", time = " << (t1 - t0).seconds() << endl;
timingResults[grainSize] = (t1 - t0).seconds();
timingResults[(int)grainSize] = (t1 - t0).seconds();
}
return timingResults;
@ -149,29 +149,29 @@ int main(int argc, char* argv[])
const vector<int> numThreads = list_of(1)(4)(8);
Results results;
BOOST_FOREACH(size_t n, numThreads)
for(size_t n: numThreads)
{
cout << "With " << n << " threads:" << endl;
tbb::task_scheduler_init init(n);
results[n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
results[n].grainSizesWithAllocation = testWithMemoryAllocation();
tbb::task_scheduler_init init((int)n);
results[(int)n].grainSizesWithoutAllocation = testWithoutMemoryAllocation();
results[(int)n].grainSizesWithAllocation = testWithMemoryAllocation();
cout << endl;
}
cout << "Summary of results:" << endl;
BOOST_FOREACH(const Results::value_type& threads_result, results)
for(const Results::value_type& threads_result: results)
{
const int threads = threads_result.first;
const ResultWithThreads& result = threads_result.second;
if(threads != 1)
{
BOOST_FOREACH(const ResultWithThreads::value_type& grainsize_time, result.grainSizesWithoutAllocation)
for(const ResultWithThreads::value_type& grainsize_time: result.grainSizesWithoutAllocation)
{
const int grainsize = grainsize_time.first;
const double speedup = results[1].grainSizesWithoutAllocation[grainsize] / grainsize_time.second;
cout << threads << " threads, without allocation, grain size = " << grainsize << ", speedup = " << speedup << endl;
}
BOOST_FOREACH(const ResultWithThreads::value_type& grainsize_time, result.grainSizesWithAllocation)
for(const ResultWithThreads::value_type& grainsize_time: result.grainSizesWithAllocation)
{
const int grainsize = grainsize_time.first;
const double speedup = results[1].grainSizesWithAllocation[grainsize] / grainsize_time.second;

6
examples/VisualISAM2Example.cpp
View File

@ -95,7 +95,7 @@ int main(int argc, char* argv[]) {
// Add an initial guess for the current pose
// Intentionally initialize the variables off from the ground truth
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
// If this is the first iteration, add a prior on the first pose to set the coordinate frame
// and a prior on the first landmark to set the scale
@ -103,7 +103,7 @@ int main(int argc, char* argv[]) {
// adding it to iSAM.
if( i == 0) {
// Add a prior on pose x0
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3),Vector3::Constant(0.1)).finished()); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Add a prior on landmark l0
@ -113,7 +113,7 @@ int main(int argc, char* argv[]) {
// Add initial guesses to all observed landmarks
// Intentionally initialize the variables off from the ground truth
for (size_t j = 0; j < points.size(); ++j)
initialEstimate.insert(Symbol('l', j), points[j].compose(Point3(-0.25, 0.20, 0.15)));
initialEstimate.insert<Point3>(Symbol('l', j), points[j] +Point3(-0.25, 0.20, 0.15));
} else {
// Update iSAM with the new factors

6
examples/VisualISAMExample.cpp
View File

@ -95,7 +95,7 @@ int main(int argc, char* argv[]) {
}
// Intentionally initialize the variables off from the ground truth
Pose3 noise(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
Pose3 noise(Rot3::Rodrigues(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20));
Pose3 initial_xi = poses[i].compose(noise);
// Add an initial guess for the current pose
@ -108,7 +108,7 @@ int main(int argc, char* argv[]) {
if (i == 0) {
// Add a prior on pose x0, with 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas(
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)));
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.add(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise));
// Add a prior on landmark l0
@ -120,7 +120,7 @@ int main(int argc, char* argv[]) {
Point3 noise(-0.25, 0.20, 0.15);
for (size_t j = 0; j < points.size(); ++j) {
// Intentionally initialize the variables off from the ground truth
Point3 initial_lj = points[j].compose(noise);
Point3 initial_lj = points[j] + noise;
initialEstimate.insert(Symbol('l', j), initial_lj);
}

20
examples/easyPoint2KalmanFilter.cpp
View File

@ -37,7 +37,7 @@ int main() {
// Create the Kalman Filter initialization point
Point2 x_initial(0.0, 0.0);
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1));
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1));
// Create Key for initial pose
Symbol x0('x',0);
@ -57,8 +57,8 @@ int main() {
// For the purposes of this example, let us assume we are using a constant-position model and
// the controls are driving the point to the right at 1 m/s. Then, F = [1 0 ; 0 1], B = [1 0 ; 0 1]
// and u = [1 ; 0]. Let us also assume that the process noise Q = [0.1 0 ; 0 0.1].
Vector u = (Vector(2) << 1.0, 0.0);
SharedDiagonal Q = noiseModel::Diagonal::Sigmas((Vector(2) << 0.1, 0.1), true);
Vector u = Vector2(1.0, 0.0);
SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector2(0.1, 0.1), true);
// This simple motion can be modeled with a BetweenFactor
// Create Key for next pose
@ -70,7 +70,7 @@ int main() {
// Predict the new value with the EKF class
Point2 x1_predict = ekf.predict(factor1);
x1_predict.print("X1 Predict");
traits<Point2>::Print(x1_predict, "X1 Predict");
@ -83,7 +83,7 @@ int main() {
// For the purposes of this example, let us assume we have something like a GPS that returns
// the current position of the robot. Then H = [1 0 ; 0 1]. Let us also assume that the measurement noise
// R = [0.25 0 ; 0 0.25].
SharedDiagonal R = noiseModel::Diagonal::Sigmas((Vector(2) << 0.25, 0.25), true);
SharedDiagonal R = noiseModel::Diagonal::Sigmas(Vector2(0.25, 0.25), true);
// This simple measurement can be modeled with a PriorFactor
Point2 z1(1.0, 0.0);
@ -91,7 +91,7 @@ int main() {
// Update the Kalman Filter with the measurement
Point2 x1_update = ekf.update(factor2);
x1_update.print("X1 Update");
traits<Point2>::Print(x1_update, "X1 Update");
@ -101,13 +101,13 @@ int main() {
difference = Point2(1,0);
BetweenFactor<Point2> factor3(x1, x2, difference, Q);
Point2 x2_predict = ekf.predict(factor1);
x2_predict.print("X2 Predict");
traits<Point2>::Print(x2_predict, "X2 Predict");
// Update
Point2 z2(2.0, 0.0);
PriorFactor<Point2> factor4(x2, z2, R);
Point2 x2_update = ekf.update(factor4);
x2_update.print("X2 Update");
traits<Point2>::Print(x2_update, "X2 Update");
@ -117,13 +117,13 @@ int main() {
difference = Point2(1,0);
BetweenFactor<Point2> factor5(x2, x3, difference, Q);
Point2 x3_predict = ekf.predict(factor5);
x3_predict.print("X3 Predict");
traits<Point2>::Print(x3_predict, "X3 Predict");
// Update
Point2 z3(3.0, 0.0);
PriorFactor<Point2> factor6(x3, z3, R);
Point2 x3_update = ekf.update(factor6);
x3_update.print("X3 Update");
traits<Point2>::Print(x3_update, "X3 Update");
return 0;
}

741
gtsam.h
View File

File diff suppressed because it is too large Load Diff

49
gtsam/3rdparty/CCOLAMD/Demo/Makefile vendored Normal file
View File

@ -0,0 +1,49 @@
#-----------------------------------------------------------------------------
# compile the CCOLAMD demo
#-----------------------------------------------------------------------------
default: all
include ../../SuiteSparse_config/SuiteSparse_config.mk
I = -I../../include
C = $(CC) $(CF) $(I)
LIB2 = $(LDFLAGS) -L../../lib -lccolamd -lsuitesparseconfig $(LDLIBS)
all: library ccolamd_example ccolamd_l_example
library:
( cd ../../SuiteSparse_config ; $(MAKE) )
( cd ../Lib ; $(MAKE) )
#------------------------------------------------------------------------------
# Create the demo program, run it, and compare the output
#------------------------------------------------------------------------------
dist:
ccolamd_example: ccolamd_example.c
$(C) -o ccolamd_example ccolamd_example.c $(LIB2)
- ./ccolamd_example > my_ccolamd_example.out
- diff ccolamd_example.out my_ccolamd_example.out
ccolamd_l_example: ccolamd_l_example.c
$(C) -o ccolamd_l_example ccolamd_l_example.c $(LIB2)
- ./ccolamd_l_example > my_ccolamd_l_example.out
- diff ccolamd_l_example.out my_ccolamd_l_example.out
#------------------------------------------------------------------------------
# Remove all but the files in the original distribution
#------------------------------------------------------------------------------
clean:
- $(RM) -r $(CLEAN)
purge: distclean
distclean: clean
- $(RM) ccolamd_example ccolamd_l_example
- $(RM) my_ccolamd_example.out my_ccolamd_l_example.out
- $(RM) -r $(PURGE)

2
gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.c vendored
View File

@ -5,8 +5,6 @@
/* ----------------------------------------------------------------------------
* CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/*

4
gtsam/3rdparty/CCOLAMD/Demo/ccolamd_example.out vendored
View File

@ -15,7 +15,7 @@ Column 3, with 2 entries:
row 1
row 3
ccolamd version 2.7, Jan 25, 2011: OK.
ccolamd version 2.9, Apr 1, 2016: OK.
ccolamd: number of dense or empty rows ignored: 0
ccolamd: number of dense or empty columns ignored: 0
ccolamd: number of garbage collections performed: 0
@ -38,7 +38,7 @@ Column 3, with 1 entries:
row 4
Column 4, with 0 entries:
csymamd version 2.7, Jan 25, 2011: OK.
csymamd version 2.9, Apr 1, 2016: OK.
csymamd: number of dense or empty rows ignored: 0
csymamd: number of dense or empty columns ignored: 0
csymamd: number of garbage collections performed: 0

22
gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.c vendored
View File

@ -1,12 +1,10 @@
/* ========================================================================== */
/* === ccolamd and csymamd example (UF_long integer version) ================ */
/* === ccolamd and csymamd example (long integer version) =================== */
/* ========================================================================== */
/* ----------------------------------------------------------------------------
* CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/*
@ -46,9 +44,6 @@
#define B_NNZ 4
#define B_N 5
/* define UF_long */
#include "UFconfig.h"
int main (void)
{
@ -56,14 +51,14 @@ int main (void)
/* input matrix A definition */
/* ====================================================================== */
UF_long A [ALEN] = {
SuiteSparse_long A [ALEN] = {
0, 1, 4, /* row indices of nonzeros in column 0 */
2, 4, /* row indices of nonzeros in column 1 */
0, 1, 2, 3, /* row indices of nonzeros in column 2 */
1, 3} ; /* row indices of nonzeros in column 3 */
UF_long p [ ] = {
SuiteSparse_long p [ ] = {
0, /* column 0 is in A [0..2] */
3, /* column 1 is in A [3..4] */
@ -75,7 +70,7 @@ int main (void)
/* input matrix B definition */
/* ====================================================================== */
UF_long B [ ] = { /* Note: only strictly lower triangular part */
SuiteSparse_long B [ ] = { /* Note: only strictly lower triangular part */
/* is included, since symamd ignores the */
/* diagonal and upper triangular part of B. */
@ -85,7 +80,7 @@ int main (void)
4 /* row indices of nonzeros in column 3 */
} ; /* row indices of nonzeros in column 4 (none) */
UF_long q [ ] = {
SuiteSparse_long q [ ] = {
0, /* column 0 is in B [0] */
1, /* column 1 is in B [1..2] */
@ -98,10 +93,9 @@ int main (void)
/* other variable definitions */
/* ====================================================================== */
UF_long perm [B_N+1] ; /* note the size is N+1 */
UF_long stats [CCOLAMD_STATS] ; /* for ccolamd and csymamd output stats */
UF_long row, col, pp, length, ok ;
SuiteSparse_long perm [B_N+1] ; /* note the size is N+1 */
SuiteSparse_long stats [CCOLAMD_STATS] ; /* ccolamd/csymamd output stats */
SuiteSparse_long row, col, pp, length, ok ;
/* ====================================================================== */
/* dump the input matrix A */

4
gtsam/3rdparty/CCOLAMD/Demo/ccolamd_l_example.out vendored
View File

@ -15,7 +15,7 @@ Column 3, with 2 entries:
row 1
row 3
ccolamd version 2.7, Jan 25, 2011: OK.
ccolamd version 2.9, Apr 1, 2016: OK.
ccolamd: number of dense or empty rows ignored: 0
ccolamd: number of dense or empty columns ignored: 0
ccolamd: number of garbage collections performed: 0
@ -38,7 +38,7 @@ Column 3, with 1 entries:
row 4
Column 4, with 0 entries:
csymamd version 2.7, Jan 25, 2011: OK.
csymamd version 2.9, Apr 1, 2016: OK.
csymamd: number of dense or empty rows ignored: 0
csymamd: number of dense or empty columns ignored: 0
csymamd: number of garbage collections performed: 0

34
gtsam/3rdparty/CCOLAMD/Doc/ChangeLog vendored
View File

@ -1,3 +1,37 @@
Apr 1, 2016: version 2.9.5
* licensing simplified (no other change); refer to CCOLAMD/Doc/License.txt
Feb 1, 2016: version 2.9.4
* update to Makefiles
Jan 30, 2016: version 2.9.3
* modifications to Makefiles
Jan 1, 2016: version 2.9.2
* modified Makefile to create shared libraries
No change to C code except version number.
The empty file ccolamd_global.c removed.
Oct 10, 2014: version 2.9.1
modified MATLAB/ccolamd_make.m. No change to C code except version number.
July 31, 2013: version 2.9.0
* changed malloc and printf pointers to use SuiteSparse_config
Jun 1, 2012: version 2.8.0
* changed from UFconfig to SuiteSparse_config
Dec 7, 2011: version 2.7.4
* fixed the Makefile to better align with CFLAGS and other standards
Jan 25, 2011: version 2.7.3
* minor fix to "make install"

21
gtsam/3rdparty/CCOLAMD/Doc/License.txt vendored Normal file
View File

@ -0,0 +1,21 @@
CCOLAMD: constrained column approximate minimum degree ordering
Copyright (C) 2005-2016, Univ. of Florida. Authors: Timothy A. Davis,
Sivasankaran Rajamanickam, and Stefan Larimore. Closely based on COLAMD by
Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert.
http://www.suitesparse.com
--------------------------------------------------------------------------------
CCOLAMD is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
CCOLAMD is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this Module; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA

283
gtsam/3rdparty/CCOLAMD/Include/ccolamd.h vendored
View File

@ -5,8 +5,6 @@
/* ----------------------------------------------------------------------------
* CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/*
@ -32,24 +30,24 @@ extern "C" {
/* All versions of CCOLAMD will include the following definitions.
* As an example, to test if the version you are using is 1.3 or later:
*
* if (CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)) ...
* if (CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)) ...
*
* This also works during compile-time:
*
* #if CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)
* printf ("This is version 1.3 or later\n") ;
* #else
* printf ("This is an early version\n") ;
* #endif
* #if CCOLAMD_VERSION >= CCOLAMD_VERSION_CODE (1,3)
* printf ("This is version 1.3 or later\n") ;
* #else
* printf ("This is an early version\n") ;
* #endif
*/
#define CCOLAMD_DATE "Jan 25, 2011"
#define CCOLAMD_DATE "Apr 1, 2016"
#define CCOLAMD_VERSION_CODE(main,sub) ((main) * 1000 + (sub))
#define CCOLAMD_MAIN_VERSION 2
#define CCOLAMD_SUB_VERSION 7
#define CCOLAMD_SUBSUB_VERSION 3
#define CCOLAMD_SUB_VERSION 9
#define CCOLAMD_SUBSUB_VERSION 5
#define CCOLAMD_VERSION \
CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION)
CCOLAMD_VERSION_CODE(CCOLAMD_MAIN_VERSION,CCOLAMD_SUB_VERSION)
/* ========================================================================== */
/* === Knob and statistics definitions ====================================== */
@ -94,106 +92,105 @@ extern "C" {
#define CCOLAMD_NEWLY_EMPTY_COL 10
/* error codes returned in stats [3]: */
#define CCOLAMD_OK (0)
#define CCOLAMD_OK_BUT_JUMBLED (1)
#define CCOLAMD_ERROR_A_not_present (-1)
#define CCOLAMD_ERROR_p_not_present (-2)
#define CCOLAMD_ERROR_nrow_negative (-3)
#define CCOLAMD_ERROR_ncol_negative (-4)
#define CCOLAMD_ERROR_nnz_negative (-5)
#define CCOLAMD_ERROR_p0_nonzero (-6)
#define CCOLAMD_ERROR_A_too_small (-7)
#define CCOLAMD_ERROR_col_length_negative (-8)
#define CCOLAMD_ERROR_row_index_out_of_bounds (-9)
#define CCOLAMD_ERROR_out_of_memory (-10)
#define CCOLAMD_ERROR_invalid_cmember (-11)
#define CCOLAMD_ERROR_internal_error (-999)
#define CCOLAMD_OK (0)
#define CCOLAMD_OK_BUT_JUMBLED (1)
#define CCOLAMD_ERROR_A_not_present (-1)
#define CCOLAMD_ERROR_p_not_present (-2)
#define CCOLAMD_ERROR_nrow_negative (-3)
#define CCOLAMD_ERROR_ncol_negative (-4)
#define CCOLAMD_ERROR_nnz_negative (-5)
#define CCOLAMD_ERROR_p0_nonzero (-6)
#define CCOLAMD_ERROR_A_too_small (-7)
#define CCOLAMD_ERROR_col_length_negative (-8)
#define CCOLAMD_ERROR_row_index_out_of_bounds (-9)
#define CCOLAMD_ERROR_out_of_memory (-10)
#define CCOLAMD_ERROR_invalid_cmember (-11)
#define CCOLAMD_ERROR_internal_error (-999)
/* ========================================================================== */
/* === Prototypes of user-callable routines ================================= */
/* ========================================================================== */
/* define UF_long */
#include "UFconfig.h"
#include "SuiteSparse_config.h"
size_t ccolamd_recommended /* returns recommended value of Alen, */
/* or 0 if input arguments are erroneous */
size_t ccolamd_recommended /* returns recommended value of Alen, */
/* or 0 if input arguments are erroneous */
(
int nnz, /* nonzeros in A */
int n_row, /* number of rows in A */
int n_col /* number of columns in A */
int nnz, /* nonzeros in A */
int n_row, /* number of rows in A */
int n_col /* number of columns in A */
) ;
size_t ccolamd_l_recommended /* returns recommended value of Alen, */
/* or 0 if input arguments are erroneous */
size_t ccolamd_l_recommended /* returns recommended value of Alen, */
/* or 0 if input arguments are erroneous */
(
UF_long nnz, /* nonzeros in A */
UF_long n_row, /* number of rows in A */
UF_long n_col /* number of columns in A */
SuiteSparse_long nnz, /* nonzeros in A */
SuiteSparse_long n_row, /* number of rows in A */
SuiteSparse_long n_col /* number of columns in A */
) ;
void ccolamd_set_defaults /* sets default parameters */
( /* knobs argument is modified on output */
double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */
void ccolamd_set_defaults /* sets default parameters */
( /* knobs argument is modified on output */
double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */
) ;
void ccolamd_l_set_defaults /* sets default parameters */
( /* knobs argument is modified on output */
double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */
void ccolamd_l_set_defaults /* sets default parameters */
( /* knobs argument is modified on output */
double knobs [CCOLAMD_KNOBS] /* parameter settings for ccolamd */
) ;
int ccolamd /* returns (1) if successful, (0) otherwise*/
( /* A and p arguments are modified on output */
int n_row, /* number of rows in A */
int n_col, /* number of columns in A */
int Alen, /* size of the array A */
int A [ ], /* row indices of A, of size Alen */
int p [ ], /* column pointers of A, of size n_col+1 */
int ccolamd /* returns (1) if successful, (0) otherwise*/
( /* A and p arguments are modified on output */
int n_row, /* number of rows in A */
int n_col, /* number of columns in A */
int Alen, /* size of the array A */
int A [ ], /* row indices of A, of size Alen */
int p [ ], /* column pointers of A, of size n_col+1 */
double knobs [CCOLAMD_KNOBS],/* parameter settings for ccolamd */
int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */
int cmember [ ] /* Constraint set of A, of size n_col */
int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */
int cmember [ ] /* Constraint set of A, of size n_col */
) ;
UF_long ccolamd_l /* same as ccolamd, but with UF_long integers */
SuiteSparse_long ccolamd_l /* as ccolamd w/ SuiteSparse_long integers */
(
UF_long n_row,
UF_long n_col,
UF_long Alen,
UF_long A [ ],
UF_long p [ ],
SuiteSparse_long n_row,
SuiteSparse_long n_col,
SuiteSparse_long Alen,
SuiteSparse_long A [ ],
SuiteSparse_long p [ ],
double knobs [CCOLAMD_KNOBS],
UF_long stats [CCOLAMD_STATS],
UF_long cmember [ ]
SuiteSparse_long stats [CCOLAMD_STATS],
SuiteSparse_long cmember [ ]
) ;
int csymamd /* return (1) if OK, (0) otherwise */
int csymamd /* return (1) if OK, (0) otherwise */
(
int n, /* number of rows and columns of A */
int A [ ], /* row indices of A */
int p [ ], /* column pointers of A */
int perm [ ], /* output permutation, size n_col+1 */
int n, /* number of rows and columns of A */
int A [ ], /* row indices of A */
int p [ ], /* column pointers of A */
int perm [ ], /* output permutation, size n_col+1 */
double knobs [CCOLAMD_KNOBS],/* parameters (uses defaults if NULL) */
int stats [CCOLAMD_STATS], /* output statistics and error codes */
int stats [CCOLAMD_STATS], /* output statistics and error codes */
void * (*allocate) (size_t, size_t), /* pointer to calloc (ANSI C) or */
/* mxCalloc (for MATLAB mexFunction) */
void (*release) (void *), /* pointer to free (ANSI C) or */
/* mxFree (for MATLAB mexFunction) */
int cmember [ ], /* Constraint set of A */
int stype /* 0: use both parts, >0: upper, <0: lower */
/* mxCalloc (for MATLAB mexFunction) */
void (*release) (void *), /* pointer to free (ANSI C) or */
/* mxFree (for MATLAB mexFunction) */
int cmember [ ], /* Constraint set of A */
int stype /* 0: use both parts, >0: upper, <0: lower */
) ;
UF_long csymamd_l /* same as csymamd, but with UF_long integers */
SuiteSparse_long csymamd_l /* as csymamd, w/ SuiteSparse_long integers */
(
UF_long n,
UF_long A [ ],
UF_long p [ ],
UF_long perm [ ],
SuiteSparse_long n,
SuiteSparse_long A [ ],
SuiteSparse_long p [ ],
SuiteSparse_long perm [ ],
double knobs [CCOLAMD_KNOBS],
UF_long stats [CCOLAMD_STATS],
SuiteSparse_long stats [CCOLAMD_STATS],
void * (*allocate) (size_t, size_t),
void (*release) (void *),
UF_long cmember [ ],
UF_long stype
SuiteSparse_long cmember [ ],
SuiteSparse_long stype
) ;
void ccolamd_report
@ -203,7 +200,7 @@ void ccolamd_report
void ccolamd_l_report
(
UF_long stats [CCOLAMD_STATS]
SuiteSparse_long stats [CCOLAMD_STATS]
) ;
void csymamd_report
@ -213,7 +210,7 @@ void csymamd_report
void csymamd_l_report
(
UF_long stats [CCOLAMD_STATS]
SuiteSparse_long stats [CCOLAMD_STATS]
) ;
@ -227,42 +224,42 @@ void csymamd_l_report
*/
int ccolamd2
( /* A and p arguments are modified on output */
int n_row, /* number of rows in A */
int n_col, /* number of columns in A */
int Alen, /* size of the array A */
int A [ ], /* row indices of A, of size Alen */
int p [ ], /* column pointers of A, of size n_col+1 */
( /* A and p arguments are modified on output */
int n_row, /* number of rows in A */
int n_col, /* number of columns in A */
int Alen, /* size of the array A */
int A [ ], /* row indices of A, of size Alen */
int p [ ], /* column pointers of A, of size n_col+1 */
double knobs [CCOLAMD_KNOBS],/* parameter settings for ccolamd */
int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */
int stats [CCOLAMD_STATS], /* ccolamd output statistics and error codes */
/* each Front_ array is of size n_col+1: */
int Front_npivcol [ ], /* # pivot cols in each front */
int Front_nrows [ ], /* # of rows in each front (incl. pivot rows) */
int Front_ncols [ ], /* # of cols in each front (incl. pivot cols) */
int Front_parent [ ], /* parent of each front */
int Front_cols [ ], /* link list of pivot columns for each front */
int *p_nfr, /* total number of frontal matrices */
int InFront [ ], /* InFront [row] = f if row in front f */
int cmember [ ] /* Constraint set of A */
int Front_npivcol [ ], /* # pivot cols in each front */
int Front_nrows [ ], /* # of rows in each front (incl. pivot rows) */
int Front_ncols [ ], /* # of cols in each front (incl. pivot cols) */
int Front_parent [ ], /* parent of each front */
int Front_cols [ ], /* link list of pivot columns for each front */
int *p_nfr, /* total number of frontal matrices */
int InFront [ ], /* InFront [row] = f if row in front f */
int cmember [ ] /* Constraint set of A */
) ;
UF_long ccolamd2_l /* same as ccolamd2, but with UF_long integers */
SuiteSparse_long ccolamd2_l /* as ccolamd2, w/ SuiteSparse_long integers */
(
UF_long n_row,
UF_long n_col,
UF_long Alen,
UF_long A [ ],
UF_long p [ ],
SuiteSparse_long n_row,
SuiteSparse_long n_col,
SuiteSparse_long Alen,
SuiteSparse_long A [ ],
SuiteSparse_long p [ ],
double knobs [CCOLAMD_KNOBS],
UF_long stats [CCOLAMD_STATS],
UF_long Front_npivcol [ ],
UF_long Front_nrows [ ],
UF_long Front_ncols [ ],
UF_long Front_parent [ ],
UF_long Front_cols [ ],
UF_long *p_nfr,
UF_long InFront [ ],
UF_long cmember [ ]
SuiteSparse_long stats [CCOLAMD_STATS],
SuiteSparse_long Front_npivcol [ ],
SuiteSparse_long Front_nrows [ ],
SuiteSparse_long Front_ncols [ ],
SuiteSparse_long Front_parent [ ],
SuiteSparse_long Front_cols [ ],
SuiteSparse_long *p_nfr,
SuiteSparse_long InFront [ ],
SuiteSparse_long cmember [ ]
) ;
void ccolamd_apply_order
@ -276,11 +273,11 @@ void ccolamd_apply_order
void ccolamd_l_apply_order
(
UF_long Front [ ],
const UF_long Order [ ],
UF_long Temp [ ],
UF_long nn,
UF_long nfr
SuiteSparse_long Front [ ],
const SuiteSparse_long Order [ ],
SuiteSparse_long Temp [ ],
SuiteSparse_long nn,
SuiteSparse_long nfr
) ;
@ -296,12 +293,12 @@ void ccolamd_fsize
void ccolamd_l_fsize
(
UF_long nn,
UF_long MaxFsize [ ],
UF_long Fnrows [ ],
UF_long Fncols [ ],
UF_long Parent [ ],
UF_long Npiv [ ]
SuiteSparse_long nn,
SuiteSparse_long MaxFsize [ ],
SuiteSparse_long Fnrows [ ],
SuiteSparse_long Fncols [ ],
SuiteSparse_long Parent [ ],
SuiteSparse_long Npiv [ ]
) ;
void ccolamd_postorder
@ -320,16 +317,16 @@ void ccolamd_postorder
void ccolamd_l_postorder
(
UF_long nn,
UF_long Parent [ ],
UF_long Npiv [ ],
UF_long Fsize [ ],
UF_long Order [ ],
UF_long Child [ ],
UF_long Sibling [ ],
UF_long Stack [ ],
UF_long Front_cols [ ],
UF_long cmember [ ]
SuiteSparse_long nn,
SuiteSparse_long Parent [ ],
SuiteSparse_long Npiv [ ],
SuiteSparse_long Fsize [ ],
SuiteSparse_long Order [ ],
SuiteSparse_long Child [ ],
SuiteSparse_long Sibling [ ],
SuiteSparse_long Stack [ ],
SuiteSparse_long Front_cols [ ],
SuiteSparse_long cmember [ ]
) ;
int ccolamd_post_tree
@ -342,22 +339,16 @@ int ccolamd_post_tree
int Stack [ ]
) ;
UF_long ccolamd_l_post_tree
SuiteSparse_long ccolamd_l_post_tree
(
UF_long root,
UF_long k,
UF_long Child [ ],
const UF_long Sibling [ ],
UF_long Order [ ],
UF_long Stack [ ]
SuiteSparse_long root,
SuiteSparse_long k,
SuiteSparse_long Child [ ],
const SuiteSparse_long Sibling [ ],
SuiteSparse_long Order [ ],
SuiteSparse_long Stack [ ]
) ;
#ifndef EXTERN
#define EXTERN extern
#endif
EXTERN int (*ccolamd_printf) (const char *, ...) ;
#ifdef __cplusplus
}
#endif

71
gtsam/3rdparty/CCOLAMD/Lib/Makefile vendored Normal file
View File

@ -0,0 +1,71 @@
#-------------------------------------------------------------------------------
# CCOLAMD Lib/Makefile
#-------------------------------------------------------------------------------
LIBRARY = libccolamd
VERSION = 2.9.5
SO_VERSION = 2
default: library
include ../../SuiteSparse_config/SuiteSparse_config.mk
# CCOLAMD depends on SuiteSparse_config
LDLIBS += -lsuitesparseconfig
# compile and install in SuiteSparse/lib
library:
$(MAKE) install INSTALL=$(SUITESPARSE)
I = -I../Include -I../../SuiteSparse_config
INC = ../Include/ccolamd.h ../../SuiteSparse_config/SuiteSparse_config.h
SRC = ../Source/ccolamd.c
OBJ = ccolamd.o ccolamd_l.o
ccolamd.o: $(SRC) $(INC)
$(CC) $(CF) $(I) -c ../Source/ccolamd.c
ccolamd_l.o: $(SRC) $(INC)
$(CC) $(CF) $(I) -c ../Source/ccolamd.c -DDLONG -o ccolamd_l.o
# creates libccolamd.a, a C-callable CCOLAMD library
$(AR_TARGET): $(OBJ)
$(ARCHIVE) $@ $^
- $(RANLIB) $@
ccode: library
clean:
- $(RM) -r $(CLEAN)
purge: distclean
distclean: clean
- $(RM) -r $(PURGE)
# install CCOLAMD
install: $(AR_TARGET) $(INSTALL_LIB)/$(SO_TARGET)
$(INSTALL_LIB)/$(SO_TARGET): $(OBJ)
@mkdir -p $(INSTALL_LIB)
@mkdir -p $(INSTALL_INCLUDE)
@mkdir -p $(INSTALL_DOC)
$(CC) $(SO_OPTS) $^ -o $@ $(LDLIBS)
( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_PLAIN) )
( cd $(INSTALL_LIB) ; ln -sf $(SO_TARGET) $(SO_MAIN) )
$(CP) ../Include/ccolamd.h $(INSTALL_INCLUDE)
$(CP) ../README.txt $(INSTALL_DOC)/CCOLAMD_README.txt
chmod 755 $(INSTALL_LIB)/$(SO_TARGET)
chmod 644 $(INSTALL_INCLUDE)/ccolamd.h
chmod 644 $(INSTALL_DOC)/CCOLAMD_README.txt
uninstall:
$(RM) $(INSTALL_LIB)/$(SO_TARGET)
$(RM) $(INSTALL_LIB)/$(SO_PLAIN)
$(RM) $(INSTALL_LIB)/$(SO_MAIN)
$(RM) $(INSTALL_INCLUDE)/ccolamd.h
$(RM) $(INSTALL_DOC)/CCOLAMD_README.txt

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.

29
gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_demo.m vendored
View File

@ -47,9 +47,8 @@ spparms ('default') ;
A = sprandn (n, n, 2/n) + speye (n) ;
b = (1:n)' ;
figure (1)
clf ;
subplot (2,2,1)
subplot (3,4,1)
spy (A)
title ('original matrix')
@ -62,7 +61,7 @@ fl = luflops (L, U) ;
x = Q * (U \ (L \ (P * b))) ;
fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ;
fprintf (1, 'residual: %e\n', norm (A*x-b));
subplot (2,2,2) ;
subplot (3,4,2) ;
spy (L|U) ;
title ('LU with ccolamd') ;
@ -76,7 +75,7 @@ fl = luflops (L, U) ;
x = Q * (U \ (L \ (P * b))) ;
fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ;
fprintf (1, 'residual: %e\n', norm (A*x-b));
subplot (2,2,3) ;
subplot (3,4,3) ;
spy (L|U) ;
title ('LU with colamd') ;
catch
@ -89,7 +88,7 @@ fl = luflops (L, U) ;
x = U \ (L \ (P * b)) ;
fprintf (1, '\nFlop count for [L,U,P] = lu (A*Q): %d\n', fl) ;
fprintf (1, 'residual: %e\n', norm (A*x-b));
subplot (2,2,4) ;
subplot (3,4,4) ;
spy (L|U) ;
title ('LU with no ordering') ;
@ -111,9 +110,7 @@ n = 1000 ;
fprintf (1, 'Generating a random %d-by-%d sparse matrix.\n', n, n) ;
A = sprandn (n, n, 2/n) + speye (n) ;
figure (2)
clf ;
subplot (2,2,1)
subplot (3,4,5)
spy (A)
title ('original matrix')
@ -121,7 +118,7 @@ fprintf (1, '\n\nUnordered matrix:\n') ;
[lnz,h,parent,post,R] = symbfact (A, 'col') ;
fprintf (1, 'nz in Cholesky factors of A''A: %d\n', sum (lnz)) ;
fprintf (1, 'flop count for Cholesky of A''A: %d\n', sum (lnz.^2)) ;
subplot (2,2,4) ;
subplot (3,4,6) ;
spy (R) ;
title ('Cholesky with no ordering') ;
@ -133,7 +130,7 @@ fprintf (1, '\n\nccolamd run time: %f\n', t) ;
fprintf (1, 'ccolamd ordering quality: \n') ;
fprintf (1, 'nz in Cholesky factors of A(:,p)''A(:,p): %d\n', sum (lnz)) ;
fprintf (1, 'flop count for Cholesky of A(:,p)''A(:,p): %d\n', sum (lnz.^2)) ;
subplot (2,2,2) ;
subplot (3,4,7) ;
spy (R) ;
title ('Cholesky with ccolamd') ;
@ -146,7 +143,7 @@ fprintf (1, '\n\ncolamd run time: %f\n', t) ;
fprintf (1, 'colamd ordering quality: \n') ;
fprintf (1, 'nz in Cholesky factors of A(:,p)''A(:,p): %d\n', sum (lnz)) ;
fprintf (1, 'flop count for Cholesky of A(:,p)''A(:,p): %d\n', sum (lnz.^2)) ;
subplot (2,2,3) ;
subplot (3,4,8) ;
spy (R) ;
title ('Cholesky with colamd') ;
catch
@ -164,9 +161,7 @@ fprintf (1, '\n-----------------------------------------------------------\n') ;
fprintf (1, 'Generating a random symmetric %d-by-%d sparse matrix.\n', n, n) ;
A = A+A' ;
figure (3)
clf ;
subplot (2,2,1)
subplot (3,4,9) ;
spy (A)
title ('original matrix')
@ -174,7 +169,7 @@ fprintf (1, '\n\nUnordered matrix:\n') ;
[lnz,h,parent,post,R] = symbfact (A, 'sym') ;
fprintf (1, 'nz in Cholesky factors of A: %d\n', sum (lnz)) ;
fprintf (1, 'flop count for Cholesky of A: %d\n', sum (lnz.^2)) ;
subplot (2,2,4) ;
subplot (3,4,10) ;
spy (R) ;
title ('Cholesky with no ordering') ;
@ -186,7 +181,7 @@ fprintf (1, '\n\ncsymamd run time: %f\n', t) ;
fprintf (1, 'csymamd ordering quality: \n') ;
fprintf (1, 'nz in Cholesky factors of A(p,p): %d\n', sum (lnz)) ;
fprintf (1, 'flop count for Cholesky of A(p,p): %d\n', sum (lnz.^2)) ;
subplot (2,2,2) ;
subplot (3,4,11) ;
spy (R) ;
title ('Cholesky with csymamd') ;
@ -199,7 +194,7 @@ fprintf (1, '\n\nsymamd run time: %f\n', t) ;
fprintf (1, 'symamd ordering quality: \n') ;
fprintf (1, 'nz in Cholesky factors of A(p,p): %d\n', sum (lnz)) ;
fprintf (1, 'flop count for Cholesky of A(p,p): %d\n', sum (lnz.^2)) ;
subplot (2,2,3) ;
subplot (3,4,12) ;
spy (R) ;
title ('Cholesky with symamd') ;
catch

23
gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_make.m vendored
View File

@ -14,14 +14,33 @@ d = '' ;
if (~isempty (strfind (computer, '64')))
d = '-largeArrayDims' ;
end
src = '../Source/ccolamd.c ../Source/ccolamd_global.c' ;
cmd = sprintf ('mex -DDLONG -O %s -I../../UFconfig -I../Include -output ', d) ;
% MATLAB 8.3.0 now has a -silent option to keep 'mex' from burbling too much
if (~verLessThan ('matlab', '8.3.0'))
d = ['-silent ' d] ;
end
src = '../Source/ccolamd.c ../../SuiteSparse_config/SuiteSparse_config.c' ;
cmd = sprintf ( ...
'mex -DDLONG -O %s -I../../SuiteSparse_config -I../Include -output ', d) ;
s = [cmd 'ccolamd ccolamdmex.c ' src] ;
if (~(ispc || ismac))
% for POSIX timing routine
s = [s ' -lrt'] ;
end
if (details)
fprintf ('%s\n', s) ;
end
eval (s) ;
s = [cmd 'csymamd csymamdmex.c ' src] ;
if (~(ispc || ismac))
% for POSIX timing routine
s = [s ' -lrt'] ;
end
if (details)
fprintf ('%s\n', s) ;
end

9
gtsam/3rdparty/CCOLAMD/MATLAB/ccolamd_test.m vendored
View File

@ -22,8 +22,13 @@ csymamd_default_knobs = [10 1 0] ;
if (~isempty (strfind (computer, '64')))
d = '-largeArrayDims' ;
end
src = '../Source/ccolamd.c ../Source/ccolamd_global.c' ;
cmd = sprintf ('mex -DDLONG -O %s -I../../UFconfig -I../Include ', d) ;
cmd = sprintf ( ...
'mex -DDLONG -O %s -I../../SuiteSparse_config -I../Include ', d) ;
src = '../Source/ccolamd.c ../../SuiteSparse_config/SuiteSparse_config.c' ;
if (~(ispc || ismac))
% for POSIX timing routine
src = [src ' -lrt'] ;
end
eval ([cmd 'ccolamdtestmex.c ' src]) ;
eval ([cmd 'csymamdtestmex.c ' src]) ;
fprintf ('Done compiling.\n') ;

52
gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdmex.c vendored
View File

@ -5,8 +5,6 @@
/* ----------------------------------------------------------------------------
* CCOLAMD, Copyright (C), Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/*
@ -26,7 +24,7 @@
#include "matrix.h"
#include <stdlib.h>
#include <string.h>
#include "UFconfig.h"
#define Long SuiteSparse_long
/* ========================================================================== */
/* === ccolamd mexFunction ================================================== */
@ -44,24 +42,24 @@ void mexFunction
{
/* === Local variables ================================================== */
UF_long *A ; /* ccolamd's copy of the matrix and workspace */
UF_long *cmember ; /* ccolamd's copy of the constraint set */
double *in_cmember ; /* input constraint set */
UF_long *p ; /* ccolamd's copy of the column pointers */
UF_long Alen ; /* size of A */
UF_long cslen ; /* size of CS */
UF_long n_col ; /* number of columns of A */
UF_long n_row ; /* number of rows of A */
UF_long nnz ; /* number of entries in A */
UF_long full ; /* TRUE if input matrix full, FALSE if sparse */
Long *A ; /* ccolamd's copy of the matrix and workspace */
Long *cmember ; /* ccolamd's copy of the constraint set */
double *in_cmember ; /* input constraint set */
Long *p ; /* ccolamd's copy of the column pointers */
Long Alen ; /* size of A */
Long cslen ; /* size of CS */
Long n_col ; /* number of columns of A */
Long n_row ; /* number of rows of A */
Long nnz ; /* number of entries in A */
Long full ; /* TRUE if input matrix full, FALSE if sparse */
double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */
double *out_perm ; /* output permutation vector */
double *out_stats ; /* output stats vector */
double *in_knobs ; /* input knobs vector */
UF_long i ; /* loop counter */
mxArray *Ainput ; /* input matrix handle */
UF_long spumoni ; /* verbosity variable */
UF_long stats [CCOLAMD_STATS] ; /* stats for ccolamd */
double *out_perm ; /* output permutation vector */
double *out_stats ; /* output stats vector */
double *in_knobs ; /* input knobs vector */
Long i ; /* loop counter */
mxArray *Ainput ; /* input matrix handle */
Long spumoni ; /* verbosity variable */
Long stats [CCOLAMD_STATS] ;/* stats for ccolamd */
/* === Check inputs ===================================================== */
@ -80,11 +78,11 @@ void mexFunction
cslen = mxGetNumberOfElements (pargin [2]) ;
if (cslen != 0)
{
cmember = (UF_long *) mxCalloc (cslen, sizeof (UF_long)) ;
cmember = (Long *) mxCalloc (cslen, sizeof (Long)) ;
for (i = 0 ; i < cslen ; i++)
{
/* convert cmember from 1-based to 0-based */
cmember[i] = ((UF_long) in_cmember [i] - 1) ;
cmember[i] = ((Long) in_cmember [i] - 1) ;
}
}
}
@ -157,10 +155,10 @@ void mexFunction
n_col = mxGetN (Ainput) ;
/* get column pointer vector */
p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ;
(void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ;
p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ;
(void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ;
nnz = p [n_col] ;
Alen = (UF_long) ccolamd_l_recommended (nnz, n_row, n_col) ;
Alen = (Long) ccolamd_l_recommended (nnz, n_row, n_col) ;
if (Alen == 0)
{
mexErrMsgTxt ("ccolamd: problem too large") ;
@ -168,8 +166,8 @@ void mexFunction
/* === Copy input matrix into workspace ================================= */
A = (UF_long *) mxCalloc (Alen, sizeof (UF_long)) ;
(void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ;
A = (Long *) mxCalloc (Alen, sizeof (Long)) ;
(void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ;
if (full)
{

68
gtsam/3rdparty/CCOLAMD/MATLAB/ccolamdtestmex.c vendored
View File

@ -5,8 +5,6 @@
/* ----------------------------------------------------------------------------
* CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/*
@ -43,7 +41,7 @@
#include "matrix.h"
#include <stdlib.h>
#include <string.h>
#include "UFconfig.h"
#define Long SuiteSparse_long
/* Here only for testing */
#undef MIN
@ -61,15 +59,15 @@
static void dump_matrix
(
UF_long A [ ],
UF_long p [ ],
UF_long n_row,
UF_long n_col,
UF_long Alen,
UF_long limit
Long A [ ],
Long p [ ],
Long n_row,
Long n_col,
Long Alen,
Long limit
)
{
UF_long col, k, row ;
Long col, k, row ;
mexPrintf ("dump matrix: nrow %d ncol %d Alen %d\n", n_row, n_col, Alen) ;
@ -102,24 +100,24 @@ void mexFunction
{
/* === Local variables ================================================== */
UF_long *A ; /* ccolamd's copy of the matrix and workspace */
UF_long *p ; /* ccolamd's copy of the column pointers */
UF_long Alen ; /* size of A */
UF_long n_col ; /* number of columns of A */
UF_long n_row ; /* number of rows of A */
UF_long nnz ; /* number of entries in A */
UF_long full ; /* TRUE if input matrix full, FALSE if sparse */
Long *A ; /* ccolamd's copy of the matrix and workspace */
Long *p ; /* ccolamd's copy of the column pointers */
Long Alen ; /* size of A */
Long n_col ; /* number of columns of A */
Long n_row ; /* number of rows of A */
Long nnz ; /* number of entries in A */
Long full ; /* TRUE if input matrix full, FALSE if sparse */
double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */
double *out_perm ; /* output permutation vector */
double *out_stats ; /* output stats vector */
double *in_knobs ; /* input knobs vector */
UF_long i ; /* loop counter */
mxArray *Ainput ; /* input matrix handle */
UF_long spumoni ; /* verbosity variable */
UF_long stats2 [CCOLAMD_STATS] ; /* stats for ccolamd */
double *out_perm ; /* output permutation vector */
double *out_stats ; /* output stats vector */
double *in_knobs ; /* input knobs vector */
Long i ; /* loop counter */
mxArray *Ainput ; /* input matrix handle */
Long spumoni ; /* verbosity variable */
Long stats2 [CCOLAMD_STATS] ; /* stats for ccolamd */
UF_long *cp, *cp_end, result, col, length, ok ;
UF_long *stats ;
Long *cp, *cp_end, result, col, length, ok ;
Long *stats ;
stats = stats2 ;
/* === Check inputs ===================================================== */
@ -199,10 +197,10 @@ void mexFunction
n_col = mxGetN (Ainput) ;
/* get column pointer vector so we can find nnz */
p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ;
(void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ;
p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ;
(void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ;
nnz = p [n_col] ;
Alen = (UF_long) ccolamd_l_recommended (nnz, n_row, n_col) ;
Alen = (Long) ccolamd_l_recommended (nnz, n_row, n_col) ;
if (Alen == 0)
{
mexErrMsgTxt ("ccolamd: problem too large") ;
@ -230,8 +228,8 @@ void mexFunction
/* === Copy input matrix into workspace ================================= */
A = (UF_long *) mxCalloc (Alen, sizeof (UF_long)) ;
(void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ;
A = (Long *) mxCalloc (Alen, sizeof (Long)) ;
(void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ;
if (full)
{
@ -261,7 +259,7 @@ void mexFunction
*/
/* jumble appropriately */
switch ((UF_long) in_knobs [6])
switch ((Long) in_knobs [6])
{
case 0 :
@ -359,7 +357,7 @@ void mexFunction
mexPrintf ("ccolamdtest: A not present\n") ;
}
result = 0 ; /* A not present */
A = (UF_long *) NULL ;
A = (Long *) NULL ;
break ;
case 8 :
@ -368,7 +366,7 @@ void mexFunction
mexPrintf ("ccolamdtest: p not present\n") ;
}
result = 0 ; /* p not present */
p = (UF_long *) NULL ;
p = (Long *) NULL ;
break ;
case 9 :
@ -456,7 +454,7 @@ void mexFunction
mexPrintf ("ccolamdtest: stats not present\n") ;
}
result = 0 ; /* stats not present */
stats = (UF_long *) NULL ;
stats = (Long *) NULL ;
break ;
case 13 :

4
gtsam/3rdparty/CCOLAMD/MATLAB/csymamd.m vendored
View File

@ -34,10 +34,10 @@ function [p, stats] = csymamd (S, knobs, cmember) %#ok
% p = csymamd(S) is about the same as p = symamd(S). knobs and its default
% values differ.
%
% Authors: S. Larimore, T. Davis (Univ of Florida), and S. Rajamanickam, in
% Authors: S. Larimore, T. Davis, and S. Rajamanickam, in
% collaboration with J. Gilbert and E. Ng. Supported by the National
% Science Foundation (DMS-9504974, DMS-9803599, CCR-0203270), and a grant
% from Sandia National Lab. See http://www.cise.ufl.edu/research/sparse
% from Sandia National Lab. See http://www.suitesparse.com
% for ccolamd, csymamd, amd, colamd, symamd, and other related orderings.
%
% See also AMD, CCOLAMD, COLAMD, SYMAMD, SYMRCM.

46
gtsam/3rdparty/CCOLAMD/MATLAB/csymamdmex.c vendored
View File

@ -5,8 +5,6 @@
/* ----------------------------------------------------------------------------
* CCOLAMD, Copyright (C), Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/*
@ -25,7 +23,7 @@
#include "mex.h"
#include "matrix.h"
#include <stdlib.h>
#include "UFconfig.h"
#define Long SuiteSparse_long
/* ========================================================================== */
/* === csymamd mexFunction ================================================== */
@ -43,23 +41,23 @@ void mexFunction
{
/* === Local variables ================================================== */
UF_long *A ; /* row indices of input matrix A */
UF_long *perm ; /* column ordering of M and ordering of A */
UF_long *cmember ; /* csymamd's copy of the constraint set */
double *in_cmember ; /* input constraint set */
UF_long *p ; /* column pointers of input matrix A */
UF_long cslen ; /* size of constraint set */
UF_long n_col ; /* number of columns of A */
UF_long n_row ; /* number of rows of A */
UF_long full ; /* TRUE if input matrix full, FALSE if sparse */
Long *A ; /* row indices of input matrix A */
Long *perm ; /* column ordering of M and ordering of A */
Long *cmember ; /* csymamd's copy of the constraint set */
double *in_cmember ; /* input constraint set */
Long *p ; /* column pointers of input matrix A */
Long cslen ; /* size of constraint set */
Long n_col ; /* number of columns of A */
Long n_row ; /* number of rows of A */
Long full ; /* TRUE if input matrix full, FALSE if sparse */
double knobs [CCOLAMD_KNOBS] ; /* csymamd user-controllable parameters */
double *out_perm ; /* output permutation vector */
double *out_stats ; /* output stats vector */
double *in_knobs ; /* input knobs vector */
UF_long i ; /* loop counter */
mxArray *Ainput ; /* input matrix handle */
UF_long spumoni ; /* verbosity variable */
UF_long stats [CCOLAMD_STATS] ; /* stats for symamd */
double *out_perm ; /* output permutation vector */
double *out_stats ; /* output stats vector */
double *in_knobs ; /* input knobs vector */
Long i ; /* loop counter */
mxArray *Ainput ; /* input matrix handle */
Long spumoni ; /* verbosity variable */
Long stats [CCOLAMD_STATS] ;/* stats for symamd */
/* === Check inputs ===================================================== */
@ -78,11 +76,11 @@ void mexFunction
cslen = mxGetNumberOfElements (pargin [2]) ;
if (cslen != 0)
{
cmember = (UF_long *) mxCalloc (cslen, sizeof (UF_long)) ;
cmember = (Long *) mxCalloc (cslen, sizeof (Long)) ;
for (i = 0 ; i < cslen ; i++)
{
/* convert cmember from 1-based to 0-based */
cmember[i] = ((UF_long) in_cmember [i] - 1) ;
cmember[i] = ((Long) in_cmember [i] - 1) ;
}
}
}
@ -153,9 +151,9 @@ void mexFunction
mexErrMsgTxt ("csymamd: cmember must be of length equal to #cols of A");
}
A = (UF_long *) mxGetIr (Ainput) ;
p = (UF_long *) mxGetJc (Ainput) ;
perm = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ;
A = (Long *) mxGetIr (Ainput) ;
p = (Long *) mxGetJc (Ainput) ;
perm = (Long *) mxCalloc (n_col+1, sizeof (Long)) ;
/* === Order the rows and columns of A (does not destroy A) ============= */

66
gtsam/3rdparty/CCOLAMD/MATLAB/csymamdtestmex.c vendored
View File

@ -5,8 +5,6 @@
/* ----------------------------------------------------------------------------
* CCOLAMD Copyright (C), Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/*
@ -37,7 +35,7 @@
#include "matrix.h"
#include <stdlib.h>
#include <string.h>
#include "UFconfig.h"
#define Long SuiteSparse_long
#ifdef MIN
#undef MIN
@ -47,15 +45,15 @@
static void dump_matrix
(
UF_long A [ ],
UF_long p [ ],
UF_long n_row,
UF_long n_col,
UF_long Alen,
UF_long limit
Long A [ ],
Long p [ ],
Long n_row,
Long n_col,
Long Alen,
Long limit
)
{
UF_long col, k, row ;
Long col, k, row ;
mexPrintf ("dump matrix: nrow %d ncol %d Alen %d\n", n_row, n_col, Alen) ;
@ -100,23 +98,23 @@ void mexFunction
{
/* === Local variables ================================================== */
UF_long *perm ; /* column ordering of M and ordering of A */
UF_long *A ; /* row indices of input matrix A */
UF_long *p ; /* column pointers of input matrix A */
UF_long n_col ; /* number of columns of A */
UF_long n_row ; /* number of rows of A */
UF_long full ; /* TRUE if input matrix full, FALSE if sparse */
Long *perm ; /* column ordering of M and ordering of A */
Long *A ; /* row indices of input matrix A */
Long *p ; /* column pointers of input matrix A */
Long n_col ; /* number of columns of A */
Long n_row ; /* number of rows of A */
Long full ; /* TRUE if input matrix full, FALSE if sparse */
double knobs [CCOLAMD_KNOBS] ; /* ccolamd user-controllable parameters */
double *out_perm ; /* output permutation vector */
double *out_stats ; /* output stats vector */
double *in_knobs ; /* input knobs vector */
UF_long i ; /* loop counter */
mxArray *Ainput ; /* input matrix handle */
UF_long spumoni ; /* verbosity variable */
UF_long stats2 [CCOLAMD_STATS] ;/* stats for csymamd */
double *out_perm ; /* output permutation vector */
double *out_stats ; /* output stats vector */
double *in_knobs ; /* input knobs vector */
Long i ; /* loop counter */
mxArray *Ainput ; /* input matrix handle */
Long spumoni ; /* verbosity variable */
Long stats2 [CCOLAMD_STATS] ;/* stats for csymamd */
UF_long *cp, *cp_end, result, nnz, col, length, ok ;
UF_long *stats ;
Long *cp, *cp_end, result, nnz, col, length, ok ;
Long *stats ;
stats = stats2 ;
/* === Check inputs ===================================================== */
@ -192,8 +190,8 @@ void mexFunction
}
/* p = mxGetJc (Ainput) ; */
p = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ;
(void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (UF_long)) ;
p = (Long *) mxCalloc (n_col+1, sizeof (Long)) ;
(void) memcpy (p, mxGetJc (Ainput), (n_col+1)*sizeof (Long)) ;
nnz = p [n_col] ;
if (spumoni)
@ -202,10 +200,10 @@ void mexFunction
}
/* A = mxGetIr (Ainput) ; */
A = (UF_long *) mxCalloc (nnz+1, sizeof (UF_long)) ;
(void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (UF_long)) ;
A = (Long *) mxCalloc (nnz+1, sizeof (Long)) ;
(void) memcpy (A, mxGetIr (Ainput), nnz*sizeof (Long)) ;
perm = (UF_long *) mxCalloc (n_col+1, sizeof (UF_long)) ;
perm = (Long *) mxCalloc (n_col+1, sizeof (Long)) ;
/* === Jumble matrix ==================================================== */
@ -230,7 +228,7 @@ void mexFunction
*/
/* jumble appropriately */
switch ((UF_long) in_knobs [3])
switch ((Long) in_knobs [3])
{
case 0 :
@ -321,7 +319,7 @@ void mexFunction
mexPrintf ("csymamdtest: A not present\n") ;
}
result = 0 ; /* A not present */
A = (UF_long *) NULL ;
A = (Long *) NULL ;
break ;
case 8 :
@ -330,7 +328,7 @@ void mexFunction
mexPrintf ("csymamdtest: p not present\n") ;
}
result = 0 ; /* p not present */
p = (UF_long *) NULL ;
p = (Long *) NULL ;
break ;
case 9 :
@ -418,7 +416,7 @@ void mexFunction
mexPrintf ("csymamdtest: stats not present\n") ;
}
result = 0 ; /* stats not present */
stats = (UF_long *) NULL ;
stats = (Long *) NULL ;
break ;
case 13 :

51
gtsam/3rdparty/CCOLAMD/Makefile vendored Normal file
View File

@ -0,0 +1,51 @@
#------------------------------------------------------------------------------
# CCOLAMD Makefile
#------------------------------------------------------------------------------
SUITESPARSE ?= $(realpath $(CURDIR)/..)
export SUITESPARSE
default: all
include ../SuiteSparse_config/SuiteSparse_config.mk
demos: all
# Compile all C code
all:
( cd Lib ; $(MAKE) )
( cd Demo ; $(MAKE) )
# compile just the C-callable libraries (not Demos)
library:
( cd Lib ; $(MAKE) )
# remove object files, but keep the compiled programs and library archives
clean:
( cd Lib ; $(MAKE) clean )
( cd Demo ; $(MAKE) clean )
( cd MATLAB ; $(RM) $(CLEAN) )
# clean, and then remove compiled programs and library archives
purge:
( cd Lib ; $(MAKE) purge )
( cd Demo ; $(MAKE) purge )
( cd MATLAB ; $(RM) $(CLEAN) ; $(RM) *.mex* )
distclean: purge
# get ready for distribution
dist: purge
( cd Demo ; $(MAKE) dist )
ccode: library
lib: library
# install CCOLAMD
install:
( cd Lib ; $(MAKE) install )
# uninstall CCOLAMD
uninstall:
( cd Lib ; $(MAKE) uninstall )

58
gtsam/3rdparty/CCOLAMD/README.txt vendored
View File

@ -1,8 +1,8 @@
CCOLAMD: constrained column approximate minimum degree ordering
Copyright (C) 2005-2011, Univ. of Florida. Authors: Timothy A. Davis,
Copyright (C) 2005-2016, Univ. of Florida. Authors: Timothy A. Davis,
Sivasankaran Rajamanickam, and Stefan Larimore. Closely based on COLAMD by
Davis, Stefan Larimore, in collaboration with Esmond Ng, and John Gilbert.
http://www.cise.ufl.edu/research/sparse
http://www.suitesparse.com
-------------------------------------------------------------------------------
The CCOLAMD column approximate minimum degree ordering algorithm computes
@ -14,7 +14,8 @@ available as a MATLAB-callable function. It constructs a matrix M such
that M'*M has the same pattern as A, and then uses CCOLAMD to compute a column
ordering of M.
Requires UFconfig, in the ../UFconfig directory relative to this directory.
Requires SuiteSparse_config, in the ../SuiteSparse_config directory relative to
this directory.
To compile and install the ccolamd m-files and mexFunctions, just cd to
CCOLAMD/MATLAB and type ccolamd_install in the MATLAB command window.
@ -22,47 +23,27 @@ A short demo will run. Optionally, type ccolamd_test to run an extensive tests.
Type "make" in Unix in the CCOLAMD directory to compile the C-callable
library and to run a short demo.
If you have MATLAB 7.2 or earlier, you must first edit UFconfig/UFconfig.h to
remove the "-largeArrayDims" option from the MEX command (or just use
ccolamd_install.m inside MATLAB).
Other "make" targets:
make mex compiles MATLAB mexFunctions only
make libccolamd.a compiles a C-callable library containing ccolamd
make clean removes all files not in the distribution, except for
libccolamd.a
make library compiles a C-callable library containing ccolamd
make clean removes all files not in the distribution
but keeps the compiled libraries.
make distclean removes all files not in the distribution
make install installs the library in /usr/local/lib and
/usr/local/include
make uninstall uninstalls the library from /usr/local/lib and
/usr/local/include
To use ccolamd and csymamd within an application written in C, all you need are
ccolamd.c and ccolamd.h, which are the C-callable ccolamd/csymamd codes.
See ccolamd.c for more information on how to call ccolamd from a C program.
It contains a complete description of the C-interface to CCOLAMD and CSYMAMD.
Copyright (c) 1998-2007 by the University of Florida.
All Rights Reserved.
Licensed under the GNU LESSER GENERAL PUBLIC LICENSE.
See CCOLAMD/Doc/License.txt for the license.
-------------------------------------------------------------------------------
This library is free software; you can redistribute it and/or
modify it under the terms of the GNU Lesser General Public
License as published by the Free Software Foundation; either
version 2.1 of the License, or (at your option) any later version.
This library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
Lesser General Public License for more details.
You should have received a copy of the GNU Lesser General Public
License along with this library; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
-------------------------------------------------------------------------------
Related papers:
T. A. Davis and W. W. Hager, Rajamanickam, Multiple-rank updates
@ -86,21 +67,18 @@ Related papers:
"An approximate minimum degree column ordering algorithm",
S. I. Larimore, MS Thesis, Dept. of Computer and Information
Science and Engineering, University of Florida, Gainesville, FL,
1998. CISE Tech Report TR-98-016. Available at
ftp://ftp.cise.ufl.edu/cis/tech-reports/tr98/tr98-016.ps
via anonymous ftp.
1998. CISE Tech Report TR-98-016.
Approximate Deficiency for Ordering the Columns of a Matrix,
J. L. Kern, Senior Thesis, Dept. of Computer and Information
Science and Engineering, University of Florida, Gainesville, FL,
1999. Available at http://www.cise.ufl.edu/~davis/Kern/kern.ps
1999.
Authors: Timothy A. Davis, Sivasankaran Rajamanickam, and Stefan Larimore.
Closely based on COLAMD by Stefan I. Larimore and Timothy A. Davis,
University of Florida, in collaboration with John Gilbert, Xerox PARC
(now at UC Santa Barbara), and Esmong Ng, Lawrence Berkeley National
Laboratory (much of this work he did while at Oak Ridge National
Laboratory).
in collaboration with John Gilbert, Xerox PARC (now at UC Santa
Barbara), and Esmong Ng, Lawrence Berkeley National Laboratory (much of
this work he did while at Oak Ridge National Laboratory).
CCOLAMD files:
@ -122,7 +100,6 @@ CCOLAMD files:
./Doc:
ChangeLog change log
lesser.txt license
./Include:
ccolamd.h include file
@ -147,4 +124,3 @@ CCOLAMD files:
./Source:
ccolamd.c primary source code
ccolamd_global.c globally defined function pointers (malloc, free, ...)

180
gtsam/3rdparty/CCOLAMD/Source/ccolamd.c vendored
View File

@ -5,8 +5,6 @@
/* ----------------------------------------------------------------------------
* CCOLAMD, Copyright (C) Univ. of Florida. Authors: Timothy A. Davis,
* Sivasankaran Rajamanickam, and Stefan Larimore
* See License.txt for the Version 2.1 of the GNU Lesser General Public License
* http://www.cise.ufl.edu/research/sparse
* -------------------------------------------------------------------------- */
/*
@ -58,39 +56,13 @@
* COLAMD is also available under alternate licenses, contact T. Davis
* for details.
*
* This library is free software; you can redistribute it and/or
* modify it under the terms of the GNU Lesser General Public
* License as published by the Free Software Foundation; either
* version 2.1 of the License, or (at your option) any later version.
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* You should have received a copy of the GNU Lesser General Public
* License along with this library; if not, write to the Free Software
* Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301
* USA
*
* Permission is hereby granted to use or copy this program under the
* terms of the GNU LGPL, provided that the Copyright, this License,
* and the Availability of the original version is retained on all copies.
* User documentation of any code that uses this code or any modified
* version of this code must cite the Copyright, this License, the
* Availability note, and "Used by permission." Permission to modify
* the code and to distribute modified code is granted, provided the
* Copyright, this License, and the Availability note are retained,
* and a notice that the code was modified is included.
* See CCOLAMD/Doc/License.txt for the license.
*
* Availability:
*
* The CCOLAMD/CSYMAMD library is available at
*
* http://www.cise.ufl.edu/research/sparse/ccolamd/
*
* This is the http://www.cise.ufl.edu/research/sparse/ccolamd/ccolamd.c
* file.
* http://www.suitesparse.com
*
* See the ChangeLog file for changes since Version 1.0.
*/
@ -99,10 +71,10 @@
/* === Description of user-callable routines ================================ */
/* ========================================================================== */
/* CCOLAMD includes both int and UF_long versions of all its routines. The
* description below is for the int version. For UF_long, all int arguments
* become UF_long integers. UF_long is normally defined as long, except for
* WIN64 */
/* CCOLAMD includes both int and SuiteSparse_long versions of all its routines.
* The description below is for the int version. For SuiteSparse_long, all
* int arguments become SuiteSparse_long integers. SuiteSparse_long is
* normally defined as long, except for WIN64 */
/* ----------------------------------------------------------------------------
* ccolamd_recommended:
@ -112,8 +84,8 @@
*
* #include "ccolamd.h"
* size_t ccolamd_recommended (int nnz, int n_row, int n_col) ;
* size_t ccolamd_l_recommended (UF_long nnz, UF_long n_row,
* UF_long n_col) ;
* size_t ccolamd_l_recommended (SuiteSparse_long nnz,
* SuiteSparse_long n_row, SuiteSparse_long n_col) ;
*
* Purpose:
*
@ -209,9 +181,12 @@
* double knobs [CCOLAMD_KNOBS], int stats [CCOLAMD_STATS],
* int *cmember) ;
*
* UF_long ccolamd_l (UF_long n_row, UF_long n_col, UF_long Alen,
* UF_long *A, UF_long *p, double knobs [CCOLAMD_KNOBS],
* UF_long stats [CCOLAMD_STATS], UF_long *cmember) ;
* SuiteSparse_long ccolamd_l (SuiteSparse_long n_row,
* SuiteSparse_long n_col, SuiteSparse_long Alen,
* SuiteSparse_long *A, SuiteSparse_long *p,
* double knobs [CCOLAMD_KNOBS],
* SuiteSparse_long stats [CCOLAMD_STATS],
* SuiteSparse_long *cmember) ;
*
* Purpose:
*
@ -385,9 +360,7 @@
*
* Example:
*
* See
* http://www.cise.ufl.edu/research/sparse/ccolamd/ccolamd_example.c
* for a complete example.
* See ccolamd_example.c for a complete example.
*
* To order the columns of a 5-by-4 matrix with 11 nonzero entries in
* the following nonzero pattern
@ -423,10 +396,12 @@
* void (*allocate) (size_t, size_t), void (*release) (void *),
* int *cmember, int stype) ;
*
* UF_long csymamd_l (UF_long n, UF_long *A, UF_long *p, UF_long *perm,
* double knobs [CCOLAMD_KNOBS], UF_long stats [CCOLAMD_STATS],
* void (*allocate) (size_t, size_t), void (*release) (void *),
* UF_long *cmember, UF_long stype) ;
* SuiteSparse_long csymamd_l (SuiteSparse_long n,
* SuiteSparse_long *A, SuiteSparse_long *p,
* SuiteSparse_long *perm, double knobs [CCOLAMD_KNOBS],
* SuiteSparse_long stats [CCOLAMD_STATS], void (*allocate)
* (size_t, size_t), void (*release) (void *),
* SuiteSparse_long *cmember, SuiteSparse_long stype) ;
*
* Purpose:
*
@ -562,7 +537,7 @@
*
* #include "ccolamd.h"
* ccolamd_report (int stats [CCOLAMD_STATS]) ;
* ccolamd_l_report (UF_long stats [CCOLAMD_STATS]) ;
* ccolamd_l_report (SuiteSparse_long stats [CCOLAMD_STATS]) ;
*
* Purpose:
*
@ -583,7 +558,7 @@
*
* #include "ccolamd.h"
* csymamd_report (int stats [CCOLAMD_STATS]) ;
* csymamd_l_report (UF_long stats [CCOLAMD_STATS]) ;
* csymamd_l_report (SuiteSparse_long stats [CCOLAMD_STATS]) ;
*
* Purpose:
*
@ -617,12 +592,11 @@
#include "ccolamd.h"
#include <stdlib.h>
#include <math.h>
#include <limits.h>
#ifdef MATLAB_MEX_FILE
#include <stdint.h>
typedef uint16_t char16_t;
#include "mex.h"
#include "matrix.h"
#endif
@ -636,17 +610,14 @@ typedef uint16_t char16_t;
#endif
/* ========================================================================== */
/* === int or UF_long ======================================================= */
/* === int or SuiteSparse_long ============================================== */
/* ========================================================================== */
/* define UF_long */
#include "UFconfig.h"
#ifdef DLONG
#define Int UF_long
#define ID UF_long_id
#define Int_MAX UF_long_max
#define Int SuiteSparse_long
#define ID SuiteSparse_long_id
#define Int_MAX SuiteSparse_long_max
#define CCOLAMD_recommended ccolamd_l_recommended
#define CCOLAMD_set_defaults ccolamd_l_set_defaults
@ -811,9 +782,6 @@ typedef struct CColamd_Row_struct
#define INDEX(i) (i)
#endif
/* All output goes through the PRINTF macro. */
#define PRINTF(params) { if (ccolamd_printf != NULL) (void) ccolamd_printf params ; }
/* ========================================================================== */
/* === Debugging prototypes and definitions ================================= */
@ -827,11 +795,11 @@ typedef struct CColamd_Row_struct
PRIVATE Int ccolamd_debug ;
/* debug print statements */
#define DEBUG0(params) { PRINTF (params) ; }
#define DEBUG1(params) { if (ccolamd_debug >= 1) PRINTF (params) ; }
#define DEBUG2(params) { if (ccolamd_debug >= 2) PRINTF (params) ; }
#define DEBUG3(params) { if (ccolamd_debug >= 3) PRINTF (params) ; }
#define DEBUG4(params) { if (ccolamd_debug >= 4) PRINTF (params) ; }
#define DEBUG0(params) { SUITESPARSE_PRINTF (params) ; }
#define DEBUG1(params) { if (ccolamd_debug >= 1) SUITESPARSE_PRINTF (params) ; }
#define DEBUG2(params) { if (ccolamd_debug >= 2) SUITESPARSE_PRINTF (params) ; }
#define DEBUG3(params) { if (ccolamd_debug >= 3) SUITESPARSE_PRINTF (params) ; }
#define DEBUG4(params) { if (ccolamd_debug >= 4) SUITESPARSE_PRINTF (params) ; }
#ifdef MATLAB_MEX_FILE
#define ASSERT(expression) (mxAssert ((expression), ""))
@ -3752,12 +3720,12 @@ PRIVATE void print_report
Int i1, i2, i3 ;
PRINTF (("\n%s version %d.%d, %s: ", method,
SUITESPARSE_PRINTF (("\n%s version %d.%d, %s: ", method,
CCOLAMD_MAIN_VERSION, CCOLAMD_SUB_VERSION, CCOLAMD_DATE)) ;
if (!stats)
{
PRINTF (("No statistics available.\n")) ;
SUITESPARSE_PRINTF (("No statistics available.\n")) ;
return ;
}
@ -3767,11 +3735,11 @@ PRIVATE void print_report
if (stats [CCOLAMD_STATUS] >= 0)
{
PRINTF(("OK. ")) ;
SUITESPARSE_PRINTF(("OK. ")) ;
}
else
{
PRINTF(("ERROR. ")) ;
SUITESPARSE_PRINTF(("ERROR. ")) ;
}
switch (stats [CCOLAMD_STATUS])
@ -3779,91 +3747,105 @@ PRIVATE void print_report
case CCOLAMD_OK_BUT_JUMBLED:
PRINTF(("Matrix has unsorted or duplicate row indices.\n")) ;
SUITESPARSE_PRINTF((
"Matrix has unsorted or duplicate row indices.\n")) ;
PRINTF(("%s: duplicate or out-of-order row indices: "ID"\n",
method, i3)) ;
SUITESPARSE_PRINTF((
"%s: duplicate or out-of-order row indices: "ID"\n",
method, i3)) ;
PRINTF(("%s: last seen duplicate or out-of-order row: "ID"\n",
method, INDEX (i2))) ;
SUITESPARSE_PRINTF((
"%s: last seen duplicate or out-of-order row: "ID"\n",
method, INDEX (i2))) ;
PRINTF(("%s: last seen in column: "ID"",
method, INDEX (i1))) ;
SUITESPARSE_PRINTF((
"%s: last seen in column: "ID"",
method, INDEX (i1))) ;
/* no break - fall through to next case instead */
case CCOLAMD_OK:
PRINTF(("\n")) ;
SUITESPARSE_PRINTF(("\n")) ;
PRINTF(("%s: number of dense or empty rows ignored: "ID"\n",
method, stats [CCOLAMD_DENSE_ROW])) ;
SUITESPARSE_PRINTF((
"%s: number of dense or empty rows ignored: "ID"\n",
method, stats [CCOLAMD_DENSE_ROW])) ;
PRINTF(("%s: number of dense or empty columns ignored: "ID"\n",
method, stats [CCOLAMD_DENSE_COL])) ;
SUITESPARSE_PRINTF((
"%s: number of dense or empty columns ignored: "ID"\n",
method, stats [CCOLAMD_DENSE_COL])) ;
PRINTF(("%s: number of garbage collections performed: "ID"\n",
method, stats [CCOLAMD_DEFRAG_COUNT])) ;
SUITESPARSE_PRINTF((
"%s: number of garbage collections performed: "ID"\n",
method, stats [CCOLAMD_DEFRAG_COUNT])) ;
break ;
case CCOLAMD_ERROR_A_not_present:
PRINTF(("Array A (row indices of matrix) not present.\n")) ;
SUITESPARSE_PRINTF((
"Array A (row indices of matrix) not present.\n")) ;
break ;
case CCOLAMD_ERROR_p_not_present:
PRINTF(("Array p (column pointers for matrix) not present.\n")) ;
SUITESPARSE_PRINTF((
"Array p (column pointers for matrix) not present.\n")) ;
break ;
case CCOLAMD_ERROR_nrow_negative:
PRINTF(("Invalid number of rows ("ID").\n", i1)) ;
SUITESPARSE_PRINTF(("Invalid number of rows ("ID").\n", i1)) ;
break ;
case CCOLAMD_ERROR_ncol_negative:
PRINTF(("Invalid number of columns ("ID").\n", i1)) ;
SUITESPARSE_PRINTF(("Invalid number of columns ("ID").\n", i1)) ;
break ;
case CCOLAMD_ERROR_nnz_negative:
PRINTF(("Invalid number of nonzero entries ("ID").\n", i1)) ;
SUITESPARSE_PRINTF((
"Invalid number of nonzero entries ("ID").\n", i1)) ;
break ;
case CCOLAMD_ERROR_p0_nonzero:
PRINTF(("Invalid column pointer, p [0] = "ID", must be 0.\n", i1)) ;
SUITESPARSE_PRINTF((
"Invalid column pointer, p [0] = "ID", must be 0.\n", i1)) ;
break ;
case CCOLAMD_ERROR_A_too_small:
PRINTF(("Array A too small.\n")) ;
PRINTF((" Need Alen >= "ID", but given only Alen = "ID".\n",
i1, i2)) ;
SUITESPARSE_PRINTF(("Array A too small.\n")) ;
SUITESPARSE_PRINTF((
" Need Alen >= "ID", but given only Alen = "ID".\n",
i1, i2)) ;
break ;
case CCOLAMD_ERROR_col_length_negative:
PRINTF(("Column "ID" has a negative number of entries ("ID").\n",
INDEX (i1), i2)) ;
SUITESPARSE_PRINTF((
"Column "ID" has a negative number of entries ("ID").\n",
INDEX (i1), i2)) ;
break ;
case CCOLAMD_ERROR_row_index_out_of_bounds:
PRINTF(("Row index (row "ID") out of bounds ("ID" to "ID") in"
"column "ID".\n", INDEX (i2), INDEX (0), INDEX (i3-1),
INDEX (i1))) ;
SUITESPARSE_PRINTF((
"Row index (row "ID") out of bounds ("ID" to "ID") in"
"column "ID".\n", INDEX (i2), INDEX (0), INDEX (i3-1),
INDEX (i1))) ;
break ;
case CCOLAMD_ERROR_out_of_memory:
PRINTF(("Out of memory.\n")) ;
SUITESPARSE_PRINTF(("Out of memory.\n")) ;
break ;
case CCOLAMD_ERROR_invalid_cmember:
PRINTF(("cmember invalid\n")) ;
SUITESPARSE_PRINTF(("cmember invalid\n")) ;
break ;
}
}

21
gtsam/3rdparty/CMakeLists.txt vendored
View File

@ -1,6 +1,6 @@
# install CCOLAMD headers
install(FILES CCOLAMD/Include/ccolamd.h DESTINATION include/gtsam/3rdparty/CCOLAMD)
install(FILES UFconfig/UFconfig.h DESTINATION include/gtsam/3rdparty/UFconfig)
install(FILES SuiteSparse_config/SuiteSparse_config.h DESTINATION include/gtsam/3rdparty/SuiteSparse_config)
if(NOT GTSAM_USE_SYSTEM_EIGEN)
# Find plain .h files
@ -18,7 +18,7 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
# do the same for the unsupported eigen folder
file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h")
file(GLOB unsupported_eigen_dir_headers_all "Eigen/unsupported/Eigen/*")
foreach(unsupported_eigen_dir ${unsupported_eigen_dir_headers_all})
get_filename_component(filename ${unsupported_eigen_dir} NAME)
@ -36,12 +36,19 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
install(DIRECTORY Eigen/Eigen
DESTINATION include/gtsam/3rdparty/Eigen
FILES_MATCHING PATTERN "*.h")
install(DIRECTORY Eigen/unsupported/Eigen
DESTINATION include/gtsam/3rdparty/Eigen/unsupported/
FILES_MATCHING PATTERN "*.h")
endif()
option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF)
if(GTSAM_SUPPORT_NESTED_DISSECTION)
add_subdirectory(metis)
endif()
add_subdirectory(ceres)
############ NOTE: When updating GeographicLib be sure to disable building their examples
############ and unit tests by commenting out their lines:
# add_subdirectory (examples)
@ -53,10 +60,10 @@ endif()
include(GeographicLib/cmake/FindGeographicLib.cmake)
# Set up the option to install GeographicLib
if(GEOGRAPHICLIB_FOUND)
set(install_geographiclib_default OFF)
else()
if(GEOGRAPHICLIB-NOTFOUND)
set(install_geographiclib_default ON)
else()
set(install_geographiclib_default OFF)
endif()
option(GTSAM_INSTALL_GEOGRAPHICLIB "Build and install the 3rd-party library GeographicLib" ${install_geographiclib_default})
@ -68,3 +75,5 @@ endif()
if(GTSAM_INSTALL_GEOGRAPHICLIB)
add_subdirectory(GeographicLib)
endif()
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)

4
gtsam/3rdparty/Eigen/.hg_archival.txt vendored
View File

@ -1,4 +1,4 @@
repo: 8a21fd850624c931e448cbcfb38168cb2717c790
node: ffa86ffb557094721ca71dcea6aed2651b9fd610
node: 07105f7124f9aef00a68c85e0fc606e65d3d6c15
branch: 3.2
tag: 3.2.0
tag: 3.2.8

8
gtsam/3rdparty/Eigen/.hgtags vendored
View File

@ -23,3 +23,11 @@ bf4cb8c934fa3a79f45f1e629610f0225e93e493 3.1.0-rc2
da195914abcc1d739027cbee7c52077aab30b336 3.2-beta1
4b687cad1d23066f66863f4f87298447298443df 3.2-rc1
1eeda7b1258bcd306018c0738e2b6a8543661141 3.2-rc2
ffa86ffb557094721ca71dcea6aed2651b9fd610 3.2.0
6b38706d90a9fe182e66ab88477b3dbde34b9f66 3.2.1
1306d75b4a21891e59ff9bd96678882cf831e39f 3.2.2
36fd1ba04c120cfdd90f3e4cede47f43b21d19ad 3.2.3
10219c95fe653d4962aa9db4946f6fbea96dd740 3.2.4
bdd17ee3b1b3a166cd5ec36dcad4fc1f3faf774a 3.2.5
c58038c56923e0fd86de3ded18e03df442e66dfb 3.2.6
b30b87236a1b1552af32ac34075ee5696a9b5a33 3.2.7

66
gtsam/3rdparty/Eigen/CMakeLists.txt vendored
View File

@ -1,6 +1,5 @@
project(Eigen)
cmake_minimum_required(VERSION 2.8.2)
cmake_minimum_required(VERSION 2.8.5)
# guard against in-source builds
@ -55,6 +54,7 @@ endif(EIGEN_HG_CHANGESET)
include(CheckCXXCompilerFlag)
include(GNUInstallDirs)
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
@ -288,25 +288,26 @@ option(EIGEN_TEST_C++0x "Enables all C++0x features." OFF)
include_directories(${CMAKE_CURRENT_SOURCE_DIR} ${CMAKE_CURRENT_BINARY_DIR})
# the user modifiable install path for header files
set(EIGEN_INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR} CACHE PATH "The directory where we install the header files (optional)")
# set the internal install path for header files which depends on wether the user modifiable
# EIGEN_INCLUDE_INSTALL_DIR has been set by the user or not.
if(EIGEN_INCLUDE_INSTALL_DIR)
set(INCLUDE_INSTALL_DIR
${EIGEN_INCLUDE_INSTALL_DIR}
CACHE INTERNAL
"The directory where we install the header files (internal)"
)
# Backward compatibility support for EIGEN_INCLUDE_INSTALL_DIR
if(EIGEN_INCLUDE_INSTALL_DIR AND NOT INCLUDE_INSTALL_DIR)
set(INCLUDE_INSTALL_DIR ${EIGEN_INCLUDE_INSTALL_DIR}
CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed")
else()
set(INCLUDE_INSTALL_DIR
"${CMAKE_INSTALL_PREFIX}/include/eigen3"
CACHE INTERNAL
"The directory where we install the header files (internal)"
)
"${CMAKE_INSTALL_INCLUDEDIR}/eigen3"
CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen header files are installed"
)
endif()
set(CMAKEPACKAGE_INSTALL_DIR
"${CMAKE_INSTALL_LIBDIR}/cmake/eigen3"
CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where Eigen3Config.cmake is installed"
)
set(PKGCONFIG_INSTALL_DIR
"${CMAKE_INSTALL_DATADIR}/pkgconfig"
CACHE PATH "The directory relative to CMAKE_PREFIX_PATH where eigen3.pc is installed"
)
# similar to set_target_properties but append the property instead of overwriting it
macro(ei_add_target_property target prop value)
@ -324,21 +325,9 @@ install(FILES
)
if(EIGEN_BUILD_PKGCONFIG)
SET(path_separator ":")
STRING(REPLACE ${path_separator} ";" pkg_config_libdir_search "$ENV{PKG_CONFIG_LIBDIR}")
message(STATUS "searching for 'pkgconfig' directory in PKG_CONFIG_LIBDIR ( $ENV{PKG_CONFIG_LIBDIR} ), ${CMAKE_INSTALL_PREFIX}/share, and ${CMAKE_INSTALL_PREFIX}/lib")
FIND_PATH(pkg_config_libdir pkgconfig ${pkg_config_libdir_search} ${CMAKE_INSTALL_PREFIX}/share ${CMAKE_INSTALL_PREFIX}/lib ${pkg_config_libdir_search})
if(pkg_config_libdir)
SET(pkg_config_install_dir ${pkg_config_libdir})
message(STATUS "found ${pkg_config_libdir}/pkgconfig" )
else(pkg_config_libdir)
SET(pkg_config_install_dir ${CMAKE_INSTALL_PREFIX}/share)
message(STATUS "pkgconfig not found; installing in ${pkg_config_install_dir}" )
endif(pkg_config_libdir)
configure_file(eigen3.pc.in eigen3.pc)
configure_file(eigen3.pc.in eigen3.pc @ONLY)
install(FILES ${CMAKE_CURRENT_BINARY_DIR}/eigen3.pc
DESTINATION ${pkg_config_install_dir}/pkgconfig
DESTINATION ${PKGCONFIG_INSTALL_DIR}
)
endif(EIGEN_BUILD_PKGCONFIG)
@ -401,12 +390,15 @@ if(cmake_generator_tolower MATCHES "makefile")
message(STATUS "--------------+--------------------------------------------------------------")
message(STATUS "Command | Description")
message(STATUS "--------------+--------------------------------------------------------------")
message(STATUS "make install | Install to ${CMAKE_INSTALL_PREFIX}. To change that:")
message(STATUS " | cmake . -DCMAKE_INSTALL_PREFIX=yourpath")
message(STATUS " | Eigen headers will then be installed to:")
message(STATUS " | ${INCLUDE_INSTALL_DIR}")
message(STATUS " | To install Eigen headers to a separate location, do:")
message(STATUS " | cmake . -DEIGEN_INCLUDE_INSTALL_DIR=yourpath")
message(STATUS "make install | Install Eigen. Headers will be installed to:")
message(STATUS " | <CMAKE_INSTALL_PREFIX>/<INCLUDE_INSTALL_DIR>")
message(STATUS " | Using the following values:")
message(STATUS " | CMAKE_INSTALL_PREFIX: ${CMAKE_INSTALL_PREFIX}")
message(STATUS " | INCLUDE_INSTALL_DIR: ${INCLUDE_INSTALL_DIR}")
message(STATUS " | Change the install location of Eigen headers using:")
message(STATUS " | cmake . -DCMAKE_INSTALL_PREFIX=yourprefix")
message(STATUS " | Or:")
message(STATUS " | cmake . -DINCLUDE_INSTALL_DIR=yourdir")
message(STATUS "make doc | Generate the API documentation, requires Doxygen & LaTeX")
message(STATUS "make check | Build and run the unit-tests. Read this page:")
message(STATUS " | http://eigen.tuxfamily.org/index.php?title=Tests")

2
gtsam/3rdparty/Eigen/Eigen/CholmodSupport vendored
View File

@ -12,7 +12,7 @@ extern "C" {
/** \ingroup Support_modules
* \defgroup CholmodSupport_Module CholmodSupport module
*
* This module provides an interface to the Cholmod library which is part of the <a href="http://www.cise.ufl.edu/research/sparse/SuiteSparse/">suitesparse</a> package.
* This module provides an interface to the Cholmod library which is part of the <a href="http://www.suitesparse.com">suitesparse</a> package.
* It provides the two following main factorization classes:
* - class CholmodSupernodalLLT: a supernodal LLT Cholesky factorization.
* - class CholmodDecomposiiton: a general L(D)LT Cholesky factorization with automatic or explicit runtime selection of the underlying factorization method (supernodal or simplicial).

2
gtsam/3rdparty/Eigen/Eigen/Core vendored
View File

@ -123,7 +123,7 @@
#undef bool
#undef vector
#undef pixel
#elif defined __ARM_NEON__
#elif defined __ARM_NEON
#define EIGEN_VECTORIZE
#define EIGEN_VECTORIZE_NEON
#include <arm_neon.h>

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