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

Merge branch 'develop' into feature/RegularFactors 

Conflicts:
	gtsam/linear/GaussianFactorGraph.cpp
	gtsam/linear/JacobianFactor.cpp
	gtsam/linear/JacobianFactor.h
	tests/testPreconditioner.cpp
release/4.3a0
Sungtae An 2015-01-01 17:50:27 -05:00
parent 7091c2bd2e b6a7dc0530
commit e13243b140
727 changed files with 34216 additions and 11195 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

548
.cproject
View File

@ -1,17 +1,19 @@
<?xml version="1.0" encoding="UTF-8" standalone="no"?>
<?fileVersion 4.0.0?><cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<?fileVersion 4.0.0?>
<cproject storage_type_id="org.eclipse.cdt.core.XmlProjectDescriptionStorage">
<storageModule moduleId="org.eclipse.cdt.core.settings">
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544">
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544" moduleId="org.eclipse.cdt.core.settings" name="MacOSX GCC">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -19,7 +21,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" 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"/>
@ -60,13 +62,13 @@
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890" moduleId="org.eclipse.cdt.core.settings" name="Timing">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -116,13 +118,13 @@
<storageModule buildSystemId="org.eclipse.cdt.managedbuilder.core.configurationDataProvider" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" moduleId="org.eclipse.cdt.core.settings" name="fast">
<externalSettings/>
<extensions>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.GASErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GLDErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GCCErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.GmakeErrorParser" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.CWDLocator" point="org.eclipse.cdt.core.ErrorParser"/>
<extension id="org.eclipse.cdt.core.ELF" point="org.eclipse.cdt.core.BinaryParser"/>
<extension id="org.eclipse.cdt.core.MachO64" point="org.eclipse.cdt.core.BinaryParser"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
@ -130,7 +132,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 +178,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"/>
@ -532,6 +526,14 @@
<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="check" path="build/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -582,7 +584,6 @@
</target>
<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -590,7 +591,6 @@
</target>
<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testBinaryBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -638,7 +638,6 @@
</target>
<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -646,7 +645,6 @@
</target>
<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testSymbolicFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -654,7 +652,6 @@
</target>
<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -670,7 +667,6 @@
</target>
<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testBayesTree</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -724,14 +720,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 +776,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 +952,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 +976,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>
@ -1006,7 +1098,6 @@
</target>
<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testErrors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1236,46 +1327,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>
@ -1358,6 +1409,7 @@
</target>
<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2DOriented.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1397,6 +1449,7 @@
</target>
<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated2D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1404,6 +1457,7 @@
</target>
<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSimulated3D.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1417,6 +1471,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>
@ -1674,7 +1768,6 @@
</target>
<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G DEB</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1682,7 +1775,6 @@
</target>
<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G RPM</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1690,7 +1782,6 @@
</target>
<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>-G TGZ</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1698,7 +1789,6 @@
</target>
<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cpack</buildCommand>
<buildArguments/>
<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -1872,6 +1962,14 @@
<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" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2031,6 +2129,38 @@
<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="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2127,6 +2257,22 @@
<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="testDiscreteBayesTree.run" path="build/gtsam/discrete/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j1</buildArguments>
@ -2207,6 +2353,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 +2505,22 @@
<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="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2425,7 +2659,6 @@
</target>
<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2433,7 +2666,6 @@
</target>
<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2441,7 +2673,6 @@
</target>
<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNetB.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -2503,6 +2734,14 @@
<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="testParticleFactor.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2511,6 +2750,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testExpressionMeta.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testExpressionMeta.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCustomChartExpression.run" path="build/gtsam_unstable/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testCustomChartExpression.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2575,10 +2830,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 +2886,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 +2902,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRangeFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>testRangeFactor.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 +3086,22 @@
<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="testLago.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2871,6 +3142,38 @@
<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="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
@ -2919,6 +3222,46 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeAdaptAutoDiff.run" path="build/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>timeAdaptAutoDiff.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeCameraExpression.run" path="build/timing" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments>
<buildTarget>timeCameraExpression.run</buildTarget>
<stopOnError>true</stopOnError>
<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="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2929,6 +3272,7 @@
</target>
<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>tests/testGaussianISAM2</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -3030,22 +3374,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSpirit.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>check.wrap</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>

3
.gitignore vendored
View File

@ -1,7 +1,8 @@
/build*
/doc*
*.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

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>

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

24
CMakeLists.txt
View File

@ -9,8 +9,8 @@ if(NOT DEFINED CMAKE_MACOSX_RPATH)
endif()
# Set the version number for the library
set (GTSAM_VERSION_MAJOR 3)
set (GTSAM_VERSION_MINOR 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,7 +59,7 @@ 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)
@ -129,7 +129,7 @@ else()
endif()
if(${Boost_VERSION} EQUAL 105600)
if(NOT (${Boost_VERSION} LESS 105600))
message("Ignoring Boost restriction on optional lvalue assignment from rvalues")
add_definitions(-DBOOST_OPTIONAL_ALLOW_BINDING_TO_RVALUES)
endif()
@ -203,13 +203,13 @@ set(GTSAM_USE_SYSTEM_EIGEN OFF)
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}")
else()
# Use bundled Eigen include paths e.g. <gtsam/3rdparty/Eigen/Eigen/Core>
set(GTSAM_EIGEN_INCLUDE_PREFIX "gtsam/3rdparty/Eigen/")
# Clear any variables set by FindEigen3
if(EIGEN3_INCLUDE_DIR)
set(EIGEN3_INCLUDE_DIR NOTFOUND CACHE STRING "" FORCE)
@ -222,7 +222,6 @@ configure_file(gtsam/3rdparty/gtsam_eigen_includes.h.in gtsam/3rdparty/gtsam_eig
# Install the configuration file for Eigen
install(FILES ${PROJECT_BINARY_DIR}/gtsam/3rdparty/gtsam_eigen_includes.h DESTINATION include/gtsam/3rdparty)
###############################################################################
# Global compile options
@ -269,18 +268,18 @@ include_directories(BEFORE ${Boost_INCLUDE_DIR})
# 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/UFconfig
gtsam/3rdparty/CCOLAMD/Include
gtsam/3rdparty/metis-5.1.0/include
gtsam/3rdparty/metis-5.1.0/libmetis
gtsam/3rdparty/metis-5.1.0/GKlib
gtsam/3rdparty/metis/include
gtsam/3rdparty/metis/libmetis
gtsam/3rdparty/metis/GKlib
${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.
@ -331,6 +330,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)

6
CppUnitLite/Test.h
View File

@ -80,6 +80,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

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 the following are defined in the traits object:
* values:
* `enum { dimension = D};`, an enum that indicates the dimensionality *n* of the manifold. In Eigen-fashion, we also support manifolds whose dimenionality 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.

96
README.md
View File

@ -1,47 +1,51 @@
README - Georgia Tech Smoothing and Mapping library
===================================================
README - Georgia Tech Smoothing and Mapping library
===================================================
What is GTSAM?
--------------
GTSAM is a library of C++ classes that implement smoothing and
mapping (SAM) in robotics and vision, using factor graphs and Bayes
networks as the underlying computing paradigm rather than sparse
matrices.
On top of the C++ library, GTSAM includes a MATLAB interface (enable
GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
is under development.
Quickstart
----------
In the root library folder execute:
```
#!bash
$ mkdir build
$ cd build
$ cmake ..
$ make check (optional, runs unit tests)
$ make install
```
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`)
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)
Additional Information
----------------------
Read about important [`GTSAM-Concepts`](GTSAM-Concepts.md) here.
See the [`INSTALL`](INSTALL) file for more detailed installation instructions.
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.
What is GTSAM?
--------------
GTSAM is a library of C++ classes that implement smoothing and
mapping (SAM) in robotics and vision, using factor graphs and Bayes
networks as the underlying computing paradigm rather than sparse
matrices.
On top of the C++ library, GTSAM includes a MATLAB interface (enable
GTSAM_INSTALL_MATLAB_TOOLBOX in CMake to build it). A Python interface
is under development.
Quickstart
----------
In the root library folder execute:
```
#!bash
$ mkdir build
$ cd build
$ cmake ..
$ make check (optional, runs unit tests)
$ make install
```
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`)
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)
Additional Information
----------------------
See the [`INSTALL`](https://bitbucket.org/gtborg/gtsam/src/develop/INSTALL) file for more detailed installation instructions.
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.
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 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).

55
THANKS
View File

@ -1,20 +1,43 @@
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
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

8
cmake/GtsamBuildTypes.cmake
View File

@ -15,8 +15,8 @@ option(GTSAM_BUILD_TYPE_POSTFIXES "Enable/Disable appending the build typ
# 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_DEBUG "/D_DEBUG /MDd /Zi /Ob0 /Od /RTC1 /W3 /GR /EHsc /MP /DWINDOWS_LEAN_AND_MEAN /DEIGEN_INITIALIZE_MATRICES_BY_NAN" 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 /DEIGEN_INITIALIZE_MATRICES_BY_NAN" 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)
@ -34,8 +34,8 @@ if(NOT FIRST_PASS_DONE)
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_DEBUG "${CMAKE_C_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" CACHE STRING "Flags used by the compiler during debug builds." FORCE)
set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS_DEBUG} -fno-inline -Wall -DEIGEN_INITIALIZE_MATRICES_BY_NAN" 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)

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 *)

4
examples/CameraResectioning.cpp
View File

@ -48,7 +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_);
Point2 reprojectionError(camera.project(P_, H, boost::none, boost::none) - p_);
return reprojectionError.vector();
}
};
@ -70,7 +70,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,

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));

6
examples/PlanarSLAMExample.cpp
View File

@ -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

88
examples/Pose2SLAMExampleExpressions.cpp Normal file
View File

@ -0,0 +1,88 @@
/* ----------------------------------------------------------------------------
* 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
* @author Yong Dian Jian
*/
// 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/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <gtsam/nonlinear/Marginals.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/inference/Key.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;
}

2
examples/Pose2SLAMExample_g2o.cpp
View File

@ -64,7 +64,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;

4
examples/Pose2SLAMExample_graph.cpp
View File

@ -31,14 +31,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

4
examples/Pose2SLAMExample_graphviz.cpp
View File

@ -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));

2
examples/Pose2SLAMExample_lago.cpp
View File

@ -43,7 +43,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();

6
examples/Pose2SLAMwSPCG.cpp
View File

@ -68,12 +68,12 @@ 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)
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));
noiseModel::Diagonal::shared_ptr odometryNoise = noiseModel::Diagonal::Sigmas(Vector3(0.2, 0.2, 0.1));
// Create odometry (Between) factors between consecutive poses
graph.push_back(BetweenFactor<Pose2>(1, 2, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
graph.push_back(BetweenFactor<Pose2>(2, 3, Pose2(2.0, 0.0, M_PI_2), odometryNoise));
@ -85,7 +85,7 @@ int main(int argc, char** argv) {
// 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

2
examples/Pose3SLAMExample_g2o.cpp
View File

@ -43,7 +43,7 @@ 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) {
std::cout << "Adding prior to g2o file " << std::endl;

2
examples/Pose3SLAMExample_initializePose3Chordal.cpp
View File

@ -43,7 +43,7 @@ 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) {
std::cout << "Adding prior to g2o file " << std::endl;

2
examples/Pose3SLAMExample_initializePose3Gradient.cpp
View File

@ -43,7 +43,7 @@ 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) {
std::cout << "Adding prior to g2o file " << std::endl;

6
examples/SFMExample.cpp
View File

@ -80,13 +80,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));
}
@ -111,6 +111,8 @@ int main(int argc, char* argv[]) {
/* 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::rodriguez(-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(Symbol('l', j), points[j].compose(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;
}
/* ************************************************************************* */

5
examples/SFMExample_SmartFactor.cpp
View File

@ -61,8 +61,7 @@ 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 gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Cal3_S2> SmartFactor;
/* ************************************************************************* */
int main(int argc, char* argv[]) {
@ -108,7 +107,7 @@ 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)));
(Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1)).finished());
graph.push_back(PriorFactor<Pose3>(0, poses[0], poseNoise));
// Because the structure-from-motion problem has a scale ambiguity, the problem is

4
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

38
examples/SolverComparer.cpp
View File

@ -72,22 +72,22 @@ 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);
//GTSAM_VALUE_EXPORT(Value);
//GTSAM_VALUE_EXPORT(Pose);
//GTSAM_VALUE_EXPORT(Rot2);
//GTSAM_VALUE_EXPORT(Point2);
//GTSAM_VALUE_EXPORT(NonlinearFactor);
//GTSAM_VALUE_EXPORT(NoiseModelFactor);
//GTSAM_VALUE_EXPORT(NM1);
//GTSAM_VALUE_EXPORT(NM2);
//GTSAM_VALUE_EXPORT(BetweenFactor<Pose>);
//GTSAM_VALUE_EXPORT(PriorFactor<Pose>);
//GTSAM_VALUE_EXPORT(BR);
//GTSAM_VALUE_EXPORT(noiseModel::Base);
//GTSAM_VALUE_EXPORT(noiseModel::Isotropic);
//GTSAM_VALUE_EXPORT(noiseModel::Gaussian);
//GTSAM_VALUE_EXPORT(noiseModel::Diagonal);
//GTSAM_VALUE_EXPORT(noiseModel::Unit);
double chi2_red(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& config) {
// Compute degrees of freedom (observations - variables)
@ -295,7 +295,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);
@ -474,7 +474,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;
@ -589,7 +589,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;

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 = LevenbergMarquardtOptimizer(graph, initial_estimate, params);
Values result = optimizer.optimize();
cout << "Final result sample:" << endl;

2
examples/VisualISAM2Example.cpp
View File

@ -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

2
examples/VisualISAMExample.cpp
View File

@ -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

8
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
@ -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);

285
gtsam.h
View File

@ -157,7 +157,7 @@ virtual class Value {
};
#include <gtsam/base/LieScalar.h>
virtual class LieScalar : gtsam::Value {
class LieScalar {
// Standard constructors
LieScalar();
LieScalar(double d);
@ -186,7 +186,7 @@ virtual class LieScalar : gtsam::Value {
};
#include <gtsam/base/LieVector.h>
virtual class LieVector : gtsam::Value {
class LieVector {
// Standard constructors
LieVector();
LieVector(Vector v);
@ -218,7 +218,7 @@ virtual class LieVector : gtsam::Value {
};
#include <gtsam/base/LieMatrix.h>
virtual class LieMatrix : gtsam::Value {
class LieMatrix {
// Standard constructors
LieMatrix();
LieMatrix(Matrix v);
@ -253,7 +253,7 @@ virtual class LieMatrix : gtsam::Value {
// geometry
//*************************************************************************
virtual class Point2 : gtsam::Value {
class Point2 {
// Standard Constructors
Point2();
Point2(double x, double y);
@ -270,8 +270,6 @@ virtual class Point2 : gtsam::Value {
gtsam::Point2 between(const gtsam::Point2& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Point2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Point2& p) const;
@ -290,7 +288,7 @@ virtual class Point2 : gtsam::Value {
void serialize() const;
};
virtual class StereoPoint2 : gtsam::Value {
class StereoPoint2 {
// Standard Constructors
StereoPoint2();
StereoPoint2(double uL, double uR, double v);
@ -325,7 +323,7 @@ virtual class StereoPoint2 : gtsam::Value {
void serialize() const;
};
virtual class Point3 : gtsam::Value {
class Point3 {
// Standard Constructors
Point3();
Point3(double x, double y, double z);
@ -342,8 +340,6 @@ virtual class Point3 : gtsam::Value {
gtsam::Point3 between(const gtsam::Point3& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Point3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Point3& p) const;
@ -361,7 +357,7 @@ virtual class Point3 : gtsam::Value {
void serialize() const;
};
virtual class Rot2 : gtsam::Value {
class Rot2 {
// Standard Constructors and Named Constructors
Rot2();
Rot2(double theta);
@ -380,8 +376,6 @@ virtual class Rot2 : gtsam::Value {
gtsam::Rot2 between(const gtsam::Rot2& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Rot2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Rot2& p) const;
@ -406,7 +400,7 @@ virtual class Rot2 : gtsam::Value {
void serialize() const;
};
virtual class Rot3 : gtsam::Value {
class Rot3 {
// Standard Constructors and Named Constructors
Rot3();
Rot3(Matrix R);
@ -433,8 +427,6 @@ virtual class Rot3 : gtsam::Value {
gtsam::Rot3 between(const gtsam::Rot3& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
//gtsam::Rot3 retractCayley(Vector v) const; // FIXME, does not exist in both Matrix and Quaternion options
gtsam::Rot3 retract(Vector v) const;
Vector localCoordinates(const gtsam::Rot3& p) const;
@ -462,7 +454,7 @@ virtual class Rot3 : gtsam::Value {
void serialize() const;
};
virtual class Pose2 : gtsam::Value {
class Pose2 {
// Standard Constructor
Pose2();
Pose2(const gtsam::Pose2& pose);
@ -482,8 +474,6 @@ virtual class Pose2 : gtsam::Value {
gtsam::Pose2 between(const gtsam::Pose2& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Pose2 retract(Vector v) const;
Vector localCoordinates(const gtsam::Pose2& p) const;
@ -512,7 +502,7 @@ virtual class Pose2 : gtsam::Value {
void serialize() const;
};
virtual class Pose3 : gtsam::Value {
class Pose3 {
// Standard Constructors
Pose3();
Pose3(const gtsam::Pose3& pose);
@ -531,10 +521,7 @@ virtual class Pose3 : gtsam::Value {
gtsam::Pose3 between(const gtsam::Pose3& p2) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Pose3 retract(Vector v) const;
gtsam::Pose3 retractFirstOrder(Vector v) const;
Vector localCoordinates(const gtsam::Pose3& T2) const;
// Lie Group
@ -564,7 +551,7 @@ virtual class Pose3 : gtsam::Value {
};
#include <gtsam/geometry/Unit3.h>
virtual class Unit3 : gtsam::Value {
class Unit3 {
// Standard Constructors
Unit3();
Unit3(const gtsam::Point3& pose);
@ -585,7 +572,7 @@ virtual class Unit3 : gtsam::Value {
};
#include <gtsam/geometry/EssentialMatrix.h>
virtual class EssentialMatrix : gtsam::Value {
class EssentialMatrix {
// Standard Constructors
EssentialMatrix(const gtsam::Rot3& aRb, const gtsam::Unit3& aTb);
@ -606,7 +593,7 @@ virtual class EssentialMatrix : gtsam::Value {
double error(Vector vA, Vector vB);
};
virtual class Cal3_S2 : gtsam::Value {
class Cal3_S2 {
// Standard Constructors
Cal3_S2();
Cal3_S2(double fx, double fy, double s, double u0, double v0);
@ -643,7 +630,7 @@ virtual class Cal3_S2 : gtsam::Value {
};
#include <gtsam/geometry/Cal3DS2.h>
virtual class Cal3DS2 : gtsam::Value {
class Cal3DS2 {
// Standard Constructors
Cal3DS2();
Cal3DS2(double fx, double fy, double s, double u0, double v0, double k1, double k2, double k3, double k4);
@ -699,7 +686,43 @@ class Cal3_S2Stereo {
double baseline() const;
};
virtual class CalibratedCamera : gtsam::Value {
#include <gtsam/geometry/Cal3Bundler.h>
class Cal3Bundler {
// Standard Constructors
Cal3Bundler();
Cal3Bundler(double fx, double k1, double k2, double u0, double v0);
// Testable
void print(string s) const;
bool equals(const gtsam::Cal3Bundler& rhs, double tol) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::Cal3Bundler retract(Vector v) const;
Vector localCoordinates(const gtsam::Cal3Bundler& c) const;
// Action on Point2
gtsam::Point2 calibrate(const gtsam::Point2& p, double tol) const;
gtsam::Point2 calibrate(const gtsam::Point2& p) const;
gtsam::Point2 uncalibrate(const gtsam::Point2& p) const;
// Standard Interface
double fx() const;
double fy() const;
double k1() const;
double k2() const;
double u0() const;
double v0() const;
Vector vector() const;
Vector k() const;
//Matrix K() const; //FIXME: Uppercase
// enabling serialization functionality
void serialize() const;
};
class CalibratedCamera {
// Standard Constructors and Named Constructors
CalibratedCamera();
CalibratedCamera(const gtsam::Pose3& pose);
@ -716,10 +739,6 @@ virtual class CalibratedCamera : gtsam::Value {
gtsam::CalibratedCamera retract(const Vector& d) const;
Vector localCoordinates(const gtsam::CalibratedCamera& T2) const;
// Group
gtsam::CalibratedCamera compose(const gtsam::CalibratedCamera& c) const;
gtsam::CalibratedCamera inverse() const;
// Action on Point3
gtsam::Point2 project(const gtsam::Point3& point) const;
static gtsam::Point2 project_to_camera(const gtsam::Point3& cameraPoint);
@ -732,7 +751,7 @@ virtual class CalibratedCamera : gtsam::Value {
void serialize() const;
};
virtual class SimpleCamera : gtsam::Value {
class SimpleCamera {
// Standard Constructors and Named Constructors
SimpleCamera();
SimpleCamera(const gtsam::Pose3& pose);
@ -771,7 +790,7 @@ virtual class SimpleCamera : gtsam::Value {
};
template<CALIBRATION = {gtsam::Cal3DS2}>
virtual class PinholeCamera : gtsam::Value {
class PinholeCamera {
// Standard Constructors and Named Constructors
PinholeCamera();
PinholeCamera(const gtsam::Pose3& pose);
@ -809,7 +828,7 @@ virtual class PinholeCamera : gtsam::Value {
void serialize() const;
};
virtual class StereoCamera : gtsam::Value {
class StereoCamera {
// Standard Constructors and Named Constructors
StereoCamera();
StereoCamera(const gtsam::Pose3& pose, const gtsam::Cal3_S2Stereo* K);
@ -862,7 +881,7 @@ virtual class SymbolicFactor {
};
#include <gtsam/symbolic/SymbolicFactorGraph.h>
class SymbolicFactorGraph {
virtual class SymbolicFactorGraph {
SymbolicFactorGraph();
SymbolicFactorGraph(const gtsam::SymbolicBayesNet& bayesNet);
SymbolicFactorGraph(const gtsam::SymbolicBayesTree& bayesTree);
@ -1192,6 +1211,7 @@ class VectorValues {
#include <gtsam/linear/GaussianFactor.h>
virtual class GaussianFactor {
gtsam::KeyVector keys() const;
void print(string s) const;
bool equals(const gtsam::GaussianFactor& lf, double tol) const;
double error(const gtsam::VectorValues& c) const;
@ -1615,6 +1635,7 @@ class NonlinearFactorGraph {
void push_back(gtsam::NonlinearFactor* factor);
void add(gtsam::NonlinearFactor* factor);
bool exists(size_t idx) const;
gtsam::KeySet keys() const;
// NonlinearFactorGraph
double error(const gtsam::Values& values) const;
@ -1664,15 +1685,12 @@ class Values {
void print(string s) const;
bool equals(const gtsam::Values& other, double tol) const;
void insert(size_t j, const gtsam::Value& value);
void insert(const gtsam::Values& values);
void update(size_t j, const gtsam::Value& val);
void update(const gtsam::Values& values);
void erase(size_t j);
void swap(gtsam::Values& values);
bool exists(size_t j) const;
gtsam::Value at(size_t j) const;
gtsam::KeyList keys() const;
gtsam::VectorValues zeroVectors() const;
@ -1682,6 +1700,57 @@ class Values {
// enabling serialization functionality
void serialize() const;
// New in 4.0, we have to specialize every insert/update/at to generate wrappers
// Instead of the old:
// void insert(size_t j, const gtsam::Value& value);
// void update(size_t j, const gtsam::Value& val);
// gtsam::Value at(size_t j) const;
void insert(size_t j, const gtsam::Point2& t);
void insert(size_t j, const gtsam::Point3& t);
void insert(size_t j, const gtsam::Rot2& t);
void insert(size_t j, const gtsam::Pose2& t);
void insert(size_t j, const gtsam::Rot3& t);
void insert(size_t j, const gtsam::Pose3& t);
void insert(size_t j, const gtsam::Cal3_S2& t);
void insert(size_t j, const gtsam::Cal3DS2& t);
void insert(size_t j, const gtsam::Cal3Bundler& t);
void insert(size_t j, const gtsam::EssentialMatrix& t);
void insert(size_t j, const gtsam::SimpleCamera& t);
void insert(size_t j, const gtsam::imuBias::ConstantBias& t);
void insert(size_t j, Vector t);
void insert(size_t j, Matrix t);
// Fixed size version
void insertFixed(size_t j, Vector t, size_t n);
void update(size_t j, const gtsam::Point2& t);
void update(size_t j, const gtsam::Point3& t);
void update(size_t j, const gtsam::Rot2& t);
void update(size_t j, const gtsam::Pose2& t);
void update(size_t j, const gtsam::Rot3& t);
void update(size_t j, const gtsam::Pose3& t);
void update(size_t j, const gtsam::Cal3_S2& t);
void update(size_t j, const gtsam::Cal3DS2& t);
void update(size_t j, const gtsam::Cal3Bundler& t);
void update(size_t j, const gtsam::EssentialMatrix& t);
void update(size_t j, const gtsam::imuBias::ConstantBias& t);
void update(size_t j, Vector t);
void update(size_t j, Matrix t);
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Pose2, gtsam::Rot3, gtsam::Pose3, gtsam::Cal3_S2, gtsam::Cal3DS2, gtsam::Cal3Bundler, gtsam::EssentialMatrix, gtsam::imuBias::ConstantBias, Vector, Matrix}>
T at(size_t j);
/// Fixed size versions, for n in 1..9
void insertFixed(size_t j, Vector t, size_t n);
/// Fixed size versions, for n in 1..9
Vector atFixed(size_t j, size_t n);
/// version for double
void insertDouble(size_t j, double c);
double atDouble(size_t j) const;
};
// Actually a FastList<Key>
@ -1779,6 +1848,7 @@ class KeyGroupMap {
class Marginals {
Marginals(const gtsam::NonlinearFactorGraph& graph,
const gtsam::Values& solution);
void print(string s) const;
Matrix marginalCovariance(size_t variable) const;
Matrix marginalInformation(size_t variable) const;
@ -2077,7 +2147,7 @@ class NonlinearISAM {
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/slam/PriorFactor.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
template<T = {Vector, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2,gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class PriorFactor : gtsam::NoiseModelFactor {
PriorFactor(size_t key, const T& prior, const gtsam::noiseModel::Base* noiseModel);
T prior() const;
@ -2088,7 +2158,7 @@ virtual class PriorFactor : gtsam::NoiseModelFactor {
#include <gtsam/slam/BetweenFactor.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
template<T = {gtsam::Point2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::imuBias::ConstantBias}>
virtual class BetweenFactor : gtsam::NoiseModelFactor {
BetweenFactor(size_t key1, size_t key2, const T& relativePose, const gtsam::noiseModel::Base* noiseModel);
T measured() const;
@ -2098,8 +2168,9 @@ virtual class BetweenFactor : gtsam::NoiseModelFactor {
};
#include <gtsam/nonlinear/NonlinearEquality.h>
template<T = {gtsam::LieScalar, gtsam::LieVector, gtsam::LieMatrix, gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
template<T = {gtsam::Point2, gtsam::StereoPoint2, gtsam::Point3, gtsam::Rot2, gtsam::Rot3, gtsam::Pose2, gtsam::Pose3, gtsam::Cal3_S2, gtsam::CalibratedCamera, gtsam::SimpleCamera, gtsam::imuBias::ConstantBias}>
virtual class NonlinearEquality : gtsam::NoiseModelFactor {
// Constructor - forces exact evaluation
NonlinearEquality(size_t j, const T& feasible);
@ -2121,10 +2192,14 @@ typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Point2> RangeFactorPosePoint2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Point3> RangeFactorPosePoint3;
typedef gtsam::RangeFactor<gtsam::Pose2, gtsam::Pose2> RangeFactorPose2;
typedef gtsam::RangeFactor<gtsam::Pose3, gtsam::Pose3> RangeFactorPose3;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
// Commented out by Frank Dec 2014: not poses!
// If needed, we need a RangeFactor that takes a camera, extracts the pose
// Should be easy with Expressions
//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::Point3> RangeFactorCalibratedCameraPoint;
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::Point3> RangeFactorSimpleCameraPoint;
//typedef gtsam::RangeFactor<gtsam::CalibratedCamera, gtsam::CalibratedCamera> RangeFactorCalibratedCamera;
//typedef gtsam::RangeFactor<gtsam::SimpleCamera, gtsam::SimpleCamera> RangeFactorSimpleCamera;
#include <gtsam/slam/BearingFactor.h>
@ -2198,7 +2273,7 @@ virtual class GeneralSFMFactor2 : gtsam::NoiseModelFactor {
};
#include <gtsam/slam/SmartProjectionPoseFactor.h>
template<POSE, LANDMARK, CALIBRATION>
template<POSE, CALIBRATION>
virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
SmartProjectionPoseFactor(double rankTol, double linThreshold,
@ -2214,7 +2289,7 @@ virtual class SmartProjectionPoseFactor : gtsam::NonlinearFactor {
//void serialize() const;
};
typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2> SmartProjectionPose3Factor;
typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Cal3_S2> SmartProjectionPose3Factor;
#include <gtsam/slam/StereoFactor.h>
@ -2280,7 +2355,7 @@ void writeG2o(const gtsam::NonlinearFactorGraph& graph,
namespace imuBias {
#include <gtsam/navigation/ImuBias.h>
virtual class ConstantBias : gtsam::Value {
class ConstantBias {
// Standard Constructor
ConstantBias();
ConstantBias(Vector biasAcc, Vector biasGyro);
@ -2296,8 +2371,6 @@ virtual class ConstantBias : gtsam::Value {
gtsam::imuBias::ConstantBias between(const gtsam::imuBias::ConstantBias& b) const;
// Manifold
static size_t Dim();
size_t dim() const;
gtsam::imuBias::ConstantBias retract(Vector v) const;
Vector localCoordinates(const gtsam::imuBias::ConstantBias& b) const;
@ -2316,19 +2389,36 @@ virtual class ConstantBias : gtsam::Value {
}///\namespace imuBias
#include <gtsam/navigation/ImuFactor.h>
class PoseVelocityBias{
PoseVelocityBias(const gtsam::Pose3& pose, Vector velocity, const gtsam::imuBias::ConstantBias& bias);
};
class ImuFactorPreintegratedMeasurements {
// Standard Constructor
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance, bool use2ndOrderIntegration);
ImuFactorPreintegratedMeasurements(const gtsam::imuBias::ConstantBias& bias, Matrix measuredAccCovariance,Matrix measuredOmegaCovariance, Matrix integrationErrorCovariance);
ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
// ImuFactorPreintegratedMeasurements(const gtsam::ImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::ImuFactorPreintegratedMeasurements& expected, double tol);
double deltaTij() const;
Matrix deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
Matrix delPdelBiasAcc() const;
Matrix delPdelBiasOmega() const;
Matrix delVdelBiasAcc() const;
Matrix delVdelBiasOmega() const;
Matrix delRdelBiasOmega() const;
Matrix preintMeasCov() const;
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
Vector gravity, Vector omegaCoriolis) const;
};
virtual class ImuFactor : gtsam::NonlinearFactor {
@ -2337,13 +2427,8 @@ virtual class ImuFactor : gtsam::NonlinearFactor {
ImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements, Vector gravity, Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::ImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
const gtsam::imuBias::ConstantBias& bias,
const gtsam::ImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
};
#include <gtsam/navigation/CombinedImuFactor.h>
@ -2366,27 +2451,101 @@ class CombinedImuFactorPreintegratedMeasurements {
Matrix biasOmegaCovariance,
Matrix biasAccOmegaInit,
bool use2ndOrderIntegration);
CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
// CombinedImuFactorPreintegratedMeasurements(const gtsam::CombinedImuFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::CombinedImuFactorPreintegratedMeasurements& expected, double tol);
double deltaTij() const;
Matrix deltaRij() const;
Vector deltaPij() const;
Vector deltaVij() const;
Vector biasHatVector() const;
Matrix delPdelBiasAcc() const;
Matrix delPdelBiasOmega() const;
Matrix delVdelBiasAcc() const;
Matrix delVdelBiasOmega() const;
Matrix delRdelBiasOmega() const;
Matrix preintMeasCov() const;
// Standard Interface
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredAcc, Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
gtsam::PoseVelocityBias predict(const gtsam::Pose3& pose_i, Vector vel_i, const gtsam::imuBias::ConstantBias& bias,
Vector gravity, Vector omegaCoriolis) const;
};
virtual class CombinedImuFactor : gtsam::NonlinearFactor {
CombinedImuFactor(size_t pose_i, size_t vel_i, size_t pose_j, size_t vel_j, size_t bias_i, size_t bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& CombinedPreintegratedMeasurements, Vector gravity, Vector omegaCoriolis);
// Standard Interface
gtsam::CombinedImuFactorPreintegratedMeasurements preintegratedMeasurements() const;
void Predict(const gtsam::Pose3& pose_i, const gtsam::LieVector& vel_i, gtsam::Pose3& pose_j, gtsam::LieVector& vel_j,
const gtsam::imuBias::ConstantBias& bias_i, const gtsam::imuBias::ConstantBias& bias_j,
const gtsam::CombinedImuFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector gravity, Vector omegaCoriolis) const;
};
#include <gtsam/navigation/AHRSFactor.h>
class AHRSFactorPreintegratedMeasurements {
// Standard Constructor
AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance);
AHRSFactorPreintegratedMeasurements(Vector bias, Matrix measuredOmegaCovariance);
AHRSFactorPreintegratedMeasurements(const gtsam::AHRSFactorPreintegratedMeasurements& rhs);
// Testable
void print(string s) const;
bool equals(const gtsam::AHRSFactorPreintegratedMeasurements& expected, double tol);
// get Data
Matrix deltaRij() const;
double deltaTij() const;
Vector biasHat() const;
// Standard Interface
void integrateMeasurement(Vector measuredOmega, double deltaT);
void integrateMeasurement(Vector measuredOmega, double deltaT, const gtsam::Pose3& body_P_sensor);
void resetIntegration() ;
};
virtual class AHRSFactor : gtsam::NonlinearFactor {
AHRSFactor(size_t rot_i, size_t rot_j,size_t bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis);
AHRSFactor(size_t rot_i, size_t rot_j, size_t bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements, Vector omegaCoriolis,
const gtsam::Pose3& body_P_sensor);
// Standard Interface
gtsam::AHRSFactorPreintegratedMeasurements preintegratedMeasurements() const;
Vector evaluateError(const gtsam::Rot3& rot_i, const gtsam::Rot3& rot_j,
Vector bias) const;
gtsam::Rot3 predict(const gtsam::Rot3& rot_i, Vector bias,
const gtsam::AHRSFactorPreintegratedMeasurements& preintegratedMeasurements,
Vector omegaCoriolis) const;
};
#include <gtsam/navigation/AttitudeFactor.h>
//virtual class AttitudeFactor : gtsam::NonlinearFactor {
// AttitudeFactor(const Unit3& nZ, const Unit3& bRef);
// AttitudeFactor();
//};
virtual class Rot3AttitudeFactor : gtsam::NonlinearFactor{
Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
const gtsam::Unit3& bRef);
Rot3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
Rot3AttitudeFactor();
void print(string s) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nZ() const;
gtsam::Unit3 bRef() const;
};
virtual class Pose3AttitudeFactor : gtsam::NonlinearFactor{
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model,
const gtsam::Unit3& bRef);
Pose3AttitudeFactor(size_t key, const gtsam::Unit3& nZ, const gtsam::noiseModel::Diagonal* model);
Pose3AttitudeFactor();
void print(string s) const;
bool equals(const gtsam::NonlinearFactor& expected, double tol) const;
gtsam::Unit3 nZ() const;
gtsam::Unit3 bRef() const;
};
//*************************************************************************

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.

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

@ -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,17 +36,17 @@ 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 "Build metis library" ON)
option(GTSAM_BUILD_METIS_EXECUTABLES "Build metis library executables" OFF)
if(GTSAM_BUILD_METIS)
add_subdirectory(metis-5.1.0)
endif(GTSAM_BUILD_METIS)
add_subdirectory(metis)
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)
@ -73,3 +73,5 @@ endif()
if(GTSAM_INSTALL_GEOGRAPHICLIB)
add_subdirectory(GeographicLib)
endif()
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)

80
gtsam/3rdparty/Eigen/Eigen/src/Core/CommaInitializer.h vendored
View File

@ -25,14 +25,10 @@ namespace Eigen {
* \sa \ref MatrixBaseCommaInitRef "MatrixBase::operator<<", CommaInitializer::finished()
*/
template<typename XprType>
struct CommaInitializer :
public internal::dense_xpr_base<CommaInitializer<XprType> >::type
struct CommaInitializer
{
typedef typename internal::dense_xpr_base<CommaInitializer<XprType> >::type Base;
EIGEN_DENSE_PUBLIC_INTERFACE(CommaInitializer)
typedef typename internal::conditional<internal::must_nest_by_value<XprType>::ret,
XprType, const XprType&>::type ExpressionTypeNested;
typedef typename XprType::InnerIterator InnerIterator;
typedef typename XprType::Scalar Scalar;
typedef typename XprType::Index Index;
inline CommaInitializer(XprType& xpr, const Scalar& s)
: m_xpr(xpr), m_row(0), m_col(1), m_currentBlockRows(1)
@ -119,82 +115,12 @@ struct CommaInitializer :
*/
inline XprType& finished() { return m_xpr; }
// The following implement the DenseBase interface
inline Index rows() const { return m_xpr.rows(); }
inline Index cols() const { return m_xpr.cols(); }
inline Index outerStride() const { return m_xpr.outerStride(); }
inline Index innerStride() const { return m_xpr.innerStride(); }
inline CoeffReturnType coeff(Index row, Index col) const
{
return m_xpr.coeff(row, col);
}
inline CoeffReturnType coeff(Index index) const
{
return m_xpr.coeff(index);
}
inline const Scalar& coeffRef(Index row, Index col) const
{
return m_xpr.const_cast_derived().coeffRef(row, col);
}
inline const Scalar& coeffRef(Index index) const
{
return m_xpr.const_cast_derived().coeffRef(index);
}
inline Scalar& coeffRef(Index row, Index col)
{
return m_xpr.const_cast_derived().coeffRef(row, col);
}
inline Scalar& coeffRef(Index index)
{
return m_xpr.const_cast_derived().coeffRef(index);
}
template<int LoadMode>
inline const PacketScalar packet(Index row, Index col) const
{
return m_xpr.template packet<LoadMode>(row, col);
}
template<int LoadMode>
inline void writePacket(Index row, Index col, const PacketScalar& x)
{
m_xpr.const_cast_derived().template writePacket<LoadMode>(row, col, x);
}
template<int LoadMode>
inline const PacketScalar packet(Index index) const
{
return m_xpr.template packet<LoadMode>(index);
}
template<int LoadMode>
inline void writePacket(Index index, const PacketScalar& x)
{
m_xpr.const_cast_derived().template writePacket<LoadMode>(index, x);
}
const XprType& _expression() const { return m_xpr; }
XprType& m_xpr; // target expression
Index m_row; // current row id
Index m_col; // current col id
Index m_currentBlockRows; // current block height
};
namespace internal {
template<typename XprType>
struct traits<CommaInitializer<XprType> > : traits<XprType>
{
};
}
/** \anchor MatrixBaseCommaInitRef
* Convenient operator to set the coefficients of a matrix.
*

2
gtsam/3rdparty/ceres/CMakeLists.txt vendored Normal file
View File

@ -0,0 +1,2 @@
file(GLOB ceres_headers "${CMAKE_CURRENT_SOURCE_DIR}/*.h")
install(FILES ${ceres_headers} DESTINATION include/gtsam/3rdparty/ceres)

314
gtsam/3rdparty/ceres/autodiff.h vendored Normal file
View File

@ -0,0 +1,314 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: keir@google.com (Keir Mierle)
//
// Computation of the Jacobian matrix for vector-valued functions of multiple
// variables, using automatic differentiation based on the implementation of
// dual numbers in jet.h. Before reading the rest of this file, it is adivsable
// to read jet.h's header comment in detail.
//
// The helper wrapper AutoDiff::Differentiate() computes the jacobian of
// functors with templated operator() taking this form:
//
// struct F {
// template<typename T>
// bool operator()(const T *x, const T *y, ..., T *z) {
// // Compute z[] based on x[], y[], ...
// // return true if computation succeeded, false otherwise.
// }
// };
//
// All inputs and outputs may be vector-valued.
//
// To understand how jets are used to compute the jacobian, a
// picture may help. Consider a vector-valued function, F, returning 3
// dimensions and taking a vector-valued parameter of 4 dimensions:
//
// y x
// [ * ] F [ * ]
// [ * ] <--- [ * ]
// [ * ] [ * ]
// [ * ]
//
// Similar to the 2-parameter example for f described in jet.h, computing the
// jacobian dy/dx is done by substutiting a suitable jet object for x and all
// intermediate steps of the computation of F. Since x is has 4 dimensions, use
// a Jet<double, 4>.
//
// Before substituting a jet object for x, the dual components are set
// appropriately for each dimension of x:
//
// y x
// [ * | * * * * ] f [ * | 1 0 0 0 ] x0
// [ * | * * * * ] <--- [ * | 0 1 0 0 ] x1
// [ * | * * * * ] [ * | 0 0 1 0 ] x2
// ---+--- [ * | 0 0 0 1 ] x3
// | ^ ^ ^ ^
// dy/dx | | | +----- infinitesimal for x3
// | | +------- infinitesimal for x2
// | +--------- infinitesimal for x1
// +----------- infinitesimal for x0
//
// The reason to set the internal 4x4 submatrix to the identity is that we wish
// to take the derivative of y separately with respect to each dimension of x.
// Each column of the 4x4 identity is therefore for a single component of the
// independent variable x.
//
// Then the jacobian of the mapping, dy/dx, is the 3x4 sub-matrix of the
// extended y vector, indicated in the above diagram.
//
// Functors with multiple parameters
// ---------------------------------
// In practice, it is often convenient to use a function f of two or more
// vector-valued parameters, for example, x[3] and z[6]. Unfortunately, the jet
// framework is designed for a single-parameter vector-valued input. The wrapper
// in this file addresses this issue adding support for functions with one or
// more parameter vectors.
//
// To support multiple parameters, all the parameter vectors are concatenated
// into one and treated as a single parameter vector, except that since the
// functor expects different inputs, we need to construct the jets as if they
// were part of a single parameter vector. The extended jets are passed
// separately for each parameter.
//
// For example, consider a functor F taking two vector parameters, p[2] and
// q[3], and producing an output y[4]:
//
// struct F {
// template<typename T>
// bool operator()(const T *p, const T *q, T *z) {
// // ...
// }
// };
//
// In this case, the necessary jet type is Jet<double, 5>. Here is a
// visualization of the jet objects in this case:
//
// Dual components for p ----+
// |
// -+-
// y [ * | 1 0 | 0 0 0 ] --- p[0]
// [ * | 0 1 | 0 0 0 ] --- p[1]
// [ * | . . | + + + ] |
// [ * | . . | + + + ] v
// [ * | . . | + + + ] <--- F(p, q)
// [ * | . . | + + + ] ^
// ^^^ ^^^^^ |
// dy/dp dy/dq [ * | 0 0 | 1 0 0 ] --- q[0]
// [ * | 0 0 | 0 1 0 ] --- q[1]
// [ * | 0 0 | 0 0 1 ] --- q[2]
// --+--
// |
// Dual components for q --------------+
//
// where the 4x2 submatrix (marked with ".") and 4x3 submatrix (marked with "+"
// of y in the above diagram are the derivatives of y with respect to p and q
// respectively. This is how autodiff works for functors taking multiple vector
// valued arguments (up to 6).
//
// Jacobian NULL pointers
// ----------------------
// In general, the functions below will accept NULL pointers for all or some of
// the Jacobian parameters, meaning that those Jacobians will not be computed.
#ifndef CERES_PUBLIC_INTERNAL_AUTODIFF_H_
#define CERES_PUBLIC_INTERNAL_AUTODIFF_H_
#include <stddef.h>
#include <gtsam/3rdparty/ceres/jet.h>
#include <gtsam/3rdparty/ceres/eigen.h>
#include <gtsam/3rdparty/ceres/fixed_array.h>
#include <gtsam/3rdparty/ceres/variadic_evaluate.h>
#define DCHECK assert
#define DCHECK_GT(a,b) assert((a)>(b))
namespace ceres {
namespace internal {
// Extends src by a 1st order pertubation for every dimension and puts it in
// dst. The size of src is N. Since this is also used for perturbations in
// blocked arrays, offset is used to shift which part of the jet the
// perturbation occurs. This is used to set up the extended x augmented by an
// identity matrix. The JetT type should be a Jet type, and T should be a
// numeric type (e.g. double). For example,
//
// 0 1 2 3 4 5 6 7 8
// dst[0] [ * | . . | 1 0 0 | . . . ]
// dst[1] [ * | . . | 0 1 0 | . . . ]
// dst[2] [ * | . . | 0 0 1 | . . . ]
//
// is what would get put in dst if N was 3, offset was 3, and the jet type JetT
// was 8-dimensional.
template <typename JetT, typename T, int N>
inline void Make1stOrderPerturbation(int offset, const T* src, JetT* dst) {
DCHECK(src);
DCHECK(dst);
for (int j = 0; j < N; ++j) {
dst[j].a = src[j];
dst[j].v.setZero();
dst[j].v[offset + j] = T(1.0);
}
}
// Takes the 0th order part of src, assumed to be a Jet type, and puts it in
// dst. This is used to pick out the "vector" part of the extended y.
template <typename JetT, typename T>
inline void Take0thOrderPart(int M, const JetT *src, T dst) {
DCHECK(src);
for (int i = 0; i < M; ++i) {
dst[i] = src[i].a;
}
}
// Takes N 1st order parts, starting at index N0, and puts them in the M x N
// matrix 'dst'. This is used to pick out the "matrix" parts of the extended y.
template <typename JetT, typename T, int N0, int N>
inline void Take1stOrderPart(const int M, const JetT *src, T *dst) {
DCHECK(src);
DCHECK(dst);
for (int i = 0; i < M; ++i) {
Eigen::Map<Eigen::Matrix<T, N, 1> >(dst + N * i, N) =
src[i].v.template segment<N>(N0);
}
}
// This is in a struct because default template parameters on a
// function are not supported in C++03 (though it is available in
// C++0x). N0 through N5 are the dimension of the input arguments to
// the user supplied functor.
template <typename Functor, typename T,
int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0,
int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0>
struct AutoDiff {
static bool Differentiate(const Functor& functor,
T const *const *parameters,
int num_outputs,
T *function_value,
T **jacobians) {
// This block breaks the 80 column rule to keep it somewhat readable.
DCHECK_GT(num_outputs, 0);
DCHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) ||
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) ||
((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0)));
typedef Jet<T, N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9> JetT;
FixedArray<JetT, (256 * 7) / sizeof(JetT)> x(
N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9 + num_outputs);
// These are the positions of the respective jets in the fixed array x.
const int jet0 = 0;
const int jet1 = N0;
const int jet2 = N0 + N1;
const int jet3 = N0 + N1 + N2;
const int jet4 = N0 + N1 + N2 + N3;
const int jet5 = N0 + N1 + N2 + N3 + N4;
const int jet6 = N0 + N1 + N2 + N3 + N4 + N5;
const int jet7 = N0 + N1 + N2 + N3 + N4 + N5 + N6;
const int jet8 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7;
const int jet9 = N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8;
const JetT *unpacked_parameters[10] = {
x.get() + jet0,
x.get() + jet1,
x.get() + jet2,
x.get() + jet3,
x.get() + jet4,
x.get() + jet5,
x.get() + jet6,
x.get() + jet7,
x.get() + jet8,
x.get() + jet9,
};
JetT* output = x.get() + N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9;
#define CERES_MAKE_1ST_ORDER_PERTURBATION(i) \
if (N ## i) { \
internal::Make1stOrderPerturbation<JetT, T, N ## i>( \
jet ## i, \
parameters[i], \
x.get() + jet ## i); \
}
CERES_MAKE_1ST_ORDER_PERTURBATION(0);
CERES_MAKE_1ST_ORDER_PERTURBATION(1);
CERES_MAKE_1ST_ORDER_PERTURBATION(2);
CERES_MAKE_1ST_ORDER_PERTURBATION(3);
CERES_MAKE_1ST_ORDER_PERTURBATION(4);
CERES_MAKE_1ST_ORDER_PERTURBATION(5);
CERES_MAKE_1ST_ORDER_PERTURBATION(6);
CERES_MAKE_1ST_ORDER_PERTURBATION(7);
CERES_MAKE_1ST_ORDER_PERTURBATION(8);
CERES_MAKE_1ST_ORDER_PERTURBATION(9);
#undef CERES_MAKE_1ST_ORDER_PERTURBATION
if (!VariadicEvaluate<Functor, JetT,
N0, N1, N2, N3, N4, N5, N6, N7, N8, N9>::Call(
functor, unpacked_parameters, output)) {
return false;
}
internal::Take0thOrderPart(num_outputs, output, function_value);
#define CERES_TAKE_1ST_ORDER_PERTURBATION(i) \
if (N ## i) { \
if (jacobians[i]) { \
internal::Take1stOrderPart<JetT, T, \
jet ## i, \
N ## i>(num_outputs, \
output, \
jacobians[i]); \
} \
}
CERES_TAKE_1ST_ORDER_PERTURBATION(0);
CERES_TAKE_1ST_ORDER_PERTURBATION(1);
CERES_TAKE_1ST_ORDER_PERTURBATION(2);
CERES_TAKE_1ST_ORDER_PERTURBATION(3);
CERES_TAKE_1ST_ORDER_PERTURBATION(4);
CERES_TAKE_1ST_ORDER_PERTURBATION(5);
CERES_TAKE_1ST_ORDER_PERTURBATION(6);
CERES_TAKE_1ST_ORDER_PERTURBATION(7);
CERES_TAKE_1ST_ORDER_PERTURBATION(8);
CERES_TAKE_1ST_ORDER_PERTURBATION(9);
#undef CERES_TAKE_1ST_ORDER_PERTURBATION
return true;
}
};
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_INTERNAL_AUTODIFF_H_

93
gtsam/3rdparty/ceres/eigen.h vendored Normal file
View File

@ -0,0 +1,93 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
#ifndef CERES_INTERNAL_EIGEN_H_
#define CERES_INTERNAL_EIGEN_H_
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
namespace ceres {
typedef Eigen::Matrix<double, Eigen::Dynamic, 1> Vector;
typedef Eigen::Matrix<double,
Eigen::Dynamic,
Eigen::Dynamic,
Eigen::RowMajor> Matrix;
typedef Eigen::Map<Vector> VectorRef;
typedef Eigen::Map<Matrix> MatrixRef;
typedef Eigen::Map<const Vector> ConstVectorRef;
typedef Eigen::Map<const Matrix> ConstMatrixRef;
// Column major matrices for DenseSparseMatrix/DenseQRSolver
typedef Eigen::Matrix<double,
Eigen::Dynamic,
Eigen::Dynamic,
Eigen::ColMajor> ColMajorMatrix;
typedef Eigen::Map<ColMajorMatrix, 0,
Eigen::Stride<Eigen::Dynamic, 1> > ColMajorMatrixRef;
typedef Eigen::Map<const ColMajorMatrix,
0,
Eigen::Stride<Eigen::Dynamic, 1> > ConstColMajorMatrixRef;
// C++ does not support templated typdefs, thus the need for this
// struct so that we can support statically sized Matrix and Maps.
template <int num_rows = Eigen::Dynamic, int num_cols = Eigen::Dynamic>
struct EigenTypes {
typedef Eigen::Matrix <double, num_rows, num_cols, Eigen::RowMajor>
Matrix;
typedef Eigen::Map<
Eigen::Matrix<double, num_rows, num_cols, Eigen::RowMajor> >
MatrixRef;
typedef Eigen::Matrix <double, num_rows, 1>
Vector;
typedef Eigen::Map <
Eigen::Matrix<double, num_rows, 1> >
VectorRef;
typedef Eigen::Map<
const Eigen::Matrix<double, num_rows, num_cols, Eigen::RowMajor> >
ConstMatrixRef;
typedef Eigen::Map <
const Eigen::Matrix<double, num_rows, 1> >
ConstVectorRef;
};
} // namespace ceres
#endif // CERES_INTERNAL_EIGEN_H_

78
gtsam/3rdparty/ceres/example.h vendored Normal file
View File

@ -0,0 +1,78 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: keir@google.com (Keir Mierle)
// sameeragarwal@google.com (Sameer Agarwal)
//
// Some Ceres Snippets copied for testing
#pragma once
#include <gtsam/3rdparty/ceres/rotation.h>
// Templated pinhole camera model for used with Ceres. The camera is
// parameterized using 9 parameters: 3 for rotation, 3 for translation, 1 for
// focal length and 2 for radial distortion. The principal point is not modeled
// (i.e. it is assumed be located at the image center).
struct SnavelyProjection {
template<typename T>
bool operator()(const T* const camera, const T* const point,
T* predicted) const {
// camera[0,1,2] are the angle-axis rotation.
T p[3];
ceres::AngleAxisRotatePoint(camera, point, p);
// camera[3,4,5] are the translation.
p[0] += camera[3];
p[1] += camera[4];
p[2] += camera[5];
// Compute the center of distortion. The sign change comes from
// the camera model that Noah Snavely's Bundler assumes, whereby
// the camera coordinate system has a negative z axis.
T xp = -p[0] / p[2];
T yp = -p[1] / p[2];
// Apply second and fourth order radial distortion.
const T& l1 = camera[7];
const T& l2 = camera[8];
T r2 = xp * xp + yp * yp;
T distortion = T(1.0) + r2 * (l1 + l2 * r2);
// Compute final projected point position.
const T& focal = camera[6];
predicted[0] = focal * distortion * xp;
predicted[1] = focal * distortion * yp;
return true;
}
};

190
gtsam/3rdparty/ceres/fixed_array.h vendored Normal file
View File

@ -0,0 +1,190 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: rennie@google.com (Jeffrey Rennie)
// Author: sanjay@google.com (Sanjay Ghemawat) -- renamed to FixedArray
#ifndef CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
#define CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_
#include <cstddef>
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
#include <gtsam/3rdparty/ceres/macros.h>
#include <gtsam/3rdparty/ceres/manual_constructor.h>
namespace ceres {
namespace internal {
// A FixedArray<T> represents a non-resizable array of T where the
// length of the array does not need to be a compile time constant.
//
// FixedArray allocates small arrays inline, and large arrays on
// the heap. It is a good replacement for non-standard and deprecated
// uses of alloca() and variable length arrays (a GCC extension).
//
// FixedArray keeps performance fast for small arrays, because it
// avoids heap operations. It also helps reduce the chances of
// accidentally overflowing your stack if large input is passed to
// your function.
//
// Also, FixedArray is useful for writing portable code. Not all
// compilers support arrays of dynamic size.
// Most users should not specify an inline_elements argument and let
// FixedArray<> automatically determine the number of elements
// to store inline based on sizeof(T).
//
// If inline_elements is specified, the FixedArray<> implementation
// will store arrays of length <= inline_elements inline.
//
// Finally note that unlike vector<T> FixedArray<T> will not zero-initialize
// simple types like int, double, bool, etc.
//
// Non-POD types will be default-initialized just like regular vectors or
// arrays.
#if defined(_WIN64)
typedef __int64 ssize_t;
#elif defined(_WIN32)
typedef __int32 ssize_t;
#endif
template <typename T, ssize_t inline_elements = -1>
class FixedArray {
public:
// For playing nicely with stl:
typedef T value_type;
typedef T* iterator;
typedef T const* const_iterator;
typedef T& reference;
typedef T const& const_reference;
typedef T* pointer;
typedef std::ptrdiff_t difference_type;
typedef size_t size_type;
// REQUIRES: n >= 0
// Creates an array object that can store "n" elements.
//
// FixedArray<T> will not zero-initialiaze POD (simple) types like int,
// double, bool, etc.
// Non-POD types will be default-initialized just like regular vectors or
// arrays.
explicit FixedArray(size_type n);
// Releases any resources.
~FixedArray();
// Returns the length of the array.
inline size_type size() const { return size_; }
// Returns the memory size of the array in bytes.
inline size_t memsize() const { return size_ * sizeof(T); }
// Returns a pointer to the underlying element array.
inline const T* get() const { return &array_[0].element; }
inline T* get() { return &array_[0].element; }
// REQUIRES: 0 <= i < size()
// Returns a reference to the "i"th element.
inline T& operator[](size_type i) {
assert(i < size_);
return array_[i].element;
}
// REQUIRES: 0 <= i < size()
// Returns a reference to the "i"th element.
inline const T& operator[](size_type i) const {
assert(i < size_);
return array_[i].element;
}
inline iterator begin() { return &array_[0].element; }
inline iterator end() { return &array_[size_].element; }
inline const_iterator begin() const { return &array_[0].element; }
inline const_iterator end() const { return &array_[size_].element; }
private:
// Container to hold elements of type T. This is necessary to handle
// the case where T is a a (C-style) array. The size of InnerContainer
// and T must be the same, otherwise callers' assumptions about use
// of this code will be broken.
struct InnerContainer {
T element;
};
// How many elements should we store inline?
// a. If not specified, use a default of 256 bytes (256 bytes
// seems small enough to not cause stack overflow or unnecessary
// stack pollution, while still allowing stack allocation for
// reasonably long character arrays.
// b. Never use 0 length arrays (not ISO C++)
static const size_type S1 = ((inline_elements < 0)
? (256/sizeof(T)) : inline_elements);
static const size_type S2 = (S1 <= 0) ? 1 : S1;
static const size_type kInlineElements = S2;
size_type const size_;
InnerContainer* const array_;
// Allocate some space, not an array of elements of type T, so that we can
// skip calling the T constructors and destructors for space we never use.
ManualConstructor<InnerContainer> inline_space_[kInlineElements];
};
// Implementation details follow
template <class T, ssize_t S>
inline FixedArray<T, S>::FixedArray(typename FixedArray<T, S>::size_type n)
: size_(n),
array_((n <= kInlineElements
? reinterpret_cast<InnerContainer*>(inline_space_)
: new InnerContainer[n])) {
// Construct only the elements actually used.
if (array_ == reinterpret_cast<InnerContainer*>(inline_space_)) {
for (size_t i = 0; i != size_; ++i) {
inline_space_[i].Init();
}
}
}
template <class T, ssize_t S>
inline FixedArray<T, S>::~FixedArray() {
if (array_ != reinterpret_cast<InnerContainer*>(inline_space_)) {
delete[] array_;
} else {
for (size_t i = 0; i != size_; ++i) {
inline_space_[i].Destroy();
}
}
}
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_INTERNAL_FIXED_ARRAY_H_

87
gtsam/3rdparty/ceres/fpclassify.h vendored Normal file
View File

@ -0,0 +1,87 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: keir@google.com (Keir Mierle)
//
// Portable floating point classification. The names are picked such that they
// do not collide with macros. For example, "isnan" in C99 is a macro and hence
// does not respect namespaces.
//
// TODO(keir): Finish porting!
#ifndef CERES_PUBLIC_FPCLASSIFY_H_
#define CERES_PUBLIC_FPCLASSIFY_H_
#if defined(_MSC_VER)
#include <float.h>
#endif
#include <limits>
namespace ceres {
#if defined(_MSC_VER)
inline bool IsFinite (double x) { return _finite(x) != 0; }
inline bool IsInfinite(double x) { return _finite(x) == 0 && _isnan(x) == 0; }
inline bool IsNaN (double x) { return _isnan(x) != 0; }
inline bool IsNormal (double x) {
int classification = _fpclass(x);
return classification == _FPCLASS_NN ||
classification == _FPCLASS_PN;
}
#elif defined(ANDROID) && defined(_STLPORT_VERSION)
// On Android, when using the STLPort, the C++ isnan and isnormal functions
// are defined as macros.
inline bool IsNaN (double x) { return isnan(x); }
inline bool IsNormal (double x) { return isnormal(x); }
// On Android NDK r6, when using STLPort, the isinf and isfinite functions are
// not available, so reimplement them.
inline bool IsInfinite(double x) {
return x == std::numeric_limits<double>::infinity() ||
x == -std::numeric_limits<double>::infinity();
}
inline bool IsFinite(double x) {
return !isnan(x) && !IsInfinite(x);
}
# else
// These definitions are for the normal Unix suspects.
inline bool IsFinite (double x) { return std::isfinite(x); }
inline bool IsInfinite(double x) { return std::isinf(x); }
inline bool IsNaN (double x) { return std::isnan(x); }
inline bool IsNormal (double x) { return std::isnormal(x); }
#endif
} // namespace ceres
#endif // CERES_PUBLIC_FPCLASSIFY_H_

670
gtsam/3rdparty/ceres/jet.h vendored Normal file
View File

@ -0,0 +1,670 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: keir@google.com (Keir Mierle)
//
// A simple implementation of N-dimensional dual numbers, for automatically
// computing exact derivatives of functions.
//
// While a complete treatment of the mechanics of automatic differentation is
// beyond the scope of this header (see
// http://en.wikipedia.org/wiki/Automatic_differentiation for details), the
// basic idea is to extend normal arithmetic with an extra element, "e," often
// denoted with the greek symbol epsilon, such that e != 0 but e^2 = 0. Dual
// numbers are extensions of the real numbers analogous to complex numbers:
// whereas complex numbers augment the reals by introducing an imaginary unit i
// such that i^2 = -1, dual numbers introduce an "infinitesimal" unit e such
// that e^2 = 0. Dual numbers have two components: the "real" component and the
// "infinitesimal" component, generally written as x + y*e. Surprisingly, this
// leads to a convenient method for computing exact derivatives without needing
// to manipulate complicated symbolic expressions.
//
// For example, consider the function
//
// f(x) = x^2 ,
//
// evaluated at 10. Using normal arithmetic, f(10) = 100, and df/dx(10) = 20.
// Next, augument 10 with an infinitesimal to get:
//
// f(10 + e) = (10 + e)^2
// = 100 + 2 * 10 * e + e^2
// = 100 + 20 * e -+-
// -- |
// | +--- This is zero, since e^2 = 0
// |
// +----------------- This is df/dx!
//
// Note that the derivative of f with respect to x is simply the infinitesimal
// component of the value of f(x + e). So, in order to take the derivative of
// any function, it is only necessary to replace the numeric "object" used in
// the function with one extended with infinitesimals. The class Jet, defined in
// this header, is one such example of this, where substitution is done with
// templates.
//
// To handle derivatives of functions taking multiple arguments, different
// infinitesimals are used, one for each variable to take the derivative of. For
// example, consider a scalar function of two scalar parameters x and y:
//
// f(x, y) = x^2 + x * y
//
// Following the technique above, to compute the derivatives df/dx and df/dy for
// f(1, 3) involves doing two evaluations of f, the first time replacing x with
// x + e, the second time replacing y with y + e.
//
// For df/dx:
//
// f(1 + e, y) = (1 + e)^2 + (1 + e) * 3
// = 1 + 2 * e + 3 + 3 * e
// = 4 + 5 * e
//
// --> df/dx = 5
//
// For df/dy:
//
// f(1, 3 + e) = 1^2 + 1 * (3 + e)
// = 1 + 3 + e
// = 4 + e
//
// --> df/dy = 1
//
// To take the gradient of f with the implementation of dual numbers ("jets") in
// this file, it is necessary to create a single jet type which has components
// for the derivative in x and y, and passing them to a templated version of f:
//
// template<typename T>
// T f(const T &x, const T &y) {
// return x * x + x * y;
// }
//
// // The "2" means there should be 2 dual number components.
// Jet<double, 2> x(0); // Pick the 0th dual number for x.
// Jet<double, 2> y(1); // Pick the 1st dual number for y.
// Jet<double, 2> z = f(x, y);
//
// LOG(INFO) << "df/dx = " << z.a[0]
// << "df/dy = " << z.a[1];
//
// Most users should not use Jet objects directly; a wrapper around Jet objects,
// which makes computing the derivative, gradient, or jacobian of templated
// functors simple, is in autodiff.h. Even autodiff.h should not be used
// directly; instead autodiff_cost_function.h is typically the file of interest.
//
// For the more mathematically inclined, this file implements first-order
// "jets". A 1st order jet is an element of the ring
//
// T[N] = T[t_1, ..., t_N] / (t_1, ..., t_N)^2
//
// which essentially means that each jet consists of a "scalar" value 'a' from T
// and a 1st order perturbation vector 'v' of length N:
//
// x = a + \sum_i v[i] t_i
//
// A shorthand is to write an element as x = a + u, where u is the pertubation.
// Then, the main point about the arithmetic of jets is that the product of
// perturbations is zero:
//
// (a + u) * (b + v) = ab + av + bu + uv
// = ab + (av + bu) + 0
//
// which is what operator* implements below. Addition is simpler:
//
// (a + u) + (b + v) = (a + b) + (u + v).
//
// The only remaining question is how to evaluate the function of a jet, for
// which we use the chain rule:
//
// f(a + u) = f(a) + f'(a) u
//
// where f'(a) is the (scalar) derivative of f at a.
//
// By pushing these things through sufficiently and suitably templated
// functions, we can do automatic differentiation. Just be sure to turn on
// function inlining and common-subexpression elimination, or it will be very
// slow!
//
// WARNING: Most Ceres users should not directly include this file or know the
// details of how jets work. Instead the suggested method for automatic
// derivatives is to use autodiff_cost_function.h, which is a wrapper around
// both jets.h and autodiff.h to make taking derivatives of cost functions for
// use in Ceres easier.
#ifndef CERES_PUBLIC_JET_H_
#define CERES_PUBLIC_JET_H_
#include <cmath>
#include <iosfwd>
#include <iostream> // NOLINT
#include <limits>
#include <string>
#include <gtsam/3rdparty/gtsam_eigen_includes.h>
#include <gtsam/3rdparty/ceres/fpclassify.h>
namespace ceres {
template <typename T, int N>
struct Jet {
enum { DIMENSION = N };
// Default-construct "a" because otherwise this can lead to false errors about
// uninitialized uses when other classes relying on default constructed T
// (where T is a Jet<T, N>). This usually only happens in opt mode. Note that
// the C++ standard mandates that e.g. default constructed doubles are
// initialized to 0.0; see sections 8.5 of the C++03 standard.
Jet() : a() {
v.setZero();
}
// Constructor from scalar: a + 0.
explicit Jet(const T& value) {
a = value;
v.setZero();
}
// Constructor from scalar plus variable: a + t_i.
Jet(const T& value, int k) {
a = value;
v.setZero();
v[k] = T(1.0);
}
// Constructor from scalar and vector part
// The use of Eigen::DenseBase allows Eigen expressions
// to be passed in without being fully evaluated until
// they are assigned to v
template<typename Derived>
EIGEN_STRONG_INLINE Jet(const T& a, const Eigen::DenseBase<Derived> &v)
: a(a), v(v) {
}
// Compound operators
Jet<T, N>& operator+=(const Jet<T, N> &y) {
*this = *this + y;
return *this;
}
Jet<T, N>& operator-=(const Jet<T, N> &y) {
*this = *this - y;
return *this;
}
Jet<T, N>& operator*=(const Jet<T, N> &y) {
*this = *this * y;
return *this;
}
Jet<T, N>& operator/=(const Jet<T, N> &y) {
*this = *this / y;
return *this;
}
// The scalar part.
T a;
// The infinitesimal part.
//
// Note the Eigen::DontAlign bit is needed here because this object
// gets allocated on the stack and as part of other arrays and
// structs. Forcing the right alignment there is the source of much
// pain and suffering. Even if that works, passing Jets around to
// functions by value has problems because the C++ ABI does not
// guarantee alignment for function arguments.
//
// Setting the DontAlign bit prevents Eigen from using SSE for the
// various operations on Jets. This is a small performance penalty
// since the AutoDiff code will still expose much of the code as
// statically sized loops to the compiler. But given the subtle
// issues that arise due to alignment, especially when dealing with
// multiple platforms, it seems to be a trade off worth making.
Eigen::Matrix<T, N, 1, Eigen::DontAlign> v;
};
// Unary +
template<typename T, int N> inline
Jet<T, N> const& operator+(const Jet<T, N>& f) {
return f;
}
// TODO(keir): Try adding __attribute__((always_inline)) to these functions to
// see if it causes a performance increase.
// Unary -
template<typename T, int N> inline
Jet<T, N> operator-(const Jet<T, N>&f) {
return Jet<T, N>(-f.a, -f.v);
}
// Binary +
template<typename T, int N> inline
Jet<T, N> operator+(const Jet<T, N>& f,
const Jet<T, N>& g) {
return Jet<T, N>(f.a + g.a, f.v + g.v);
}
// Binary + with a scalar: x + s
template<typename T, int N> inline
Jet<T, N> operator+(const Jet<T, N>& f, T s) {
return Jet<T, N>(f.a + s, f.v);
}
// Binary + with a scalar: s + x
template<typename T, int N> inline
Jet<T, N> operator+(T s, const Jet<T, N>& f) {
return Jet<T, N>(f.a + s, f.v);
}
// Binary -
template<typename T, int N> inline
Jet<T, N> operator-(const Jet<T, N>& f,
const Jet<T, N>& g) {
return Jet<T, N>(f.a - g.a, f.v - g.v);
}
// Binary - with a scalar: x - s
template<typename T, int N> inline
Jet<T, N> operator-(const Jet<T, N>& f, T s) {
return Jet<T, N>(f.a - s, f.v);
}
// Binary - with a scalar: s - x
template<typename T, int N> inline
Jet<T, N> operator-(T s, const Jet<T, N>& f) {
return Jet<T, N>(s - f.a, -f.v);
}
// Binary *
template<typename T, int N> inline
Jet<T, N> operator*(const Jet<T, N>& f,
const Jet<T, N>& g) {
return Jet<T, N>(f.a * g.a, f.a * g.v + f.v * g.a);
}
// Binary * with a scalar: x * s
template<typename T, int N> inline
Jet<T, N> operator*(const Jet<T, N>& f, T s) {
return Jet<T, N>(f.a * s, f.v * s);
}
// Binary * with a scalar: s * x
template<typename T, int N> inline
Jet<T, N> operator*(T s, const Jet<T, N>& f) {
return Jet<T, N>(f.a * s, f.v * s);
}
// Binary /
template<typename T, int N> inline
Jet<T, N> operator/(const Jet<T, N>& f,
const Jet<T, N>& g) {
// This uses:
//
// a + u (a + u)(b - v) (a + u)(b - v)
// ----- = -------------- = --------------
// b + v (b + v)(b - v) b^2
//
// which holds because v*v = 0.
const T g_a_inverse = T(1.0) / g.a;
const T f_a_by_g_a = f.a * g_a_inverse;
return Jet<T, N>(f.a * g_a_inverse, (f.v - f_a_by_g_a * g.v) * g_a_inverse);
}
// Binary / with a scalar: s / x
template<typename T, int N> inline
Jet<T, N> operator/(T s, const Jet<T, N>& g) {
const T minus_s_g_a_inverse2 = -s / (g.a * g.a);
return Jet<T, N>(s / g.a, g.v * minus_s_g_a_inverse2);
}
// Binary / with a scalar: x / s
template<typename T, int N> inline
Jet<T, N> operator/(const Jet<T, N>& f, T s) {
const T s_inverse = 1.0 / s;
return Jet<T, N>(f.a * s_inverse, f.v * s_inverse);
}
// Binary comparison operators for both scalars and jets.
#define CERES_DEFINE_JET_COMPARISON_OPERATOR(op) \
template<typename T, int N> inline \
bool operator op(const Jet<T, N>& f, const Jet<T, N>& g) { \
return f.a op g.a; \
} \
template<typename T, int N> inline \
bool operator op(const T& s, const Jet<T, N>& g) { \
return s op g.a; \
} \
template<typename T, int N> inline \
bool operator op(const Jet<T, N>& f, const T& s) { \
return f.a op s; \
}
CERES_DEFINE_JET_COMPARISON_OPERATOR( < ) // NOLINT
CERES_DEFINE_JET_COMPARISON_OPERATOR( <= ) // NOLINT
CERES_DEFINE_JET_COMPARISON_OPERATOR( > ) // NOLINT
CERES_DEFINE_JET_COMPARISON_OPERATOR( >= ) // NOLINT
CERES_DEFINE_JET_COMPARISON_OPERATOR( == ) // NOLINT
CERES_DEFINE_JET_COMPARISON_OPERATOR( != ) // NOLINT
#undef CERES_DEFINE_JET_COMPARISON_OPERATOR
// Pull some functions from namespace std.
//
// This is necessary because we want to use the same name (e.g. 'sqrt') for
// double-valued and Jet-valued functions, but we are not allowed to put
// Jet-valued functions inside namespace std.
//
// TODO(keir): Switch to "using".
inline double abs (double x) { return std::abs(x); }
inline double log (double x) { return std::log(x); }
inline double exp (double x) { return std::exp(x); }
inline double sqrt (double x) { return std::sqrt(x); }
inline double cos (double x) { return std::cos(x); }
inline double acos (double x) { return std::acos(x); }
inline double sin (double x) { return std::sin(x); }
inline double asin (double x) { return std::asin(x); }
inline double tan (double x) { return std::tan(x); }
inline double atan (double x) { return std::atan(x); }
inline double sinh (double x) { return std::sinh(x); }
inline double cosh (double x) { return std::cosh(x); }
inline double tanh (double x) { return std::tanh(x); }
inline double pow (double x, double y) { return std::pow(x, y); }
inline double atan2(double y, double x) { return std::atan2(y, x); }
// In general, f(a + h) ~= f(a) + f'(a) h, via the chain rule.
// abs(x + h) ~= x + h or -(x + h)
template <typename T, int N> inline
Jet<T, N> abs(const Jet<T, N>& f) {
return f.a < T(0.0) ? -f : f;
}
// log(a + h) ~= log(a) + h / a
template <typename T, int N> inline
Jet<T, N> log(const Jet<T, N>& f) {
const T a_inverse = T(1.0) / f.a;
return Jet<T, N>(log(f.a), f.v * a_inverse);
}
// exp(a + h) ~= exp(a) + exp(a) h
template <typename T, int N> inline
Jet<T, N> exp(const Jet<T, N>& f) {
const T tmp = exp(f.a);
return Jet<T, N>(tmp, tmp * f.v);
}
// sqrt(a + h) ~= sqrt(a) + h / (2 sqrt(a))
template <typename T, int N> inline
Jet<T, N> sqrt(const Jet<T, N>& f) {
const T tmp = sqrt(f.a);
const T two_a_inverse = T(1.0) / (T(2.0) * tmp);
return Jet<T, N>(tmp, f.v * two_a_inverse);
}
// cos(a + h) ~= cos(a) - sin(a) h
template <typename T, int N> inline
Jet<T, N> cos(const Jet<T, N>& f) {
return Jet<T, N>(cos(f.a), - sin(f.a) * f.v);
}
// acos(a + h) ~= acos(a) - 1 / sqrt(1 - a^2) h
template <typename T, int N> inline
Jet<T, N> acos(const Jet<T, N>& f) {
const T tmp = - T(1.0) / sqrt(T(1.0) - f.a * f.a);
return Jet<T, N>(acos(f.a), tmp * f.v);
}
// sin(a + h) ~= sin(a) + cos(a) h
template <typename T, int N> inline
Jet<T, N> sin(const Jet<T, N>& f) {
return Jet<T, N>(sin(f.a), cos(f.a) * f.v);
}
// asin(a + h) ~= asin(a) + 1 / sqrt(1 - a^2) h
template <typename T, int N> inline
Jet<T, N> asin(const Jet<T, N>& f) {
const T tmp = T(1.0) / sqrt(T(1.0) - f.a * f.a);
return Jet<T, N>(asin(f.a), tmp * f.v);
}
// tan(a + h) ~= tan(a) + (1 + tan(a)^2) h
template <typename T, int N> inline
Jet<T, N> tan(const Jet<T, N>& f) {
const T tan_a = tan(f.a);
const T tmp = T(1.0) + tan_a * tan_a;
return Jet<T, N>(tan_a, tmp * f.v);
}
// atan(a + h) ~= atan(a) + 1 / (1 + a^2) h
template <typename T, int N> inline
Jet<T, N> atan(const Jet<T, N>& f) {
const T tmp = T(1.0) / (T(1.0) + f.a * f.a);
return Jet<T, N>(atan(f.a), tmp * f.v);
}
// sinh(a + h) ~= sinh(a) + cosh(a) h
template <typename T, int N> inline
Jet<T, N> sinh(const Jet<T, N>& f) {
return Jet<T, N>(sinh(f.a), cosh(f.a) * f.v);
}
// cosh(a + h) ~= cosh(a) + sinh(a) h
template <typename T, int N> inline
Jet<T, N> cosh(const Jet<T, N>& f) {
return Jet<T, N>(cosh(f.a), sinh(f.a) * f.v);
}
// tanh(a + h) ~= tanh(a) + (1 - tanh(a)^2) h
template <typename T, int N> inline
Jet<T, N> tanh(const Jet<T, N>& f) {
const T tanh_a = tanh(f.a);
const T tmp = T(1.0) - tanh_a * tanh_a;
return Jet<T, N>(tanh_a, tmp * f.v);
}
// Jet Classification. It is not clear what the appropriate semantics are for
// these classifications. This picks that IsFinite and isnormal are "all"
// operations, i.e. all elements of the jet must be finite for the jet itself
// to be finite (or normal). For IsNaN and IsInfinite, the answer is less
// clear. This takes a "any" approach for IsNaN and IsInfinite such that if any
// part of a jet is nan or inf, then the entire jet is nan or inf. This leads
// to strange situations like a jet can be both IsInfinite and IsNaN, but in
// practice the "any" semantics are the most useful for e.g. checking that
// derivatives are sane.
// The jet is finite if all parts of the jet are finite.
template <typename T, int N> inline
bool IsFinite(const Jet<T, N>& f) {
if (!IsFinite(f.a)) {
return false;
}
for (int i = 0; i < N; ++i) {
if (!IsFinite(f.v[i])) {
return false;
}
}
return true;
}
// The jet is infinite if any part of the jet is infinite.
template <typename T, int N> inline
bool IsInfinite(const Jet<T, N>& f) {
if (IsInfinite(f.a)) {
return true;
}
for (int i = 0; i < N; i++) {
if (IsInfinite(f.v[i])) {
return true;
}
}
return false;
}
// The jet is NaN if any part of the jet is NaN.
template <typename T, int N> inline
bool IsNaN(const Jet<T, N>& f) {
if (IsNaN(f.a)) {
return true;
}
for (int i = 0; i < N; ++i) {
if (IsNaN(f.v[i])) {
return true;
}
}
return false;
}
// The jet is normal if all parts of the jet are normal.
template <typename T, int N> inline
bool IsNormal(const Jet<T, N>& f) {
if (!IsNormal(f.a)) {
return false;
}
for (int i = 0; i < N; ++i) {
if (!IsNormal(f.v[i])) {
return false;
}
}
return true;
}
// atan2(b + db, a + da) ~= atan2(b, a) + (- b da + a db) / (a^2 + b^2)
//
// In words: the rate of change of theta is 1/r times the rate of
// change of (x, y) in the positive angular direction.
template <typename T, int N> inline
Jet<T, N> atan2(const Jet<T, N>& g, const Jet<T, N>& f) {
// Note order of arguments:
//
// f = a + da
// g = b + db
T const tmp = T(1.0) / (f.a * f.a + g.a * g.a);
return Jet<T, N>(atan2(g.a, f.a), tmp * (- g.a * f.v + f.a * g.v));
}
// pow -- base is a differentiable function, exponent is a constant.
// (a+da)^p ~= a^p + p*a^(p-1) da
template <typename T, int N> inline
Jet<T, N> pow(const Jet<T, N>& f, double g) {
T const tmp = g * pow(f.a, g - T(1.0));
return Jet<T, N>(pow(f.a, g), tmp * f.v);
}
// pow -- base is a constant, exponent is a differentiable function.
// (a)^(p+dp) ~= a^p + a^p log(a) dp
template <typename T, int N> inline
Jet<T, N> pow(double f, const Jet<T, N>& g) {
T const tmp = pow(f, g.a);
return Jet<T, N>(tmp, log(f) * tmp * g.v);
}
// pow -- both base and exponent are differentiable functions.
// (a+da)^(b+db) ~= a^b + b * a^(b-1) da + a^b log(a) * db
template <typename T, int N> inline
Jet<T, N> pow(const Jet<T, N>& f, const Jet<T, N>& g) {
T const tmp1 = pow(f.a, g.a);
T const tmp2 = g.a * pow(f.a, g.a - T(1.0));
T const tmp3 = tmp1 * log(f.a);
return Jet<T, N>(tmp1, tmp2 * f.v + tmp3 * g.v);
}
// Define the helper functions Eigen needs to embed Jet types.
//
// NOTE(keir): machine_epsilon() and precision() are missing, because they don't
// work with nested template types (e.g. where the scalar is itself templated).
// Among other things, this means that decompositions of Jet's does not work,
// for example
//
// Matrix<Jet<T, N> ... > A, x, b;
// ...
// A.solve(b, &x)
//
// does not work and will fail with a strange compiler error.
//
// TODO(keir): This is an Eigen 2.0 limitation that is lifted in 3.0. When we
// switch to 3.0, also add the rest of the specialization functionality.
template<typename T, int N> inline const Jet<T, N>& ei_conj(const Jet<T, N>& x) { return x; } // NOLINT
template<typename T, int N> inline const Jet<T, N>& ei_real(const Jet<T, N>& x) { return x; } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_imag(const Jet<T, N>& ) { return Jet<T, N>(0.0); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_abs (const Jet<T, N>& x) { return fabs(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_abs2(const Jet<T, N>& x) { return x * x; } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_sqrt(const Jet<T, N>& x) { return sqrt(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_exp (const Jet<T, N>& x) { return exp(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_log (const Jet<T, N>& x) { return log(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_sin (const Jet<T, N>& x) { return sin(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_cos (const Jet<T, N>& x) { return cos(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_tan (const Jet<T, N>& x) { return tan(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_atan(const Jet<T, N>& x) { return atan(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_sinh(const Jet<T, N>& x) { return sinh(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_cosh(const Jet<T, N>& x) { return cosh(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_tanh(const Jet<T, N>& x) { return tanh(x); } // NOLINT
template<typename T, int N> inline Jet<T, N> ei_pow (const Jet<T, N>& x, Jet<T, N> y) { return pow(x, y); } // NOLINT
// Note: This has to be in the ceres namespace for argument dependent lookup to
// function correctly. Otherwise statements like CHECK_LE(x, 2.0) fail with
// strange compile errors.
template <typename T, int N>
inline std::ostream &operator<<(std::ostream &s, const Jet<T, N>& z) {
return s << "[" << z.a << " ; " << z.v.transpose() << "]";
}
} // namespace ceres
namespace Eigen {
// Creating a specialization of NumTraits enables placing Jet objects inside
// Eigen arrays, getting all the goodness of Eigen combined with autodiff.
template<typename T, int N>
struct NumTraits<ceres::Jet<T, N> > {
typedef ceres::Jet<T, N> Real;
typedef ceres::Jet<T, N> NonInteger;
typedef ceres::Jet<T, N> Nested;
static typename ceres::Jet<T, N> dummy_precision() {
return ceres::Jet<T, N>(1e-12);
}
static inline Real epsilon() {
return Real(std::numeric_limits<T>::epsilon());
}
enum {
IsComplex = 0,
IsInteger = 0,
IsSigned,
ReadCost = 1,
AddCost = 1,
// For Jet types, multiplication is more expensive than addition.
MulCost = 3,
HasFloatingPoint = 1,
RequireInitialization = 1
};
};
} // namespace Eigen
#endif // CERES_PUBLIC_JET_H_

170
gtsam/3rdparty/ceres/macros.h vendored Normal file
View File

@ -0,0 +1,170 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
//
// Various Google-specific macros.
//
// This code is compiled directly on many platforms, including client
// platforms like Windows, Mac, and embedded systems. Before making
// any changes here, make sure that you're not breaking any platforms.
#ifndef CERES_PUBLIC_INTERNAL_MACROS_H_
#define CERES_PUBLIC_INTERNAL_MACROS_H_
#include <cstddef> // For size_t.
// A macro to disallow the copy constructor and operator= functions
// This should be used in the private: declarations for a class
//
// For disallowing only assign or copy, write the code directly, but declare
// the intend in a comment, for example:
//
// void operator=(const TypeName&); // _DISALLOW_ASSIGN
// Note, that most uses of CERES_DISALLOW_ASSIGN and CERES_DISALLOW_COPY
// are broken semantically, one should either use disallow both or
// neither. Try to avoid these in new code.
#define CERES_DISALLOW_COPY_AND_ASSIGN(TypeName) \
TypeName(const TypeName&); \
void operator=(const TypeName&)
// A macro to disallow all the implicit constructors, namely the
// default constructor, copy constructor and operator= functions.
//
// This should be used in the private: declarations for a class
// that wants to prevent anyone from instantiating it. This is
// especially useful for classes containing only static methods.
#define CERES_DISALLOW_IMPLICIT_CONSTRUCTORS(TypeName) \
TypeName(); \
CERES_DISALLOW_COPY_AND_ASSIGN(TypeName)
// The arraysize(arr) macro returns the # of elements in an array arr.
// The expression is a compile-time constant, and therefore can be
// used in defining new arrays, for example. If you use arraysize on
// a pointer by mistake, you will get a compile-time error.
//
// One caveat is that arraysize() doesn't accept any array of an
// anonymous type or a type defined inside a function. In these rare
// cases, you have to use the unsafe ARRAYSIZE() macro below. This is
// due to a limitation in C++'s template system. The limitation might
// eventually be removed, but it hasn't happened yet.
// This template function declaration is used in defining arraysize.
// Note that the function doesn't need an implementation, as we only
// use its type.
template <typename T, size_t N>
char (&ArraySizeHelper(T (&array)[N]))[N];
// That gcc wants both of these prototypes seems mysterious. VC, for
// its part, can't decide which to use (another mystery). Matching of
// template overloads: the final frontier.
#ifndef _WIN32
template <typename T, size_t N>
char (&ArraySizeHelper(const T (&array)[N]))[N];
#endif
#define arraysize(array) (sizeof(ArraySizeHelper(array)))
// ARRAYSIZE performs essentially the same calculation as arraysize,
// but can be used on anonymous types or types defined inside
// functions. It's less safe than arraysize as it accepts some
// (although not all) pointers. Therefore, you should use arraysize
// whenever possible.
//
// The expression ARRAYSIZE(a) is a compile-time constant of type
// size_t.
//
// ARRAYSIZE catches a few type errors. If you see a compiler error
//
// "warning: division by zero in ..."
//
// when using ARRAYSIZE, you are (wrongfully) giving it a pointer.
// You should only use ARRAYSIZE on statically allocated arrays.
//
// The following comments are on the implementation details, and can
// be ignored by the users.
//
// ARRAYSIZE(arr) works by inspecting sizeof(arr) (the # of bytes in
// the array) and sizeof(*(arr)) (the # of bytes in one array
// element). If the former is divisible by the latter, perhaps arr is
// indeed an array, in which case the division result is the # of
// elements in the array. Otherwise, arr cannot possibly be an array,
// and we generate a compiler error to prevent the code from
// compiling.
//
// Since the size of bool is implementation-defined, we need to cast
// !(sizeof(a) & sizeof(*(a))) to size_t in order to ensure the final
// result has type size_t.
//
// This macro is not perfect as it wrongfully accepts certain
// pointers, namely where the pointer size is divisible by the pointee
// size. Since all our code has to go through a 32-bit compiler,
// where a pointer is 4 bytes, this means all pointers to a type whose
// size is 3 or greater than 4 will be (righteously) rejected.
//
// Kudos to Jorg Brown for this simple and elegant implementation.
//
// - wan 2005-11-16
//
// Starting with Visual C++ 2005, WinNT.h includes ARRAYSIZE. However,
// the definition comes from the over-broad windows.h header that
// introduces a macro, ERROR, that conflicts with the logging framework
// that Ceres uses. Instead, rename ARRAYSIZE to CERES_ARRAYSIZE.
#define CERES_ARRAYSIZE(a) \
((sizeof(a) / sizeof(*(a))) / \
static_cast<size_t>(!(sizeof(a) % sizeof(*(a)))))
// Tell the compiler to warn about unused return values for functions
// declared with this macro. The macro should be used on function
// declarations following the argument list:
//
// Sprocket* AllocateSprocket() MUST_USE_RESULT;
//
#if (__GNUC__ > 3 || (__GNUC__ == 3 && __GNUC_MINOR__ >= 4)) \
&& !defined(COMPILER_ICC)
#define CERES_MUST_USE_RESULT __attribute__ ((warn_unused_result))
#else
#define CERES_MUST_USE_RESULT
#endif
// Platform independent macros to get aligned memory allocations.
// For example
//
// MyFoo my_foo CERES_ALIGN_ATTRIBUTE(16);
//
// Gives us an instance of MyFoo which is aligned at a 16 byte
// boundary.
#if defined(_MSC_VER)
#define CERES_ALIGN_ATTRIBUTE(n) __declspec(align(n))
#define CERES_ALIGN_OF(T) __alignof(T)
#elif defined(__GNUC__)
#define CERES_ALIGN_ATTRIBUTE(n) __attribute__((aligned(n)))
#define CERES_ALIGN_OF(T) __alignof(T)
#endif
#endif // CERES_PUBLIC_INTERNAL_MACROS_H_

208
gtsam/3rdparty/ceres/manual_constructor.h vendored Normal file
View File

@ -0,0 +1,208 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: kenton@google.com (Kenton Varda)
//
// ManualConstructor statically-allocates space in which to store some
// object, but does not initialize it. You can then call the constructor
// and destructor for the object yourself as you see fit. This is useful
// for memory management optimizations, where you want to initialize and
// destroy an object multiple times but only allocate it once.
//
// (When I say ManualConstructor statically allocates space, I mean that
// the ManualConstructor object itself is forced to be the right size.)
#ifndef CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_
#define CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_
#include <new>
namespace ceres {
namespace internal {
// ------- Define CERES_ALIGNED_CHAR_ARRAY --------------------------------
#ifndef CERES_ALIGNED_CHAR_ARRAY
// Because MSVC and older GCCs require that the argument to their alignment
// construct to be a literal constant integer, we use a template instantiated
// at all the possible powers of two.
template<int alignment, int size> struct AlignType { };
template<int size> struct AlignType<0, size> { typedef char result[size]; };
#if !defined(CERES_ALIGN_ATTRIBUTE)
#define CERES_ALIGNED_CHAR_ARRAY you_must_define_CERES_ALIGNED_CHAR_ARRAY_for_your_compiler
#else // !defined(CERES_ALIGN_ATTRIBUTE)
#define CERES_ALIGN_TYPE_TEMPLATE(X) \
template<int size> struct AlignType<X, size> { \
typedef CERES_ALIGN_ATTRIBUTE(X) char result[size]; \
}
CERES_ALIGN_TYPE_TEMPLATE(1);
CERES_ALIGN_TYPE_TEMPLATE(2);
CERES_ALIGN_TYPE_TEMPLATE(4);
CERES_ALIGN_TYPE_TEMPLATE(8);
CERES_ALIGN_TYPE_TEMPLATE(16);
CERES_ALIGN_TYPE_TEMPLATE(32);
CERES_ALIGN_TYPE_TEMPLATE(64);
CERES_ALIGN_TYPE_TEMPLATE(128);
CERES_ALIGN_TYPE_TEMPLATE(256);
CERES_ALIGN_TYPE_TEMPLATE(512);
CERES_ALIGN_TYPE_TEMPLATE(1024);
CERES_ALIGN_TYPE_TEMPLATE(2048);
CERES_ALIGN_TYPE_TEMPLATE(4096);
CERES_ALIGN_TYPE_TEMPLATE(8192);
// Any larger and MSVC++ will complain.
#undef CERES_ALIGN_TYPE_TEMPLATE
#define CERES_ALIGNED_CHAR_ARRAY(T, Size) \
typename AlignType<CERES_ALIGN_OF(T), sizeof(T) * Size>::result
#endif // !defined(CERES_ALIGN_ATTRIBUTE)
#endif // CERES_ALIGNED_CHAR_ARRAY
template <typename Type>
class ManualConstructor {
public:
// No constructor or destructor because one of the most useful uses of
// this class is as part of a union, and members of a union cannot have
// constructors or destructors. And, anyway, the whole point of this
// class is to bypass these.
inline Type* get() {
return reinterpret_cast<Type*>(space_);
}
inline const Type* get() const {
return reinterpret_cast<const Type*>(space_);
}
inline Type* operator->() { return get(); }
inline const Type* operator->() const { return get(); }
inline Type& operator*() { return *get(); }
inline const Type& operator*() const { return *get(); }
// This is needed to get around the strict aliasing warning GCC generates.
inline void* space() {
return reinterpret_cast<void*>(space_);
}
// You can pass up to four constructor arguments as arguments of Init().
inline void Init() {
new(space()) Type;
}
template <typename T1>
inline void Init(const T1& p1) {
new(space()) Type(p1);
}
template <typename T1, typename T2>
inline void Init(const T1& p1, const T2& p2) {
new(space()) Type(p1, p2);
}
template <typename T1, typename T2, typename T3>
inline void Init(const T1& p1, const T2& p2, const T3& p3) {
new(space()) Type(p1, p2, p3);
}
template <typename T1, typename T2, typename T3, typename T4>
inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4) {
new(space()) Type(p1, p2, p3, p4);
}
template <typename T1, typename T2, typename T3, typename T4, typename T5>
inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
const T5& p5) {
new(space()) Type(p1, p2, p3, p4, p5);
}
template <typename T1, typename T2, typename T3, typename T4, typename T5,
typename T6>
inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
const T5& p5, const T6& p6) {
new(space()) Type(p1, p2, p3, p4, p5, p6);
}
template <typename T1, typename T2, typename T3, typename T4, typename T5,
typename T6, typename T7>
inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
const T5& p5, const T6& p6, const T7& p7) {
new(space()) Type(p1, p2, p3, p4, p5, p6, p7);
}
template <typename T1, typename T2, typename T3, typename T4, typename T5,
typename T6, typename T7, typename T8>
inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
const T5& p5, const T6& p6, const T7& p7, const T8& p8) {
new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8);
}
template <typename T1, typename T2, typename T3, typename T4, typename T5,
typename T6, typename T7, typename T8, typename T9>
inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
const T5& p5, const T6& p6, const T7& p7, const T8& p8,
const T9& p9) {
new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9);
}
template <typename T1, typename T2, typename T3, typename T4, typename T5,
typename T6, typename T7, typename T8, typename T9, typename T10>
inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
const T5& p5, const T6& p6, const T7& p7, const T8& p8,
const T9& p9, const T10& p10) {
new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10);
}
template <typename T1, typename T2, typename T3, typename T4, typename T5,
typename T6, typename T7, typename T8, typename T9, typename T10,
typename T11>
inline void Init(const T1& p1, const T2& p2, const T3& p3, const T4& p4,
const T5& p5, const T6& p6, const T7& p7, const T8& p8,
const T9& p9, const T10& p10, const T11& p11) {
new(space()) Type(p1, p2, p3, p4, p5, p6, p7, p8, p9, p10, p11);
}
inline void Destroy() {
get()->~Type();
}
private:
CERES_ALIGNED_CHAR_ARRAY(Type, 1) space_;
};
#undef CERES_ALIGNED_CHAR_ARRAY
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_INTERNAL_MANUAL_CONSTRUCTOR_H_

647
gtsam/3rdparty/ceres/rotation.h vendored Normal file
View File

@ -0,0 +1,647 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: keir@google.com (Keir Mierle)
// sameeragarwal@google.com (Sameer Agarwal)
//
// Templated functions for manipulating rotations. The templated
// functions are useful when implementing functors for automatic
// differentiation.
//
// In the following, the Quaternions are laid out as 4-vectors, thus:
//
// q[0] scalar part.
// q[1] coefficient of i.
// q[2] coefficient of j.
// q[3] coefficient of k.
//
// where: i*i = j*j = k*k = -1 and i*j = k, j*k = i, k*i = j.
#ifndef CERES_PUBLIC_ROTATION_H_
#define CERES_PUBLIC_ROTATION_H_
#include <algorithm>
#include <cmath>
#include <limits>
#include <assert.h>
#define DCHECK assert
namespace ceres {
// Trivial wrapper to index linear arrays as matrices, given a fixed
// column and row stride. When an array "T* array" is wrapped by a
//
// (const) MatrixAdapter<T, row_stride, col_stride> M"
//
// the expression M(i, j) is equivalent to
//
// arrary[i * row_stride + j * col_stride]
//
// Conversion functions to and from rotation matrices accept
// MatrixAdapters to permit using row-major and column-major layouts,
// and rotation matrices embedded in larger matrices (such as a 3x4
// projection matrix).
template <typename T, int row_stride, int col_stride>
struct MatrixAdapter;
// Convenience functions to create a MatrixAdapter that treats the
// array pointed to by "pointer" as a 3x3 (contiguous) column-major or
// row-major matrix.
template <typename T>
MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer);
template <typename T>
MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer);
// Convert a value in combined axis-angle representation to a quaternion.
// The value angle_axis is a triple whose norm is an angle in radians,
// and whose direction is aligned with the axis of rotation,
// and quaternion is a 4-tuple that will contain the resulting quaternion.
// The implementation may be used with auto-differentiation up to the first
// derivative, higher derivatives may have unexpected results near the origin.
template<typename T>
void AngleAxisToQuaternion(const T* angle_axis, T* quaternion);
// Convert a quaternion to the equivalent combined axis-angle representation.
// The value quaternion must be a unit quaternion - it is not normalized first,
// and angle_axis will be filled with a value whose norm is the angle of
// rotation in radians, and whose direction is the axis of rotation.
// The implemention may be used with auto-differentiation up to the first
// derivative, higher derivatives may have unexpected results near the origin.
template<typename T>
void QuaternionToAngleAxis(const T* quaternion, T* angle_axis);
// Conversions between 3x3 rotation matrix (in column major order) and
// axis-angle rotation representations. Templated for use with
// autodifferentiation.
template <typename T>
void RotationMatrixToAngleAxis(const T* R, T* angle_axis);
template <typename T, int row_stride, int col_stride>
void RotationMatrixToAngleAxis(
const MatrixAdapter<const T, row_stride, col_stride>& R,
T* angle_axis);
template <typename T>
void AngleAxisToRotationMatrix(const T* angle_axis, T* R);
template <typename T, int row_stride, int col_stride>
void AngleAxisToRotationMatrix(
const T* angle_axis,
const MatrixAdapter<T, row_stride, col_stride>& R);
// Conversions between 3x3 rotation matrix (in row major order) and
// Euler angle (in degrees) rotation representations.
//
// The {pitch,roll,yaw} Euler angles are rotations around the {x,y,z}
// axes, respectively. They are applied in that same order, so the
// total rotation R is Rz * Ry * Rx.
template <typename T>
void EulerAnglesToRotationMatrix(const T* euler, int row_stride, T* R);
template <typename T, int row_stride, int col_stride>
void EulerAnglesToRotationMatrix(
const T* euler,
const MatrixAdapter<T, row_stride, col_stride>& R);
// Convert a 4-vector to a 3x3 scaled rotation matrix.
//
// The choice of rotation is such that the quaternion [1 0 0 0] goes to an
// identity matrix and for small a, b, c the quaternion [1 a b c] goes to
// the matrix
//
// [ 0 -c b ]
// I + 2 [ c 0 -a ] + higher order terms
// [ -b a 0 ]
//
// which corresponds to a Rodrigues approximation, the last matrix being
// the cross-product matrix of [a b c]. Together with the property that
// R(q1 * q2) = R(q1) * R(q2) this uniquely defines the mapping from q to R.
//
// The rotation matrix is row-major.
//
// No normalization of the quaternion is performed, i.e.
// R = ||q||^2 * Q, where Q is an orthonormal matrix
// such that det(Q) = 1 and Q*Q' = I
template <typename T> inline
void QuaternionToScaledRotation(const T q[4], T R[3 * 3]);
template <typename T, int row_stride, int col_stride> inline
void QuaternionToScaledRotation(
const T q[4],
const MatrixAdapter<T, row_stride, col_stride>& R);
// Same as above except that the rotation matrix is normalized by the
// Frobenius norm, so that R * R' = I (and det(R) = 1).
template <typename T> inline
void QuaternionToRotation(const T q[4], T R[3 * 3]);
template <typename T, int row_stride, int col_stride> inline
void QuaternionToRotation(
const T q[4],
const MatrixAdapter<T, row_stride, col_stride>& R);
// Rotates a point pt by a quaternion q:
//
// result = R(q) * pt
//
// Assumes the quaternion is unit norm. This assumption allows us to
// write the transform as (something)*pt + pt, as is clear from the
// formula below. If you pass in a quaternion with |q|^2 = 2 then you
// WILL NOT get back 2 times the result you get for a unit quaternion.
template <typename T> inline
void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]);
// With this function you do not need to assume that q has unit norm.
// It does assume that the norm is non-zero.
template <typename T> inline
void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]);
// zw = z * w, where * is the Quaternion product between 4 vectors.
template<typename T> inline
void QuaternionProduct(const T z[4], const T w[4], T zw[4]);
// xy = x cross y;
template<typename T> inline
void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]);
template<typename T> inline
T DotProduct(const T x[3], const T y[3]);
// y = R(angle_axis) * x;
template<typename T> inline
void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]);
// --- IMPLEMENTATION
template<typename T, int row_stride, int col_stride>
struct MatrixAdapter {
T* pointer_;
explicit MatrixAdapter(T* pointer)
: pointer_(pointer)
{}
T& operator()(int r, int c) const {
return pointer_[r * row_stride + c * col_stride];
}
};
template <typename T>
MatrixAdapter<T, 1, 3> ColumnMajorAdapter3x3(T* pointer) {
return MatrixAdapter<T, 1, 3>(pointer);
}
template <typename T>
MatrixAdapter<T, 3, 1> RowMajorAdapter3x3(T* pointer) {
return MatrixAdapter<T, 3, 1>(pointer);
}
template<typename T>
inline void AngleAxisToQuaternion(const T* angle_axis, T* quaternion) {
const T& a0 = angle_axis[0];
const T& a1 = angle_axis[1];
const T& a2 = angle_axis[2];
const T theta_squared = a0 * a0 + a1 * a1 + a2 * a2;
// For points not at the origin, the full conversion is numerically stable.
if (theta_squared > T(0.0)) {
const T theta = sqrt(theta_squared);
const T half_theta = theta * T(0.5);
const T k = sin(half_theta) / theta;
quaternion[0] = cos(half_theta);
quaternion[1] = a0 * k;
quaternion[2] = a1 * k;
quaternion[3] = a2 * k;
} else {
// At the origin, sqrt() will produce NaN in the derivative since
// the argument is zero. By approximating with a Taylor series,
// and truncating at one term, the value and first derivatives will be
// computed correctly when Jets are used.
const T k(0.5);
quaternion[0] = T(1.0);
quaternion[1] = a0 * k;
quaternion[2] = a1 * k;
quaternion[3] = a2 * k;
}
}
template<typename T>
inline void QuaternionToAngleAxis(const T* quaternion, T* angle_axis) {
const T& q1 = quaternion[1];
const T& q2 = quaternion[2];
const T& q3 = quaternion[3];
const T sin_squared_theta = q1 * q1 + q2 * q2 + q3 * q3;
// For quaternions representing non-zero rotation, the conversion
// is numerically stable.
if (sin_squared_theta > T(0.0)) {
const T sin_theta = sqrt(sin_squared_theta);
const T& cos_theta = quaternion[0];
// If cos_theta is negative, theta is greater than pi/2, which
// means that angle for the angle_axis vector which is 2 * theta
// would be greater than pi.
//
// While this will result in the correct rotation, it does not
// result in a normalized angle-axis vector.
//
// In that case we observe that 2 * theta ~ 2 * theta - 2 * pi,
// which is equivalent saying
//
// theta - pi = atan(sin(theta - pi), cos(theta - pi))
// = atan(-sin(theta), -cos(theta))
//
const T two_theta =
T(2.0) * ((cos_theta < 0.0)
? atan2(-sin_theta, -cos_theta)
: atan2(sin_theta, cos_theta));
const T k = two_theta / sin_theta;
angle_axis[0] = q1 * k;
angle_axis[1] = q2 * k;
angle_axis[2] = q3 * k;
} else {
// For zero rotation, sqrt() will produce NaN in the derivative since
// the argument is zero. By approximating with a Taylor series,
// and truncating at one term, the value and first derivatives will be
// computed correctly when Jets are used.
const T k(2.0);
angle_axis[0] = q1 * k;
angle_axis[1] = q2 * k;
angle_axis[2] = q3 * k;
}
}
// The conversion of a rotation matrix to the angle-axis form is
// numerically problematic when then rotation angle is close to zero
// or to Pi. The following implementation detects when these two cases
// occurs and deals with them by taking code paths that are guaranteed
// to not perform division by a small number.
template <typename T>
inline void RotationMatrixToAngleAxis(const T* R, T* angle_axis) {
RotationMatrixToAngleAxis(ColumnMajorAdapter3x3(R), angle_axis);
}
template <typename T, int row_stride, int col_stride>
void RotationMatrixToAngleAxis(
const MatrixAdapter<const T, row_stride, col_stride>& R,
T* angle_axis) {
// x = k * 2 * sin(theta), where k is the axis of rotation.
angle_axis[0] = R(2, 1) - R(1, 2);
angle_axis[1] = R(0, 2) - R(2, 0);
angle_axis[2] = R(1, 0) - R(0, 1);
static const T kOne = T(1.0);
static const T kTwo = T(2.0);
// Since the right hand side may give numbers just above 1.0 or
// below -1.0 leading to atan misbehaving, we threshold.
T costheta = std::min(std::max((R(0, 0) + R(1, 1) + R(2, 2) - kOne) / kTwo,
T(-1.0)),
kOne);
// sqrt is guaranteed to give non-negative results, so we only
// threshold above.
T sintheta = std::min(sqrt(angle_axis[0] * angle_axis[0] +
angle_axis[1] * angle_axis[1] +
angle_axis[2] * angle_axis[2]) / kTwo,
kOne);
// Use the arctan2 to get the right sign on theta
const T theta = atan2(sintheta, costheta);
// Case 1: sin(theta) is large enough, so dividing by it is not a
// problem. We do not use abs here, because while jets.h imports
// std::abs into the namespace, here in this file, abs resolves to
// the int version of the function, which returns zero always.
//
// We use a threshold much larger then the machine epsilon, because
// if sin(theta) is small, not only do we risk overflow but even if
// that does not occur, just dividing by a small number will result
// in numerical garbage. So we play it safe.
static const double kThreshold = 1e-12;
if ((sintheta > kThreshold) || (sintheta < -kThreshold)) {
const T r = theta / (kTwo * sintheta);
for (int i = 0; i < 3; ++i) {
angle_axis[i] *= r;
}
return;
}
// Case 2: theta ~ 0, means sin(theta) ~ theta to a good
// approximation.
if (costheta > 0.0) {
const T kHalf = T(0.5);
for (int i = 0; i < 3; ++i) {
angle_axis[i] *= kHalf;
}
return;
}
// Case 3: theta ~ pi, this is the hard case. Since theta is large,
// and sin(theta) is small. Dividing by theta by sin(theta) will
// either give an overflow or worse still numerically meaningless
// results. Thus we use an alternate more complicated formula
// here.
// Since cos(theta) is negative, division by (1-cos(theta)) cannot
// overflow.
const T inv_one_minus_costheta = kOne / (kOne - costheta);
// We now compute the absolute value of coordinates of the axis
// vector using the diagonal entries of R. To resolve the sign of
// these entries, we compare the sign of angle_axis[i]*sin(theta)
// with the sign of sin(theta). If they are the same, then
// angle_axis[i] should be positive, otherwise negative.
for (int i = 0; i < 3; ++i) {
angle_axis[i] = theta * sqrt((R(i, i) - costheta) * inv_one_minus_costheta);
if (((sintheta < 0.0) && (angle_axis[i] > 0.0)) ||
((sintheta > 0.0) && (angle_axis[i] < 0.0))) {
angle_axis[i] = -angle_axis[i];
}
}
}
template <typename T>
inline void AngleAxisToRotationMatrix(const T* angle_axis, T* R) {
AngleAxisToRotationMatrix(angle_axis, ColumnMajorAdapter3x3(R));
}
template <typename T, int row_stride, int col_stride>
void AngleAxisToRotationMatrix(
const T* angle_axis,
const MatrixAdapter<T, row_stride, col_stride>& R) {
static const T kOne = T(1.0);
const T theta2 = DotProduct(angle_axis, angle_axis);
if (theta2 > T(std::numeric_limits<double>::epsilon())) {
// We want to be careful to only evaluate the square root if the
// norm of the angle_axis vector is greater than zero. Otherwise
// we get a division by zero.
const T theta = sqrt(theta2);
const T wx = angle_axis[0] / theta;
const T wy = angle_axis[1] / theta;
const T wz = angle_axis[2] / theta;
const T costheta = cos(theta);
const T sintheta = sin(theta);
R(0, 0) = costheta + wx*wx*(kOne - costheta);
R(1, 0) = wz*sintheta + wx*wy*(kOne - costheta);
R(2, 0) = -wy*sintheta + wx*wz*(kOne - costheta);
R(0, 1) = wx*wy*(kOne - costheta) - wz*sintheta;
R(1, 1) = costheta + wy*wy*(kOne - costheta);
R(2, 1) = wx*sintheta + wy*wz*(kOne - costheta);
R(0, 2) = wy*sintheta + wx*wz*(kOne - costheta);
R(1, 2) = -wx*sintheta + wy*wz*(kOne - costheta);
R(2, 2) = costheta + wz*wz*(kOne - costheta);
} else {
// Near zero, we switch to using the first order Taylor expansion.
R(0, 0) = kOne;
R(1, 0) = angle_axis[2];
R(2, 0) = -angle_axis[1];
R(0, 1) = -angle_axis[2];
R(1, 1) = kOne;
R(2, 1) = angle_axis[0];
R(0, 2) = angle_axis[1];
R(1, 2) = -angle_axis[0];
R(2, 2) = kOne;
}
}
template <typename T>
inline void EulerAnglesToRotationMatrix(const T* euler,
const int row_stride_parameter,
T* R) {
DCHECK(row_stride_parameter==3);
EulerAnglesToRotationMatrix(euler, RowMajorAdapter3x3(R));
}
template <typename T, int row_stride, int col_stride>
void EulerAnglesToRotationMatrix(
const T* euler,
const MatrixAdapter<T, row_stride, col_stride>& R) {
const double kPi = 3.14159265358979323846;
const T degrees_to_radians(kPi / 180.0);
const T pitch(euler[0] * degrees_to_radians);
const T roll(euler[1] * degrees_to_radians);
const T yaw(euler[2] * degrees_to_radians);
const T c1 = cos(yaw);
const T s1 = sin(yaw);
const T c2 = cos(roll);
const T s2 = sin(roll);
const T c3 = cos(pitch);
const T s3 = sin(pitch);
R(0, 0) = c1*c2;
R(0, 1) = -s1*c3 + c1*s2*s3;
R(0, 2) = s1*s3 + c1*s2*c3;
R(1, 0) = s1*c2;
R(1, 1) = c1*c3 + s1*s2*s3;
R(1, 2) = -c1*s3 + s1*s2*c3;
R(2, 0) = -s2;
R(2, 1) = c2*s3;
R(2, 2) = c2*c3;
}
template <typename T> inline
void QuaternionToScaledRotation(const T q[4], T R[3 * 3]) {
QuaternionToScaledRotation(q, RowMajorAdapter3x3(R));
}
template <typename T, int row_stride, int col_stride> inline
void QuaternionToScaledRotation(
const T q[4],
const MatrixAdapter<T, row_stride, col_stride>& R) {
// Make convenient names for elements of q.
T a = q[0];
T b = q[1];
T c = q[2];
T d = q[3];
// This is not to eliminate common sub-expression, but to
// make the lines shorter so that they fit in 80 columns!
T aa = a * a;
T ab = a * b;
T ac = a * c;
T ad = a * d;
T bb = b * b;
T bc = b * c;
T bd = b * d;
T cc = c * c;
T cd = c * d;
T dd = d * d;
R(0, 0) = aa + bb - cc - dd; R(0, 1) = T(2) * (bc - ad); R(0, 2) = T(2) * (ac + bd); // NOLINT
R(1, 0) = T(2) * (ad + bc); R(1, 1) = aa - bb + cc - dd; R(1, 2) = T(2) * (cd - ab); // NOLINT
R(2, 0) = T(2) * (bd - ac); R(2, 1) = T(2) * (ab + cd); R(2, 2) = aa - bb - cc + dd; // NOLINT
}
template <typename T> inline
void QuaternionToRotation(const T q[4], T R[3 * 3]) {
QuaternionToRotation(q, RowMajorAdapter3x3(R));
}
template <typename T, int row_stride, int col_stride> inline
void QuaternionToRotation(const T q[4],
const MatrixAdapter<T, row_stride, col_stride>& R) {
QuaternionToScaledRotation(q, R);
T normalizer = q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3];
CHECK_NE(normalizer, T(0));
normalizer = T(1) / normalizer;
for (int i = 0; i < 3; ++i) {
for (int j = 0; j < 3; ++j) {
R(i, j) *= normalizer;
}
}
}
template <typename T> inline
void UnitQuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
const T t2 = q[0] * q[1];
const T t3 = q[0] * q[2];
const T t4 = q[0] * q[3];
const T t5 = -q[1] * q[1];
const T t6 = q[1] * q[2];
const T t7 = q[1] * q[3];
const T t8 = -q[2] * q[2];
const T t9 = q[2] * q[3];
const T t1 = -q[3] * q[3];
result[0] = T(2) * ((t8 + t1) * pt[0] + (t6 - t4) * pt[1] + (t3 + t7) * pt[2]) + pt[0]; // NOLINT
result[1] = T(2) * ((t4 + t6) * pt[0] + (t5 + t1) * pt[1] + (t9 - t2) * pt[2]) + pt[1]; // NOLINT
result[2] = T(2) * ((t7 - t3) * pt[0] + (t2 + t9) * pt[1] + (t5 + t8) * pt[2]) + pt[2]; // NOLINT
}
template <typename T> inline
void QuaternionRotatePoint(const T q[4], const T pt[3], T result[3]) {
// 'scale' is 1 / norm(q).
const T scale = T(1) / sqrt(q[0] * q[0] +
q[1] * q[1] +
q[2] * q[2] +
q[3] * q[3]);
// Make unit-norm version of q.
const T unit[4] = {
scale * q[0],
scale * q[1],
scale * q[2],
scale * q[3],
};
UnitQuaternionRotatePoint(unit, pt, result);
}
template<typename T> inline
void QuaternionProduct(const T z[4], const T w[4], T zw[4]) {
zw[0] = z[0] * w[0] - z[1] * w[1] - z[2] * w[2] - z[3] * w[3];
zw[1] = z[0] * w[1] + z[1] * w[0] + z[2] * w[3] - z[3] * w[2];
zw[2] = z[0] * w[2] - z[1] * w[3] + z[2] * w[0] + z[3] * w[1];
zw[3] = z[0] * w[3] + z[1] * w[2] - z[2] * w[1] + z[3] * w[0];
}
// xy = x cross y;
template<typename T> inline
void CrossProduct(const T x[3], const T y[3], T x_cross_y[3]) {
x_cross_y[0] = x[1] * y[2] - x[2] * y[1];
x_cross_y[1] = x[2] * y[0] - x[0] * y[2];
x_cross_y[2] = x[0] * y[1] - x[1] * y[0];
}
template<typename T> inline
T DotProduct(const T x[3], const T y[3]) {
return (x[0] * y[0] + x[1] * y[1] + x[2] * y[2]);
}
template<typename T> inline
void AngleAxisRotatePoint(const T angle_axis[3], const T pt[3], T result[3]) {
const T theta2 = DotProduct(angle_axis, angle_axis);
if (theta2 > T(std::numeric_limits<double>::epsilon())) {
// Away from zero, use the rodriguez formula
//
// result = pt costheta +
// (w x pt) * sintheta +
// w (w . pt) (1 - costheta)
//
// We want to be careful to only evaluate the square root if the
// norm of the angle_axis vector is greater than zero. Otherwise
// we get a division by zero.
//
const T theta = sqrt(theta2);
const T costheta = cos(theta);
const T sintheta = sin(theta);
const T theta_inverse = 1.0 / theta;
const T w[3] = { angle_axis[0] * theta_inverse,
angle_axis[1] * theta_inverse,
angle_axis[2] * theta_inverse };
// Explicitly inlined evaluation of the cross product for
// performance reasons.
const T w_cross_pt[3] = { w[1] * pt[2] - w[2] * pt[1],
w[2] * pt[0] - w[0] * pt[2],
w[0] * pt[1] - w[1] * pt[0] };
const T tmp =
(w[0] * pt[0] + w[1] * pt[1] + w[2] * pt[2]) * (T(1.0) - costheta);
result[0] = pt[0] * costheta + w_cross_pt[0] * sintheta + w[0] * tmp;
result[1] = pt[1] * costheta + w_cross_pt[1] * sintheta + w[1] * tmp;
result[2] = pt[2] * costheta + w_cross_pt[2] * sintheta + w[2] * tmp;
} else {
// Near zero, the first order Taylor approximation of the rotation
// matrix R corresponding to a vector w and angle w is
//
// R = I + hat(w) * sin(theta)
//
// But sintheta ~ theta and theta * w = angle_axis, which gives us
//
// R = I + hat(w)
//
// and actually performing multiplication with the point pt, gives us
// R * pt = pt + w x pt.
//
// Switching to the Taylor expansion near zero provides meaningful
// derivatives when evaluated using Jets.
//
// Explicitly inlined evaluation of the cross product for
// performance reasons.
const T w_cross_pt[3] = { angle_axis[1] * pt[2] - angle_axis[2] * pt[1],
angle_axis[2] * pt[0] - angle_axis[0] * pt[2],
angle_axis[0] * pt[1] - angle_axis[1] * pt[0] };
result[0] = pt[0] + w_cross_pt[0];
result[1] = pt[1] + w_cross_pt[1];
result[2] = pt[2] + w_cross_pt[2];
}
}
} // namespace ceres
#endif // CERES_PUBLIC_ROTATION_H_

181
gtsam/3rdparty/ceres/variadic_evaluate.h vendored Normal file
View File

@ -0,0 +1,181 @@
// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2013 Google Inc. All rights reserved.
// http://code.google.com/p/ceres-solver/
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
//
// * Redistributions of source code must retain the above copyright notice,
// this list of conditions and the following disclaimer.
// * Redistributions in binary form must reproduce the above copyright notice,
// this list of conditions and the following disclaimer in the documentation
// and/or other materials provided with the distribution.
// * Neither the name of Google Inc. nor the names of its contributors may be
// used to endorse or promote products derived from this software without
// specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
// POSSIBILITY OF SUCH DAMAGE.
//
// Author: sameeragarwal@google.com (Sameer Agarwal)
// mierle@gmail.com (Keir Mierle)
#ifndef CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
#define CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_
#include <stddef.h>
#include <gtsam/3rdparty/ceres/jet.h>
#include <gtsam/3rdparty/ceres/eigen.h>
#include <gtsam/3rdparty/ceres/fixed_array.h>
namespace ceres {
namespace internal {
// This block of quasi-repeated code calls the user-supplied functor, which may
// take a variable number of arguments. This is accomplished by specializing the
// struct based on the size of the trailing parameters; parameters with 0 size
// are assumed missing.
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
int N5, int N6, int N7, int N8, int N9>
struct VariadicEvaluate {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
input[2],
input[3],
input[4],
input[5],
input[6],
input[7],
input[8],
input[9],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
int N5, int N6, int N7, int N8>
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, N8, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
input[2],
input[3],
input[4],
input[5],
input[6],
input[7],
input[8],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
int N5, int N6, int N7>
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, N7, 0, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
input[2],
input[3],
input[4],
input[5],
input[6],
input[7],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
int N5, int N6>
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, N6, 0, 0, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
input[2],
input[3],
input[4],
input[5],
input[6],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4,
int N5>
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, N5, 0, 0, 0, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
input[2],
input[3],
input[4],
input[5],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2, int N3, int N4>
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, N4, 0, 0, 0, 0, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
input[2],
input[3],
input[4],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2, int N3>
struct VariadicEvaluate<Functor, T, N0, N1, N2, N3, 0, 0, 0, 0, 0, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
input[2],
input[3],
output);
}
};
template<typename Functor, typename T, int N0, int N1, int N2>
struct VariadicEvaluate<Functor, T, N0, N1, N2, 0, 0, 0, 0, 0, 0, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
input[2],
output);
}
};
template<typename Functor, typename T, int N0, int N1>
struct VariadicEvaluate<Functor, T, N0, N1, 0, 0, 0, 0, 0, 0, 0, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
input[1],
output);
}
};
template<typename Functor, typename T, int N0>
struct VariadicEvaluate<Functor, T, N0, 0, 0, 0, 0, 0, 0, 0, 0, 0> {
static bool Call(const Functor& functor, T const *const *input, T* output) {
return functor(input[0],
output);
}
};
} // namespace internal
} // namespace ceres
#endif // CERES_PUBLIC_INTERNAL_VARIADIC_EVALUATE_H_

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

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

16
gtsam/3rdparty/metis-5.1.0/libmetis/CMakeLists.txt vendored
View File

@ -1,16 +0,0 @@
# Add this directory for internal users.
include_directories(.)
# Find sources.
file(GLOB metis_sources *.c)
# Build libmetis.
add_library(metis ${METIS_LIBRARY_TYPE} ${GKlib_sources} ${metis_sources})
if(UNIX)
target_link_libraries(metis m)
endif()
install(TARGETS metis
LIBRARY DESTINATION include/gtsam/3rdparty/metis/lib
RUNTIME DESTINATION include/gtsam/3rdparty/metis/lib
ARCHIVE DESTINATION include/gtsam/3rdparty/metis/lib)

0
gtsam/3rdparty/metis-5.1.0/BUILD-Windows.txt → gtsam/3rdparty/metis/BUILD-Windows.txt vendored
View File

0
gtsam/3rdparty/metis-5.1.0/BUILD.txt → gtsam/3rdparty/metis/BUILD.txt vendored
View File

3
gtsam/3rdparty/metis-5.1.0/CMakeLists.txt → gtsam/3rdparty/metis/CMakeLists.txt vendored
View File

@ -46,3 +46,6 @@ add_subdirectory("libmetis")
if(GTSAM_BUILD_METIS_EXECUTABLES)
add_subdirectory("programs")
endif()
set(GTSAM_EXPORTED_TARGETS "${GTSAM_EXPORTED_TARGETS}" PARENT_SCOPE)

0
gtsam/3rdparty/metis-5.1.0/Changelog → gtsam/3rdparty/metis/Changelog vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/BUILD.txt → gtsam/3rdparty/metis/GKlib/BUILD.txt vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/CMakeLists.txt → gtsam/3rdparty/metis/GKlib/CMakeLists.txt vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/GKlib.h → gtsam/3rdparty/metis/GKlib/GKlib.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/GKlibSystem.cmake → gtsam/3rdparty/metis/GKlib/GKlibSystem.cmake vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/Makefile → gtsam/3rdparty/metis/GKlib/Makefile vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/b64.c → gtsam/3rdparty/metis/GKlib/b64.c vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/blas.c → gtsam/3rdparty/metis/GKlib/blas.c vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/conf/check_thread_storage.c → gtsam/3rdparty/metis/GKlib/conf/check_thread_storage.c vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/csr.c → gtsam/3rdparty/metis/GKlib/csr.c vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/error.c → gtsam/3rdparty/metis/GKlib/error.c vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/evaluate.c → gtsam/3rdparty/metis/GKlib/evaluate.c vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/fkvkselect.c → gtsam/3rdparty/metis/GKlib/fkvkselect.c vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/fs.c → gtsam/3rdparty/metis/GKlib/fs.c vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/getopt.c → gtsam/3rdparty/metis/GKlib/getopt.c vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gk_arch.h → gtsam/3rdparty/metis/GKlib/gk_arch.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gk_defs.h → gtsam/3rdparty/metis/GKlib/gk_defs.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gk_externs.h → gtsam/3rdparty/metis/GKlib/gk_externs.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gk_getopt.h → gtsam/3rdparty/metis/GKlib/gk_getopt.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gk_macros.h → gtsam/3rdparty/metis/GKlib/gk_macros.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkblas.h → gtsam/3rdparty/metis/GKlib/gk_mkblas.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkmemory.h → gtsam/3rdparty/metis/GKlib/gk_mkmemory.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkpqueue.h → gtsam/3rdparty/metis/GKlib/gk_mkpqueue.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkpqueue2.h → gtsam/3rdparty/metis/GKlib/gk_mkpqueue2.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkrandom.h → gtsam/3rdparty/metis/GKlib/gk_mkrandom.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gk_mksort.h → gtsam/3rdparty/metis/GKlib/gk_mksort.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gk_mkutils.h → gtsam/3rdparty/metis/GKlib/gk_mkutils.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gk_proto.h → gtsam/3rdparty/metis/GKlib/gk_proto.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gk_struct.h → gtsam/3rdparty/metis/GKlib/gk_struct.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gk_types.h → gtsam/3rdparty/metis/GKlib/gk_types.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gkregex.c → gtsam/3rdparty/metis/GKlib/gkregex.c vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/gkregex.h → gtsam/3rdparty/metis/GKlib/gkregex.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/graph.c → gtsam/3rdparty/metis/GKlib/graph.c vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/htable.c → gtsam/3rdparty/metis/GKlib/htable.c vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/io.c → gtsam/3rdparty/metis/GKlib/io.c vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/itemsets.c → gtsam/3rdparty/metis/GKlib/itemsets.c vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/mcore.c → gtsam/3rdparty/metis/GKlib/mcore.c vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/memory.c → gtsam/3rdparty/metis/GKlib/memory.c vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/ms_inttypes.h → gtsam/3rdparty/metis/GKlib/ms_inttypes.h vendored
View File

0
gtsam/3rdparty/metis-5.1.0/GKlib/ms_stat.h → gtsam/3rdparty/metis/GKlib/ms_stat.h vendored
View File

81
gtsam/3rdparty/metis-5.1.0/GKlib/ms_stdint.h → gtsam/3rdparty/metis/GKlib/ms_stdint.h vendored
View File

@ -1,7 +1,7 @@
// ISO C9x compliant stdint.h for Microsoft Visual Studio
// Based on ISO/IEC 9899:TC2 Committee draft (May 6, 2005) WG14/N1124
//
// Copyright (c) 2006 Alexander Chemeris
// Copyright (c) 2006-2013 Alexander Chemeris
//
// Redistribution and use in source and binary forms, with or without
// modification, are permitted provided that the following conditions are met:
@ -13,8 +13,9 @@
// notice, this list of conditions and the following disclaimer in the
// documentation and/or other materials provided with the distribution.
//
// 3. The name of the author may be used to endorse or promote products
// derived from this software without specific prior written permission.
// 3. Neither the name of the product nor the names of its contributors may
// be used to endorse or promote products derived from this software
// without specific prior written permission.
//
// THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``AS IS'' AND ANY EXPRESS OR IMPLIED
// WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
@ -40,30 +41,59 @@
#pragma once
#endif
#if _MSC_VER >= 1600 // [
#include <stdint.h>
#else // ] _MSC_VER >= 1600 [
#include <limits.h>
// For Visual Studio 6 in C++ mode wrap <wchar.h> include with 'extern "C++" {}'
// For Visual Studio 6 in C++ mode and for many Visual Studio versions when
// compiling for ARM we should wrap <wchar.h> include with 'extern "C++" {}'
// or compiler give many errors like this:
// error C2733: second C linkage of overloaded function 'wmemchr' not allowed
#if (_MSC_VER < 1300) && defined(__cplusplus)
extern "C++" {
#endif
# include <wchar.h>
#if (_MSC_VER < 1300) && defined(__cplusplus)
}
#ifdef __cplusplus
extern "C" {
#endif
# include <wchar.h>
#ifdef __cplusplus
}
#endif
// Define _W64 macros to mark types changing their size, like intptr_t.
#ifndef _W64
# if !defined(__midl) && (defined(_X86_) || defined(_M_IX86)) && _MSC_VER >= 1300
# define _W64 __w64
# else
# define _W64
# endif
#endif
// 7.18.1 Integer types
// 7.18.1.1 Exact-width integer types
typedef __int8 int8_t;
typedef __int16 int16_t;
typedef __int32 int32_t;
typedef __int64 int64_t;
// Visual Studio 6 and Embedded Visual C++ 4 doesn't
// realize that, e.g. char has the same size as __int8
// so we give up on __intX for them.
#if (_MSC_VER < 1300)
typedef signed char int8_t;
typedef signed short int16_t;
typedef signed int int32_t;
typedef unsigned char uint8_t;
typedef unsigned short uint16_t;
typedef unsigned int uint32_t;
#else
typedef signed __int8 int8_t;
typedef signed __int16 int16_t;
typedef signed __int32 int32_t;
typedef unsigned __int8 uint8_t;
typedef unsigned __int16 uint16_t;
typedef unsigned __int32 uint32_t;
typedef unsigned __int64 uint64_t;
#endif
typedef signed __int64 int64_t;
typedef unsigned __int64 uint64_t;
// 7.18.1.2 Minimum-width integer types
typedef int8_t int_least8_t;
@ -87,11 +117,11 @@ typedef uint64_t uint_fast64_t;
// 7.18.1.4 Integer types capable of holding object pointers
#ifdef _WIN64 // [
typedef __int64 intptr_t;
typedef unsigned __int64 uintptr_t;
typedef signed __int64 intptr_t;
typedef unsigned __int64 uintptr_t;
#else // _WIN64 ][
typedef int intptr_t;
typedef unsigned int uintptr_t;
typedef _W64 signed int intptr_t;
typedef _W64 unsigned int uintptr_t;
#endif // _WIN64 ]
// 7.18.1.5 Greatest-width integer types
@ -213,10 +243,17 @@ typedef uint64_t uintmax_t;
#define UINT64_C(val) val##ui64
// 7.18.4.2 Macros for greatest-width integer constants
#define INTMAX_C INT64_C
#define UINTMAX_C UINT64_C
// These #ifndef's are needed to prevent collisions with <boost/cstdint.hpp>.
// Check out Issue 9 for the details.
#ifndef INTMAX_C // [
# define INTMAX_C INT64_C
#endif // INTMAX_C ]
#ifndef UINTMAX_C // [
# define UINTMAX_C UINT64_C
#endif // UINTMAX_C ]
#endif // __STDC_CONSTANT_MACROS ]
#endif // _MSC_VER >= 1600 ]
#endif // _MSC_STDINT_H_ ]
#endif // _MSC_STDINT_H_ ]

0
gtsam/3rdparty/metis-5.1.0/GKlib/omp.c → gtsam/3rdparty/metis/GKlib/omp.c vendored
View File

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