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

Merging 2.0_prep branch into trunk, with dynamic Value and Values class, integer nonlinear keys, key print formatters, and new CMake build process. 

Merge commit '2cf01d1ca075a3da909a10c58acb2792b62f6456' into trunk

Conflicts:
	.gitattributes
	.gitignore
	gtsam/slam/GeneralSFMFactor.h
	tests/CMakeLists.txt
release/4.3a0
Richard Roberts 2012-02-24 21:09:20 +00:00
parent 9ef8ce5fa6 9121df7883
commit 4d117037a5
187 changed files with 4646 additions and 9121 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

162
.cproject
View File

@ -21,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="${workspace_loc:/gtsam/build}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.319933862" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<builder 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"/>
<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">
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1032375444" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">
@ -39,6 +39,7 @@
<listOptionValue builtIn="false" value="/usr/include/c++/4.6.1"/>
<listOptionValue builtIn="false" value="/usr/include/c++/4.6"/>
<listOptionValue builtIn="false" value="/usr/include/c++/4.6/backward"/>
<listOptionValue builtIn="false" value="&quot;${ProjDirPath}&quot;"/>
</option>
<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.1833545667" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
</tool>
@ -71,7 +72,7 @@
<folderInfo id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.1441575890." name="/" resourcePath="">
<toolChain id="cdt.managedbuild.toolchain.gnu.macosx.base.1257341773" 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.1756820285" name="Debug Platform" osList="macosx" superClass="cdt.managedbuild.target.gnu.platform.macosx.base"/>
<builder arguments="" buildPath="${workspace_loc:/gtsam/build-timing}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.404194139" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="2" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<builder arguments="" buildPath="${workspace_loc:/gtsam/build-timing}" command="make" id="cdt.managedbuild.target.gnu.builder.macosx.base.404194139" keepEnvironmentInBuildfile="false" managedBuildOn="false" name="Gnu Make Builder" parallelBuildOn="true" parallelizationNumber="5" superClass="cdt.managedbuild.target.gnu.builder.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.c.linker.macosx.base.879403713" name="MacOS X C Linker" superClass="cdt.managedbuild.tool.macosx.c.linker.macosx.base"/>
<tool id="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base.48676242" name="MacOS X C++ Linker" superClass="cdt.managedbuild.tool.macosx.cpp.linker.macosx.base">
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.1326666213" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">
@ -98,11 +99,63 @@
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
</cconfiguration>
<cconfiguration id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216">
<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"/>
</extensions>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<configuration artifactName="gtsam" buildProperties="" description="" id="cdt.managedbuild.toolchain.gnu.macosx.base.1359703544.127261216" name="fast" parent="org.eclipse.cdt.build.core.emptycfg">
<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"/>
<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">
<inputType id="cdt.managedbuild.tool.macosx.cpp.linker.input.2101986359" superClass="cdt.managedbuild.tool.macosx.cpp.linker.input">
<additionalInput kind="additionalinputdependency" paths="$(USER_OBJS)"/>
<additionalInput kind="additionalinput" paths="$(LIBS)"/>
</inputType>
</tool>
<tool id="cdt.managedbuild.tool.gnu.assembler.macosx.base.1665834395" name="GCC Assembler" superClass="cdt.managedbuild.tool.gnu.assembler.macosx.base">
<inputType id="cdt.managedbuild.tool.gnu.assembler.input.1096646805" superClass="cdt.managedbuild.tool.gnu.assembler.input"/>
</tool>
<tool id="cdt.managedbuild.tool.gnu.archiver.macosx.base.1662212593" name="GCC Archiver" superClass="cdt.managedbuild.tool.gnu.archiver.macosx.base"/>
<tool id="cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base.165336396" name="GCC C++ Compiler" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.macosx.base">
<option id="gnu.cpp.compiler.option.include.paths.1336290606" name="Include paths (-I)" superClass="gnu.cpp.compiler.option.include.paths" valueType="includePath">
<listOptionValue builtIn="false" value="&quot;${workspace_loc:/gtsam}&quot;"/>
<listOptionValue builtIn="false" value="/usr/include/c++/4.6.1"/>
<listOptionValue builtIn="false" value="/usr/include/c++/4.6"/>
<listOptionValue builtIn="false" value="/usr/include/c++/4.6/backward"/>
</option>
<inputType id="cdt.managedbuild.tool.gnu.cpp.compiler.input.515209182" superClass="cdt.managedbuild.tool.gnu.cpp.compiler.input"/>
</tool>
<tool id="cdt.managedbuild.tool.gnu.c.compiler.macosx.base.302039063" name="GCC C Compiler" superClass="cdt.managedbuild.tool.gnu.c.compiler.macosx.base">
<inputType id="cdt.managedbuild.tool.gnu.c.compiler.input.623172045" superClass="cdt.managedbuild.tool.gnu.c.compiler.input"/>
</tool>
</toolChain>
</folderInfo>
</configuration>
</storageModule>
<storageModule moduleId="org.eclipse.cdt.core.externalSettings"/>
<storageModule moduleId="org.eclipse.cdt.core.language.mapping"/>
<storageModule moduleId="org.eclipse.cdt.internal.ui.text.commentOwnerProjectMappings"/>
</cconfiguration>
</storageModule>
<storageModule moduleId="cdtBuildSystem" version="4.0.0">
<project id="gtsam.null.1344331286" name="gtsam"/>
</storageModule>
<storageModule moduleId="refreshScope"/>
<storageModule moduleId="refreshScope" versionNumber="1">
<resource resourceType="PROJECT" workspacePath="/gtsam"/>
</storageModule>
<storageModule moduleId="scannerConfiguration">
<autodiscovery enabled="true" problemReportingEnabled="true" selectedProfileId="org.eclipse.cdt.make.core.GCCStandardMakePerProjectProfile"/>
<profile id="org.eclipse.cdt.make.core.GCCStandardMakePerFileProfile">
@ -258,6 +311,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -284,7 +345,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>
@ -292,7 +352,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>
@ -340,7 +399,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>
@ -348,7 +406,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>
@ -356,7 +413,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>
@ -372,20 +428,11 @@
</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>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -412,6 +459,7 @@
</target>
<target name="testGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -483,6 +531,7 @@
</target>
<target name="testInference.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -490,6 +539,7 @@
</target>
<target name="testGaussianFactor.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testGaussianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -497,6 +547,7 @@
</target>
<target name="testJunctionTree.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -504,6 +555,7 @@
</target>
<target name="testSymbolicBayesNet.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicBayesNet.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -511,6 +563,7 @@
</target>
<target name="testSymbolicFactorGraph.run" path="tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>testSymbolicFactorGraph.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
@ -580,22 +633,6 @@
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -612,6 +649,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="tests/testPose3.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>tests/testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -764,14 +817,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>vSFMexample.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -780,6 +825,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="vSFMexample.run" path="build/examples/vSLAMexample" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
<buildTarget>vSFMexample.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -1038,7 +1091,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>
@ -1494,6 +1546,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>
@ -1533,6 +1586,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>
@ -1540,6 +1594,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>
@ -2170,6 +2225,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>
@ -2231,6 +2287,21 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="cmake" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cmake</buildCommand>
<buildArguments>..</buildArguments>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="nonlinear.testValues.run" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments/>
<buildTarget>nonlinear.testValues.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2359,6 +2430,13 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="cmake" path="build" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>cmake</buildCommand>
<buildArguments>..</buildArguments>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>

4
.project
View File

@ -23,7 +23,7 @@
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildArguments</key>
<value>-j2</value>
<value>-j5</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildCommand</key>
@ -31,7 +31,7 @@
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.buildLocation</key>
<value>${workspace_loc:/gtsam/build}</value>
<value>${ProjDirPath}/build</value>
</dictionary>
<dictionary>
<key>org.eclipse.cdt.make.core.cleanBuildTarget</key>

22
CppUnitLite/Makefile.am
View File

@ -1,22 +0,0 @@
#----------------------------------------------------------------------------------------------------
# CppUnitLite
# replaced Makefile with automake for easy linking
#----------------------------------------------------------------------------------------------------
headers = TestHarness.h
sources = Failure.cpp SimpleString.cpp Test.cpp TestRegistry.cpp TestResult.cpp
headers += $(sources:.cpp=.h)
if ENABLE_INSTALL_CPPUNITLITE
CppUnitLitedir = $(includedir)/CppUnitLite
lib_LIBRARIES = libCppUnitLite.a
CppUnitLite_HEADERS = $(headers)
libCppUnitLite_a_SOURCES = $(sources)
else
noinst_LIBRARIES = libCppUnitLite.a
noinst_HEADERS = $(headers)
libCppUnitLite_a_SOURCES = $(sources)
endif

32
Makefile.am
View File

@ -1,32 +0,0 @@
#----------------------------------------------------------------------------------------------------
# GTSAM top-level automake file
#----------------------------------------------------------------------------------------------------
#The option -I m4 tells Autoconf to look for additional Autoconf macros in the m4 subdirectory.
ACLOCAL_AMFLAGS = -I m4
# make automake install some standard but missing files
# also use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = foreign nostdinc
# For Doxygen integration
#include aminclude.am
# All the sub-directories that need to be built
SUBDIRS = CppUnitLite gtsam tests examples
if ENABLE_BUILD_TOOLBOX
SUBDIRS += wrap
endif
# Add these files to make sure they're in the distribution
EXTRA_DIST = autogen.sh configure.ac COPYING INSTALL LGPL LICENSE README THANKS USAGE doc
# For Doxygen integration
#MOSTLYCLEANFILES = $(DX_CLEANFILES)
#EXTRA_DIST += $(DX_CONFIG)
# Remove .svn directories from dist
dist-hook:
rm -rf `find $(distdir)/doc -type d -name .svn`

186
aminclude.am
View File

@ -1,186 +0,0 @@
# Copyright (C) 2004 Oren Ben-Kiki
# This file is distributed under the same terms as the Automake macro files.
# Generate automatic documentation using Doxygen. Goals and variables values
# are controlled by the various DX_COND_??? conditionals set by autoconf.
#
# The provided goals are:
# doc: Generate all doxygen documentation.
# doxygen-run: Run doxygen, which will generate some of the documentation
# (HTML, CHM, CHI, MAN, RTF, XML) but will not do the post
# processing required for the rest of it (PS, PDF, and some MAN).
# doxygen-man: Rename some doxygen generated man pages.
# doxygen-ps: Generate doxygen PostScript documentation.
# doxygen-pdf: Generate doxygen PDF documentation.
#
# Note that by default these are not integrated into the automake goals. If
# doxygen is used to generate man pages, you can achieve this integration by
# setting man3_MANS to the list of man pages generated and then adding the
# dependency:
#
# $(man3_MANS): doxygen-doc
#
# This will cause make to run doxygen and generate all the documentation.
#
# The following variable is intended for use in Makefile.am:
#
# DX_CLEANFILES = everything to clean.
#
# This is usually added to MOSTLYCLEANFILES.
## --------------------------------- ##
## Format-independent Doxygen rules. ##
## --------------------------------- ##
if DX_COND_doc
## ------------------------------- ##
## Rules specific for HTML output. ##
## ------------------------------- ##
if DX_COND_html
DX_CLEAN_HTML = @DX_DOCDIR@/html
endif DX_COND_html
## ------------------------------ ##
## Rules specific for CHM output. ##
## ------------------------------ ##
if DX_COND_chm
DX_CLEAN_CHM = @DX_DOCDIR@/chm
if DX_COND_chi
DX_CLEAN_CHI = @DX_DOCDIR@/@PACKAGE@.chi
endif DX_COND_chi
endif DX_COND_chm
## ------------------------------ ##
## Rules specific for MAN output. ##
## ------------------------------ ##
if DX_COND_man
DX_CLEAN_MAN = @DX_DOCDIR@/man
endif DX_COND_man
## ------------------------------ ##
## Rules specific for RTF output. ##
## ------------------------------ ##
if DX_COND_rtf
DX_CLEAN_RTF = @DX_DOCDIR@/rtf
endif DX_COND_rtf
## ------------------------------ ##
## Rules specific for XML output. ##
## ------------------------------ ##
if DX_COND_xml
DX_CLEAN_XML = @DX_DOCDIR@/xml
endif DX_COND_xml
## ----------------------------- ##
## Rules specific for PS output. ##
## ----------------------------- ##
if DX_COND_ps
DX_CLEAN_PS = @DX_DOCDIR@/@PACKAGE@.ps
DX_PS_GOAL = doxygen-ps
doxygen-ps: @DX_DOCDIR@/@PACKAGE@.ps
@DX_DOCDIR@/@PACKAGE@.ps: @DX_DOCDIR@/@PACKAGE@.tag
cd @DX_DOCDIR@/latex; \
rm -f *.aux *.toc *.idx *.ind *.ilg *.log *.out; \
$(DX_LATEX) refman.tex; \
$(MAKEINDEX_PATH) refman.idx; \
$(DX_LATEX) refman.tex; \
countdown=5; \
while $(DX_EGREP) 'Rerun (LaTeX|to get cross-references right)' \
refman.log > /dev/null 2>&1 \
&& test $$countdown -gt 0; do \
$(DX_LATEX) refman.tex; \
countdown=`expr $$countdown - 1`; \
done; \
$(DX_DVIPS) -o ../@PACKAGE@.ps refman.dvi
endif DX_COND_ps
## ------------------------------ ##
## Rules specific for PDF output. ##
## ------------------------------ ##
if DX_COND_pdf
DX_CLEAN_PDF = @DX_DOCDIR@/@PACKAGE@.pdf
DX_PDF_GOAL = doxygen-pdf
doxygen-pdf: @DX_DOCDIR@/@PACKAGE@.pdf
@DX_DOCDIR@/@PACKAGE@.pdf: @DX_DOCDIR@/@PACKAGE@.tag
cd @DX_DOCDIR@/latex; \
rm -f *.aux *.toc *.idx *.ind *.ilg *.log *.out; \
$(DX_PDFLATEX) refman.tex; \
$(DX_MAKEINDEX) refman.idx; \
$(DX_PDFLATEX) refman.tex; \
countdown=5; \
while $(DX_EGREP) 'Rerun (LaTeX|to get cross-references right)' \
refman.log > /dev/null 2>&1 \
&& test $$countdown -gt 0; do \
$(DX_PDFLATEX) refman.tex; \
countdown=`expr $$countdown - 1`; \
done; \
mv refman.pdf ../@PACKAGE@.pdf
endif DX_COND_pdf
## ------------------------------------------------- ##
## Rules specific for LaTeX (shared for PS and PDF). ##
## ------------------------------------------------- ##
if DX_COND_latex
DX_CLEAN_LATEX = @DX_DOCDIR@/latex
endif DX_COND_latex
.PHONY: doxygen-run doc $(DX_PS_GOAL) $(DX_PDF_GOAL)
.INTERMEDIATE: doxygen-run $(DX_PS_GOAL) $(DX_PDF_GOAL)
doxygen-run: @DX_DOCDIR@/@PACKAGE@.tag
doc: doxygen-run $(DX_PS_GOAL) $(DX_PDF_GOAL)
@DX_DOCDIR@/@PACKAGE@.tag: $(DX_CONFIG) $(pkginclude_HEADERS)
rm -rf @DX_DOCDIR@
$(DX_ENV) $(DX_DOXYGEN) $(srcdir)/$(DX_CONFIG)
DX_CLEANFILES = \
@DX_DOCDIR@/@PACKAGE@.tag \
-r \
$(DX_CLEAN_HTML) \
$(DX_CLEAN_CHM) \
$(DX_CLEAN_CHI) \
$(DX_CLEAN_MAN) \
$(DX_CLEAN_RTF) \
$(DX_CLEAN_XML) \
$(DX_CLEAN_PS) \
$(DX_CLEAN_PDF) \
$(DX_CLEAN_LATEX)
endif DX_COND_doc

2
autogen.sh
View File

@ -1,2 +0,0 @@
#!/bin/sh
autoreconf --force --install -I config -I m4

191
configure.ac
View File

@ -1,191 +0,0 @@
# -*- Autoconf -*-
# Process this file with autoconf to produce a configure script.
AC_PREREQ(2.59)
AC_INIT(gtsam, 0.9.3, dellaert@cc.gatech.edu)
AM_INIT_AUTOMAKE(gtsam, 0.9.3)
AC_CONFIG_MACRO_DIR([m4])
AC_CONFIG_HEADER([config.h])
AC_CONFIG_SRCDIR([CppUnitLite/Test.cpp])
AC_CONFIG_SRCDIR([wrap/wrap.cpp])
AC_CONFIG_SRCDIR([gtsam/base/DSFVector.cpp])
AC_CONFIG_SRCDIR([gtsam/geometry/Cal3_S2.cpp])
AC_CONFIG_SRCDIR([gtsam/inference/SymbolicFactorGraph.cpp])
AC_CONFIG_SRCDIR([gtsam/linear/GaussianFactor.cpp])
AC_CONFIG_SRCDIR([gtsam/nonlinear/NonlinearOptimizer.cpp])
AC_CONFIG_SRCDIR([gtsam/slam/pose2SLAM.cpp])
AC_CONFIG_SRCDIR([tests/testTupleValues.cpp])
AC_CONFIG_SRCDIR([examples/SimpleRotation.cpp])
AC_CONFIG_SRCDIR([gtsam/3rdparty/Makefile.am])
# For doxygen support
#DX_HTML_FEATURE(ON)
#DX_CHM_FEATURE(OFF)
#DX_CHI_FEATURE(OFF)
#DX_MAN_FEATURE(OFF)
#DX_RTF_FEATURE(OFF)
#DX_XML_FEATURE(OFF)
#DX_PDF_FEATURE(OFF)
#DX_PS_FEATURE(OFF)
#DX_INIT_DOXYGEN(gtsam, Doxyfile, doc)
# Check for OS
# needs to be called at some point earlier
AC_CANONICAL_HOST
AM_CONDITIONAL([DARWIN], [case $host_os in darwin*) true;; *) false;; esac])
AM_CONDITIONAL([LINUX], [case $host_os in linux*) true;; *) false;; esac])
AM_CONDITIONAL([IS_64BIT], [case $host_cpu in *x86_64*) true;; *) false;; esac])
# enable debug variable
AC_ARG_ENABLE([debug],
[ --enable-debug Turn on debugging],
[case "${enableval}" in
yes) debug=true ;;
no) debug=false ;;
*) AC_MSG_ERROR([bad value ${enableval} for --enable-debug]) ;;
esac],[debug=false])
AM_CONDITIONAL([DEBUG], [test x$debug = xtrue])
# enable profiling
AC_ARG_ENABLE([profiling],
[ --enable-profiling Enable profiling],
[case "${enableval}" in
yes) profiling=true ;;
no) profiling=false ;;
*) AC_MSG_ERROR([bad value ${enableval} for --enable-profiling]) ;;
esac],[profiling=false])
AM_CONDITIONAL([USE_PROFILING], [test x$profiling = xtrue])
# enable serialization in serialization test
AC_ARG_ENABLE([serialization],
[ --enable-serialization Enable serialization with boost serialization],
[case "${enableval}" in
yes) serialization=true ;;
no) serialization=false ;;
*) AC_MSG_ERROR([bad value ${enableval} for --enable-serialization]) ;;
esac],[serialization=false])
AM_CONDITIONAL([ENABLE_SERIALIZATION], [test x$serialization = xtrue])
# enable installation of CppUnitLite with gtsam
AC_ARG_ENABLE([install_cppunitlite],
[ --enable-install-cppunitlite Enable installation of CppUnitLite],
[case "${enableval}" in
yes) install_cppunitlite=true ;;
no) install_cppunitlite=false ;;
*) AC_MSG_ERROR([bad value ${enableval} for --enable-install-cppunitlite]) ;;
esac],[install_cppunitlite=false])
AM_CONDITIONAL([ENABLE_INSTALL_CPPUNITLITE], [test x$install_cppunitlite = xtrue])
# enable matlab toolbox generation
AC_ARG_ENABLE([build_toolbox],
[ --enable-build-toolbox Enable building of the Matlab toolbox],
[case "${enableval}" in
yes) build_toolbox=true ;;
no) build_toolbox=false ;;
*) AC_MSG_ERROR([bad value ${enableval} for --enable-build-toolbox]) ;;
esac],[build_toolbox=false])
AM_CONDITIONAL([ENABLE_BUILD_TOOLBOX], [test x$build_toolbox = xtrue])
# enable installation of matlab tests
AC_ARG_ENABLE([install_matlab_tests],
[ --enable-install-matlab-tests Enable installation of tests for the Matlab toolbox],
[case "${enableval}" in
yes) install_matlab_tests=true ;;
no) install_matlab_tests=false ;;
*) AC_MSG_ERROR([bad value ${enableval} for --enable-install-matlab-tests]) ;;
esac],[install_matlab_tests=true])
AM_CONDITIONAL([ENABLE_INSTALL_MATLAB_TESTS], [test x$install_matlab_tests = xtrue])
# enable installation of matlab examples
AC_ARG_ENABLE([install_matlab_examples],
[ --enable-install-matlab-examples Enable installation of examples for the Matlab toolbox],
[case "${enableval}" in
yes) install_matlab_examples=true ;;
no) install_matlab_examples=false ;;
*) AC_MSG_ERROR([bad value ${enableval} for --enable-install-matlab-examples]) ;;
esac],[install_matlab_examples=true])
AM_CONDITIONAL([ENABLE_INSTALL_MATLAB_EXAMPLES], [test x$install_matlab_examples = xtrue])
# Matlab toolbox: optional flag to change location of toolbox, defaults to install prefix
AC_ARG_WITH([toolbox],
[AS_HELP_STRING([--with-toolbox],
[specify the matlab toolbox directory for installation])],
[toolbox=$withval],
[toolbox=$prefix])
AC_SUBST([toolbox])
# enable installation of the wrap utility
AC_ARG_ENABLE([install_wrap],
[ --enable-install-wrap Enable installation of the wrap tool for generating matlab interfaces],
[case "${enableval}" in
yes) install_wrap=true ;;
no) install_wrap=false ;;
*) AC_MSG_ERROR([bad value ${enableval} for --enable-install-wrap]) ;;
esac],[install_wrap=false])
AM_CONDITIONAL([ENABLE_INSTALL_WRAP], [test x$install_wrap = xtrue])
# enable unsafe mode for wrap
AC_ARG_ENABLE([unsafe_wrap],
[ --enable-unsafe-wrap Enable using unsafe mode in wrap],
[case "${enableval}" in
yes) unsafe_wrap=true ;;
no) unsafe_wrap=false ;;
*) AC_MSG_ERROR([bad value ${enableval} for --enable-unsafe-wrap]) ;;
esac],[unsafe_wrap=false])
AM_CONDITIONAL([ENABLE_UNSAFE_WRAP], [test x$unsafe_wrap = xtrue])
# wrap install path: optional flag to change location of wrap, defaults to install prefix/bin
AC_ARG_WITH([wrap],
[AS_HELP_STRING([--with-wrap],
[specify the wrap directory for installation])],
[wrap=$withval],
[wrap=$prefix/bin])
AC_SUBST([wrap])
# Checks for programs.
AC_PROG_CXX
AC_PROG_CC
# Checks for libraries.
LT_INIT
# Checks for header files.
AC_HEADER_STDC
AC_CHECK_HEADERS([string.h])
# Checks for typedefs, structures, and compiler characteristics.
AC_HEADER_STDBOOL
AC_C_CONST
AC_C_INLINE
AC_TYPE_SIZE_T
# Checks for library functions.
AC_FUNC_ERROR_AT_LINE
AC_CHECK_FUNCS([pow sqrt])
# Check for boost
AX_BOOST_BASE([1.40])
AC_CONFIG_FILES([CppUnitLite/Makefile \
wrap/Makefile \
gtsam/3rdparty/Makefile \
gtsam/base/Makefile \
gtsam/geometry/Makefile \
gtsam/inference/Makefile \
gtsam/linear/Makefile \
gtsam/nonlinear/Makefile \
gtsam/slam/Makefile gtsam/Makefile \
tests/Makefile \
examples/Makefile \
examples/vSLAMexample/Makefile \
Makefile])
AC_OUTPUT

26
examples/CameraResectioning.cpp
View File

@ -16,38 +16,35 @@
* @date Aug 23, 2011
*/
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/NonlinearFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
using namespace gtsam;
typedef TypedSymbol<Pose3, 'x'> PoseKey;
typedef Values<PoseKey> PoseValues;
/**
* Unary factor for the pose.
*/
class ResectioningFactor: public NonlinearFactor1<PoseValues, PoseKey> {
typedef NonlinearFactor1<PoseValues, PoseKey> Base;
class ResectioningFactor: public NoiseModelFactor1<Pose3> {
typedef NoiseModelFactor1<Pose3> Base;
shared_ptrK K_; // camera's intrinsic parameters
Point3 P_; // 3D point on the calibration rig
Point2 p_; // 2D measurement of the 3D point
public:
ResectioningFactor(const SharedNoiseModel& model, const PoseKey& key,
ResectioningFactor(const SharedNoiseModel& model, const Symbol& key,
const shared_ptrK& calib, const Point2& p, const Point3& P) :
Base(model, key), K_(calib), P_(P), p_(p) {
}
virtual Vector evaluateError(const Pose3& X, boost::optional<Matrix&> H =
boost::none) const {
virtual ~ResectioningFactor() {}
virtual Vector evaluateError(const Pose3& X, boost::optional<Matrix&> H = boost::none) const {
SimpleCamera camera(*K_, X);
Point2 reprojectionError(camera.project(P_, H) - p_);
return reprojectionError.vector();
@ -69,10 +66,10 @@ int main(int argc, char* argv[]) {
/* create keys for variables */
// we have only 1 variable to solve: the camera pose
PoseKey X(1);
Symbol X('x',1);
/* 1. create graph */
NonlinearFactorGraph<PoseValues> graph;
NonlinearFactorGraph graph;
/* 2. add factors to the graph */
// add measurement factors
@ -92,14 +89,13 @@ int main(int argc, char* argv[]) {
graph.push_back(factor);
/* 3. Create an initial estimate for the camera pose */
PoseValues initial;
Values initial;
initial.insert(X, Pose3(Rot3(1.,0.,0.,
0.,-1.,0.,
0.,0.,-1.), Point3(0.,0.,2.0)));
/* 4. Optimize the graph using Levenberg-Marquardt*/
PoseValues result = optimize<NonlinearFactorGraph<PoseValues> , PoseValues> (
graph, initial);
Values result = optimize<NonlinearFactorGraph> (graph, initial);
result.print("Final result: ");
return 0;

45
examples/Makefile.am
View File

@ -1,45 +0,0 @@
#----------------------------------------------------------------------------------------------------
# GTSAM Examples
#----------------------------------------------------------------------------------------------------
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
sources =
check_PROGRAMS =
# Examples
noinst_PROGRAMS = SimpleRotation # Optimizes a single nonlinear rotation variable
noinst_PROGRAMS += PlanarSLAMExample_easy # Solves SLAM example from tutorial by using planarSLAM
noinst_PROGRAMS += PlanarSLAMSelfContained_advanced # Solves SLAM example from tutorial with all typedefs in the script
noinst_PROGRAMS += Pose2SLAMExample_easy # Solves SLAM example from tutorial by using Pose2SLAM and easy optimization interface
noinst_PROGRAMS += Pose2SLAMExample_advanced # Solves SLAM example from tutorial by using Pose2SLAM and advanced optimization interface
noinst_PROGRAMS += Pose2SLAMwSPCG_easy # Solves a simple Pose2 SLAM example with advanced SPCG solver
noinst_PROGRAMS += Pose2SLAMwSPCG_advanced # Solves a simple Pose2 SLAM example with easy SPCG solver
noinst_PROGRAMS += elaboratePoint2KalmanFilter # simple linear Kalman filter on a moving 2D point, but done using factor graphs
noinst_PROGRAMS += easyPoint2KalmanFilter # uses the cool generic templated Kalman filter class to do the same
noinst_PROGRAMS += CameraResectioning
EXTRA_DIST = Data
dist-hook:
rm -rf $(distdir)/Data/.svn
SUBDIRS = vSLAMexample # visual SLAM examples with 3D point landmarks and 6D camera poses
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------
AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir)
AM_LDFLAGS = $(BOOST_LDFLAGS)
LDADD = ../gtsam/libgtsam.la
AM_DEFAULT_SOURCE_EXT = .cpp
# rule to run an executable
%.run: % $(LDADD)
./$^
# rule to run executable with valgrind
%.valgrind: % $(LDADD)
valgrind ./%
#----------------------------------------------------------------------------------------------------

29
examples/PlanarSLAMExample_easy.cpp
View File

@ -36,25 +36,22 @@ using namespace planarSLAM;
* - Landmarks are 2 meters away from the robot trajectory
*/
int main(int argc, char** argv) {
// create keys for variables
PoseKey x1(1), x2(2), x3(3);
PointKey l1(1), l2(2);
// create graph container and add factors to it
// create graph container and add factors to it
Graph graph;
/* add prior */
// gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
graph.addPrior(x1, prior_measurement, prior_model); // add directly to graph
graph.addPrior(1, prior_measurement, prior_model); // add directly to graph
/* add odometry */
// general noisemodel for odometry
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
graph.addOdometry(x1, x2, odom_measurement, odom_model);
graph.addOdometry(x2, x3, odom_measurement, odom_model);
graph.addOdometry(1, 2, odom_measurement, odom_model);
graph.addOdometry(2, 3, odom_measurement, odom_model);
/* add measurements */
// general noisemodel for measurements
@ -69,24 +66,24 @@ int main(int argc, char** argv) {
range32 = 2.0;
// create bearing/range factors and add them
graph.addBearingRange(x1, l1, bearing11, range11, meas_model);
graph.addBearingRange(x2, l1, bearing21, range21, meas_model);
graph.addBearingRange(x3, l2, bearing32, range32, meas_model);
graph.addBearingRange(1, 1, bearing11, range11, meas_model);
graph.addBearingRange(2, 1, bearing21, range21, meas_model);
graph.addBearingRange(3, 2, bearing32, range32, meas_model);
graph.print("full graph");
// initialize to noisy points
planarSLAM::Values initialEstimate;
initialEstimate.insert(x1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insert(l1, Point2(1.8, 2.1));
initialEstimate.insert(l2, Point2(4.1, 1.8));
initialEstimate.insertPose(1, Pose2(0.5, 0.0, 0.2));
initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
initialEstimate.insertPoint(1, Point2(1.8, 2.1));
initialEstimate.insertPoint(2, Point2(4.1, 1.8));
initialEstimate.print("initial estimate");
// optimize using Levenberg-Marquardt optimization with an ordering from colamd
planarSLAM::Values result = optimize<Graph, planarSLAM::Values>(graph, initialEstimate);
planarSLAM::Values result = optimize(graph, initialEstimate);
result.print("final result");
return 0;

33
examples/PlanarSLAMSelfContained_advanced.cpp
View File

@ -19,7 +19,7 @@
#include <iostream>
// for all nonlinear keys
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
// for points and poses
#include <gtsam/geometry/Point2.h>
@ -34,21 +34,14 @@
#include <gtsam/slam/BearingRangeFactor.h>
// implementations for structures - needed if self-contained, and these should be included last
#include <gtsam/nonlinear/TupleValues.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianMultifrontalSolver.h>
// Main typedefs
typedef gtsam::TypedSymbol<gtsam::Pose2, 'x'> PoseKey; // Key for poses, with type included
typedef gtsam::TypedSymbol<gtsam::Point2,'l'> PointKey; // Key for points, with type included
typedef gtsam::Values<PoseKey> PoseValues; // config type for poses
typedef gtsam::Values<PointKey> PointValues; // config type for points
typedef gtsam::TupleValues2<PoseValues, PointValues> PlanarValues; // main config with two variable classes
typedef gtsam::NonlinearFactorGraph<PlanarValues> Graph; // graph structure
typedef gtsam::NonlinearOptimizer<Graph,PlanarValues,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential; // optimization engine for this domain
typedef gtsam::NonlinearOptimizer<Graph,PlanarValues,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal; // optimization engine for this domain
typedef gtsam::NonlinearOptimizer<gtsam::NonlinearFactorGraph,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential; // optimization engine for this domain
typedef gtsam::NonlinearOptimizer<gtsam::NonlinearFactorGraph,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal; // optimization engine for this domain
using namespace std;
using namespace gtsam;
@ -64,17 +57,17 @@ using namespace gtsam;
*/
int main(int argc, char** argv) {
// create keys for variables
PoseKey x1(1), x2(2), x3(3);
PointKey l1(1), l2(2);
Symbol x1('x',1), x2('x',2), x3('x',3);
Symbol l1('l',1), l2('l',2);
// create graph container and add factors to it
Graph::shared_ptr graph(new Graph);
NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
/* add prior */
// gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
PriorFactor<PlanarValues, PoseKey> posePrior(x1, prior_measurement, prior_model); // create the factor
PriorFactor<Pose2> posePrior(x1, prior_measurement, prior_model); // create the factor
graph->add(posePrior); // add the factor to the graph
/* add odometry */
@ -82,8 +75,8 @@ int main(int argc, char** argv) {
SharedDiagonal odom_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.2, 0.2, 0.1));
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
// create between factors to represent odometry
BetweenFactor<PlanarValues,PoseKey> odom12(x1, x2, odom_measurement, odom_model);
BetweenFactor<PlanarValues,PoseKey> odom23(x2, x3, odom_measurement, odom_model);
BetweenFactor<Pose2> odom12(x1, x2, odom_measurement, odom_model);
BetweenFactor<Pose2> odom23(x2, x3, odom_measurement, odom_model);
graph->add(odom12); // add both to graph
graph->add(odom23);
@ -100,9 +93,9 @@ int main(int argc, char** argv) {
range32 = 2.0;
// create bearing/range factors
BearingRangeFactor<PlanarValues, PoseKey, PointKey> meas11(x1, l1, bearing11, range11, meas_model);
BearingRangeFactor<PlanarValues, PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model);
BearingRangeFactor<PlanarValues, PoseKey, PointKey> meas32(x3, l2, bearing32, range32, meas_model);
BearingRangeFactor<Pose2, Point2> meas11(x1, l1, bearing11, range11, meas_model);
BearingRangeFactor<Pose2, Point2> meas21(x2, l1, bearing21, range21, meas_model);
BearingRangeFactor<Pose2, Point2> meas32(x3, l2, bearing32, range32, meas_model);
// add the factors
graph->add(meas11);
@ -112,7 +105,7 @@ int main(int argc, char** argv) {
graph->print("Full Graph");
// initialize to noisy points
boost::shared_ptr<PlanarValues> initial(new PlanarValues);
boost::shared_ptr<Values> initial(new Values);
initial->insert(x1, Pose2(0.5, 0.0, 0.2));
initial->insert(x2, Pose2(2.3, 0.1,-0.2));
initial->insert(x3, Pose2(4.1, 0.1, 0.1));

21
examples/Pose2SLAMExample_advanced.cpp
View File

@ -33,9 +33,6 @@ using namespace boost;
using namespace pose2SLAM;
int main(int argc, char** argv) {
// create keys for robot positions
PoseKey x1(1), x2(2), x3(3);
/* 1. create graph container and add factors to it */
shared_ptr<Graph> graph(new Graph);
@ -43,7 +40,7 @@ int main(int argc, char** argv) {
// gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
graph->addPrior(x1, prior_measurement, prior_model); // add directly to graph
graph->addPrior(1, prior_measurement, prior_model); // add directly to graph
/* 2.b add odometry */
// general noisemodel for odometry
@ -51,16 +48,16 @@ int main(int argc, char** argv) {
/* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
graph->addOdometry(x1, x2, odom_measurement, odom_model);
graph->addOdometry(x2, x3, odom_measurement, odom_model);
graph->addOdometry(1, 2, odom_measurement, odom_model);
graph->addOdometry(2, 3, odom_measurement, odom_model);
graph->print("full graph");
/* 3. Create the data structure to hold the initial estimate to the solution
* initialize to noisy points */
shared_ptr<pose2SLAM::Values> initial(new pose2SLAM::Values);
initial->insert(x1, Pose2(0.5, 0.0, 0.2));
initial->insert(x2, Pose2(2.3, 0.1,-0.2));
initial->insert(x3, Pose2(4.1, 0.1, 0.1));
initial->insertPose(1, Pose2(0.5, 0.0, 0.2));
initial->insertPose(2, Pose2(2.3, 0.1,-0.2));
initial->insertPose(3, Pose2(4.1, 0.1, 0.1));
initial->print("initial estimate");
/* 4.2.1 Alternatively, you can go through the process step by step
@ -68,7 +65,7 @@ int main(int argc, char** argv) {
Ordering::shared_ptr ordering = graph->orderingCOLAMD(*initial);
/* 4.2.2 set up solver and optimize */
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDrecreaseThresholds(1e-15, 1e-15);
NonlinearOptimizationParameters::shared_ptr params = NonlinearOptimizationParameters::newDecreaseThresholds(1e-15, 1e-15);
Optimizer optimizer(graph, initial, ordering, params);
Optimizer optimizer_result = optimizer.levenbergMarquardt();
@ -76,8 +73,8 @@ int main(int argc, char** argv) {
result.print("final result");
/* Get covariances */
Matrix covariance1 = optimizer_result.marginalCovariance(x1);
Matrix covariance2 = optimizer_result.marginalCovariance(x2);
Matrix covariance1 = optimizer_result.marginalCovariance(PoseKey(1));
Matrix covariance2 = optimizer_result.marginalCovariance(PoseKey(1));
print(covariance1, "Covariance1");
print(covariance2, "Covariance2");

16
examples/Pose2SLAMExample_easy.cpp
View File

@ -31,8 +31,6 @@ using namespace gtsam;
using namespace pose2SLAM;
int main(int argc, char** argv) {
// create keys for robot positions
PoseKey x1(1), x2(2), x3(3);
/* 1. create graph container and add factors to it */
Graph graph ;
@ -41,7 +39,7 @@ int main(int argc, char** argv) {
// gaussian for prior
SharedDiagonal prior_model = noiseModel::Diagonal::Sigmas(Vector_(3, 0.3, 0.3, 0.1));
Pose2 prior_measurement(0.0, 0.0, 0.0); // prior at origin
graph.addPrior(x1, prior_measurement, prior_model); // add directly to graph
graph.addPrior(1, prior_measurement, prior_model); // add directly to graph
/* 2.b add odometry */
// general noisemodel for odometry
@ -49,21 +47,21 @@ int main(int argc, char** argv) {
/* Pose2 measurements take (x,y,theta), where theta is taken from the positive x-axis*/
Pose2 odom_measurement(2.0, 0.0, 0.0); // create a measurement for both factors (the same in this case)
graph.addOdometry(x1, x2, odom_measurement, odom_model);
graph.addOdometry(x2, x3, odom_measurement, odom_model);
graph.addOdometry(1, 2, odom_measurement, odom_model);
graph.addOdometry(2, 3, odom_measurement, odom_model);
graph.print("full graph");
/* 3. Create the data structure to hold the initial estinmate to the solution
* initialize to noisy points */
pose2SLAM::Values initial;
initial.insert(x1, Pose2(0.5, 0.0, 0.2));
initial.insert(x2, Pose2(2.3, 0.1,-0.2));
initial.insert(x3, Pose2(4.1, 0.1, 0.1));
initial.insertPose(1, Pose2(0.5, 0.0, 0.2));
initial.insertPose(2, Pose2(2.3, 0.1,-0.2));
initial.insertPose(3, Pose2(4.1, 0.1, 0.1));
initial.print("initial estimate");
/* 4 Single Step Optimization
* optimize using Levenberg-Marquardt optimization with an ordering from colamd */
pose2SLAM::Values result = optimize<Graph, pose2SLAM::Values>(graph, initial);
pose2SLAM::Values result = optimize<Graph>(graph, initial);
result.print("final result");

10
examples/Pose2SLAMwSPCG_advanced.cpp
View File

@ -31,17 +31,17 @@ using namespace gtsam;
using namespace pose2SLAM;
typedef boost::shared_ptr<Graph> sharedGraph ;
typedef boost::shared_ptr<pose2SLAM::Values> sharedValue ;
typedef boost::shared_ptr<Values> sharedValue ;
//typedef NonlinearOptimizer<Graph, Values, SubgraphPreconditioner, SubgraphSolver<Graph,Values> > SPCGOptimizer;
typedef SubgraphSolver<Graph, GaussianFactorGraph, pose2SLAM::Values> Solver;
typedef SubgraphSolver<Graph, GaussianFactorGraph, Values> Solver;
typedef boost::shared_ptr<Solver> sharedSolver ;
typedef NonlinearOptimizer<Graph, pose2SLAM::Values, GaussianFactorGraph, Solver> SPCGOptimizer;
typedef NonlinearOptimizer<Graph, Values, GaussianFactorGraph, Solver> SPCGOptimizer;
sharedGraph graph;
sharedValue initial;
pose2SLAM::Values result;
Values result;
/* ************************************************************************* */
int main(void) {
@ -51,7 +51,7 @@ int main(void) {
Key x1(1), x2(2), x3(3), x4(4), x5(5), x6(6), x7(7), x8(8), x9(9);
graph = boost::make_shared<Graph>() ;
initial = boost::make_shared<pose2SLAM::Values>() ;
initial = boost::make_shared<Values>() ;
// create a 3 by 3 grid
// x3 x6 x9

2
examples/Pose2SLAMwSPCG_easy.cpp
View File

@ -31,7 +31,7 @@ using namespace gtsam;
using namespace pose2SLAM;
Graph graph;
pose2SLAM::Values initial, result;
Values initial, result;
/* ************************************************************************* */
int main(void) {

41
examples/SimpleRotation.cpp
View File

@ -22,8 +22,7 @@
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/geometry/Rot2.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimization.h>
@ -36,26 +35,6 @@
using namespace std;
using namespace gtsam;
/**
* Step 1: Setup basic types for optimization of a single variable type
* This can be considered to be specifying the domain of the problem we wish
* to solve. In this case, we will create a very simple domain that operates
* on variables of a specific type, in this case, Rot2.
*
* To create a domain:
* - variable types need to have a key defined to act as a label in graphs
* - a "RotValues" structure needs to be defined to store the system state
* - a graph container acting on a given RotValues
*
* In a typical scenario, these typedefs could be placed in a header
* file and reused between projects. Also, RotValues can be combined to
* form a "TupleValues" to enable multiple variable types, such as 2D points
* and 2D poses.
*/
typedef TypedSymbol<Rot2, 'x'> Key; /// Variable labels for a specific type
typedef Values<Key> RotValues; /// Class to store values - acts as a state for the system
typedef NonlinearFactorGraph<RotValues> Graph; /// Graph container for constraints - needs to know type of variables
const double degree = M_PI / 180;
int main() {
@ -66,7 +45,7 @@ int main() {
*/
/**
* Step 2: create a factor on to express a unary constraint
* Step 1: create a factor on to express a unary constraint
* The "prior" in this case is the measurement from a sensor,
* with a model of the noise on the measurement.
*
@ -82,11 +61,11 @@ int main() {
Rot2 prior = Rot2::fromAngle(30 * degree);
prior.print("goal angle");
SharedDiagonal model = noiseModel::Isotropic::Sigma(1, 1 * degree);
Key key(1);
PriorFactor<RotValues, Key> factor(key, prior, model);
Symbol key('x',1);
PriorFactor<Rot2> factor(key, prior, model);
/**
* Step 3: create a graph container and add the factor to it
* Step 2: create a graph container and add the factor to it
* Before optimizing, all factors need to be added to a Graph container,
* which provides the necessary top-level functionality for defining a
* system of constraints.
@ -94,12 +73,12 @@ int main() {
* In this case, there is only one factor, but in a practical scenario,
* many more factors would be added.
*/
Graph graph;
NonlinearFactorGraph graph;
graph.add(factor);
graph.print("full graph");
/**
* Step 4: create an initial estimate
* Step 3: create an initial estimate
* An initial estimate of the solution for the system is necessary to
* start optimization. This system state is the "RotValues" structure,
* which is similar in structure to a STL map, in that it maps
@ -114,19 +93,19 @@ int main() {
* on the type of key used to find the appropriate value map if there
* are multiple types of variables.
*/
RotValues initial;
Values initial;
initial.insert(key, Rot2::fromAngle(20 * degree));
initial.print("initial estimate");
/**
* Step 5: optimize
* Step 4: optimize
* After formulating the problem with a graph of constraints
* and an initial estimate, executing optimization is as simple
* as calling a general optimization function with the graph and
* initial estimate. This will yield a new RotValues structure
* with the final state of the optimization.
*/
RotValues result = optimize<Graph, RotValues>(graph, initial);
Values result = optimize(graph, initial);
result.print("final result");
return 0;

23
examples/easyPoint2KalmanFilter.cpp
View File

@ -24,15 +24,12 @@
#include <gtsam/nonlinear/ExtendedKalmanFilter-inl.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/geometry/Point2.h>
using namespace std;
using namespace gtsam;
// Define Types for Linear System Test
typedef TypedSymbol<Point2, 'x'> LinearKey;
typedef Values<LinearKey> LinearValues;
typedef Point2 LinearMeasurement;
int main() {
@ -42,7 +39,7 @@ int main() {
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
// Create an ExtendedKalmanFilter object
ExtendedKalmanFilter<LinearValues,LinearKey> ekf(x_initial, P_initial);
ExtendedKalmanFilter<Point2> ekf(x_initial, P_initial);
@ -63,11 +60,11 @@ int main() {
// This simple motion can be modeled with a BetweenFactor
// Create Keys
LinearKey x0(0), x1(1);
Symbol x0('x',0), x1('x',1);
// Predict delta based on controls
Point2 difference(1,0);
// Create Factor
BetweenFactor<LinearValues,LinearKey> factor1(x0, x1, difference, Q);
BetweenFactor<Point2> factor1(x0, x1, difference, Q);
// Predict the new value with the EKF class
Point2 x1_predict = ekf.predict(factor1);
@ -88,7 +85,7 @@ int main() {
// This simple measurement can be modeled with a PriorFactor
Point2 z1(1.0, 0.0);
PriorFactor<LinearValues,LinearKey> factor2(x1, z1, R);
PriorFactor<Point2> factor2(x1, z1, R);
// Update the Kalman Filter with the measurement
Point2 x1_update = ekf.update(factor2);
@ -98,15 +95,15 @@ int main() {
// Do the same thing two more times...
// Predict
LinearKey x2(2);
Symbol x2('x',2);
difference = Point2(1,0);
BetweenFactor<LinearValues,LinearKey> factor3(x1, x2, difference, Q);
BetweenFactor<Point2> factor3(x1, x2, difference, Q);
Point2 x2_predict = ekf.predict(factor1);
x2_predict.print("X2 Predict");
// Update
Point2 z2(2.0, 0.0);
PriorFactor<LinearValues,LinearKey> factor4(x2, z2, R);
PriorFactor<Point2> factor4(x2, z2, R);
Point2 x2_update = ekf.update(factor4);
x2_update.print("X2 Update");
@ -114,15 +111,15 @@ int main() {
// Do the same thing one more time...
// Predict
LinearKey x3(3);
Symbol x3('x',3);
difference = Point2(1,0);
BetweenFactor<LinearValues,LinearKey> factor5(x2, x3, difference, Q);
BetweenFactor<Point2> factor5(x2, x3, difference, Q);
Point2 x3_predict = ekf.predict(factor5);
x3_predict.print("X3 Predict");
// Update
Point2 z3(3.0, 0.0);
PriorFactor<LinearValues,LinearKey> factor6(x3, z3, R);
PriorFactor<Point2> factor6(x3, z3, R);
Point2 x3_update = ekf.update(factor6);
x3_update.print("X3 Update");

42
examples/elaboratePoint2KalmanFilter.cpp
View File

@ -22,9 +22,8 @@
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Key.h>
#include <gtsam/nonlinear/Symbol.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianFactorGraph.h>
@ -34,9 +33,6 @@
using namespace std;
using namespace gtsam;
typedef TypedSymbol<Point2, 'x'> Key; /// Variable labels for a specific type
typedef Values<Key> KalmanValues; /// Class to store values - acts as a state for the system
int main() {
// [code below basically does SRIF with LDL]
@ -48,7 +44,7 @@ int main() {
Ordering::shared_ptr ordering(new Ordering);
// Create a structure to hold the linearization points
KalmanValues linearizationPoints;
Values linearizationPoints;
// Ground truth example
// Start at origin, move to the right (x-axis): 0,0 0,1 0,2
@ -57,14 +53,14 @@ int main() {
// i.e., we should get 0,0 0,1 0,2 if there is no noise
// Create new state variable
Key x0(0);
Symbol x0('x',0);
ordering->insert(x0, 0);
// Initialize state x0 (2D point) at origin by adding a prior factor, i.e., Bayes net P(x0)
// This is equivalent to x_0 and P_0
Point2 x_initial(0,0);
SharedDiagonal P_initial = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
PriorFactor<KalmanValues, Key> factor1(x0, x_initial, P_initial);
PriorFactor<Point2> factor1(x0, x_initial, P_initial);
// Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x0, x_initial);
linearFactorGraph->push_back(factor1.linearize(linearizationPoints, *ordering));
@ -90,12 +86,12 @@ int main() {
// so, difference = x_{t+1} - x_{t} = F*x_{t} + B*u_{t} - I*x_{t}
// = (F - I)*x_{t} + B*u_{t}
// = B*u_{t} (for our example)
Key x1(1);
Symbol x1('x',1);
ordering->insert(x1, 1);
Point2 difference(1,0);
SharedDiagonal Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
BetweenFactor<KalmanValues, Key> factor2(x0, x1, difference, Q);
BetweenFactor<Point2> factor2(x0, x1, difference, Q);
// Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x1, x_initial);
linearFactorGraph->push_back(factor2.linearize(linearizationPoints, *ordering));
@ -118,7 +114,7 @@ int main() {
// Extract the current estimate of x1,P1 from the Bayes Network
VectorValues result = optimize(*linearBayesNet);
Point2 x1_predict = linearizationPoints[x1].retract(result[ordering->at(x1)]);
Point2 x1_predict = linearizationPoints.at<Point2>(x1).retract(result[ordering->at(x1)]);
x1_predict.print("X1 Predict");
// Update the new linearization point to the new estimate
@ -173,7 +169,7 @@ int main() {
// This can be modeled using the PriorFactor, where the mean is z_{t} and the covariance is R.
Point2 z1(1.0, 0.0);
SharedDiagonal R1 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
PriorFactor<KalmanValues, Key> factor4(x1, z1, R1);
PriorFactor<Point2> factor4(x1, z1, R1);
// Linearize the factor and add it to the linear factor graph
linearFactorGraph->push_back(factor4.linearize(linearizationPoints, *ordering));
@ -190,7 +186,7 @@ int main() {
// Extract the current estimate of x1 from the Bayes Network
result = optimize(*linearBayesNet);
Point2 x1_update = linearizationPoints[x1].retract(result[ordering->at(x1)]);
Point2 x1_update = linearizationPoints.at<Point2>(x1).retract(result[ordering->at(x1)]);
x1_update.print("X1 Update");
// Update the linearization point to the new estimate
@ -215,7 +211,7 @@ int main() {
linearFactorGraph->add(0, tmpPrior1.getA(tmpPrior1.begin()), tmpPrior1.getb() - tmpPrior1.getA(tmpPrior1.begin()) * result[ordering->at(x1)], tmpPrior1.get_model());
// Create a key for the new state
Key x2(2);
Symbol x2('x',2);
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
@ -225,7 +221,7 @@ int main() {
// Create a nonlinear factor describing the motion model
difference = Point2(1,0);
Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
BetweenFactor<KalmanValues, Key> factor6(x1, x2, difference, Q);
BetweenFactor<Point2> factor6(x1, x2, difference, Q);
// Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x2, x1_update);
@ -237,7 +233,7 @@ int main() {
// Extract the current estimate of x2 from the Bayes Network
result = optimize(*linearBayesNet);
Point2 x2_predict = linearizationPoints[x2].retract(result[ordering->at(x2)]);
Point2 x2_predict = linearizationPoints.at<Point2>(x2).retract(result[ordering->at(x2)]);
x2_predict.print("X2 Predict");
// Update the linearization point to the new estimate
@ -263,7 +259,7 @@ int main() {
// And update using z2 ...
Point2 z2(2.0, 0.0);
SharedDiagonal R2 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
PriorFactor<KalmanValues, Key> factor8(x2, z2, R2);
PriorFactor<Point2> factor8(x2, z2, R2);
// Linearize the factor and add it to the linear factor graph
linearFactorGraph->push_back(factor8.linearize(linearizationPoints, *ordering));
@ -281,7 +277,7 @@ int main() {
// Extract the current estimate of x2 from the Bayes Network
result = optimize(*linearBayesNet);
Point2 x2_update = linearizationPoints[x2].retract(result[ordering->at(x2)]);
Point2 x2_update = linearizationPoints.at<Point2>(x2).retract(result[ordering->at(x2)]);
x2_update.print("X2 Update");
// Update the linearization point to the new estimate
@ -304,7 +300,7 @@ int main() {
linearFactorGraph->add(0, tmpPrior3.getA(tmpPrior3.begin()), tmpPrior3.getb() - tmpPrior3.getA(tmpPrior3.begin()) * result[ordering->at(x2)], tmpPrior3.get_model());
// Create a key for the new state
Key x3(3);
Symbol x3('x',3);
// Create the desired ordering
ordering = Ordering::shared_ptr(new Ordering);
@ -314,7 +310,7 @@ int main() {
// Create a nonlinear factor describing the motion model
difference = Point2(1,0);
Q = noiseModel::Diagonal::Sigmas(Vector_(2, 0.1, 0.1));
BetweenFactor<KalmanValues, Key> factor10(x2, x3, difference, Q);
BetweenFactor<Point2> factor10(x2, x3, difference, Q);
// Linearize the factor and add it to the linear factor graph
linearizationPoints.insert(x3, x2_update);
@ -326,7 +322,7 @@ int main() {
// Extract the current estimate of x3 from the Bayes Network
result = optimize(*linearBayesNet);
Point2 x3_predict = linearizationPoints[x3].retract(result[ordering->at(x3)]);
Point2 x3_predict = linearizationPoints.at<Point2>(x3).retract(result[ordering->at(x3)]);
x3_predict.print("X3 Predict");
// Update the linearization point to the new estimate
@ -352,7 +348,7 @@ int main() {
// And update using z3 ...
Point2 z3(3.0, 0.0);
SharedDiagonal R3 = noiseModel::Diagonal::Sigmas(Vector_(2, 0.25, 0.25));
PriorFactor<KalmanValues, Key> factor12(x3, z3, R3);
PriorFactor<Point2> factor12(x3, z3, R3);
// Linearize the factor and add it to the linear factor graph
linearFactorGraph->push_back(factor12.linearize(linearizationPoints, *ordering));
@ -370,7 +366,7 @@ int main() {
// Extract the current estimate of x2 from the Bayes Network
result = optimize(*linearBayesNet);
Point2 x3_update = linearizationPoints[x3].retract(result[ordering->at(x3)]);
Point2 x3_update = linearizationPoints.at<Point2>(x3).retract(result[ordering->at(x3)]);
x3_update.print("X3 Update");
// Update the linearization point to the new estimate

6
examples/matlab/PlanarSLAMExample_easy.m
View File

@ -28,20 +28,20 @@ graph = planarSLAMGraph;
%% Add prior
% gaussian for prior
prior_model = gtsamSharedNoiseModel_sharedSigmas([0.3; 0.3; 0.1]);
prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph
%% Add odometry
% general noisemodel for odometry
odom_model = gtsamSharedNoiseModel_sharedSigmas([0.2; 0.2; 0.1]);
odom_model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
graph.addOdometry(x1, x2, odom_measurement, odom_model);
graph.addOdometry(x2, x3, odom_measurement, odom_model);
%% Add measurements
% general noisemodel for measurements
meas_model = gtsamSharedNoiseModel_sharedSigmas([0.1; 0.2]);
meas_model = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]);
% create the measurement values - indices are (pose id, landmark id)
degrees = pi/180;

18
examples/matlab/Pose2SLAMExample_advanced.m
View File

@ -28,20 +28,20 @@ graph = pose2SLAMGraph;
%% Add prior
% gaussian for prior
prior_model = gtsamSharedNoiseModel_sharedSigmas([0.3; 0.3; 0.1]);
prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph
%% Add odometry
% general noisemodel for odometry
odom_model = gtsamSharedNoiseModel_sharedSigmas([0.2; 0.2; 0.1]);
odom_model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
graph.addOdometry(x1, x2, odom_measurement, odom_model);
graph.addOdometry(x2, x3, odom_measurement, odom_model);
%% Add measurements
% general noisemodel for measurements
meas_model = gtsamSharedNoiseModel_sharedSigmas([0.1; 0.2]);
meas_model = gtsamSharedNoiseModel_Sigmas([0.1; 0.2]);
% print
graph.print('full graph');
@ -55,19 +55,19 @@ initialEstimate.insertPose(x3, gtsamPose2(4.1, 0.1, 0.1));
initialEstimate.print('initial estimate');
%% set up solver, choose ordering and optimize
params = NonlinearOptimizationParameters_newDrecreaseThresholds(1e-15, 1e-15);
params = gtsamNonlinearOptimizationParameters_newDecreaseThresholds(1e-15, 1e-15);
ord = graph.orderingCOLAMD(initialEstimate);
result = pose2SLAMOptimizer(graph,initialEstimate,ord,params);
result.print('final result');
%result = pose2SLAMOptimizer(graph,initialEstimate,ord,params);
%result.print('final result');
% %
% % disp('\\\');
% %
% % %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
% % result = graph.optimize(initialEstimate);
% % result.print('final result');
result = graph.optimize(initialEstimate);
result.print('final result');
%% Get the corresponding dense matrix
ord = graph.orderingCOLAMD(result);

4
examples/matlab/Pose2SLAMExample_easy.m
View File

@ -26,13 +26,13 @@ graph = pose2SLAMGraph;
%% Add prior
% gaussian for prior
prior_model = gtsamSharedNoiseModel_sharedSigmas([0.3; 0.3; 0.1]);
prior_model = gtsamSharedNoiseModel_Sigmas([0.3; 0.3; 0.1]);
prior_measurement = gtsamPose2(0.0, 0.0, 0.0); % prior at origin
graph.addPrior(x1, prior_measurement, prior_model); % add directly to graph
%% Add odometry
% general noisemodel for odometry
odom_model = gtsamSharedNoiseModel_sharedSigmas([0.2; 0.2; 0.1]);
odom_model = gtsamSharedNoiseModel_Sigmas([0.2; 0.2; 0.1]);
odom_measurement = gtsamPose2(2.0, 0.0, 0.0); % create a measurement for both factors (the same in this case)
graph.addOdometry(x1, x2, odom_measurement, odom_model);
graph.addOdometry(x2, x3, odom_measurement, odom_model);

39
examples/vSLAMexample/Makefile.am
View File

@ -1,39 +0,0 @@
#----------------------------------------------------------------------------------------------------
# GTSAM Examples
#----------------------------------------------------------------------------------------------------
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
sources =
check_PROGRAMS =
# Examples
noinst_PROGRAMS = vSFMexample
vSFMexample_SOURCES = vSFMexample.cpp vSLAMutils.cpp
noinst_PROGRAMS += vISAMexample
vISAMexample_SOURCES = vISAMexample.cpp vSLAMutils.cpp
headers += Feature2D.h vSLAMutils.h
noinst_HEADERS = $(headers)
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------
AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir)
AM_LDFLAGS = $(BOOST_LDFLAGS)
LDADD = ../../gtsam/libgtsam.la
AM_DEFAULT_SOURCE_EXT = .cpp
# rule to run an executable
%.run: % $(LDADD)
./%
# rule to run executable with valgrind
%.valgrind: % $(LDADD)
valgrind ./$^
#----------------------------------------------------------------------------------------------------

17
examples/vSLAMexample/vISAMexample.cpp
View File

@ -20,9 +20,6 @@
#include <boost/foreach.hpp>
using namespace boost;
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
#define GTSAM_MAGIC_KEY
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/slam/visualSLAM.h>
@ -79,7 +76,7 @@ void readAllDataISAM() {
/**
* Setup newFactors and initialValues for each new pose and set of measurements at each frame.
*/
void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<visualSLAM::Values>& initialValues,
void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<Values>& initialValues,
int pose_id, const Pose3& pose, const std::vector<Feature2D>& measurements, SharedNoiseModel measurementSigma, shared_ptrK calib) {
// Create a graph of newFactors with new measurements
@ -100,10 +97,10 @@ void createNewFactors(shared_ptr<Graph>& newFactors, boost::shared_ptr<visualSLA
}
// Create initial values for all nodes in the newFactors
initialValues = shared_ptr<visualSLAM::Values> (new visualSLAM::Values());
initialValues->insert(pose_id, pose);
initialValues = shared_ptr<Values> (new Values());
initialValues->insert(PoseKey(pose_id), pose);
for (size_t i = 0; i < measurements.size(); i++) {
initialValues->insert(measurements[i].m_idLandmark, g_landmarks[measurements[i].m_idLandmark]);
initialValues->insert(PointKey(measurements[i].m_idLandmark), g_landmarks[measurements[i].m_idLandmark]);
}
}
@ -123,7 +120,7 @@ int main(int argc, char* argv[]) {
// Create a NonlinearISAM which will be relinearized and reordered after every "relinearizeInterval" updates
int relinearizeInterval = 3;
NonlinearISAM<visualSLAM::Values> isam(relinearizeInterval);
NonlinearISAM<> isam(relinearizeInterval);
// At each frame (poseId) with new camera pose and set of associated measurements,
// create a graph of new factors and update ISAM
@ -131,12 +128,12 @@ int main(int argc, char* argv[]) {
BOOST_FOREACH(const FeatureMap::value_type& features, g_measurements) {
const int poseId = features.first;
shared_ptr<Graph> newFactors;
shared_ptr<visualSLAM::Values> initialValues;
shared_ptr<Values> initialValues;
createNewFactors(newFactors, initialValues, poseId, g_poses[poseId],
features.second, measurementSigma, g_calib);
isam.update(*newFactors, *initialValues);
visualSLAM::Values currentEstimate = isam.estimate();
Values currentEstimate = isam.estimate();
cout << "****************************************************" << endl;
currentEstimate.print("Current estimate: ");
}

13
examples/vSLAMexample/vSFMexample.cpp
View File

@ -19,9 +19,6 @@
#include <boost/shared_ptr.hpp>
using namespace boost;
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
#define GTSAM_MAGIC_KEY
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/NonlinearOptimizer.h>
#include <gtsam/slam/visualSLAM.h>
@ -102,17 +99,17 @@ Graph setupGraph(std::vector<Feature2D>& measurements, SharedNoiseModel measurem
* Create a structure of initial estimates for all nodes (landmarks and poses) in the graph.
* The returned Values structure contains all initial values for all nodes.
*/
visualSLAM::Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
Values initialize(std::map<int, Point3> landmarks, std::map<int, Pose3> poses) {
visualSLAM::Values initValues;
Values initValues;
// Initialize landmarks 3D positions.
for (map<int, Point3>::iterator lmit = landmarks.begin(); lmit != landmarks.end(); lmit++)
initValues.insert(lmit->first, lmit->second);
initValues.insert(PointKey(lmit->first), lmit->second);
// Initialize camera poses.
for (map<int, Pose3>::iterator poseit = poses.begin(); poseit != poses.end(); poseit++)
initValues.insert(poseit->first, poseit->second);
initValues.insert(PoseKey(poseit->first), poseit->second);
return initValues;
}
@ -135,7 +132,7 @@ int main(int argc, char* argv[]) {
boost::shared_ptr<Graph> graph(new Graph(setupGraph(g_measurements, measurementSigma, g_calib)));
// Create an initial Values structure using groundtruth values as the initial estimates
boost::shared_ptr<visualSLAM::Values> initialEstimates(new visualSLAM::Values(initialize(g_landmarks, g_poses)));
boost::shared_ptr<Values> initialEstimates(new Values(initialize(g_landmarks, g_poses)));
cout << "*******************************************************" << endl;
initialEstimates->print("INITIAL ESTIMATES: ");

20
gtsam.h
View File

@ -251,13 +251,13 @@ class SharedDiagonal {
};
class SharedNoiseModel {
static gtsam::SharedNoiseModel sharedSigmas(Vector sigmas);
static gtsam::SharedNoiseModel sharedSigma(size_t dim, double sigma);
static gtsam::SharedNoiseModel sharedPrecisions(Vector precisions);
static gtsam::SharedNoiseModel sharedPrecision(size_t dim, double precision);
static gtsam::SharedNoiseModel sharedUnit(size_t dim);
static gtsam::SharedNoiseModel sharedSqrtInformation(Matrix R);
static gtsam::SharedNoiseModel sharedCovariance(Matrix covariance);
static gtsam::SharedNoiseModel Sigmas(Vector sigmas);
static gtsam::SharedNoiseModel Sigma(size_t dim, double sigma);
static gtsam::SharedNoiseModel Precisions(Vector precisions);
static gtsam::SharedNoiseModel Precision(size_t dim, double precision);
static gtsam::SharedNoiseModel Unit(size_t dim);
static gtsam::SharedNoiseModel SqrtInformation(Matrix R);
static gtsam::SharedNoiseModel Covariance(Matrix covariance);
void print(string s) const;
};
@ -411,6 +411,8 @@ class NonlinearOptimizationParameters {
NonlinearOptimizationParameters(double absDecrease, double relDecrease,
double sumError, int iIters, double lambda, double lambdaFactor);
void print(string s) const;
static gtsam::NonlinearOptimizationParameters* newDecreaseThresholds(double absDecrease,
double relDecrease);
};
@ -659,8 +661,8 @@ class Graph {
// GaussianFactor* linearize(const gtsam::Pose2Values& config) const;
//};
//
//class gtsam::Pose2Graph{
// Pose2Graph();
//class gtsam::pose2SLAM::Graph{
// pose2SLAM::Graph();
// void print(string s) const;
// GaussianFactorGraph* linearize_(const gtsam::Pose2Values& config) const;
// void push_back(Pose2Factor* factor);

287
gtsam/3rdparty/Makefile.am vendored
View File

@ -1,287 +0,0 @@
# 3rd Party libraries to be built and installed along with gtsam
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
# set up the folders for includes
3rdpartydir = $(pkgincludedir)/3rdparty
3rdparty_includedir = $(includedir)/gtsam/3rdparty
nobase_3rdparty_HEADERS =
# CCOLAMD (with UFconfig files included)
# FIXME: ccolamd requires a -I setting for every header file
ccolamd_inc = $(top_srcdir)/gtsam/3rdparty/CCOLAMD/Include
ufconfig_inc = $(top_srcdir)/gtsam/3rdparty/UFconfig
headers = CCOLAMD/Include/ccolamd.h UFconfig/UFconfig.h
sources = CCOLAMD/Source/ccolamd.c CCOLAMD/Source/ccolamd_global.c UFconfig/UFconfig.c
#----------------------------------------------------------------------------------------------------
# Create a libtool library that is not installed
# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am
# The headers are installed in $(includedir)/gtsam/3rdparty:
#----------------------------------------------------------------------------------------------------
nobase_3rdparty_HEADERS += $(headers)
noinst_LTLIBRARIES = libccolamd.la
libccolamd_la_SOURCES = $(sources)
AM_CPPFLAGS =
AM_CPPFLAGS += -I$(ccolamd_inc) -I$(ufconfig_inc) $(BOOST_CPPFLAGS) -I$(top_srcdir)
AM_LDFLAGS = $(BOOST_LDFLAGS)
# Eigen Installation - just copies the headers
eigen_path =
eigen_path += Eigen/Eigen
nobase_3rdparty_HEADERS += $(eigen_path)/Array $(eigen_path)/LeastSquares
nobase_3rdparty_HEADERS += $(eigen_path)/Cholesky $(eigen_path)/LU
nobase_3rdparty_HEADERS += $(eigen_path)/Core $(eigen_path)/QR
nobase_3rdparty_HEADERS += $(eigen_path)/Dense $(eigen_path)/QtAlignedMalloc
nobase_3rdparty_HEADERS += $(eigen_path)/Eigen $(eigen_path)/Sparse
nobase_3rdparty_HEADERS += $(eigen_path)/Eigen2Support $(eigen_path)/StdDeque
nobase_3rdparty_HEADERS += $(eigen_path)/Eigenvalues $(eigen_path)/StdList
nobase_3rdparty_HEADERS += $(eigen_path)/Geometry $(eigen_path)/StdVector
nobase_3rdparty_HEADERS += $(eigen_path)/Householder $(eigen_path)/SVD
nobase_3rdparty_HEADERS += $(eigen_path)/Jacobi
##./src/Cholesky:
nobase_3rdparty_HEADERS += $(eigen_path)/src/Cholesky/LDLT.h $(eigen_path)/src/Cholesky/LLT.h
##./src/Core:
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/ArrayBase.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Array.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/ArrayWrapper.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Assign.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/BandMatrix.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Block.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/BooleanRedux.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/CommaInitializer.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/CwiseBinaryOp.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/CwiseNullaryOp.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/CwiseUnaryOp.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/CwiseUnaryView.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/DenseBase.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/DenseCoeffsBase.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/DenseStorage.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Diagonal.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/DiagonalMatrix.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/DiagonalProduct.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Dot.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/EigenBase.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Flagged.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/ForceAlignedAccess.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Functors.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Fuzzy.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/GenericPacketMath.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/GlobalFunctions.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/IO.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/MapBase.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Map.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/MathFunctions.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/MatrixBase.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Matrix.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/NestByValue.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/NoAlias.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/NumTraits.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/PermutationMatrix.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/PlainObjectBase.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/ProductBase.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Product.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Random.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Redux.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Replicate.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/ReturnByValue.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Reverse.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Select.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/SelfAdjointView.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/SelfCwiseBinaryOp.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/SolveTriangular.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/StableNorm.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Stride.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Swap.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Transpose.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Transpositions.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/TriangularMatrix.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/VectorBlock.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/VectorwiseOp.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/Visitor.h
##./src/Core/arch/AltiVec:
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/AltiVec/Complex.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/AltiVec/PacketMath.h
##./src/Core/arch/Default:
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/Default/Settings.h
##./src/Core/arch/NEON:
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/NEON/Complex.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/NEON/PacketMath.h
##./src/Core/arch/SSE:
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/SSE/Complex.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/SSE/MathFunctions.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/arch/SSE/PacketMath.h
##./src/Core/products:
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/CoeffBasedProduct.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/GeneralBlockPanelKernel.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/GeneralMatrixMatrix.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/GeneralMatrixMatrixTriangular.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/GeneralMatrixVector.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/Parallelizer.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/SelfadjointMatrixMatrix.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/SelfadjointMatrixVector.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/SelfadjointProduct.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/SelfadjointRank2Update.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/TriangularMatrixMatrix.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/TriangularMatrixVector.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/TriangularSolverMatrix.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/products/TriangularSolverVector.h
##./src/Core/util:
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/BlasUtil.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/Constants.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/DisableStupidWarnings.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/ForwardDeclarations.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/Macros.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/Memory.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/Meta.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/ReenableStupidWarnings.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/StaticAssert.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Core/util/XprHelper.h
##./src/Eigen2Support:
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Block.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Cwise.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/CwiseOperators.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Lazy.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/LeastSquares.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/LU.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Macros.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/MathFunctions.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Memory.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Meta.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Minor.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/QR.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/SVD.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/TriangularSolver.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/VectorBlock.h
##./src/Eigen2Support/Geometry:
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/AlignedBox.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/All.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/AngleAxis.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Hyperplane.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/ParametrizedLine.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Quaternion.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Rotation2D.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/RotationBase.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Scaling.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Transform.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigen2Support/Geometry/Translation.h
##./src/Eigenvalues:
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/ComplexEigenSolver.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/ComplexSchur.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/EigenSolver.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/EigenvaluesCommon.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/GeneralizedSelfAdjointEigenSolver.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/HessenbergDecomposition.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/MatrixBaseEigenvalues.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/RealSchur.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/SelfAdjointEigenSolver.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Eigenvalues/Tridiagonalization.h
##./src/Geometry:
nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/AlignedBox.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/AngleAxis.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/EulerAngles.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Homogeneous.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Hyperplane.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/OrthoMethods.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/ParametrizedLine.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Quaternion.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Rotation2D.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/RotationBase.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Scaling.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Transform.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Translation.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/Umeyama.h
##./src/Geometry/arch:
nobase_3rdparty_HEADERS += $(eigen_path)/src/Geometry/arch/Geometry_SSE.h
##./src/Householder:
nobase_3rdparty_HEADERS += $(eigen_path)/src/Householder/BlockHouseholder.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Householder/Householder.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Householder/HouseholderSequence.h
##./src/Jacobi:
nobase_3rdparty_HEADERS += $(eigen_path)/src/Jacobi/Jacobi.h
##./src/LU:
nobase_3rdparty_HEADERS += $(eigen_path)/src/LU/Determinant.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/LU/FullPivLU.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/LU/Inverse.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/LU/PartialPivLU.h
##./src/LU/arch:
nobase_3rdparty_HEADERS += $(eigen_path)/src/LU/arch/Inverse_SSE.h
##./src/misc:
nobase_3rdparty_HEADERS += $(eigen_path)/src/misc/Image.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/misc/Kernel.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/misc/Solve.h
##./src/plugins:
nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/ArrayCwiseBinaryOps.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/ArrayCwiseUnaryOps.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/BlockMethods.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/CommonCwiseBinaryOps.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/CommonCwiseUnaryOps.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/MatrixCwiseBinaryOps.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/plugins/MatrixCwiseUnaryOps.h
##./src/QR:
nobase_3rdparty_HEADERS += $(eigen_path)/src/QR/ColPivHouseholderQR.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/QR/FullPivHouseholderQR.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/QR/HouseholderQR.h
##./src/Sparse:
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/AmbiVector.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/CompressedStorage.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/CoreIterators.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/DynamicSparseMatrix.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/MappedSparseMatrix.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseAssign.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseBlock.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseCwiseBinaryOp.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseCwiseUnaryOp.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseDenseProduct.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseDiagonalProduct.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseDot.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseFuzzy.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseMatrixBase.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseMatrix.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseProduct.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseRedux.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseSelfAdjointView.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseSparseProduct.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseTranspose.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseTriangularView.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseUtil.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseVector.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/SparseView.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/Sparse/TriangularSolver.h
##./src/StlSupport:
nobase_3rdparty_HEADERS += $(eigen_path)/src/StlSupport/details.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/StlSupport/StdDeque.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/StlSupport/StdList.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/StlSupport/StdVector.h
##./src/SVD:
nobase_3rdparty_HEADERS += $(eigen_path)/src/SVD/JacobiSVD.h
nobase_3rdparty_HEADERS += $(eigen_path)/src/SVD/UpperBidiagonalization.h

15
gtsam/Makefile.am
View File

@ -1,15 +0,0 @@
# All the sub-directories that need to be built
SUBDIRS = 3rdparty base geometry inference linear nonlinear slam
# And the corresponding libraries produced
SUBLIBS = 3rdparty/libccolamd.la base/libbase.la geometry/libgeometry.la \
inference/libinference.la linear/liblinear.la nonlinear/libnonlinear.la \
slam/libslam.la
# The following lines specify the actual shared library to be built with libtool
lib_LTLIBRARIES = libgtsam.la
libgtsam_la_SOURCES =
nodist_EXTRA_libgtsam_la_SOURCES = dummy.cxx
libgtsam_la_LIBADD = $(SUBLIBS) $(BOOST_LDFLAGS)
libgtsam_la_LDFLAGS = -no-undefined -version-info 0:0:0

106
gtsam/base/DerivedValue.h Normal file
View File

@ -0,0 +1,106 @@
/*
* DerivedValue.h
*
* Created on: Jan 26, 2012
* Author: thduynguyen
*/
#pragma once
#include <boost/pool/singleton_pool.hpp>
#include <gtsam/base/Value.h>
namespace gtsam {
template<class DERIVED>
class DerivedValue : public Value {
private:
/// Fake Tag struct for singleton pool allocator. In fact, it is never used!
struct PoolTag { };
protected:
DerivedValue() {}
public:
virtual ~DerivedValue() {}
/**
* Create a duplicate object returned as a pointer to the generic Value interface.
* For the sake of performance, this function use singleton pool allocator instead of the normal heap allocator
*/
virtual Value* clone_() const {
void *place = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
DERIVED* ptr = new(place) DERIVED(static_cast<const DERIVED&>(*this));
return ptr;
}
/**
* Destroy and deallocate this object, only if it was originally allocated using clone_().
*/
virtual void deallocate_() const {
this->~Value();
boost::singleton_pool<PoolTag, sizeof(DERIVED)>::free((void*)this);
}
/// equals implementing generic Value interface
virtual bool equals_(const Value& p, double tol = 1e-9) const {
// Cast the base class Value pointer to a derived class pointer
const DERIVED& derivedValue2 = dynamic_cast<const DERIVED&>(p);
// Return the result of calling equals on the derived class
return (static_cast<const DERIVED*>(this))->equals(derivedValue2, tol);
}
/// Generic Value interface version of retract
virtual Value* retract_(const Vector& delta) const {
// Call retract on the derived class
const DERIVED retractResult = (static_cast<const DERIVED*>(this))->retract(delta);
// Create a Value pointer copy of the result
void* resultAsValuePlace = boost::singleton_pool<PoolTag, sizeof(DERIVED)>::malloc();
Value* resultAsValue = new(resultAsValuePlace) DERIVED(retractResult);
// Return the pointer to the Value base class
return resultAsValue;
}
/// Generic Value interface version of localCoordinates
virtual Vector localCoordinates_(const Value& value2) const {
// Cast the base class Value pointer to a derived class pointer
const DERIVED& derivedValue2 = dynamic_cast<const DERIVED&>(value2);
// Return the result of calling localCoordinates on the derived class
return (static_cast<const DERIVED*>(this))->localCoordinates(derivedValue2);
}
/// Assignment operator
virtual Value& operator=(const Value& rhs) {
// Cast the base class Value pointer to a derived class pointer
const DERIVED& derivedRhs = dynamic_cast<const DERIVED&>(rhs);
// Do the assignment and return the result
return (static_cast<DERIVED*>(this))->operator=(derivedRhs);
}
/// Conversion to the derived class
operator const DERIVED& () const {
return static_cast<const DERIVED&>(*this);
}
/// Conversion to the derived class
operator DERIVED& () {
return static_cast<DERIVED&>(*this);
}
protected:
/// Assignment operator, protected because only the Value or DERIVED
/// assignment operators should be used.
DerivedValue<DERIVED>& operator=(const DerivedValue<DERIVED>& rhs) {
// Nothing to do, do not call base class assignment operator
return *this;
}
};
} /* namespace gtsam */

10
gtsam/base/FastList.h
View File

@ -20,6 +20,8 @@
#include <list>
#include <boost/pool/pool_alloc.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/list.hpp>
namespace gtsam {
@ -51,6 +53,14 @@ public:
/** Copy constructor from the base map class */
FastList(const Base& x) : Base(x) {}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
};
}

9
gtsam/base/FastMap.h
View File

@ -20,7 +20,8 @@
#include <map>
#include <boost/pool/pool_alloc.hpp>
#include <boost/serialization/base_object.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/map.hpp>
namespace gtsam {
@ -52,12 +53,12 @@ public:
/** Copy constructor from the base map class */
FastMap(const Base& x) : Base(x) {}
private:
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::base_object<Base>(*this);
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
};

10
gtsam/base/FastSet.h
View File

@ -25,6 +25,8 @@
#include <boost/pool/pool_alloc.hpp>
#include <boost/mpl/has_xxx.hpp>
#include <boost/utility/enable_if.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/set.hpp>
BOOST_MPL_HAS_XXX_TRAIT_DEF(print)
@ -74,6 +76,14 @@ public:
/** Check for equality within tolerance to implement Testable */
bool equals(const FastSet<VALUE>& other, double tol = 1e-9) const { return FastSetTestableHelper<VALUE>::equals(*this, other, tol); }
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
};
// This is the default Testable interface for *non*Testable elements, which

29
gtsam/base/FastVector.h
View File

@ -20,6 +20,8 @@
#include <vector>
#include <boost/pool/pool_alloc.hpp>
#include <boost/serialization/nvp.hpp>
#include <boost/serialization/vector.hpp>
namespace gtsam {
@ -48,14 +50,37 @@ public:
/** Constructor from a range, passes through to base class */
template<typename INPUTITERATOR>
explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) : Base(first, last) {}
explicit FastVector(INPUTITERATOR first, INPUTITERATOR last) {
// This if statement works around a bug in boost pool allocator and/or
// STL vector where if the size is zero, the pool allocator will allocate
// huge amounts of memory.
if(first != last)
Base::assign(first, last);
}
/** Copy constructor from another FastSet */
FastVector(const FastVector<VALUE>& x) : Base(x) {}
/** Copy constructor from the base map class */
/** Copy constructor from the base class */
FastVector(const Base& x) : Base(x) {}
/** Copy constructor from a standard STL container */
FastVector(const std::vector<VALUE>& x) {
// This if statement works around a bug in boost pool allocator and/or
// STL vector where if the size is zero, the pool allocator will allocate
// huge amounts of memory.
if(x.size() > 0)
Base::assign(x.begin(), x.end());
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
}
};
}

2
gtsam/base/Lie.h
View File

@ -133,7 +133,7 @@ template <class T> Matrix wedge(const Vector& x);
template <class T>
T expm(const Vector& x, int K=7) {
Matrix xhat = wedge<T>(x);
return expm(xhat,K);
return T(expm(xhat,K));
}
} // namespace gtsam

3
gtsam/base/LieVector.h
View File

@ -19,13 +19,14 @@
#include <gtsam/base/Lie.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/DerivedValue.h>
namespace gtsam {
/**
* LieVector is a wrapper around vector to allow it to be a Lie type
*/
struct LieVector : public Vector {
struct LieVector : public Vector, public DerivedValue<LieVector> {
/** default constructor - should be unnecessary */
LieVector() {}

74
gtsam/base/Makefile.am
View File

@ -1,74 +0,0 @@
#----------------------------------------------------------------------------------------------------
# GTSAM base
# provides some base Math and data structures, as well as test-related utilities
#----------------------------------------------------------------------------------------------------
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
sources =
check_PROGRAMS =
# base Math
headers += FixedVector.h types.h blockMatrices.h
sources += Vector.cpp Matrix.cpp
sources += cholesky.cpp
check_PROGRAMS += tests/testFixedVector tests/testVector tests/testMatrix tests/testBlockMatrices
check_PROGRAMS += tests/testCholesky
check_PROGRAMS += tests/testNumericalDerivative
# Testing
headers += Testable.h TestableAssertions.h numericalDerivative.h
sources += timing.cpp debug.cpp
check_PROGRAMS += tests/testDebug tests/testTestableAssertions
# Manifolds and Lie Groups
headers += Manifold.h Group.h
headers += Lie.h Lie-inl.h lieProxies.h LieScalar.h
sources += LieVector.cpp
check_PROGRAMS += tests/testLieVector tests/testLieScalar
# Data structures
headers += BTree.h DSF.h FastMap.h FastSet.h FastList.h FastVector.h
sources += DSFVector.cpp
check_PROGRAMS += tests/testBTree tests/testDSF tests/testDSFVector
# Timing tests
noinst_PROGRAMS = tests/timeMatrix tests/timeVirtual tests/timeVirtual2 tests/timeTest
noinst_PROGRAMS += tests/timeMatrixOps
#----------------------------------------------------------------------------------------------------
# Create a libtool library that is not installed
# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am
# The headers are installed in $(includedir)/gtsam:
#----------------------------------------------------------------------------------------------------
headers += $(sources:.cpp=.h)
basedir = $(pkgincludedir)/base
base_HEADERS = $(headers)
noinst_LTLIBRARIES = libbase.la
libbase_la_SOURCES = $(sources)
AM_CPPFLAGS =
AM_CPPFLAGS += $(BOOST_CPPFLAGS) -I$(top_srcdir)
AM_LDFLAGS = $(BOOST_LDFLAGS)
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------
TESTS = $(check_PROGRAMS)
AM_DEFAULT_SOURCE_EXT = .cpp
AM_LDFLAGS += $(boost_serialization)
LDADD = libbase.la ../../CppUnitLite/libCppUnitLite.a
# rule to run an executable
%.run: % $(LDADD)
./$^
# rule to run executable with valgrind
%.valgrind: % $(LDADD)
valgrind ./$^
#----------------------------------------------------------------------------------------------------

3
gtsam/base/Testable.h
View File

@ -23,7 +23,7 @@
* void print(const std::string& name) const = 0;
*
* equality up to tolerance
* tricky to implement, see NonlinearFactor1 for an example
* tricky to implement, see NoiseModelFactor1 for an example
* equals is not supposed to print out *anything*, just return true|false
* bool equals(const Derived& expected, double tol) const = 0;
*
@ -35,6 +35,7 @@
#include <boost/shared_ptr.hpp>
#include <stdio.h>
#include <string>
#define GTSAM_PRINT(x)((x).print(#x))

184
gtsam/base/Value.h Normal file
View File

@ -0,0 +1,184 @@
/* ----------------------------------------------------------------------------
* 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 Value.h
* @brief The interface class for any variable that can be optimized or used in a factor.
* @author Richard Roberts
* @date Jan 14, 2012
*/
#pragma once
#include <memory>
#include <boost/serialization/assume_abstract.hpp>
#include <gtsam/base/Vector.h>
namespace gtsam {
/**
* This is the interface class for any value that may be used as a variable
* assignment in a factor graph, and which you must derive to create new
* variable types to use with gtsam. Examples of built-in classes
* implementing this are mainly in geometry, including Rot3, Pose2, etc.
*
* This interface specifies pure virtual retract_(), localCoordinates_() and
* equals_() functions that work with pointers and references to this interface
* class, i.e. the base class. These functions allow containers, such as
* Values can operate generically on Value objects, retracting or computing
* local coordinates for many Value objects of different types.
*
* When you implement retract_(), localCoordinates_(), and equals_(), we
* suggest first implementing versions of these functions that work directly
* with derived objects, then using the provided helper functions to
* implement the generic Value versions. This makes your implementation
* easier, and also improves performance in situations where the derived type
* is in fact known, such as in most implementations of \c evaluateError() in
* classes derived from NonlinearFactor.
*
* Using the above practice, here is an example of implementing a typical
* class derived from Value:
* \code
class Rot3 : public Value {
public:
// Constructor, there is never a need to call the Value base class constructor.
Rot3() { ... }
// Print for unit tests and debugging (virtual, implements Value::print())
virtual void print(const std::string& str = "") const;
// Equals working directly with Rot3 objects (non-virtual, non-overriding!)
bool equals(const Rot3& other, double tol = 1e-9) const;
// Tangent space dimensionality (virtual, implements Value::dim())
virtual size_t dim() const {
return 3;
}
// retract working directly with Rot3 objects (non-virtual, non-overriding!)
Rot3 retract(const Vector& delta) const {
// Math to implement a 3D rotation retraction e.g. exponential map
return Rot3(result);
}
// localCoordinates working directly with Rot3 objects (non-virtual, non-overriding!)
Vector localCoordinates(const Rot3& r2) const {
// Math to implement 3D rotation localCoordinates, e.g. logarithm map
return Vector(result);
}
// Equals implementing the generic Value interface (virtual, implements Value::equals_())
virtual bool equals_(const Value& other, double tol = 1e-9) const {
// Call our provided helper function to call your Rot3-specific
// equals with appropriate casting.
return CallDerivedEquals(this, other, tol);
}
// retract implementing the generic Value interface (virtual, implements Value::retract_())
virtual std::auto_ptr<Value> retract_(const Vector& delta) const {
// Call our provided helper function to call your Rot3-specific
// retract and do the appropriate casting and allocation.
return CallDerivedRetract(this, delta);
}
// localCoordinates implementing the generic Value interface (virtual, implements Value::localCoordinates_())
virtual Vector localCoordinates_(const Value& value) const {
// Call our provided helper function to call your Rot3-specific
// localCoordinates and do the appropriate casting.
return CallDerivedLocalCoordinates(this, value);
}
};
\endcode
*/
class Value {
public:
/** Allocate and construct a clone of this value */
virtual Value* clone_() const = 0;
/** Deallocate a raw pointer of this value */
virtual void deallocate_() const = 0;
/** Compare this Value with another for equality. */
virtual bool equals_(const Value& other, double tol = 1e-9) const = 0;
/** Print this value, for debugging and unit tests */
virtual void print(const std::string& str = "") const = 0;
/** Return the dimensionality of the tangent space of this value. This is
* the dimensionality of \c delta passed into retract() and of the vector
* returned by localCoordinates().
* @return The dimensionality of the tangent space
*/
virtual size_t dim() const = 0;
/** Increment the value, by mapping from the vector delta in the tangent
* space of the current value back to the manifold to produce a new,
* incremented value.
* @param delta The delta vector in the tangent space of this value, by
* which to increment this value.
*/
virtual Value* retract_(const Vector& delta) const = 0;
/** Compute the coordinates in the tangent space of this value that
* retract() would map to \c value.
* @param value The value whose coordinates should be determined in the
* tangent space of the value on which this function is called.
* @return The coordinates of \c value in the tangent space of \c this.
*/
virtual Vector localCoordinates_(const Value& value) const = 0;
/** Assignment operator */
virtual Value& operator=(const Value& rhs) = 0;
/** Virutal destructor */
virtual ~Value() {}
private:
/** Empty serialization function.
*
* There are two important things that users need to do to serialize derived objects in Values successfully:
* (Those derived objects are stored in Values as pointer to this abstract base class Value)
*
* 1. All DERIVED classes derived from Value must put the following line in their serialization function:
* \code
ar & boost::serialization::make_nvp("DERIVED", boost::serialization::base_object<Value>(*this));
\endcode
* or, alternatively
* \code
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value);
\endcode
* See: http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#runtimecasting
*
* 2. The source module that includes archive class headers to serialize objects of derived classes
* (boost/archive/text_oarchive.h, for example) must *export* all derived classes, using either
* BOOST_CLASS_EXPORT or BOOST_CLASS_EXPORT_GUID macros:
\code
BOOST_CLASS_EXPORT(DERIVED_CLASS_1)
BOOST_CLASS_EXPORT_GUID(DERIVED_CLASS_2, "DERIVED_CLASS_2_ID_STRING")
\endcode
* See: http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#derivedpointers
* http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#export
* http://www.boost.org/doc/libs/release/libs/serialization/doc/serialization.html#instantiation\
* http://www.boost.org/doc/libs/release/libs/serialization/doc/special.html#export
* http://www.boost.org/doc/libs/release/libs/serialization/doc/traits.html#export
* The last two links explain why these export lines have to be in the same source module that includes
* any of the archive class headers.
* */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {}
};
} /* namespace gtsam */
BOOST_SERIALIZATION_ASSUME_ABSTRACT(gtsam::Value)

25
gtsam/base/Vector.cpp
View File

@ -42,15 +42,14 @@ namespace gtsam {
/* ************************************************************************* */
void odprintf_(const char *format, ostream& stream, ...) {
char buf[4096], *p = buf;
int n;
char buf[4096], *p = buf;
va_list args;
va_start(args, stream);
#ifdef WIN32
n = _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
_vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#else
n = vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#endif
va_end(args);
@ -63,17 +62,15 @@ void odprintf_(const char *format, ostream& stream, ...) {
/* ************************************************************************* */
void odprintf(const char *format, ...)
{
char buf[4096], *p = buf;
int n;
void odprintf(const char *format, ...) {
char buf[4096], *p = buf;
va_list args;
va_start(args, format);
#ifdef WIN32
n = _vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
_vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#else
n = vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
vsnprintf(p, sizeof buf - 3, format, args); // buf-3 is room for CR/LF/NUL
#endif
va_end(args);
@ -86,11 +83,6 @@ void odprintf(const char *format, ...)
/* ************************************************************************* */
Vector Vector_( size_t m, const double* const data) {
// Vector v(m);
// for (size_t i=0; i<m; ++i)
// v(i) = data[i];
// return v;
Vector A(m);
copy(data, data+m, A.data());
return A;
@ -133,9 +125,6 @@ Vector repeat(size_t n, double value) {
/* ************************************************************************* */
Vector delta(size_t n, size_t i, double value) {
return Vector::Unit(n, i) * value;
// Vector v = zero(n);
// v(i) = value;
// return v;
}
/* ************************************************************************* */

148
gtsam/base/serializationTestHelpers.h Normal file
View File

@ -0,0 +1,148 @@
/* ----------------------------------------------------------------------------
* 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 serializationTestHelpers.h
* @brief
* @author Richard Roberts
* @date Feb 7, 2012
*/
#pragma once
#include <sstream>
#include <string>
// includes for standard serialization types
#include <boost/serialization/export.hpp>
#include <boost/serialization/optional.hpp>
#include <boost/serialization/shared_ptr.hpp>
#include <boost/serialization/vector.hpp>
#include <boost/serialization/map.hpp>
#include <boost/serialization/list.hpp>
#include <boost/serialization/deque.hpp>
#include <boost/serialization/weak_ptr.hpp>
#include <boost/archive/text_oarchive.hpp>
#include <boost/archive/text_iarchive.hpp>
#include <boost/archive/xml_iarchive.hpp>
#include <boost/archive/xml_oarchive.hpp>
// whether to print the serialized text to stdout
const bool verbose = false;
namespace gtsam { namespace serializationTestHelpers {
/* ************************************************************************* */
// Serialization testing code.
/* ************************************************************************* */
template<class T>
std::string serialize(const T& input) {
std::ostringstream out_archive_stream;
boost::archive::text_oarchive out_archive(out_archive_stream);
out_archive << input;
return out_archive_stream.str();
}
template<class T>
void deserialize(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
boost::archive::text_iarchive in_archive(in_archive_stream);
in_archive >> output;
}
// Templated round-trip serialization
template<class T>
void roundtrip(const T& input, T& output) {
// Serialize
std::string serialized = serialize(input);
if (verbose) std::cout << serialized << std::endl << std::endl;
deserialize(serialized, output);
}
// This version requires equality operator
template<class T>
bool equality(const T& input = T()) {
T output;
roundtrip<T>(input,output);
return input==output;
}
// This version requires equals
template<class T>
bool equalsObj(const T& input = T()) {
T output;
roundtrip<T>(input,output);
return input.equals(output);
}
// De-referenced version for pointers
template<class T>
bool equalsDereferenced(const T& input) {
T output;
roundtrip<T>(input,output);
return input->equals(*output);
}
/* ************************************************************************* */
template<class T>
std::string serializeXML(const T& input) {
std::ostringstream out_archive_stream;
boost::archive::xml_oarchive out_archive(out_archive_stream);
out_archive << boost::serialization::make_nvp("data", input);
return out_archive_stream.str();
}
template<class T>
void deserializeXML(const std::string& serialized, T& output) {
std::istringstream in_archive_stream(serialized);
boost::archive::xml_iarchive in_archive(in_archive_stream);
in_archive >> boost::serialization::make_nvp("data", output);
}
// Templated round-trip serialization using XML
template<class T>
void roundtripXML(const T& input, T& output) {
// Serialize
std::string serialized = serializeXML<T>(input);
if (verbose) std::cout << serialized << std::endl << std::endl;
// De-serialize
deserializeXML(serialized, output);
}
// This version requires equality operator
template<class T>
bool equalityXML(const T& input = T()) {
T output;
roundtripXML<T>(input,output);
return input==output;
}
// This version requires equals
template<class T>
bool equalsXML(const T& input = T()) {
T output;
roundtripXML<T>(input,output);
return input.equals(output);
}
// This version is for pointers
template<class T>
bool equalsDereferencedXML(const T& input = T()) {
T output;
roundtripXML<T>(input,output);
return input->equals(*output);
}
} }

95
gtsam/base/tests/testSerializationBase.cpp Normal file
View File

@ -0,0 +1,95 @@
/* ----------------------------------------------------------------------------
* 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 testSerializationBase.cpp
* @brief
* @author Richard Roberts
* @date Feb 7, 2012
*/
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/FastList.h>
#include <gtsam/base/FastMap.h>
#include <gtsam/base/FastSet.h>
#include <gtsam/base/FastVector.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
Vector v1 = Vector_(2, 1.0, 2.0);
Vector v2 = Vector_(2, 3.0, 4.0);
Vector v3 = Vector_(2, 5.0, 6.0);
/* ************************************************************************* */
TEST (Serialization, FastList) {
FastList<Vector> list;
list.push_back(v1);
list.push_back(v2);
list.push_back(v3);
EXPECT(equality(list));
EXPECT(equalityXML(list));
}
/* ************************************************************************* */
TEST (Serialization, FastMap) {
FastMap<int, Vector> map;
map.insert(make_pair(1, v1));
map.insert(make_pair(2, v2));
map.insert(make_pair(3, v3));
EXPECT(equality(map));
EXPECT(equalityXML(map));
}
/* ************************************************************************* */
TEST (Serialization, FastSet) {
FastSet<double> set;
set.insert(1.0);
set.insert(2.0);
set.insert(3.0);
EXPECT(equality(set));
EXPECT(equalityXML(set));
}
/* ************************************************************************* */
TEST (Serialization, FastVector) {
FastVector<Vector> vector;
vector.push_back(v1);
vector.push_back(v2);
vector.push_back(v3);
EXPECT(equality(vector));
EXPECT(equalityXML(vector));
}
/* ************************************************************************* */
TEST (Serialization, matrix_vector) {
EXPECT(equality<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
EXPECT(equality<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityXML<Vector>(Vector_(4, 1.0, 2.0, 3.0, 4.0)));
EXPECT(equalityXML<Matrix>(Matrix_(2, 2, 1.0, 2.0, 3.0, 4.0)));
}
/* ************************************************************************* */
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

7
gtsam/geometry/Cal3Bundler.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -27,7 +28,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Cal3Bundler {
class Cal3Bundler : public DerivedValue<Cal3Bundler> {
private:
double f_, k1_, k2_ ;
@ -94,7 +95,7 @@ public:
Vector localCoordinates(const Cal3Bundler& T2) const ;
///TODO: comment
int dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
virtual size_t dim() const { return 3 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
///TODO: comment
static size_t Dim() { return 3; } //TODO: make a final dimension variable
@ -110,6 +111,8 @@ private:
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Cal3Bundler",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(f_);
ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_);

7
gtsam/geometry/Cal3DS2.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -27,7 +28,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Cal3DS2 {
class Cal3DS2 : public DerivedValue<Cal3DS2> {
private:
@ -97,7 +98,7 @@ public:
Vector localCoordinates(const Cal3DS2& T2) const ;
///TODO: comment
int dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
virtual size_t dim() const { return 9 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2)
///TODO: comment
static size_t Dim() { return 9; } //TODO: make a final dimension variable
@ -113,6 +114,8 @@ private:
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Cal3DS2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);

5
gtsam/geometry/Cal3_S2.h
View File

@ -21,6 +21,7 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -30,7 +31,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Cal3_S2 {
class Cal3_S2 : public DerivedValue<Cal3_S2> {
private:
double fx_, fy_, s_, u0_, v0_;
@ -175,6 +176,8 @@ namespace gtsam {
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Cal3_S2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(fx_);
ar & BOOST_SERIALIZATION_NVP(fy_);
ar & BOOST_SERIALIZATION_NVP(s_);

2
gtsam/geometry/Cal3_S2Stereo.h
View File

@ -81,6 +81,8 @@ namespace gtsam {
template<class Archive>
void serialize(Archive & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Cal3_S2Stereo",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(b_);
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Cal3_S2);
}

16
gtsam/geometry/CalibratedCamera.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Pose3.h>
@ -35,7 +36,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class CalibratedCamera {
class CalibratedCamera : public DerivedValue<CalibratedCamera> {
private:
Pose3 pose_; // 6DOF pose
@ -48,22 +49,23 @@ namespace gtsam {
CalibratedCamera() {}
/// construct with pose
CalibratedCamera(const Pose3& pose);
explicit CalibratedCamera(const Pose3& pose);
/// @}
/// @name Advanced Constructors
/// @{
/// construct from vector
CalibratedCamera(const Vector &v);
explicit CalibratedCamera(const Vector &v);
/// @}
/// @name Testable
/// @{
void print(const std::string& s="") const {
pose_.print();
}
virtual void print(const std::string& s = "") const {
pose_.print(s);
}
/// check equality to another camera
bool equals (const CalibratedCamera &camera, double tol = 1e-9) const {
return pose_.equals(camera.pose(), tol) ;
@ -155,6 +157,8 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("CalibratedCamera",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(pose_);
}

71
gtsam/geometry/Makefile.am
View File

@ -1,71 +0,0 @@
#----------------------------------------------------------------------------------------------------
# GTSAM geometry
#----------------------------------------------------------------------------------------------------
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
sources =
check_PROGRAMS =
# Concept check
headers += concepts.h
# Points and poses
sources += Point2.cpp Rot2.cpp Pose2.cpp Point3.cpp Pose3.cpp
check_PROGRAMS += tests/testPoint2 tests/testRot2 tests/testPose2 tests/testPoint3 tests/testRot3M tests/testRot3Q tests/testPose3
# Cameras
headers += GeneralCameraT.h Cal3_S2Stereo.h PinholeCamera.h
sources += Cal3_S2.cpp Cal3DS2.cpp Cal3Bundler.cpp CalibratedCamera.cpp SimpleCamera.cpp
check_PROGRAMS += tests/testCal3DS2 tests/testCal3_S2 tests/testCal3Bundler tests/testCalibratedCamera tests/testSimpleCamera
# Stereo
sources += StereoPoint2.cpp StereoCamera.cpp
check_PROGRAMS += tests/testStereoCamera tests/testStereoPoint2
# Tensors
headers += tensors.h Tensor1.h Tensor2.h Tensor3.h Tensor4.h Tensor5.h
headers += Tensor1Expression.h Tensor2Expression.h Tensor3Expression.h Tensor5Expression.h
sources += projectiveGeometry.cpp tensorInterface.cpp
check_PROGRAMS += tests/testTensors tests/testHomography2 tests/testFundamental
# Timing tests
noinst_PROGRAMS = tests/timeRot3 tests/timePose3 tests/timeCalibratedCamera tests/timeStereoCamera
# Rot3M and Rot3Q both use Rot3.h, they do not have individual header files
allsources = $(sources)
allsources += Rot3M.cpp Rot3Q.cpp
headers += Rot3.h
#----------------------------------------------------------------------------------------------------
# Create a libtool library that is not installed
# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am
# The headers are installed in $(includedir)/gtsam:
#----------------------------------------------------------------------------------------------------
headers += $(sources:.cpp=.h)
geometrydir = $(pkgincludedir)/geometry
geometry_HEADERS = $(headers)
noinst_LTLIBRARIES = libgeometry.la
libgeometry_la_SOURCES = $(allsources)
AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir)
AM_LDFLAGS = $(BOOST_LDFLAGS)
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------
TESTS = $(check_PROGRAMS)
AM_LDFLAGS += $(boost_serialization)
LDADD = libgeometry.la ../base/libbase.la ../../CppUnitLite/libCppUnitLite.a
AM_DEFAULT_SOURCE_EXT = .cpp
# rule to run an executable
%.run: % $(LDADD)
./$^
# rule to run executable with valgrind
%.valgrind: % $(LDADD)
valgrind ./$^
#----------------------------------------------------------------------------------------------------

9
gtsam/geometry/PinholeCamera.h
View File

@ -20,6 +20,8 @@
#include <cmath>
#include <boost/optional.hpp>
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Vector.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/geometry/Point2.h>
@ -35,7 +37,7 @@ namespace gtsam {
* \nosubgrouping
*/
template <typename Calibration>
class PinholeCamera {
class PinholeCamera : public DerivedValue<PinholeCamera<Calibration> > {
private:
Pose3 pose_;
Calibration k_;
@ -49,7 +51,7 @@ namespace gtsam {
PinholeCamera() {}
/** constructor with pose */
PinholeCamera(const Pose3& pose):pose_(pose){}
explicit PinholeCamera(const Pose3& pose):pose_(pose){}
/** constructor with pose and calibration */
PinholeCamera(const Pose3& pose, const Calibration& k):pose_(pose),k_(k) {}
@ -61,7 +63,7 @@ namespace gtsam {
/// @name Advanced Constructors
/// @{
PinholeCamera(const Vector &v){
explicit PinholeCamera(const Vector &v){
pose_ = Pose3::Expmap(v.head(Pose3::Dim()));
if ( v.size() > Pose3::Dim()) {
k_ = Calibration(v.tail(Calibration::Dim()));
@ -278,6 +280,7 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Value);
ar & BOOST_SERIALIZATION_NVP(pose_);
ar & BOOST_SERIALIZATION_NVP(k_);
}

6
gtsam/geometry/Point2.h
View File

@ -18,6 +18,8 @@
#pragma once
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/Lie.h>
@ -30,7 +32,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Point2 {
class Point2 : public DerivedValue<Point2> {
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 2;
@ -187,6 +189,8 @@ private:
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Point2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(x_);
ar & BOOST_SERIALIZATION_NVP(y_);
}

5
gtsam/geometry/Point3.h
View File

@ -24,6 +24,7 @@
#include <boost/serialization/nvp.hpp>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/base/Lie.h>
namespace gtsam {
@ -33,7 +34,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Point3 {
class Point3 : public DerivedValue<Point3> {
public:
/// dimension of the variable - used to autodetect sizes
static const size_t dimension = 3;
@ -203,6 +204,8 @@ namespace gtsam {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Point3",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(x_);
ar & BOOST_SERIALIZATION_NVP(y_);
ar & BOOST_SERIALIZATION_NVP(z_);

5
gtsam/geometry/Pose2.h
View File

@ -22,6 +22,7 @@
#include <boost/optional.hpp>
#include <gtsam/base/Matrix.h>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Rot2.h>
@ -32,7 +33,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Pose2 {
class Pose2 : public DerivedValue<Pose2> {
public:
static const size_t dimension = 3;
@ -268,6 +269,8 @@ private:
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Pose2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(t_);
ar & BOOST_SERIALIZATION_NVP(r_);
}

5
gtsam/geometry/Pose3.h
View File

@ -22,6 +22,7 @@
#define POSE3_DEFAULT_COORDINATES_MODE Pose3::FIRST_ORDER
#endif
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Rot3.h>
@ -34,7 +35,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Pose3 {
class Pose3 : public DerivedValue<Pose3> {
public:
static const size_t dimension = 6;
@ -232,6 +233,8 @@ namespace gtsam {
friend class boost::serialization::access;
template<class Archive>
void serialize(Archive & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Pose3",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(R_);
ar & BOOST_SERIALIZATION_NVP(t_);
}

6
gtsam/geometry/Rot2.h
View File

@ -19,6 +19,8 @@
#pragma once
#include <boost/optional.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -29,7 +31,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Rot2 {
class Rot2 : public DerivedValue<Rot2> {
public:
/** get the dimension by the type */
@ -237,6 +239,8 @@ namespace gtsam {
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("Rot2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(c_);
ar & BOOST_SERIALIZATION_NVP(s_);
}

8
gtsam/geometry/Rot3.h
View File

@ -32,6 +32,7 @@
#endif
#endif
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/3rdparty/Eigen/Eigen/Geometry>
@ -49,7 +50,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class Rot3 {
class Rot3 : public DerivedValue<Rot3> {
public:
static const size_t dimension = 3;
@ -92,6 +93,9 @@ namespace gtsam {
*/
Rot3(const Quaternion& q);
/** Virtual destructor */
virtual ~Rot3() {}
/* Static member function to generate some well known rotations */
/// Rotation around X axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
@ -358,6 +362,8 @@ namespace gtsam {
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version)
{
ar & boost::serialization::make_nvp("Rot3",
boost::serialization::base_object<Value>(*this));
#ifndef GTSAM_DEFAULT_QUATERNIONS
ar & BOOST_SERIALIZATION_NVP(r1_);
ar & BOOST_SERIALIZATION_NVP(r2_);

2
gtsam/geometry/StereoCamera.h
View File

@ -18,6 +18,8 @@
#pragma once
#include <boost/tuple/tuple.hpp>
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/StereoPoint2.h>

5
gtsam/geometry/StereoPoint2.h
View File

@ -18,6 +18,7 @@
#pragma once
#include <gtsam/base/DerivedValue.h>
#include <gtsam/geometry/Point2.h>
namespace gtsam {
@ -27,7 +28,7 @@ namespace gtsam {
* @ingroup geometry
* \nosubgrouping
*/
class StereoPoint2 {
class StereoPoint2 : public DerivedValue<StereoPoint2> {
public:
static const size_t dimension = 3;
private:
@ -147,6 +148,8 @@ namespace gtsam {
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("StereoPoint2",
boost::serialization::base_object<Value>(*this));
ar & BOOST_SERIALIZATION_NVP(uL_);
ar & BOOST_SERIALIZATION_NVP(uR_);
ar & BOOST_SERIALIZATION_NVP(v_);

115
gtsam/geometry/tests/testSerializationGeometry.cpp Normal file
View File

@ -0,0 +1,115 @@
/* ----------------------------------------------------------------------------
* 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 testSerializationGeometry.cpp
* @brief
* @author Richard Roberts
* @date Feb 7, 2012
*/
#include <gtsam/geometry/Point2.h>
#include <gtsam/geometry/Point3.h>
#include <gtsam/geometry/Pose2.h>
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/Cal3_S2Stereo.h>
#include <gtsam/geometry/CalibratedCamera.h>
#include <gtsam/geometry/PinholeCamera.h>
#include <gtsam/geometry/Cal3DS2.h>
#include <gtsam/geometry/Cal3Bundler.h>
#include <gtsam/geometry/StereoCamera.h>
#include <gtsam/geometry/StereoPoint2.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */
// Export all classes derived from Value
BOOST_CLASS_EXPORT(gtsam::Cal3_S2)
BOOST_CLASS_EXPORT(gtsam::Cal3_S2Stereo)
BOOST_CLASS_EXPORT(gtsam::Cal3Bundler)
BOOST_CLASS_EXPORT(gtsam::CalibratedCamera)
BOOST_CLASS_EXPORT(gtsam::Point2)
BOOST_CLASS_EXPORT(gtsam::Point3)
BOOST_CLASS_EXPORT(gtsam::Pose2)
BOOST_CLASS_EXPORT(gtsam::Pose3)
BOOST_CLASS_EXPORT(gtsam::Rot2)
BOOST_CLASS_EXPORT(gtsam::Rot3)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3_S2>)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3DS2>)
BOOST_CLASS_EXPORT(gtsam::PinholeCamera<Cal3Bundler>)
BOOST_CLASS_EXPORT(gtsam::StereoPoint2)
/* ************************************************************************* */
Point3 pt3(1.0, 2.0, 3.0);
Rot3 rt3 = Rot3::RzRyRx(1.0, 3.0, 2.0);
Pose3 pose3(rt3, pt3);
Cal3_S2 cal1(1.0, 2.0, 0.3, 0.1, 0.5);
Cal3DS2 cal2(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0);
Cal3Bundler cal3(1.0, 2.0, 3.0);
Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0);
Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4));
CalibratedCamera cal5(Pose3(rt3, pt3));
PinholeCamera<Cal3_S2> cam1(pose3, cal1);
StereoCamera cam2(pose3, cal4ptr);
StereoPoint2 spt(1.0, 2.0, 3.0);
/* ************************************************************************* */
TEST (Serialization, text_geometry) {
EXPECT(equalsObj<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsObj<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsObj<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
EXPECT(equalsObj(pt3));
EXPECT(equalsObj<gtsam::Rot3>(rt3));
EXPECT(equalsObj<gtsam::Pose3>(Pose3(rt3, pt3)));
EXPECT(equalsObj(cal1));
EXPECT(equalsObj(cal2));
EXPECT(equalsObj(cal3));
EXPECT(equalsObj(cal4));
EXPECT(equalsObj(cal5));
EXPECT(equalsObj(cam1));
EXPECT(equalsObj(cam2));
EXPECT(equalsObj(spt));
}
/* ************************************************************************* */
TEST (Serialization, xml_geometry) {
EXPECT(equalsXML<gtsam::Point2>(Point2(1.0, 2.0)));
EXPECT(equalsXML<gtsam::Pose2>(Pose2(1.0, 2.0, 0.3)));
EXPECT(equalsXML<gtsam::Rot2>(Rot2::fromDegrees(30.0)));
EXPECT(equalsXML<gtsam::Point3>(pt3));
EXPECT(equalsXML<gtsam::Rot3>(rt3));
EXPECT(equalsXML<gtsam::Pose3>(Pose3(rt3, pt3)));
EXPECT(equalsXML(cal1));
EXPECT(equalsXML(cal2));
EXPECT(equalsXML(cal3));
EXPECT(equalsXML(cal4));
EXPECT(equalsXML(cal5));
EXPECT(equalsXML(cam1));
EXPECT(equalsXML(cam2));
EXPECT(equalsXML(spt));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

1
gtsam/inference/ClusterTree.h
View File

@ -21,6 +21,7 @@
#include <list>
#include <vector>
#include <string>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>

28
gtsam/inference/Conditional.h
View File

@ -46,7 +46,7 @@ private:
/** Create keys by adding key in front */
template<typename ITERATOR>
static std::vector<KEY> MakeKeys(KEY key, ITERATOR firstParent, ITERATOR lastParent) {
std::vector<Key> keys((lastParent - firstParent) + 1);
std::vector<KeyType> keys((lastParent - firstParent) + 1);
std::copy(firstParent, lastParent, keys.begin() + 1);
keys[0] = key;
return keys;
@ -62,16 +62,16 @@ protected:
public:
typedef KEY Key;
typedef Conditional<Key> This;
typedef Factor<Key> Base;
typedef KEY KeyType;
typedef Conditional<KeyType> This;
typedef Factor<KeyType> Base;
/**
* Typedef to the factor type that produces this conditional and that this
* conditional can be converted to using a factor constructor. Derived
* classes must redefine this.
*/
typedef gtsam::Factor<Key> FactorType;
typedef gtsam::Factor<KeyType> FactorType;
/** A shared_ptr to this class. Derived classes must redefine this. */
typedef boost::shared_ptr<This> shared_ptr;
@ -95,23 +95,23 @@ public:
Conditional() : nrFrontals_(0) { assertInvariants(); }
/** No parents */
Conditional(Key key) : FactorType(key), nrFrontals_(1) { assertInvariants(); }
Conditional(KeyType key) : FactorType(key), nrFrontals_(1) { assertInvariants(); }
/** Single parent */
Conditional(Key key, Key parent) : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); }
Conditional(KeyType key, KeyType parent) : FactorType(key, parent), nrFrontals_(1) { assertInvariants(); }
/** Two parents */
Conditional(Key key, Key parent1, Key parent2) : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); }
Conditional(KeyType key, KeyType parent1, KeyType parent2) : FactorType(key, parent1, parent2), nrFrontals_(1) { assertInvariants(); }
/** Three parents */
Conditional(Key key, Key parent1, Key parent2, Key parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); }
Conditional(KeyType key, KeyType parent1, KeyType parent2, KeyType parent3) : FactorType(key, parent1, parent2, parent3), nrFrontals_(1) { assertInvariants(); }
/// @}
/// @name Advanced Constructors
/// @{
/** Constructor from a frontal variable and a vector of parents */
Conditional(Key key, const std::vector<Key>& parents) :
Conditional(KeyType key, const std::vector<KeyType>& parents) :
FactorType(MakeKeys(key, parents.begin(), parents.end())), nrFrontals_(1) {
assertInvariants();
}
@ -145,8 +145,8 @@ public:
size_t nrParents() const { return FactorType::size() - nrFrontals_; }
/** Special accessor when there is only one frontal variable. */
Key firstFrontalKey() const { assert(nrFrontals_>0); return FactorType::front(); }
Key lastFrontalKey() const { assert(nrFrontals_>0); return *(endFrontals()-1); }
KeyType firstFrontalKey() const { assert(nrFrontals_>0); return FactorType::front(); }
KeyType lastFrontalKey() const { assert(nrFrontals_>0); return *(endFrontals()-1); }
/** return a view of the frontal keys */
Frontals frontals() const {
@ -198,9 +198,9 @@ private:
template<typename KEY>
void Conditional<KEY>::print(const std::string& s) const {
std::cout << s << " P(";
BOOST_FOREACH(Key key, frontals()) std::cout << " " << key;
BOOST_FOREACH(KeyType key, frontals()) std::cout << " " << key;
if (nrParents()>0) std::cout << " |";
BOOST_FOREACH(Key parent, parents()) std::cout << " " << parent;
BOOST_FOREACH(KeyType parent, parents()) std::cout << " " << parent;
std::cout << ")" << std::endl;
}

1417
gtsam/inference/Doxyfile
View File

File diff suppressed because it is too large Load Diff

4
gtsam/inference/Factor-inl.h
View File

@ -45,8 +45,8 @@ namespace gtsam {
void Factor<KEY>::assertInvariants() const {
#ifndef NDEBUG
// Check that keys are all unique
std::multiset < Key > nonunique(keys_.begin(), keys_.end());
std::set < Key > unique(keys_.begin(), keys_.end());
std::multiset < KeyType > nonunique(keys_.begin(), keys_.end());
std::set < KeyType > unique(keys_.begin(), keys_.end());
bool correct = (nonunique.size() == unique.size())
&& std::equal(nonunique.begin(), nonunique.end(), unique.begin());
if (!correct) throw std::logic_error(

36
gtsam/inference/Factor.h
View File

@ -55,28 +55,28 @@ class Factor {
public:
typedef KEY Key; ///< The KEY template parameter
typedef Factor<Key> This; ///< This class
typedef KEY KeyType; ///< The KEY template parameter
typedef Factor<KeyType> This; ///< This class
/**
* Typedef to the conditional type obtained by eliminating this factor,
* derived classes must redefine this.
*/
typedef Conditional<Key> ConditionalType;
typedef Conditional<KeyType> ConditionalType;
/// A shared_ptr to this class, derived classes must redefine this.
typedef boost::shared_ptr<Factor> shared_ptr;
/// Iterator over keys
typedef typename std::vector<Key>::iterator iterator;
typedef typename std::vector<KeyType>::iterator iterator;
/// Const iterator over keys
typedef typename std::vector<Key>::const_iterator const_iterator;
typedef typename std::vector<KeyType>::const_iterator const_iterator;
protected:
/// The keys involved in this factor
std::vector<Key> keys_;
std::vector<KeyType> keys_;
friend class JacobianFactor;
friend class HessianFactor;
@ -102,19 +102,19 @@ public:
Factor() {}
/** Construct unary factor */
Factor(Key key) : keys_(1) {
Factor(KeyType key) : keys_(1) {
keys_[0] = key; assertInvariants(); }
/** Construct binary factor */
Factor(Key key1, Key key2) : keys_(2) {
Factor(KeyType key1, KeyType key2) : keys_(2) {
keys_[0] = key1; keys_[1] = key2; assertInvariants(); }
/** Construct ternary factor */
Factor(Key key1, Key key2, Key key3) : keys_(3) {
Factor(KeyType key1, KeyType key2, KeyType key3) : keys_(3) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; assertInvariants(); }
/** Construct 4-way factor */
Factor(Key key1, Key key2, Key key3, Key key4) : keys_(4) {
Factor(KeyType key1, KeyType key2, KeyType key3, KeyType key4) : keys_(4) {
keys_[0] = key1; keys_[1] = key2; keys_[2] = key3; keys_[3] = key4; assertInvariants(); }
/// @}
@ -122,13 +122,13 @@ public:
/// @{
/** Construct n-way factor */
Factor(const std::set<Key>& keys) {
BOOST_FOREACH(const Key& key, keys) keys_.push_back(key);
Factor(const std::set<KeyType>& keys) {
BOOST_FOREACH(const KeyType& key, keys) keys_.push_back(key);
assertInvariants();
}
/** Construct n-way factor */
Factor(const std::vector<Key>& keys) : keys_(keys) {
Factor(const std::vector<KeyType>& keys) : keys_(keys) {
assertInvariants();
}
@ -157,16 +157,16 @@ public:
/// @{
/// First key
Key front() const { return keys_.front(); }
KeyType front() const { return keys_.front(); }
/// Last key
Key back() const { return keys_.back(); }
KeyType back() const { return keys_.back(); }
/// find
const_iterator find(Key key) const { return std::find(begin(), end(), key); }
const_iterator find(KeyType key) const { return std::find(begin(), end(), key); }
///TODO: comment
const std::vector<Key>& keys() const { return keys_; }
const std::vector<KeyType>& keys() const { return keys_; }
/** iterators */
const_iterator begin() const { return keys_.begin(); } ///TODO: comment
@ -194,7 +194,7 @@ public:
/**
* @return keys involved in this factor
*/
std::vector<Key>& keys() { return keys_; }
std::vector<KeyType>& keys() { return keys_; }
/** mutable iterators */
iterator begin() { return keys_.begin(); } ///TODO: comment

10
gtsam/inference/IndexConditional.cpp
View File

@ -43,11 +43,11 @@ namespace gtsam {
/* ************************************************************************* */
bool IndexConditional::permuteSeparatorWithInverse(const Permutation& inversePermutation) {
#ifndef NDEBUG
BOOST_FOREACH(Key key, frontals()) { assert(key == inversePermutation[key]); }
BOOST_FOREACH(KeyType key, frontals()) { assert(key == inversePermutation[key]); }
#endif
bool parentChanged = false;
BOOST_FOREACH(Key& parent, parents()) {
Key newParent = inversePermutation[parent];
BOOST_FOREACH(KeyType& parent, parents()) {
KeyType newParent = inversePermutation[parent];
if(parent != newParent) {
parentChanged = true;
parent = newParent;
@ -61,8 +61,8 @@ namespace gtsam {
void IndexConditional::permuteWithInverse(const Permutation& inversePermutation) {
// The permutation may not move the separators into the frontals
#ifndef NDEBUG
BOOST_FOREACH(const Key frontal, this->frontals()) {
BOOST_FOREACH(const Key separator, this->parents()) {
BOOST_FOREACH(const KeyType frontal, this->frontals()) {
BOOST_FOREACH(const KeyType separator, this->parents()) {
assert(inversePermutation[frontal] < inversePermutation[separator]);
}
}

83
gtsam/inference/Makefile.am
View File

@ -1,83 +0,0 @@
#----------------------------------------------------------------------------------------------------
# GTSAM core functionality: base classes for inference, as well as symbolic and discrete
#----------------------------------------------------------------------------------------------------
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
sources =
check_PROGRAMS =
#----------------------------------------------------------------------------------------------------
# base
#----------------------------------------------------------------------------------------------------
# GTSAM core
headers += Factor.h Factor-inl.h Conditional.h
# Symbolic Inference
sources += SymbolicFactorGraph.cpp SymbolicMultifrontalSolver.cpp SymbolicSequentialSolver.cpp
check_PROGRAMS += tests/testSymbolicFactor tests/testSymbolicFactorGraph tests/testConditional
check_PROGRAMS += tests/testSymbolicBayesNet tests/testVariableIndex tests/testVariableSlots
# Inference
headers += GenericMultifrontalSolver.h GenericMultifrontalSolver-inl.h GenericSequentialSolver.h GenericSequentialSolver-inl.h
sources += inference.cpp VariableSlots.cpp Permutation.cpp VariableIndex.cpp
sources += IndexFactor.cpp IndexConditional.cpp
headers += graph.h graph-inl.h
headers += FactorGraph.h FactorGraph-inl.h
headers += ClusterTree.h ClusterTree-inl.h
headers += JunctionTree.h JunctionTree-inl.h
headers += EliminationTree.h EliminationTree-inl.h
headers += BayesNet.h BayesNet-inl.h
headers += BayesTree.h BayesTree-inl.h
headers += BayesTreeCliqueBase.h BayesTreeCliqueBase-inl.h
headers += ISAM.h ISAM-inl.h
check_PROGRAMS += tests/testInference
check_PROGRAMS += tests/testFactorGraph
check_PROGRAMS += tests/testFactorGraph
check_PROGRAMS += tests/testBayesTree
check_PROGRAMS += tests/testISAM
check_PROGRAMS += tests/testEliminationTree
check_PROGRAMS += tests/testClusterTree
check_PROGRAMS += tests/testJunctionTree
check_PROGRAMS += tests/testPermutation
#----------------------------------------------------------------------------------------------------
# Create a libtool library that is not installed
# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am
# The headers are installed in $(includedir)/gtsam:
#----------------------------------------------------------------------------------------------------
# CCOLAMD include flags are needed due to the bad include paths within the library
# but will not be exposed to users.
ccolamd_inc = -I$(top_srcdir)/gtsam/3rdparty/CCOLAMD/Include -I$(top_srcdir)/gtsam/3rdparty/UFconfig
headers += $(sources:.cpp=.h)
inferencedir = $(pkgincludedir)/inference
inference_HEADERS = $(headers)
noinst_LTLIBRARIES = libinference.la
libinference_la_SOURCES = $(sources)
AM_CPPFLAGS = $(ccolamd_inc) $(BOOST_CPPFLAGS) -I$(top_srcdir)
AM_LDFLAGS = $(BOOST_LDFLAGS)
AM_CXXFLAGS =
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------
TESTS = $(check_PROGRAMS)
AM_LDFLAGS += $(boost_serialization)
LDADD = libinference.la ../base/libbase.la ../3rdparty/libccolamd.la
LDADD += ../../CppUnitLite/libCppUnitLite.a
AM_DEFAULT_SOURCE_EXT = .cpp
# rule to run an executable
%.run: % $(LDADD)
./$^
# rule to run executable with valgrind
%.valgrind: % $(LDADD)
valgrind ./$^

32
gtsam/inference/graph-inl.h
View File

@ -139,43 +139,43 @@ predecessorMap2Graph(const PredecessorMap<KEY>& p_map) {
}
/* ************************************************************************* */
template <class V,class POSE, class VALUES>
template <class V, class POSE, class KEY>
class compose_key_visitor : public boost::default_bfs_visitor {
private:
boost::shared_ptr<VALUES> config_;
boost::shared_ptr<Values> config_;
public:
compose_key_visitor(boost::shared_ptr<VALUES> config_in) {config_ = config_in;}
compose_key_visitor(boost::shared_ptr<Values> config_in) {config_ = config_in;}
template <typename Edge, typename Graph> void tree_edge(Edge edge, const Graph& g) const {
typename VALUES::Key key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
typename VALUES::Key key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
KEY key_from = boost::get(boost::vertex_name, g, boost::source(edge, g));
KEY key_to = boost::get(boost::vertex_name, g, boost::target(edge, g));
POSE relativePose = boost::get(boost::edge_weight, g, edge);
config_->insert(key_to, (*config_)[key_from].compose(relativePose));
config_->insert(key_to, config_->at<POSE>(key_from).compose(relativePose));
}
};
/* ************************************************************************* */
template<class G, class Factor, class POSE, class VALUES>
boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<typename VALUES::Key>& tree,
template<class G, class Factor, class POSE, class KEY>
boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>& tree,
const POSE& rootPose) {
//TODO: change edge_weight_t to edge_pose_t
typedef typename boost::adjacency_list<
boost::vecS, boost::vecS, boost::directedS,
boost::property<boost::vertex_name_t, typename VALUES::Key>,
boost::property<boost::vertex_name_t, KEY>,
boost::property<boost::edge_weight_t, POSE> > PoseGraph;
typedef typename boost::graph_traits<PoseGraph>::vertex_descriptor PoseVertex;
typedef typename boost::graph_traits<PoseGraph>::edge_descriptor PoseEdge;
PoseGraph g;
PoseVertex root;
map<typename VALUES::Key, PoseVertex> key2vertex;
map<KEY, PoseVertex> key2vertex;
boost::tie(g, root, key2vertex) =
predecessorMap2Graph<PoseGraph, PoseVertex, typename VALUES::Key>(tree);
predecessorMap2Graph<PoseGraph, PoseVertex, KEY>(tree);
// attach the relative poses to the edges
PoseEdge edge12, edge21;
@ -189,8 +189,8 @@ boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<type
boost::shared_ptr<Factor> factor = boost::dynamic_pointer_cast<Factor>(nl_factor);
if (!factor) continue;
typename VALUES::Key key1 = factor->key1();
typename VALUES::Key key2 = factor->key2();
KEY key1 = factor->key1();
KEY key2 = factor->key2();
PoseVertex v1 = key2vertex.find(key1)->second;
PoseVertex v2 = key2vertex.find(key2)->second;
@ -207,10 +207,10 @@ boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<type
}
// compose poses
boost::shared_ptr<VALUES> config(new VALUES);
typename VALUES::Key rootKey = boost::get(boost::vertex_name, g, root);
boost::shared_ptr<Values> config(new Values);
KEY rootKey = boost::get(boost::vertex_name, g, root);
config->insert(rootKey, rootPose);
compose_key_visitor<PoseVertex, POSE, VALUES> vis(config);
compose_key_visitor<PoseVertex, POSE, KEY> vis(config);
boost::breadth_first_search(g, root, boost::visitor(vis));
return config;

7
gtsam/inference/graph.h
View File

@ -25,6 +25,7 @@
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/shared_ptr.hpp>
#include <gtsam/nonlinear/Values.h>
namespace gtsam {
@ -87,9 +88,9 @@ namespace gtsam {
/**
* Compose the poses by following the chain specified by the spanning tree
*/
template<class G, class Factor, class POSE, class VALUES>
boost::shared_ptr<VALUES>
composePoses(const G& graph, const PredecessorMap<typename VALUES::Key>& tree, const POSE& rootPose);
template<class G, class Factor, class POSE, class KEY>
boost::shared_ptr<Values>
composePoses(const G& graph, const PredecessorMap<KEY>& tree, const POSE& rootPose);
/**

3
gtsam/inference/tests/testFactorGraph.cpp
View File

@ -25,9 +25,6 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
#define GTSAM_MAGIC_KEY
#include <gtsam/inference/SymbolicFactorGraph.h>
using namespace std;

3
gtsam/inference/tests/testISAM.cpp
View File

@ -21,9 +21,6 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
#define GTSAM_MAGIC_KEY
#include <gtsam/inference/BayesNet.h>
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/SymbolicFactorGraph.h>

4
gtsam/inference/tests/testJunctionTree.cpp
View File

@ -24,10 +24,6 @@ using namespace boost::assign;
#include <CppUnitLite/TestHarness.h>
#include <gtsam/base/TestableAssertions.h>
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
#define GTSAM_MAGIC_KEY
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/JunctionTree.h>
#include <gtsam/inference/ClusterTree.h>

85
gtsam/inference/tests/testSerializationInference.cpp Normal file
View File

@ -0,0 +1,85 @@
/* ----------------------------------------------------------------------------
* 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 testSerializationInference.cpp
* @brief
* @author Richard Roberts
* @date Feb 7, 2012
*/
#include <gtsam/inference/IndexConditional.h>
#include <gtsam/inference/SymbolicFactorGraph.h>
#include <gtsam/inference/BayesTree.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */
TEST (Serialization, symbolic_graph) {
// construct expected symbolic graph
SymbolicFactorGraph sfg;
sfg.push_factor(0);
sfg.push_factor(0,1);
sfg.push_factor(0,2);
sfg.push_factor(2,1);
EXPECT(equalsObj(sfg));
EXPECT(equalsXML(sfg));
}
/* ************************************************************************* */
TEST (Serialization, symbolic_bn) {
IndexConditional::shared_ptr x2(new IndexConditional(1, 2, 0));
IndexConditional::shared_ptr l1(new IndexConditional(2, 0));
IndexConditional::shared_ptr x1(new IndexConditional(0));
SymbolicBayesNet sbn;
sbn.push_back(x2);
sbn.push_back(l1);
sbn.push_back(x1);
EXPECT(equalsObj(sbn));
EXPECT(equalsXML(sbn));
}
/* ************************************************************************* */
TEST (Serialization, symbolic_bayes_tree ) {
typedef BayesTree<IndexConditional> SymbolicBayesTree;
static const Index _X_=0, _T_=1, _S_=2, _E_=3, _L_=4, _B_=5;
IndexConditional::shared_ptr
B(new IndexConditional(_B_)),
L(new IndexConditional(_L_, _B_)),
E(new IndexConditional(_E_, _L_, _B_)),
S(new IndexConditional(_S_, _L_, _B_)),
T(new IndexConditional(_T_, _E_, _L_)),
X(new IndexConditional(_X_, _E_));
// Bayes Tree for Asia example
SymbolicBayesTree bayesTree;
SymbolicBayesTree::insert(bayesTree, B);
SymbolicBayesTree::insert(bayesTree, L);
SymbolicBayesTree::insert(bayesTree, E);
SymbolicBayesTree::insert(bayesTree, S);
SymbolicBayesTree::insert(bayesTree, T);
SymbolicBayesTree::insert(bayesTree, X);
EXPECT(equalsObj(bayesTree));
EXPECT(equalsXML(bayesTree));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

190
gtsam/inference/tests/timeSymbolMaps.cpp
View File

@ -1,190 +0,0 @@
/* ----------------------------------------------------------------------------
* 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 timeSymbolMaps.cpp
* @date Jan 20, 2010
* @author richard
*/
#include <boost/unordered_map.hpp>
#include <string>
#include <boost/timer.hpp>
#include <boost/lexical_cast.hpp>
#include <vector>
#include <map>
#include <gtsam/nonlinear/Key.h>
using namespace std;
using namespace boost;
using namespace gtsam;
template<class T>
class SymbolMapExp {
private:
typedef map<unsigned char, vector<T> > Map;
typedef vector<T> Vec;
Map values_;
public:
typedef pair<Symbol, T> value_type;
SymbolMapExp() {}
T& at(const Symbol& key) {
typename Map::iterator it = values_.find(key.chr());
if(it != values_.end())
return it->second.at(key.index());
else
throw invalid_argument("Key " + (string)key + " not present");
}
void set(const Symbol& key, const T& value) {
Vec& vec(values_[key.chr()]);
//vec.reserve(10000);
if(key.index() >= vec.size()) {
vec.reserve(key.index()+1);
vec.resize(key.index());
vec.push_back(value);
} else
vec[key.index()] = value;
}
};
template<class T>
class SymbolMapBinary : public std::map<Symbol, T> {
private:
typedef std::map<Symbol, T> Base;
public:
SymbolMapBinary() : std::map<Symbol, T>() {}
T& at(const Symbol& key) {
typename Base::iterator it = Base::find(key);
if (it == Base::end())
throw(std::invalid_argument("SymbolMap::[] invalid key: " + (std::string)key));
return it->second;
}
};
struct SymbolHash : public std::unary_function<Symbol, std::size_t> {
std::size_t operator()(Symbol const& x) const {
std::size_t seed = 0;
boost::hash_combine(seed, x.chr());
boost::hash_combine(seed, x.index());
return ((size_t(x.chr()) << 24) & x.index());
}
};
template<class T>
class SymbolMapHash : public boost::unordered_map<Symbol, T, SymbolHash> {
public:
SymbolMapHash() : boost::unordered_map<Symbol, T, SymbolHash>(60000) {}
};
struct Value {
double v;
Value() : v(0.0) {}
Value(double vi) : v(vi) {}
operator string() { return lexical_cast<string>(v); }
bool operator!=(const Value& vc) { return v != vc.v; }
};
#define ELEMS 3000
#define TIMEAT 300
int main(int argc, char *argv[]) {
timer tmr;
// pre-allocate
cout << "Generating test data ..." << endl;
vector<pair<Symbol, Value> > values;
for(size_t i=0; i<ELEMS; i++) {
values.push_back(make_pair(Symbol('a',i), (double)i));
values.push_back(make_pair(Symbol('b',i), (double)i));
values.push_back(make_pair(Symbol('c',i), (double)i));
}
// time binary map
cout << "Timing binary map ..." << endl;
{
SymbolMapBinary<Value> binary;
for(size_t i=0; i<ELEMS*3; ) {
size_t stop = i + TIMEAT;
tmr.restart();
for( ; i<stop; i++)
binary.insert(values[i]);
double time = tmr.elapsed();
cout << i << " values, avg " << (time/(double)TIMEAT)*1e6 << " mu-s per insert" << endl;
tmr.restart();
for(size_t j=0; j<i; j++)
if(values[j].second != binary[values[j].first]) {
cout << "Wrong value! At key " << (string)values[j].first <<
" expecting " << (string)values[j].second <<
" got " << (string)binary[values[j].first] << endl;
}
time = tmr.elapsed();
cout << i << " values, avg " << (time)*1e3 << " ms per lookup" << endl;
}
}
// time hash map
cout << "Timing hash map ..." << endl;
{
SymbolMapHash<Value> hash;
for(size_t i=0; i<ELEMS*3; ) {
size_t stop = i + TIMEAT;
tmr.restart();
for( ; i<stop; i++)
hash.insert(values[i]);
double time = tmr.elapsed();
cout << i << " values, avg " << (time/(double)TIMEAT)*1e6 << " mu-s per insert" << endl;
tmr.restart();
for(size_t j=0; j<i; j++)
if(values[j].second != hash[values[j].first]) {
cout << "Wrong value! At key " << (string)values[j].first <<
" expecting " << (string)values[j].second <<
" got " << (string)hash[values[j].first] << endl;
}
time = tmr.elapsed();
cout << i << " values, avg " << (time/(double)i)*1e6 << " mu-s per lookup" << endl;
}
}
// time experimental map
cout << "Timing experimental map ..." << endl;
{
SymbolMapExp<Value> experimental;
for(size_t i=0; i<ELEMS*3; ) {
size_t stop = i + TIMEAT;
tmr.restart();
for( ; i<stop; i++)
experimental.set(values[i].first, values[i].second);
double time = tmr.elapsed();
cout << i << " values, avg " << (time/(double)TIMEAT)*1e6 << " mu-s per insert" << endl;
tmr.restart();
for(size_t j=0; j<i; j++)
if(values[j].second != experimental.at(values[j].first)) {
cout << "Wrong value! At key " << (string)values[j].first <<
" expecting " << (string)values[j].second <<
" got " << (string)experimental.at(values[j].first) << endl;
}
time = tmr.elapsed();
cout << i << " values, avg " << (time/(double)i)*1e6 << " mu-s per lookup" << endl;
}
}
return 0;
}

9
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -49,11 +49,10 @@ namespace gtsam {
/* ************************************************************************* */
void GaussianFactorGraph::permuteWithInverse(
const Permutation& inversePermutation) {
BOOST_FOREACH(const sharedFactor& factor, factors_)
{
factor->permuteWithInverse(inversePermutation);
}
const Permutation& inversePermutation) {
BOOST_FOREACH(const sharedFactor& factor, factors_) {
factor->permuteWithInverse(inversePermutation);
}
}
/* ************************************************************************* */

77
gtsam/linear/Makefile.am
View File

@ -1,77 +0,0 @@
#----------------------------------------------------------------------------------------------------
# GTSAM linear: inference in Gaussian factor graphs
#----------------------------------------------------------------------------------------------------
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
sources =
check_PROGRAMS =
# Noise Model
headers += SharedGaussian.h SharedDiagonal.h SharedNoiseModel.h
sources += NoiseModel.cpp Errors.cpp Sampler.cpp
check_PROGRAMS += tests/testNoiseModel tests/testErrors tests/testSampler
# Vector Configurations
sources += VectorValues.cpp
check_PROGRAMS += tests/testVectorValues
# Solvers
sources += GaussianSequentialSolver.cpp GaussianMultifrontalSolver.cpp
# Gaussian Factor Graphs
sources += JacobianFactor.cpp HessianFactor.cpp
sources += GaussianFactor.cpp GaussianFactorGraph.cpp
sources += GaussianJunctionTree.cpp
sources += GaussianConditional.cpp GaussianDensity.cpp GaussianBayesNet.cpp
sources += GaussianISAM.cpp
check_PROGRAMS += tests/testHessianFactor tests/testJacobianFactor tests/testGaussianConditional
check_PROGRAMS += tests/testGaussianDensity tests/testGaussianFactorGraph tests/testGaussianJunctionTree
# Kalman Filter
sources += KalmanFilter.cpp
check_PROGRAMS += tests/testKalmanFilter
# Iterative Methods
headers += iterative-inl.h
sources += iterative.cpp SubgraphPreconditioner.cpp SubgraphSolver.cpp
headers += IterativeSolver.h IterativeOptimizationParameters.h
headers += SubgraphSolver-inl.h
# Timing tests
noinst_PROGRAMS = tests/timeGaussianFactor tests/timeFactorOverhead tests/timeSLAMlike
#----------------------------------------------------------------------------------------------------
# Create a libtool library that is not installed
# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am
# The headers are installed in $(includedir)/gtsam:
#----------------------------------------------------------------------------------------------------
headers += $(sources:.cpp=.h)
lineardir = $(pkgincludedir)/linear
linear_HEADERS = $(headers)
noinst_LTLIBRARIES = liblinear.la
liblinear_la_SOURCES = $(sources)
AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir)
AM_LDFLAGS = $(BOOST_LDFLAGS)
AM_CXXFLAGS =
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------
TESTS = $(check_PROGRAMS)
AM_LDFLAGS += $(boost_serialization)
LDADD = liblinear.la ../inference/libinference.la ../base/libbase.la ../3rdparty/libccolamd.la
LDADD += ../../CppUnitLite/libCppUnitLite.a
AM_DEFAULT_SOURCE_EXT = .cpp
# rule to run an executable
%.run: % $(LDADD)
./$^
# rule to run executable with valgrind
%.valgrind: % $(LDADD)
valgrind ./$^

14
gtsam/linear/SharedNoiseModel.h
View File

@ -50,31 +50,31 @@ namespace gtsam { // note, deliberately not in noiseModel namespace
// Static syntactic sugar functions to create noisemodels directly
// These should only be used with the Matlab interface
static inline SharedNoiseModel sharedSigmas(const Vector& sigmas, bool smart=false) {
static inline SharedNoiseModel Sigmas(const Vector& sigmas, bool smart=false) {
return noiseModel::Diagonal::Sigmas(sigmas, smart);
}
static inline SharedNoiseModel sharedSigma(size_t dim, double sigma) {
static inline SharedNoiseModel Sigma(size_t dim, double sigma) {
return noiseModel::Isotropic::Sigma(dim, sigma);
}
static inline SharedNoiseModel sharedPrecisions(const Vector& precisions) {
static inline SharedNoiseModel Precisions(const Vector& precisions) {
return noiseModel::Diagonal::Precisions(precisions);
}
static inline SharedNoiseModel sharedPrecision(size_t dim, double precision) {
static inline SharedNoiseModel Precision(size_t dim, double precision) {
return noiseModel::Isotropic::Precision(dim, precision);
}
static inline SharedNoiseModel sharedUnit(size_t dim) {
static inline SharedNoiseModel Unit(size_t dim) {
return noiseModel::Unit::Create(dim);
}
static inline SharedNoiseModel sharedSqrtInformation(const Matrix& R) {
static inline SharedNoiseModel SqrtInformation(const Matrix& R) {
return noiseModel::Gaussian::SqrtInformation(R);
}
static inline SharedNoiseModel sharedCovariance(const Matrix& covariance, bool smart=false) {
static inline SharedNoiseModel Covariance(const Matrix& covariance, bool smart=false) {
return noiseModel::Gaussian::Covariance(covariance, smart);
}

24
gtsam/linear/SubgraphSolver-inl.h
View File

@ -21,8 +21,8 @@ using namespace std;
namespace gtsam {
template<class GRAPH, class LINEAR, class VALUES>
void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
template<class GRAPH, class LINEAR, class KEY>
void SubgraphSolver<GRAPH,LINEAR,KEY>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>();
GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared<GaussianFactorGraph>();
@ -47,8 +47,8 @@ void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::
Ab1->dynamicCastFactors<FactorGraph<JacobianFactor> >(), Ab2->dynamicCastFactors<FactorGraph<JacobianFactor> >(),Rc1,xbar);
}
template<class GRAPH, class LINEAR, class VALUES>
VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() {
template<class GRAPH, class LINEAR, class KEY>
VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,KEY>::optimize() const {
// preconditioned conjugate gradient
VectorValues zeros = pc_->zero();
@ -60,21 +60,21 @@ VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() {
return xbar;
}
template<class GRAPH, class LINEAR, class VALUES>
void SubgraphSolver<GRAPH,LINEAR,VALUES>::initialize(const GRAPH& G, const VALUES& theta0) {
template<class GRAPH, class LINEAR, class KEY>
void SubgraphSolver<GRAPH,LINEAR,KEY>::initialize(const GRAPH& G, const Values& theta0) {
// generate spanning tree
PredecessorMap<Key> tree_ = gtsam::findMinimumSpanningTree<GRAPH, Key, Constraint>(G);
PredecessorMap<KEY> tree_ = gtsam::findMinimumSpanningTree<GRAPH, KEY, Constraint>(G);
// make the ordering
list<Key> keys = predecessorMap2Keys(tree_);
ordering_ = boost::make_shared<Ordering>(list<Symbol>(keys.begin(), keys.end()));
list<KEY> keys = predecessorMap2Keys(tree_);
ordering_ = boost::make_shared<Ordering>(list<Key>(keys.begin(), keys.end()));
// build factor pairs
pairs_.clear();
typedef pair<Key,Key> EG ;
typedef pair<KEY,KEY> EG ;
BOOST_FOREACH( const EG &eg, tree_ ) {
Symbol key1 = Symbol(eg.first),
key2 = Symbol(eg.second) ;
Key key1 = Key(eg.first),
key2 = Key(eg.second) ;
pairs_.insert(pair<Index, Index>((*ordering_)[key1], (*ordering_)[key2])) ;
}
}

2
gtsam/linear/VectorValues.h
View File

@ -175,7 +175,7 @@ namespace gtsam {
/// @name Advanced Constructors
/// @{
/** Construct from a container of variable dimensions (in variable order). */
/** Construct from a container of variable dimensions (in variable order), without initializing any values. */
template<class CONTAINER>
VectorValues(const CONTAINER& dimensions) { append(dimensions); }

161
gtsam/linear/tests/testSerializationLinear.cpp Normal file
View File

@ -0,0 +1,161 @@
/* ----------------------------------------------------------------------------
* 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 testSerializationLinear.cpp
* @brief
* @author Richard Roberts
* @date Feb 7, 2012
*/
#include <gtsam/linear/VectorValues.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/linear/HessianFactor.h>
#include <gtsam/linear/GaussianSequentialSolver.h>
#include <gtsam/linear/GaussianISAM.h>
#include <gtsam/linear/NoiseModel.h>
#include <gtsam/linear/SharedNoiseModel.h>
#include <gtsam/linear/SharedDiagonal.h>
#include <gtsam/linear/SharedGaussian.h>
#include <gtsam/base/serializationTestHelpers.h>
#include <CppUnitLite/TestHarness.h>
using namespace std;
using namespace gtsam;
using namespace gtsam::serializationTestHelpers;
/* ************************************************************************* */
// Noise model components
/* ************************************************************************* */
/* ************************************************************************* */
// Export Noisemodels
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Constrained, "gtsam_noiseModel_Constrained");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Diagonal, "gtsam_noiseModel_Diagonal");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Gaussian, "gtsam_noiseModel_Gaussian");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Unit, "gtsam_noiseModel_Unit");
BOOST_CLASS_EXPORT_GUID(gtsam::noiseModel::Isotropic, "gtsam_noiseModel_Isotropic");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedNoiseModel, "gtsam_SharedNoiseModel");
BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsam_SharedDiagonal");
/* ************************************************************************* */
// example noise models
noiseModel::Diagonal::shared_ptr diag3 = noiseModel::Diagonal::Sigmas(Vector_(3, 0.1, 0.2, 0.3));
noiseModel::Gaussian::shared_ptr gaussian3 = noiseModel::Gaussian::SqrtInformation(2.0 * eye(3,3));
noiseModel::Isotropic::shared_ptr iso3 = noiseModel::Isotropic::Sigma(3, 0.2);
noiseModel::Constrained::shared_ptr constrained3 = noiseModel::Constrained::MixedSigmas(Vector_(3, 0.0, 0.0, 0.1));
noiseModel::Unit::shared_ptr unit3 = noiseModel::Unit::Create(3);
/* ************************************************************************* */
TEST (Serialization, noiseModels) {
// tests using pointers to the derived class
EXPECT( equalsDereferenced<noiseModel::Diagonal::shared_ptr>(diag3));
EXPECT(equalsDereferencedXML<noiseModel::Diagonal::shared_ptr>(diag3));
EXPECT( equalsDereferenced<noiseModel::Gaussian::shared_ptr>(gaussian3));
EXPECT(equalsDereferencedXML<noiseModel::Gaussian::shared_ptr>(gaussian3));
EXPECT( equalsDereferenced<noiseModel::Isotropic::shared_ptr>(iso3));
EXPECT(equalsDereferencedXML<noiseModel::Isotropic::shared_ptr>(iso3));
EXPECT( equalsDereferenced<noiseModel::Constrained::shared_ptr>(constrained3));
EXPECT(equalsDereferencedXML<noiseModel::Constrained::shared_ptr>(constrained3));
EXPECT( equalsDereferenced<noiseModel::Unit::shared_ptr>(unit3));
EXPECT(equalsDereferencedXML<noiseModel::Unit::shared_ptr>(unit3));
}
/* ************************************************************************* */
TEST (Serialization, SharedNoiseModel_noiseModels) {
SharedNoiseModel diag3_sg = diag3;
EXPECT(equalsDereferenced<SharedNoiseModel>(diag3_sg));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3_sg));
EXPECT(equalsDereferenced<SharedNoiseModel>(diag3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(diag3));
EXPECT(equalsDereferenced<SharedNoiseModel>(iso3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(iso3));
EXPECT(equalsDereferenced<SharedNoiseModel>(gaussian3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(gaussian3));
EXPECT(equalsDereferenced<SharedNoiseModel>(unit3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(unit3));
EXPECT(equalsDereferenced<SharedNoiseModel>(constrained3));
EXPECT(equalsDereferencedXML<SharedNoiseModel>(constrained3));
}
/* ************************************************************************* */
TEST (Serialization, SharedDiagonal_noiseModels) {
EXPECT(equalsDereferenced<SharedDiagonal>(diag3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(diag3));
EXPECT(equalsDereferenced<SharedDiagonal>(iso3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(iso3));
EXPECT(equalsDereferenced<SharedDiagonal>(unit3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(unit3));
EXPECT(equalsDereferenced<SharedDiagonal>(constrained3));
EXPECT(equalsDereferencedXML<SharedDiagonal>(constrained3));
}
/* ************************************************************************* */
// Linear components
/* ************************************************************************* */
/* Create GUIDs for factors */
/* ************************************************************************* */
BOOST_CLASS_EXPORT_GUID(gtsam::JacobianFactor, "gtsam::JacobianFactor");
BOOST_CLASS_EXPORT_GUID(gtsam::HessianFactor , "gtsam::HessianFactor");
/* ************************************************************************* */
TEST (Serialization, linear_factors) {
VectorValues values;
values.insert(0, Vector_(1, 1.0));
values.insert(1, Vector_(2, 2.0,3.0));
values.insert(2, Vector_(2, 4.0,5.0));
EXPECT(equalsObj<VectorValues>(values));
EXPECT(equalsXML<VectorValues>(values));
Index i1 = 4, i2 = 7;
Matrix A1 = eye(3), A2 = -1.0 * eye(3);
Vector b = ones(3);
SharedDiagonal model = noiseModel::Diagonal::Sigmas(Vector_(3, 1.0, 2.0, 3.0));
JacobianFactor jacobianfactor(i1, A1, i2, A2, b, model);
EXPECT(equalsObj(jacobianfactor));
EXPECT(equalsXML(jacobianfactor));
HessianFactor hessianfactor(jacobianfactor);
EXPECT(equalsObj(hessianfactor));
EXPECT(equalsXML(hessianfactor));
}
/* ************************************************************************* */
TEST (Serialization, gaussian_conditional) {
Matrix A1 = Matrix_(2,2, 1., 2., 3., 4.);
Matrix A2 = Matrix_(2,2, 6., 0.2, 8., 0.4);
Matrix R = Matrix_(2,2, 0.1, 0.3, 0.0, 0.34);
Vector d(2); d << 0.2, 0.5;
GaussianConditional cg(0, d, R, 1, A1, 2, A2, ones(2));
EXPECT(equalsObj(cg));
EXPECT(equalsXML(cg));
}
/* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
/* ************************************************************************* */

3
gtsam/linear/tests/timeGaussianFactor.cpp
View File

@ -15,9 +15,6 @@
* @author Alireza Fathi
*/
// Magically casts strings like "x3" to a Symbol('x',3) key, see Key.h
#define GTSAM_MAGIC_KEY
#include <time.h>
/*STL/C++*/

2
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -7,7 +7,7 @@
#include <gtsam/linear/GaussianBayesNet.h>
#include <gtsam/linear/GaussianISAM.h> // To get optimize(BayesTree<GaussianConditional>)
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
//#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/nonlinear/Ordering.h>
namespace gtsam {

30
gtsam/nonlinear/ExtendedKalmanFilter-inl.h
View File

@ -27,10 +27,10 @@
namespace gtsam {
/* ************************************************************************* */
template<class VALUES, class KEY>
typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::solve_(
template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::solve_(
const GaussianFactorGraph& linearFactorGraph, const Ordering& ordering,
const VALUES& linearizationPoints, const KEY& lastKey,
const Values& linearizationPoints, Key lastKey,
JacobianFactor::shared_ptr& newPrior) const {
// Extract the Index of the provided last key
@ -43,7 +43,7 @@ namespace gtsam {
// Extract the current estimate of x1,P1 from the Bayes Network
VectorValues result = optimize(*linearBayesNet);
T x = linearizationPoints[lastKey].retract(result[lastIndex]);
T x = linearizationPoints.at<T>(lastKey).retract(result[lastIndex]);
// Create a Jacobian Factor from the root node of the produced Bayes Net.
// This will act as a prior for the next iteration.
@ -66,8 +66,8 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class VALUES, class KEY>
ExtendedKalmanFilter<VALUES, KEY>::ExtendedKalmanFilter(T x_initial,
template<class VALUE>
ExtendedKalmanFilter<VALUE>::ExtendedKalmanFilter(T x_initial,
noiseModel::Gaussian::shared_ptr P_initial) {
// Set the initial linearization point to the provided mean
@ -82,8 +82,8 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class VALUES, class KEY>
typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::predict(
template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::predict(
const MotionFactor& motionFactor) {
// TODO: This implementation largely ignores the actual factor symbols.
@ -91,8 +91,8 @@ namespace gtsam {
// different keys will still compute as if a common key-set was used
// Create Keys
KEY x0 = motionFactor.key1();
KEY x1 = motionFactor.key2();
Key x0 = motionFactor.key1();
Key x1 = motionFactor.key2();
// Create an elimination ordering
Ordering ordering;
@ -100,7 +100,7 @@ namespace gtsam {
ordering.insert(x1, 1);
// Create a set of linearization points
VALUES linearizationPoints;
Values linearizationPoints;
linearizationPoints.insert(x0, x_);
linearizationPoints.insert(x1, x_); // TODO should this really be x_ ?
@ -122,8 +122,8 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class VALUES, class KEY>
typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::update(
template<class VALUE>
typename ExtendedKalmanFilter<VALUE>::T ExtendedKalmanFilter<VALUE>::update(
const MeasurementFactor& measurementFactor) {
// TODO: This implementation largely ignores the actual factor symbols.
@ -131,14 +131,14 @@ namespace gtsam {
// different keys will still compute as if a common key-set was used
// Create Keys
KEY x0 = measurementFactor.key();
Key x0 = measurementFactor.key();
// Create an elimination ordering
Ordering ordering;
ordering.insert(x0, 0);
// Create a set of linearization points
VALUES linearizationPoints;
Values linearizationPoints;
linearizationPoints.insert(x0, x_);
// Create a Gaussian Factor Graph

14
gtsam/nonlinear/ExtendedKalmanFilter.h
View File

@ -40,22 +40,22 @@ namespace gtsam {
* \nosubgrouping
*/
template<class VALUES, class KEY>
template<class VALUE>
class ExtendedKalmanFilter {
public:
typedef boost::shared_ptr<ExtendedKalmanFilter<VALUES, KEY> > shared_ptr;
typedef typename KEY::Value T;
typedef NonlinearFactor2<VALUES, KEY, KEY> MotionFactor;
typedef NonlinearFactor1<VALUES, KEY> MeasurementFactor;
typedef boost::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;
typedef VALUE T;
typedef NoiseModelFactor2<VALUE, VALUE> MotionFactor;
typedef NoiseModelFactor1<VALUE> MeasurementFactor;
protected:
T x_; // linearization point
JacobianFactor::shared_ptr priorFactor_; // density
T solve_(const GaussianFactorGraph& linearFactorGraph,
const Ordering& ordering, const VALUES& linearizationPoints,
const KEY& x, JacobianFactor::shared_ptr& newPrior) const;
const Ordering& ordering, const Values& linearizationPoints,
Key x, JacobianFactor::shared_ptr& newPrior) const;
public:

2
gtsam/nonlinear/GaussianISAM2-inl.h
View File

@ -121,7 +121,7 @@ namespace gtsam {
///* ************************************************************************* */
//boost::shared_ptr<VectorValues> optimize2(const GaussianISAM2::sharedClique& root) {
// boost::shared_ptr<VectorValues> delta(new VectorValues());
// set<Symbol> changed;
// set<Key> changed;
// // starting from the root, call optimize on each conditional
// optimize2(root, delta);
// return delta;

10
gtsam/nonlinear/GaussianISAM2.h
View File

@ -36,19 +36,19 @@ namespace gtsam {
* variables.
* @tparam GRAPH The NonlinearFactorGraph structure to store factors. Defaults to standard NonlinearFactorGraph<VALUES>
*/
template <class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES, GRAPH> {
typedef ISAM2<GaussianConditional, VALUES, GRAPH> Base;
template <class GRAPH = NonlinearFactorGraph>
class GaussianISAM2 : public ISAM2<GaussianConditional, GRAPH> {
typedef ISAM2<GaussianConditional, GRAPH> Base;
public:
/// @name Standard Constructors
/// @{
/** Create an empty ISAM2 instance */
GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, VALUES, GRAPH>(params) {}
GaussianISAM2(const ISAM2Params& params) : ISAM2<GaussianConditional, GRAPH>(params) {}
/** Create an empty ISAM2 instance using the default set of parameters (see ISAM2Params) */
GaussianISAM2() : ISAM2<GaussianConditional, VALUES, GRAPH>() {}
GaussianISAM2() : ISAM2<GaussianConditional, GRAPH>() {}
/// @}
/// @name Advanced Interface

140
gtsam/nonlinear/ISAM2-impl-inl.h
View File

@ -19,10 +19,10 @@ namespace gtsam {
using namespace std;
template<class CONDITIONAL, class VALUES, class GRAPH>
struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
template<class CONDITIONAL, class GRAPH>
struct ISAM2<CONDITIONAL, GRAPH>::Impl {
typedef ISAM2<CONDITIONAL, VALUES, GRAPH> ISAM2Type;
typedef ISAM2<CONDITIONAL, GRAPH> ISAM2Type;
struct PartialSolveResult {
typename ISAM2Type::sharedClique bayesTree;
@ -44,8 +44,10 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
* @param delta Current linear delta to be augmented with zeros
* @param ordering Current ordering to be augmented with new variables
* @param nodes Current BayesTree::Nodes index to be augmented with slots for new variables
* @param keyFormatter Formatter for printing nonlinear keys during debugging
*/
static void AddVariables(const VALUES& newTheta, VALUES& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes);
static void AddVariables(const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering,
typename Base::Nodes& nodes, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
* Extract the set of variable indices from a NonlinearFactorGraph. For each Symbol
@ -61,10 +63,12 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
* Any variables in the VectorValues delta whose vector magnitude is greater than
* or equal to relinearizeThreshold are returned.
* @param delta The linear delta to check against the threshold
* @param keyFormatter Formatter for printing nonlinear keys during debugging
* @return The set of variable indices in delta whose magnitude is greater than or
* equal to relinearizeThreshold
*/
static FastSet<Index> CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold);
static FastSet<Index> CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
* Recursively search this clique and its children for marked keys appearing
@ -94,10 +98,12 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
* expmapped deltas will be set to an invalid value (infinity) to catch bugs
* where we might expmap something twice, or expmap it but then not
* recalculate its delta.
* @param keyFormatter Formatter for printing nonlinear keys during debugging
*/
static void ExpmapMasked(VALUES& values, const Permuted<VectorValues>& delta,
static void ExpmapMasked(Values& values, const Permuted<VectorValues>& delta,
const Ordering& ordering, const std::vector<bool>& mask,
boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>());
boost::optional<Permuted<VectorValues>&> invalidateIfDebug = boost::optional<Permuted<VectorValues>&>(),
const KeyFormatter& keyFormatter = DefaultKeyFormatter);
/**
* Reorder and eliminate factors. These factors form a subset of the full
@ -118,25 +124,9 @@ struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
};
/* ************************************************************************* */
struct _VariableAdder {
Ordering& ordering;
Permuted<VectorValues>& vconfig;
Index nextVar;
_VariableAdder(Ordering& _ordering, Permuted<VectorValues>& _vconfig, Index _nextVar) : ordering(_ordering), vconfig(_vconfig), nextVar(_nextVar){}
template<typename I>
void operator()(I xIt) {
const bool debug = ISDEBUG("ISAM2 AddVariables");
vconfig.permutation()[nextVar] = nextVar;
ordering.insert(xIt->first, nextVar);
if(debug) cout << "Adding variable " << (string)xIt->first << " with order " << nextVar << endl;
++ nextVar;
}
};
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
const VALUES& newTheta, VALUES& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes) {
template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
const bool debug = ISDEBUG("ISAM2 AddVariables");
theta.insert(newTheta);
@ -151,8 +141,13 @@ void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
delta.container().vector().segment(originalDim, newDim).operator=(Vector::Zero(newDim));
delta.permutation().resize(originalnVars + newTheta.size());
{
_VariableAdder vadder(ordering, delta, originalnVars);
newTheta.apply(vadder);
Index nextVar = originalnVars;
BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, newTheta) {
delta.permutation()[nextVar] = nextVar;
ordering.insert(key_value.first, nextVar);
if(debug) cout << "Adding variable " << keyFormatter(key_value.first) << " with order " << nextVar << endl;
++ nextVar;
}
assert(delta.permutation().size() == delta.container().size());
assert(ordering.nVars() == delta.size());
assert(ordering.size() == delta.size());
@ -162,11 +157,11 @@ void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::AddVariables(
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
template<class CONDITIONAL, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
FastSet<Index> indices;
BOOST_FOREACH(const typename NonlinearFactor<VALUES>::shared_ptr& factor, factors) {
BOOST_FOREACH(const Symbol& key, factor->keys()) {
BOOST_FOREACH(const typename NonlinearFactor::shared_ptr& factor, factors) {
BOOST_FOREACH(Key key, factor->keys()) {
indices.insert(ordering[key]);
}
}
@ -174,8 +169,9 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::IndicesFromFactors(const O
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
template<class CONDITIONAL, class GRAPH>
FastSet<Index> ISAM2<CONDITIONAL,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering,
const ISAM2Params::RelinearizationThreshold& relinearizeThreshold, const KeyFormatter& keyFormatter) {
FastSet<Index> relinKeys;
if(relinearizeThreshold.type() == typeid(double)) {
@ -189,7 +185,7 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(const
} else if(relinearizeThreshold.type() == typeid(FastMap<char,Vector>)) {
const FastMap<char,Vector>& thresholds = boost::get<FastMap<char,Vector> >(relinearizeThreshold);
BOOST_FOREACH(const Ordering::value_type& key_index, ordering) {
const Vector& threshold = thresholds.find(key_index.first.chr())->second;
const Vector& threshold = thresholds.find(Symbol(key_index.first).chr())->second;
Index j = key_index.second;
if(threshold.rows() != delta[j].rows())
throw std::invalid_argument("Relinearization threshold vector dimensionality passed into iSAM2 parameters does not match actual variable dimensionality");
@ -202,8 +198,8 @@ FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(const
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
static const bool debug = false;
// does the separator contain any of the variables?
bool found = false;
@ -223,42 +219,9 @@ void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITI
}
/* ************************************************************************* */
// Internal class used in ExpmapMasked()
// This debug version sets delta entries that are applied to "Inf". The
// idea is that if a delta is applied, the variable is being relinearized,
// so the same delta should not be re-applied because it will be recalc-
// ulated. This is a debug check to prevent against a mix-up of indices
// or not keeping track of recalculated variables.
struct _SelectiveExpmapAndClear {
const Permuted<VectorValues>& delta;
const Ordering& ordering;
const vector<bool>& mask;
const boost::optional<Permuted<VectorValues>&> invalidate;
_SelectiveExpmapAndClear(const Permuted<VectorValues>& _delta, const Ordering& _ordering, const vector<bool>& _mask, boost::optional<Permuted<VectorValues>&> _invalidate) :
delta(_delta), ordering(_ordering), mask(_mask), invalidate(_invalidate) {}
template<typename I>
void operator()(I it_x) {
Index var = ordering[it_x->first];
if(ISDEBUG("ISAM2 update verbose")) {
if(mask[var])
cout << "expmap " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
else
cout << " " << (string)it_x->first << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
}
assert(delta[var].size() == (int)it_x->second.dim());
assert(delta[var].unaryExpr(&isfinite<double>).all());
if(mask[var]) {
it_x->second = it_x->second.retract(delta[var]);
if(invalidate)
(*invalidate)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)
}
}
};
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
void ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::ExpmapMasked(VALUES& values, const Permuted<VectorValues>& delta,
const Ordering& ordering, const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug) {
template<class CONDITIONAL, class GRAPH>
void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta, const Ordering& ordering,
const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug, const KeyFormatter& keyFormatter) {
// If debugging, invalidate if requested, otherwise do not invalidate.
// Invalidating means setting expmapped entries to Inf, to trigger assertions
// if we try to re-use them.
@ -266,14 +229,35 @@ void ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::ExpmapMasked(VALUES& values, const
invalidateIfDebug = boost::optional<Permuted<VectorValues>&>();
#endif
_SelectiveExpmapAndClear selectiveExpmapper(delta, ordering, mask, invalidateIfDebug);
values.apply(selectiveExpmapper);
assert(values.size() == ordering.size());
Values::iterator key_value;
Ordering::const_iterator key_index;
for(key_value = values.begin(), key_index = ordering.begin();
key_value != values.end() && key_index != ordering.end(); ++key_value, ++key_index) {
assert(key_value->first == key_index->first);
const Index var = key_index->second;
if(ISDEBUG("ISAM2 update verbose")) {
if(mask[var])
cout << "expmap " << keyFormatter(key_value->first) << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
else
cout << " " << keyFormatter(key_value->first) << " (j = " << var << "), delta = " << delta[var].transpose() << endl;
}
assert(delta[var].size() == (int)key_value->second.dim());
assert(delta[var].unaryExpr(&isfinite<double>).all());
if(mask[var]) {
Value* retracted = key_value->second.retract_(delta[var]);
key_value->second = *retracted;
retracted->deallocate_();
if(invalidateIfDebug)
(*invalidateIfDebug)[var].operator=(Vector::Constant(delta[var].rows(), numeric_limits<double>::infinity())); // Strange syntax to work with clang++ (bug in clang?)
}
}
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
typename ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolveResult
ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
template<class CONDITIONAL, class GRAPH>
typename ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolveResult
ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
const FastSet<Index>& keys, const ReorderingMode& reorderingMode) {
static const bool debug = ISDEBUG("ISAM2 recalculate");

56
gtsam/nonlinear/ISAM2-inl.h
View File

@ -39,8 +39,8 @@ static const bool disableReordering = false;
static const double batchThreshold = 0.65;
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2(const ISAM2Params& params):
template<class CONDITIONAL, class GRAPH>
ISAM2<CONDITIONAL, GRAPH>::ISAM2(const ISAM2Params& params):
delta_(Permutation(), deltaUnpermuted_), params_(params) {
// See note in gtsam/base/boost_variant_with_workaround.h
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
@ -48,8 +48,8 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2(const ISAM2Params& params):
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2():
template<class CONDITIONAL, class GRAPH>
ISAM2<CONDITIONAL, GRAPH>::ISAM2():
delta_(Permutation(), deltaUnpermuted_) {
// See note in gtsam/base/boost_variant_with_workaround.h
if(params_.optimizationParams.type() == typeid(ISAM2DoglegParams))
@ -57,14 +57,14 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2():
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
FastList<size_t> ISAM2<CONDITIONAL, VALUES, GRAPH>::getAffectedFactors(const FastList<Index>& keys) const {
template<class CONDITIONAL, class GRAPH>
FastList<size_t> ISAM2<CONDITIONAL, GRAPH>::getAffectedFactors(const FastList<Index>& keys) const {
static const bool debug = false;
if(debug) cout << "Getting affected factors for ";
if(debug) { BOOST_FOREACH(const Index key, keys) { cout << key << " "; } }
if(debug) cout << endl;
FactorGraph<NonlinearFactor<VALUES> > allAffected;
FactorGraph<NonlinearFactor > allAffected;
FastList<size_t> indices;
BOOST_FOREACH(const Index key, keys) {
// const list<size_t> l = nonlinearFactors_.factors(key);
@ -86,9 +86,9 @@ FastList<size_t> ISAM2<CONDITIONAL, VALUES, GRAPH>::getAffectedFactors(const Fas
/* ************************************************************************* */
// retrieve all factors that ONLY contain the affected variables
// (note that the remaining stuff is summarized in the cached factors)
template<class CONDITIONAL, class VALUES, class GRAPH>
template<class CONDITIONAL, class GRAPH>
FactorGraph<GaussianFactor>::shared_ptr
ISAM2<CONDITIONAL, VALUES, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
ISAM2<CONDITIONAL, GRAPH>::relinearizeAffectedFactors(const FastList<Index>& affectedKeys) const {
tic(1,"getAffectedFactors");
FastList<size_t> candidates = getAffectedFactors(affectedKeys);
@ -105,7 +105,7 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::relinearizeAffectedFactors(const FastList<Ind
tic(3,"check candidates");
BOOST_FOREACH(size_t idx, candidates) {
bool inside = true;
BOOST_FOREACH(const Symbol& key, nonlinearFactors_[idx]->keys()) {
BOOST_FOREACH(Key key, nonlinearFactors_[idx]->keys()) {
Index var = ordering_[key];
if (affectedKeysSet.find(var) == affectedKeysSet.end()) {
inside = false;
@ -125,9 +125,9 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::relinearizeAffectedFactors(const FastList<Ind
/* ************************************************************************* */
// find intermediate (linearized) factors from cache that are passed into the affected area
template<class CONDITIONAL, class VALUES, class GRAPH>
FactorGraph<typename ISAM2<CONDITIONAL, VALUES, GRAPH>::CacheFactor>
ISAM2<CONDITIONAL, VALUES, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
template<class CONDITIONAL, class GRAPH>
FactorGraph<typename ISAM2<CONDITIONAL, GRAPH>::CacheFactor>
ISAM2<CONDITIONAL, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
static const bool debug = false;
@ -151,8 +151,8 @@ ISAM2<CONDITIONAL, VALUES, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
return cachedBoundary;
}
template<class CONDITIONAL, class VALUES, class GRAPH>
boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculate(
template<class CONDITIONAL, class GRAPH>
boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, GRAPH>::recalculate(
const FastSet<Index>& markedKeys, const FastVector<Index>& newKeys, const FactorGraph<GaussianFactor>::shared_ptr newFactors,
const boost::optional<FastSet<Index> >& constrainKeys, ISAM2Result& result) {
@ -395,10 +395,10 @@ boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculat
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
template<class CONDITIONAL, class GRAPH>
ISAM2Result ISAM2<CONDITIONAL, GRAPH>::update(
const GRAPH& newFactors, const Values& newTheta, const FastVector<size_t>& removeFactorIndices,
const boost::optional<FastSet<Symbol> >& constrainedKeys, bool force_relinearize) {
const boost::optional<FastSet<Key> >& constrainedKeys, bool force_relinearize) {
static const bool debug = ISDEBUG("ISAM2 update");
static const bool verbose = ISDEBUG("ISAM2 update verbose");
@ -519,7 +519,7 @@ ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
boost::optional<FastSet<Index> > constrainedIndices;
if(constrainedKeys) {
constrainedIndices.reset(FastSet<Index>());
BOOST_FOREACH(const Symbol& key, *constrainedKeys) {
BOOST_FOREACH(Key key, *constrainedKeys) {
constrainedIndices->insert(ordering_[key]);
}
}
@ -581,36 +581,36 @@ ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateEstimate() const {
template<class CONDITIONAL, class GRAPH>
Values ISAM2<CONDITIONAL, GRAPH>::calculateEstimate() const {
// We use ExpmapMasked here instead of regular expmap because the former
// handles Permuted<VectorValues>
VALUES ret(theta_);
Values ret(theta_);
vector<bool> mask(ordering_.nVars(), true);
Impl::ExpmapMasked(ret, delta_, ordering_, mask);
return ret;
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
template<class CONDITIONAL, class GRAPH>
template<class KEY>
typename KEY::Value ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateEstimate(const KEY& key) const {
typename KEY::Value ISAM2<CONDITIONAL, GRAPH>::calculateEstimate(const KEY& key) const {
const Index index = getOrdering()[key];
const SubVector delta = getDelta()[index];
return getLinearizationPoint()[key].retract(delta);
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateBestEstimate() const {
template<class CONDITIONAL, class GRAPH>
Values ISAM2<CONDITIONAL, GRAPH>::calculateBestEstimate() const {
VectorValues delta(theta_.dims(ordering_));
optimize2(this->root(), delta);
return theta_.retract(delta, ordering_);
}
/* ************************************************************************* */
template<class CONDITIONAL, class VALUES, class GRAPH>
VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam) {
template<class CONDITIONAL, class GRAPH>
VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam) {
VectorValues delta = *allocateVectorValues(isam);
optimize2(isam.root(), delta);
return delta;

36
gtsam/nonlinear/ISAM2.h
View File

@ -106,16 +106,19 @@ struct ISAM2Params {
bool evaluateNonlinearError; ///< Whether to evaluate the nonlinear error before and after the update, to return in ISAM2Result from update()
KeyFormatter keyFormatter; ///< A KeyFormatter for when keys are printed during debugging (default: DefaultKeyFormatter)
/** Specify parameters as constructor arguments */
ISAM2Params(
OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params public variables, ISAM2Params::optimizationParams
RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params public variables, ISAM2Params::relinearizeThreshold
int _relinearizeSkip = 10, ///< see ISAM2Params public variables, ISAM2Params::relinearizeSkip
bool _enableRelinearization = true, ///< see ISAM2Params public variables, ISAM2Params::enableRelinearization
bool _evaluateNonlinearError = false ///< see ISAM2Params public variables, ISAM2Params::evaluateNonlinearError
OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams
RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params::relinearizeThreshold
int _relinearizeSkip = 10, ///< see ISAM2Params::relinearizeSkip
bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization
bool _evaluateNonlinearError = false, ///< see ISAM2Params::evaluateNonlinearError
const KeyFormatter& _keyFormatter = DefaultKeyFormatter ///< see ISAM2::Params::keyFormatter
) : optimizationParams(_optimizationParams), relinearizeThreshold(_relinearizeThreshold),
relinearizeSkip(_relinearizeSkip), enableRelinearization(_enableRelinearization),
evaluateNonlinearError(_evaluateNonlinearError) {}
evaluateNonlinearError(_evaluateNonlinearError), keyFormatter(_keyFormatter) {}
};
/**
@ -266,13 +269,13 @@ private:
* estimate of all variables.
*
*/
template<class CONDITIONAL, class VALUES, class GRAPH = NonlinearFactorGraph<VALUES> >
template<class CONDITIONAL, class GRAPH = NonlinearFactorGraph>
class ISAM2: public BayesTree<CONDITIONAL, ISAM2Clique<CONDITIONAL> > {
protected:
/** The current linearization point */
VALUES theta_;
Values theta_;
/** VariableIndex lets us look up factors by involved variable and keeps track of dimensions */
VariableIndex variableIndex_;
@ -314,8 +317,7 @@ private:
public:
typedef BayesTree<CONDITIONAL,ISAM2Clique<CONDITIONAL> > Base; ///< The BayesTree base class
typedef ISAM2<CONDITIONAL, VALUES> This; ///< This class
typedef VALUES Values;
typedef ISAM2<CONDITIONAL> This; ///< This class
typedef GRAPH Graph;
/** Create an empty ISAM2 instance */
@ -368,19 +370,19 @@ public:
* (Params::relinearizeSkip).
* @return An ISAM2Result struct containing information about the update
*/
ISAM2Result update(const GRAPH& newFactors = GRAPH(), const VALUES& newTheta = VALUES(),
ISAM2Result update(const GRAPH& newFactors = GRAPH(), const Values& newTheta = Values(),
const FastVector<size_t>& removeFactorIndices = FastVector<size_t>(),
const boost::optional<FastSet<Symbol> >& constrainedKeys = boost::none,
const boost::optional<FastSet<Key> >& constrainedKeys = boost::none,
bool force_relinearize = false);
/** Access the current linearization point */
const VALUES& getLinearizationPoint() const {return theta_;}
const Values& getLinearizationPoint() const {return theta_;}
/** Compute an estimate from the incomplete linear delta computed during the last update.
* This delta is incomplete because it was not updated below wildfire_threshold. If only
* a single variable is needed, it is faster to call calculateEstimate(const KEY&).
*/
VALUES calculateEstimate() const;
Values calculateEstimate() const;
/** Compute an estimate for a single variable using its incomplete linear delta computed
* during the last update. This is faster than calling the no-argument version of
@ -399,7 +401,7 @@ public:
/** Compute an estimate using a complete delta computed by a full back-substitution.
*/
VALUES calculateBestEstimate() const;
Values calculateBestEstimate() const;
/** Access the current delta, computed during the last call to update */
const Permuted<VectorValues>& getDelta() const { return delta_; }
@ -435,8 +437,8 @@ private:
}; // ISAM2
/** Get the linear delta for the ISAM2 object, unpermuted the delta returned by ISAM2::getDelta() */
template<class CONDITIONAL, class VALUES, class GRAPH>
VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam);
template<class CONDITIONAL, class GRAPH>
VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam);
} /// namespace gtsam

34
gtsam/nonlinear/Key.cpp Normal file
View File

@ -0,0 +1,34 @@
/* ----------------------------------------------------------------------------
* 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 Key.h
* @brief
* @author Richard Roberts
* @date Feb 20, 2012
*/
#include <gtsam/nonlinear/Key.h>
#include <boost/lexical_cast.hpp>
#include <gtsam/nonlinear/Symbol.h>
namespace gtsam {
std::string _defaultKeyFormatter(Key key) {
const Symbol asSymbol(key);
if(asSymbol.chr() > 0)
return (std::string)asSymbol;
else
return boost::lexical_cast<std::string>(key);
}
}

348
gtsam/nonlinear/Key.h
View File

@ -11,348 +11,30 @@
/**
* @file Key.h
* @date Jan 12, 2010
* @author: Frank Dellaert
* @author: Richard Roberts
* @brief
* @author Richard Roberts
* @date Feb 20, 2012
*/
#pragma once
#include <list>
#include <iostream>
#include <boost/mpl/char.hpp>
#include <boost/format.hpp>
#include <boost/serialization/nvp.hpp>
#ifdef GTSAM_MAGIC_KEY
#include <boost/lexical_cast.hpp>
#endif
#define ALPHA '\224'
#include <boost/function.hpp>
#include <string>
namespace gtsam {
/**
* TypedSymbol key class is templated on
* 1) the type T it is supposed to retrieve, for extra type checking
* 2) the character constant used for its string representation
*/
template<class T, char C>
class TypedSymbol {
/// Integer nonlinear key type
typedef size_t Key;
protected:
size_t j_;
/// Typedef for a function to format a key, i.e. to convert it to a string
typedef boost::function<std::string(Key)> KeyFormatter;
public:
// Helper function for DefaultKeyFormatter
std::string _defaultKeyFormatter(Key key);
// typedefs
typedef T Value;
typedef boost::mpl::char_<C> Chr; // to reconstruct the type: use Chr::value
/// The default KeyFormatter, which is used if no KeyFormatter is passed to
/// a nonlinear 'print' function. Automatically detects plain integer keys
/// and Symbol keys.
static const KeyFormatter DefaultKeyFormatter = &_defaultKeyFormatter;
// Constructors:
TypedSymbol() :
j_(0) {
}
TypedSymbol(size_t j) :
j_(j) {
}
virtual ~TypedSymbol() {}
// Get stuff:
///TODO: comment
size_t index() const {
return j_;
}
static char chr() {
return C;
}
const char* c_str() const {
return ((std::string) (*this)).c_str();
}
operator std::string() const {
return (boost::format("%c%d") % C % j_).str();
}
std::string latex() const {
return (boost::format("%c_{%d}") % C % j_).str();
}
// logic:
bool operator<(const TypedSymbol& compare) const {
return j_ < compare.j_;
}
bool operator==(const TypedSymbol& compare) const {
return j_ == compare.j_;
}
bool operator!=(const TypedSymbol& compare) const {
return j_ != compare.j_;
}
int compare(const TypedSymbol& compare) const {
return j_ - compare.j_;
}
// Testable Requirements
virtual void print(const std::string& s = "") const {
std::cout << s << ": " << (std::string) (*this) << std::endl;
}
bool equals(const TypedSymbol& expected, double tol = 0.0) const {
return (*this) == expected;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(j_);
}
};
/** forward declaration to avoid circular dependencies */
template<class T, char C, typename L>
class TypedLabeledSymbol;
/**
* Character and index key used in VectorValues, GaussianFactorGraph,
* GaussianFactor, etc. These keys are generated at runtime from TypedSymbol
* keys when linearizing a nonlinear factor graph. This key is not type
* safe, so cannot be used with any Nonlinear* classes.
*/
class Symbol {
protected:
unsigned char c_;
size_t j_;
public:
/** Default constructor */
Symbol() :
c_(0), j_(0) {
}
/** Copy constructor */
Symbol(const Symbol& key) :
c_(key.c_), j_(key.j_) {
}
/** Constructor */
Symbol(unsigned char c, size_t j) :
c_(c), j_(j) {
}
/** Casting constructor from TypedSymbol */
template<class T, char C>
Symbol(const TypedSymbol<T, C>& symbol) :
c_(C), j_(symbol.index()) {
}
/** Casting constructor from TypedLabeledSymbol */
template<class T, char C, typename L>
Symbol(const TypedLabeledSymbol<T, C, L>& symbol) :
c_(C), j_(symbol.encode()) {
}
/** "Magic" key casting constructor from string */
#ifdef GTSAM_MAGIC_KEY
Symbol(const std::string& str) {
if(str.length() < 1)
throw std::invalid_argument("Cannot parse string key '" + str + "'");
else {
const char *c_str = str.c_str();
c_ = c_str[0];
if(str.length() > 1)
j_ = boost::lexical_cast<size_t>(c_str+1);
else
j_ = 0;
}
}
Symbol(const char *c_str) {
std::string str(c_str);
if(str.length() < 1)
throw std::invalid_argument("Cannot parse string key '" + str + "'");
else {
c_ = c_str[0];
if(str.length() > 1)
j_ = boost::lexical_cast<size_t>(c_str+1);
else
j_ = 0;
}
}
#endif
// Testable Requirements
void print(const std::string& s = "") const {
std::cout << s << ": " << (std::string) (*this) << std::endl;
}
bool equals(const Symbol& expected, double tol = 0.0) const {
return (*this) == expected;
}
/** Retrieve key character */
unsigned char chr() const {
return c_;
}
/** Retrieve key index */
size_t index() const {
return j_;
}
/** Create a string from the key */
operator std::string() const {
return str(boost::format("%c%d") % c_ % j_);
}
/** Comparison for use in maps */
bool operator<(const Symbol& comp) const {
return c_ < comp.c_ || (comp.c_ == c_ && j_ < comp.j_);
}
bool operator==(const Symbol& comp) const {
return comp.c_ == c_ && comp.j_ == j_;
}
bool operator!=(const Symbol& comp) const {
return comp.c_ != c_ || comp.j_ != j_;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & BOOST_SERIALIZATION_NVP(c_);
ar & BOOST_SERIALIZATION_NVP(j_);
}
};
// Conversion utilities
template<class KEY> Symbol key2symbol(KEY key) {
return Symbol(key);
}
template<class KEY> std::list<Symbol> keys2symbols(std::list<KEY> keys) {
std::list<Symbol> symbols;
std::transform(keys.begin(), keys.end(), std::back_inserter(symbols),
key2symbol<KEY> );
return symbols;
}
/**
* TypedLabeledSymbol is a variation of the TypedSymbol that allows
* for a runtime label to be placed on the label, so as to express
* "Pose 5 for robot 3"
* Labels should be kept to base datatypes (int, char, etc) to
* minimize cost of comparisons
*
* The labels will be compared first when comparing Keys, followed by the
* index
*/
template<class T, char C, typename L>
class TypedLabeledSymbol: public TypedSymbol<T, C> {
protected:
// Label
L label_;
public:
typedef TypedSymbol<T, C> Base;
// Constructors:
TypedLabeledSymbol() {
}
TypedLabeledSymbol(size_t j, L label) :
Base(j), label_(label) {
}
/** Constructor that decodes encoded labels */
TypedLabeledSymbol(const Symbol& sym) :
TypedSymbol<T, C> (0) {
size_t shift = (sizeof(size_t) - sizeof(short)) * 8;
this->j_ = (sym.index() << shift) >> shift; // truncate upper bits
label_ = (L) (sym.index() >> shift); // remove lower bits
}
/** Constructor to upgrade an existing typed label with a label */
TypedLabeledSymbol(const Base& key, L label) :
Base(key.index()), label_(label) {
}
// Get stuff:
L label() const {
return label_;
}
const char* c_str() const {
return ((std::string)(*this)).c_str();
}
operator std::string() const {
std::string label_s = (boost::format("%1%") % label_).str();
return (boost::format("%c%s_%d") % C % label_s % this->j_).str();
}
std::string latex() const {
std::string label_s = (boost::format("%1%") % label_).str();
return (boost::format("%c%s_{%d}") % C % label_s % this->j_).str();
}
// Needed for conversion to LabeledSymbol
size_t convertLabel() const {
return label_;
}
/**
* Encoding two numbers into a single size_t for conversion to Symbol
* Stores the label in the upper bytes of the index
*/
size_t encode() const {
short label = (short) label_; //bound size of label to 2 bytes
size_t shift = (sizeof(size_t) - sizeof(short)) * 8;
size_t modifier = ((size_t) label) << shift;
return this->j_ + modifier;
}
// logic:
bool operator<(const TypedLabeledSymbol& compare) const {
if (label_ == compare.label_) // sort by label first
return this->j_ < compare.j_;
else
return label_ < compare.label_;
}
bool operator==(const TypedLabeledSymbol& compare) const {
return this->j_ == compare.j_ && label_ == compare.label_;
}
int compare(const TypedLabeledSymbol& compare) const {
if (label_ == compare.label_) // sort by label first
return this->j_ - compare.j_;
else
return label_ - compare.label_;
}
// Testable Requirements
void print(const std::string& s = "") const {
std::cout << s << ": " << (std::string) (*this) << std::endl;
}
bool equals(const TypedLabeledSymbol& expected, double tol = 0.0) const {
return (*this) == expected;
}
private:
/** Serialization function */
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
typedef TypedSymbol<T, C> Base;
ar & boost::serialization::make_nvp("TypedLabeledSymbol",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(label_);
}
};
} // namespace gtsam

76
gtsam/nonlinear/Makefile.am
View File

@ -1,76 +0,0 @@
#----------------------------------------------------------------------------------------------------
# GTSAM nonlinear
# Non-linear optimization
#----------------------------------------------------------------------------------------------------
# use nostdinc to turn off -I. and -I.., we do not need them because
# header files are qualified so they can be included in external projects.
AUTOMAKE_OPTIONS = nostdinc
headers =
sources =
check_PROGRAMS =
#----------------------------------------------------------------------------------------------------
# nonlinear
#----------------------------------------------------------------------------------------------------
# Lie Groups
headers += Values.h Values-inl.h TupleValues.h TupleValues-inl.h
check_PROGRAMS += tests/testValues tests/testKey tests/testOrdering
# Nonlinear nonlinear
headers += Key.h
headers += NonlinearFactorGraph.h NonlinearFactorGraph-inl.h
headers += NonlinearOptimizer-inl.h NonlinearOptimization.h NonlinearOptimization-inl.h
headers += NonlinearFactor.h
sources += NonlinearOptimizer.cpp Ordering.cpp DoglegOptimizerImpl.cpp NonlinearOptimizationParameters.cpp
headers += DoglegOptimizer.h DoglegOptimizer-inl.h
# Nonlinear iSAM(2)
headers += NonlinearISAM.h NonlinearISAM-inl.h
headers += ISAM2.h ISAM2-inl.h ISAM2-impl-inl.h
sources += GaussianISAM2.cpp
headers += GaussianISAM2-inl.h
# Nonlinear constraints
headers += NonlinearEquality.h
# White noise factor
headers += WhiteNoiseFactor.h
#check_PROGRAMS += tests/testWhiteFactor
# Kalman Filter
headers += ExtendedKalmanFilter.h ExtendedKalmanFilter-inl.h
#----------------------------------------------------------------------------------------------------
# Create a libtool library that is not installed
# It will be packaged in the toplevel libgtsam.la as specfied in ../Makefile.am
# The headers are installed in $(includedir)/gtsam:
#----------------------------------------------------------------------------------------------------
headers += $(sources:.cpp=.h)
nonlineardir = $(pkgincludedir)/nonlinear
nonlinear_HEADERS = $(headers)
noinst_LTLIBRARIES = libnonlinear.la
libnonlinear_la_SOURCES = $(sources)
AM_CPPFLAGS = $(BOOST_CPPFLAGS) -I$(top_srcdir)
AM_LDFLAGS = $(BOOST_LDFLAGS)
AM_CXXFLAGS =
#----------------------------------------------------------------------------------------------------
# rules to build local programs
#----------------------------------------------------------------------------------------------------
TESTS = $(check_PROGRAMS)
AM_LDFLAGS += $(boost_serialization)
LDADD = libnonlinear.la ../linear/liblinear.la ../inference/libinference.la ../base/libbase.la ../3rdparty/libccolamd.la
LDADD += ../../CppUnitLite/libCppUnitLite.a
AM_DEFAULT_SOURCE_EXT = .cpp
# rule to run an executable
%.run: % $(LDADD)
./$^
# rule to run executable with valgrind
%.valgrind: % $(LDADD)
valgrind ./$^

77
gtsam/nonlinear/NonlinearEquality.h
View File

@ -47,11 +47,11 @@ namespace gtsam {
*
* \nosubgrouping
*/
template<class VALUES, class KEY>
class NonlinearEquality: public NonlinearFactor1<VALUES, KEY> {
template<class VALUE>
class NonlinearEquality: public NoiseModelFactor1<VALUE> {
public:
typedef typename KEY::Value T;
typedef VALUE T;
private:
@ -64,6 +64,12 @@ namespace gtsam {
// error gain in allow error case
double error_gain_;
// typedef to this class
typedef NonlinearEquality<VALUE> This;
// typedef to base class
typedef NoiseModelFactor1<VALUE> Base;
public:
/**
@ -71,7 +77,6 @@ namespace gtsam {
*/
bool (*compare_)(const T& a, const T& b);
typedef NonlinearFactor1<VALUES, KEY> Base;
/** default constructor - only for serialization */
NonlinearEquality() {}
@ -84,7 +89,7 @@ namespace gtsam {
/**
* Constructor - forces exact evaluation
*/
NonlinearEquality(const KEY& j, const T& feasible, bool (*_compare)(const T&, const T&) = compare<T>) :
NonlinearEquality(Key j, const T& feasible, bool (*_compare)(const T&, const T&) = compare<T>) :
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(false), error_gain_(0.0),
compare_(_compare) {
@ -93,7 +98,7 @@ namespace gtsam {
/**
* Constructor - allows inexact evaluation
*/
NonlinearEquality(const KEY& j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare<T>) :
NonlinearEquality(Key j, const T& feasible, double error_gain, bool (*_compare)(const T&, const T&) = compare<T>) :
Base(noiseModel::Constrained::All(feasible.dim()), j), feasible_(feasible),
allow_error_(true), error_gain_(error_gain),
compare_(_compare) {
@ -103,17 +108,17 @@ namespace gtsam {
/// @name Testable
/// @{
void print(const std::string& s = "") const {
std::cout << "Constraint: " << s << " on [" << (std::string)(this->key_) << "]\n";
virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << "Constraint: " << s << " on [" << keyFormatter(this->key()) << "]\n";
gtsam::print(feasible_,"Feasible Point");
std::cout << "Variable Dimension: " << feasible_.dim() << std::endl;
}
/** Check if two factors are equal */
bool equals(const NonlinearEquality<VALUES,KEY>& f, double tol = 1e-9) const {
if (!Base::equals(f)) return false;
return feasible_.equals(f.feasible_, tol) &&
fabs(error_gain_ - f.error_gain_) < tol;
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
const This* e = dynamic_cast<const This*>(&f);
return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) &&
fabs(error_gain_ - e->error_gain_) < tol;
}
/// @}
@ -121,8 +126,8 @@ namespace gtsam {
/// @{
/** actual error function calculation */
virtual double error(const VALUES& c) const {
const T& xj = c[this->key_];
virtual double error(const Values& c) const {
const T& xj = c.at<T>(this->key());
Vector e = this->unwhitenedError(c);
if (allow_error_ || !compare_(xj, feasible_)) {
return error_gain_ * dot(e,e);
@ -132,7 +137,7 @@ namespace gtsam {
}
/** error function */
inline Vector evaluateError(const T& xj, boost::optional<Matrix&> H = boost::none) const {
Vector evaluateError(const T& xj, boost::optional<Matrix&> H = boost::none) const {
size_t nj = feasible_.dim();
if (allow_error_) {
if (H) *H = eye(nj); // FIXME: this is not the right linearization for nonlinear compare
@ -142,18 +147,18 @@ namespace gtsam {
return zero(nj); // set error to zero if equal
} else {
if (H) throw std::invalid_argument(
"Linearization point not feasible for " + (std::string)(this->key_) + "!");
"Linearization point not feasible for " + DefaultKeyFormatter(this->key()) + "!");
return repeat(nj, std::numeric_limits<double>::infinity()); // set error to infinity if not equal
}
}
// Linearize is over-written, because base linearization tries to whiten
virtual GaussianFactor::shared_ptr linearize(const VALUES& x, const Ordering& ordering) const {
const T& xj = x[this->key_];
virtual GaussianFactor::shared_ptr linearize(const Values& x, const Ordering& ordering) const {
const T& xj = x.at<T>(this->key());
Matrix A;
Vector b = evaluateError(xj, A);
SharedDiagonal model = noiseModel::Constrained::All(b.size());
return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key_], A, b, model));
return GaussianFactor::shared_ptr(new JacobianFactor(ordering[this->key()], A, b, model));
}
/// @}
@ -164,7 +169,7 @@ namespace gtsam {
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor1",
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(feasible_);
ar & BOOST_SERIALIZATION_NVP(allow_error_);
@ -177,14 +182,14 @@ namespace gtsam {
/**
* Simple unary equality constraint - fixes a value for a variable
*/
template<class VALUES, class KEY>
class NonlinearEquality1 : public NonlinearFactor1<VALUES, KEY> {
template<class VALUE>
class NonlinearEquality1 : public NoiseModelFactor1<VALUE> {
public:
typedef typename KEY::Value X;
typedef VALUE X;
protected:
typedef NonlinearFactor1<VALUES, KEY> Base;
typedef NoiseModelFactor1<VALUE> Base;
/** default constructor to allow for serialization */
NonlinearEquality1() {}
@ -196,10 +201,10 @@ namespace gtsam {
public:
typedef boost::shared_ptr<NonlinearEquality1<VALUES, KEY> > shared_ptr;
typedef boost::shared_ptr<NonlinearEquality1<VALUE> > shared_ptr;
///TODO: comment
NonlinearEquality1(const X& value, const KEY& key1, double mu = 1000.0)
NonlinearEquality1(const X& value, Key key1, double mu = 1000.0)
: Base(noiseModel::Constrained::All(value.dim(), fabs(mu)), key1), value_(value) {}
virtual ~NonlinearEquality1() {}
@ -212,9 +217,9 @@ namespace gtsam {
}
/** Print */
virtual void print(const std::string& s = "") const {
virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << ": NonlinearEquality1("
<< (std::string) this->key_ << "),"<< "\n";
<< keyFormatter(this->key()) << "),"<< "\n";
this->noiseModel_->print();
value_.print("Value");
}
@ -225,7 +230,7 @@ namespace gtsam {
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor1",
ar & boost::serialization::make_nvp("NoiseModelFactor1",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(value_);
}
@ -236,13 +241,13 @@ namespace gtsam {
* Simple binary equality constraint - this constraint forces two factors to
* be the same.
*/
template<class VALUES, class KEY>
class NonlinearEquality2 : public NonlinearFactor2<VALUES, KEY, KEY> {
template<class VALUE>
class NonlinearEquality2 : public NoiseModelFactor2<VALUE, VALUE> {
public:
typedef typename KEY::Value X;
typedef VALUE X;
protected:
typedef NonlinearFactor2<VALUES, KEY, KEY> Base;
typedef NoiseModelFactor2<VALUE, VALUE> Base;
GTSAM_CONCEPT_MANIFOLD_TYPE(X);
@ -251,10 +256,10 @@ namespace gtsam {
public:
typedef boost::shared_ptr<NonlinearEquality2<VALUES, KEY> > shared_ptr;
typedef boost::shared_ptr<NonlinearEquality2<VALUE> > shared_ptr;
///TODO: comment
NonlinearEquality2(const KEY& key1, const KEY& key2, double mu = 1000.0)
NonlinearEquality2(Key key1, Key key2, double mu = 1000.0)
: Base(noiseModel::Constrained::All(X::Dim(), fabs(mu)), key1, key2) {}
virtual ~NonlinearEquality2() {}
@ -274,7 +279,7 @@ namespace gtsam {
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NonlinearFactor2",
ar & boost::serialization::make_nvp("NoiseModelFactor2",
boost::serialization::base_object<Base>(*this));
}
}; // \NonlinearEquality2

463
gtsam/nonlinear/NonlinearFactor.h
View File

@ -24,30 +24,19 @@
#include <limits>
#include <boost/serialization/base_object.hpp>
#include <boost/tuple/tuple.hpp>
#include <boost/function.hpp>
#include <gtsam/inference/Factor-inl.h>
#include <gtsam/inference/IndexFactor.h>
#include <gtsam/linear/SharedNoiseModel.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/nonlinear/Values.h>
#include <gtsam/nonlinear/Ordering.h>
#include <gtsam/nonlinear/Symbol.h>
namespace gtsam {
using boost::make_tuple;
// Helper function to fill a vector from a tuple function of any length
template<typename CONS>
inline void __fill_from_tuple(std::vector<Symbol>& vector, size_t position, const CONS& tuple) {
vector[position] = tuple.get_head();
__fill_from_tuple<typename CONS::tail_type>(vector, position+1, tuple.get_tail());
}
template<>
inline void __fill_from_tuple<boost::tuples::null_type>(std::vector<Symbol>& vector, size_t position, const boost::tuples::null_type& tuple) {
// Do nothing
}
/* ************************************************************************* */
/**
* Nonlinear factor base class
@ -57,18 +46,17 @@ inline void __fill_from_tuple<boost::tuples::null_type>(std::vector<Symbol>& vec
* which are objects in non-linear manifolds (Lie groups).
* \nosubgrouping
*/
template<class VALUES>
class NonlinearFactor: public Factor<Symbol> {
class NonlinearFactor: public Factor<Key> {
protected:
// Some handy typedefs
typedef Factor<Symbol> Base;
typedef NonlinearFactor<VALUES> This;
typedef Factor<Key> Base;
typedef NonlinearFactor This;
public:
typedef boost::shared_ptr<NonlinearFactor<VALUES> > shared_ptr;
typedef boost::shared_ptr<NonlinearFactor> shared_ptr;
/// @name Standard Constructors
/// @{
@ -77,18 +65,6 @@ public:
NonlinearFactor() {
}
/**
* Constructor
* @param keys A boost::tuple containing the variables involved in this factor,
* example: <tt>NonlinearFactor(make_tuple(symbol1, symbol2, symbol3))</tt>
*/
template<class U1, class U2>
NonlinearFactor(const boost::tuples::cons<U1,U2>& keys) {
this->keys_.resize(boost::tuples::length<boost::tuples::cons<U1,U2> >::value);
// Use helper function to fill key vector, using 'cons' representation of tuple
__fill_from_tuple(this->keys(), 0, keys);
}
/**
* Constructor
* @param keys The variables involved in this factor
@ -103,8 +79,15 @@ public:
/// @{
/** print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor\n";
virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << s << "keys = { ";
BOOST_FOREACH(Key key, this->keys()) { std::cout << keyFormatter(key) << " "; }
std::cout << "}" << endl;
}
/** Check if two factors are equal */
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
return Base::equals(f);
}
/// @}
@ -120,7 +103,7 @@ public:
* This is typically equal to log-likelihood, e.g. 0.5(h(x)-z)^2/sigma^2 in case of Gaussian.
* You can override this for systems with unusual noise models.
*/
virtual double error(const VALUES& c) const = 0;
virtual double error(const Values& c) const = 0;
/** get the dimension of the factor (number of rows on linearization) */
virtual size_t dim() const = 0;
@ -135,11 +118,11 @@ public:
* when the constraint is *NOT* fulfilled.
* @return true if the constraint is active
*/
virtual bool active(const VALUES& c) const { return true; }
virtual bool active(const Values& c) const { return true; }
/** linearize to a GaussianFactor */
virtual boost::shared_ptr<GaussianFactor>
linearize(const VALUES& c, const Ordering& ordering) const = 0;
linearize(const Values& c, const Ordering& ordering) const = 0;
/**
* Create a symbolic factor using the given ordering to determine the
@ -165,20 +148,19 @@ public:
* The noise model is typically Gaussian, but robust and constrained error models are also supported.
*/
template<class VALUES>
class NoiseModelFactor: public NonlinearFactor<VALUES> {
class NoiseModelFactor: public NonlinearFactor {
protected:
// handy typedefs
typedef NonlinearFactor<VALUES> Base;
typedef NoiseModelFactor<VALUES> This;
typedef NonlinearFactor Base;
typedef NoiseModelFactor This;
SharedNoiseModel noiseModel_; /** Noise model */
public:
typedef boost::shared_ptr<NoiseModelFactor<VALUES> > shared_ptr;
typedef boost::shared_ptr<NoiseModelFactor > shared_ptr;
/** Default constructor for I/O only */
NoiseModelFactor() {
@ -187,16 +169,6 @@ public:
/** Destructor */
virtual ~NoiseModelFactor() {}
/**
* Constructor
* @param keys A boost::tuple containing the variables involved in this factor,
* example: <tt>NoiseModelFactor(noiseModel, make_tuple(symbol1, symbol2, symbol3)</tt>
*/
template<class U1, class U2>
NoiseModelFactor(const SharedNoiseModel& noiseModel, const boost::tuples::cons<U1,U2>& keys)
: Base(keys), noiseModel_(noiseModel) {
}
/**
* Constructor
* @param keys The variables involved in this factor
@ -216,17 +188,15 @@ protected:
public:
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NoiseModelFactor\n";
std::cout << " ";
BOOST_FOREACH(const Symbol& key, this->keys()) { std::cout << (std::string)key << " "; }
std::cout << "\n";
virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
Base::print(s, keyFormatter);
this->noiseModel_->print(" noise model: ");
}
/** Check if two factors are equal */
virtual bool equals(const NoiseModelFactor<VALUES>& f, double tol = 1e-9) const {
return noiseModel_->equals(*f.noiseModel_, tol) && Base::equals(f, tol);
virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
const NoiseModelFactor* e = dynamic_cast<const NoiseModelFactor*>(&f);
return e && Base::equals(f, tol) && noiseModel_->equals(*e->noiseModel_, tol);
}
/** get the dimension of the factor (number of rows on linearization) */
@ -245,13 +215,13 @@ public:
* If any of the optional Matrix reference arguments are specified, it should compute
* both the function evaluation and its derivative(s) in X1 (and/or X2, X3...).
*/
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const = 0;
/**
* Vector of errors, whitened
* This is the raw error, i.e., i.e. \f$ (h(x)-z)/\sigma \f$ in case of a Gaussian
*/
Vector whitenedError(const VALUES& c) const {
Vector whitenedError(const Values& c) const {
return noiseModel_->whiten(unwhitenedError(c));
}
@ -261,7 +231,7 @@ public:
* In this class, we take the raw prediction error \f$ h(x)-z \f$, ask the noise model
* to transform it to \f$ (h(x)-z)^2/\sigma^2 \f$, and then multiply by 0.5.
*/
virtual double error(const VALUES& c) const {
virtual double error(const Values& c) const {
if (this->active(c))
return 0.5 * noiseModel_->distance(unwhitenedError(c));
else
@ -273,7 +243,7 @@ public:
* \f$ Ax-b \approx h(x+\delta x)-z = h(x) + A \delta x - z \f$
* Hence \f$ b = z - h(x) = - \mathtt{error\_vector}(x) \f$
*/
boost::shared_ptr<GaussianFactor> linearize(const VALUES& x, const Ordering& ordering) const {
boost::shared_ptr<GaussianFactor> linearize(const Values& x, const Ordering& ordering) const {
// Only linearize if the factor is active
if (!this->active(x))
return boost::shared_ptr<JacobianFactor>();
@ -321,45 +291,44 @@ private:
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 1
* variable. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY>
class NonlinearFactor1: public NoiseModelFactor<VALUES> {
template<class VALUE>
class NoiseModelFactor1: public NoiseModelFactor {
public:
// typedefs for value types pulled from keys
typedef typename KEY::Value X;
typedef VALUE X;
protected:
// The value of the key. Not const to allow serialization
KEY key_;
typedef NoiseModelFactor<VALUES> Base;
typedef NonlinearFactor1<VALUES, KEY> This;
typedef NoiseModelFactor Base;
typedef NoiseModelFactor1<VALUE> This;
public:
/** Default constructor for I/O only */
NonlinearFactor1() {}
NoiseModelFactor1() {}
virtual ~NonlinearFactor1() {}
virtual ~NoiseModelFactor1() {}
inline const KEY& key() const { return key_; }
inline Key key() const { return keys_[0]; }
/**
* Constructor
* @param z measurement
* @param key by which to look up X value in Values
*/
NonlinearFactor1(const SharedNoiseModel& noiseModel, const KEY& key1) :
Base(noiseModel, make_tuple(key1)), key_(key1) {
NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) :
Base(noiseModel) {
keys_.resize(1);
keys_[0] = key1;
}
/** Calls the 1-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
const X& x1 = x[key_];
const X& x1 = x.at<X>(keys_[0]);
if(H) {
return evaluateError(x1, (*H)[0]);
} else {
@ -370,12 +339,6 @@ public:
}
}
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor1(" << (std::string) this->key_ << ")\n";
this->noiseModel_->print(" noise model: ");
}
/**
* Override this method to finish implementing a unary factor.
* If the optional Matrix reference argument is specified, it should compute
@ -392,59 +355,58 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key_);
}
};// \class NonlinearFactor1
};// \class NoiseModelFactor1
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 2
* variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2>
class NonlinearFactor2: public NoiseModelFactor<VALUES> {
template<class VALUE1, class VALUE2>
class NoiseModelFactor2: public NoiseModelFactor {
public:
// typedefs for value types pulled from keys
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef VALUE1 X1;
typedef VALUE2 X2;
protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
typedef NoiseModelFactor<VALUES> Base;
typedef NonlinearFactor2<VALUES, KEY1, KEY2> This;
typedef NoiseModelFactor Base;
typedef NoiseModelFactor2<VALUE1, VALUE2> This;
public:
/**
* Default Constructor for I/O
*/
NonlinearFactor2() {}
NoiseModelFactor2() {}
/**
* Constructor
* @param j1 key of the first variable
* @param j2 key of the second variable
*/
NonlinearFactor2(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2) :
Base(noiseModel, make_tuple(j1,j2)), key1_(j1), key2_(j2) {}
NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) :
Base(noiseModel) {
keys_.resize(2);
keys_[0] = j1;
keys_[1] = j2;
}
virtual ~NonlinearFactor2() {}
virtual ~NoiseModelFactor2() {}
/** methods to retrieve both keys */
inline const KEY1& key1() const { return key1_; }
inline const KEY2& key2() const { return key2_; }
inline Key key1() const { return keys_[0]; }
inline Key key2() const { return keys_[1]; }
/** Calls the 2-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
const X1& x1 = x[key1_];
const X2& x2 = x[key2_];
const X1& x1 = x.at<X1>(keys_[0]);
const X2& x2 = x.at<X2>(keys_[1]);
if(H) {
return evaluateError(x1, x2, (*H)[0], (*H)[1]);
} else {
@ -455,14 +417,6 @@ public:
}
}
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor2("
<< (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ")\n";
this->noiseModel_->print(" noise model: ");
}
/**
* Override this method to finish implementing a binary factor.
* If any of the optional Matrix reference arguments are specified, it should compute
@ -480,40 +434,33 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
}
}; // \class NonlinearFactor2
}; // \class NoiseModelFactor2
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 3
* variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2, class KEY3>
class NonlinearFactor3: public NoiseModelFactor<VALUES> {
template<class VALUE1, class VALUE2, class VALUE3>
class NoiseModelFactor3: public NoiseModelFactor {
public:
// typedefs for value types pulled from keys
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef typename KEY3::Value X3;
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef VALUE3 X3;
protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
typedef NoiseModelFactor<VALUES> Base;
typedef NonlinearFactor3<VALUES, KEY1, KEY2, KEY3> This;
typedef NoiseModelFactor Base;
typedef NoiseModelFactor3<VALUE1, VALUE2, VALUE3> This;
public:
/**
* Default Constructor for I/O
*/
NonlinearFactor3() {}
NoiseModelFactor3() {}
/**
* Constructor
@ -521,39 +468,34 @@ public:
* @param j2 key of the second variable
* @param j3 key of the third variable
*/
NonlinearFactor3(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3) :
Base(noiseModel, make_tuple(j1,j2,j3)), key1_(j1), key2_(j2), key3_(j3) {}
NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) :
Base(noiseModel) {
keys_.resize(3);
keys_[0] = j1;
keys_[1] = j2;
keys_[2] = j3;
}
virtual ~NonlinearFactor3() {}
virtual ~NoiseModelFactor3() {}
/** methods to retrieve keys */
inline const KEY1& key1() const { return key1_; }
inline const KEY2& key2() const { return key2_; }
inline const KEY3& key3() const { return key3_; }
inline Key key1() const { return keys_[0]; }
inline Key key2() const { return keys_[1]; }
inline Key key3() const { return keys_[2]; }
/** Calls the 3-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], (*H)[0], (*H)[1], (*H)[2]);
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), (*H)[0], (*H)[1], (*H)[2]);
else
return evaluateError(x[key1_], x[key2_], x[key3_]);
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]));
} else {
return zero(this->dim());
}
}
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor3("
<< (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ","
<< (std::string) this->key3_ << ")\n";
this->noiseModel_->print(" noise model: ");
}
/**
* Override this method to finish implementing a trinary factor.
* If any of the optional Matrix reference arguments are specified, it should compute
@ -573,43 +515,34 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
}
}; // \class NonlinearFactor3
}; // \class NoiseModelFactor3
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 4
* variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4>
class NonlinearFactor4: public NoiseModelFactor<VALUES> {
template<class VALUE1, class VALUE2, class VALUE3, class VALUE4>
class NoiseModelFactor4: public NoiseModelFactor {
public:
// typedefs for value types pulled from keys
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef typename KEY3::Value X3;
typedef typename KEY4::Value X4;
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef VALUE3 X3;
typedef VALUE4 X4;
protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
KEY4 key4_;
typedef NoiseModelFactor<VALUES> Base;
typedef NonlinearFactor4<VALUES, KEY1, KEY2, KEY3, KEY4> This;
typedef NoiseModelFactor Base;
typedef NoiseModelFactor4<VALUE1, VALUE2, VALUE3, VALUE4> This;
public:
/**
* Default Constructor for I/O
*/
NonlinearFactor4() {}
NoiseModelFactor4() {}
/**
* Constructor
@ -618,40 +551,36 @@ public:
* @param j3 key of the third variable
* @param j4 key of the fourth variable
*/
NonlinearFactor4(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4) :
Base(noiseModel, make_tuple(j1,j2,j3,j4)), key1_(j1), key2_(j2), key3_(j3), key4_(j4) {}
NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) :
Base(noiseModel) {
keys_.resize(4);
keys_[0] = j1;
keys_[1] = j2;
keys_[2] = j3;
keys_[3] = j4;
}
virtual ~NonlinearFactor4() {}
virtual ~NoiseModelFactor4() {}
/** methods to retrieve keys */
inline const KEY1& key1() const { return key1_; }
inline const KEY2& key2() const { return key2_; }
inline const KEY3& key3() const { return key3_; }
inline const KEY4& key4() const { return key4_; }
inline Key key1() const { return keys_[0]; }
inline Key key2() const { return keys_[1]; }
inline Key key3() const { return keys_[2]; }
inline Key key4() const { return keys_[3]; }
/** Calls the 4-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), (*H)[0], (*H)[1], (*H)[2], (*H)[3]);
else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_]);
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]));
} else {
return zero(this->dim());
}
}
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor4("
<< (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ","
<< (std::string) this->key3_ << ","
<< (std::string) this->key4_ << ")\n";
this->noiseModel_->print(" noise model: ");
}
/**
* Override this method to finish implementing a 4-way factor.
* If any of the optional Matrix reference arguments are specified, it should compute
@ -672,46 +601,35 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
ar & BOOST_SERIALIZATION_NVP(key4_);
}
}; // \class NonlinearFactor4
}; // \class NoiseModelFactor4
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 5
* variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4, class KEY5>
class NonlinearFactor5: public NoiseModelFactor<VALUES> {
template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5>
class NoiseModelFactor5: public NoiseModelFactor {
public:
// typedefs for value types pulled from keys
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef typename KEY3::Value X3;
typedef typename KEY4::Value X4;
typedef typename KEY5::Value X5;
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef VALUE3 X3;
typedef VALUE4 X4;
typedef VALUE5 X5;
protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
KEY4 key4_;
KEY5 key5_;
typedef NoiseModelFactor<VALUES> Base;
typedef NonlinearFactor5<VALUES, KEY1, KEY2, KEY3, KEY4, KEY5> This;
typedef NoiseModelFactor Base;
typedef NoiseModelFactor5<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5> This;
public:
/**
* Default Constructor for I/O
*/
NonlinearFactor5() {}
NoiseModelFactor5() {}
/**
* Constructor
@ -721,42 +639,38 @@ public:
* @param j4 key of the fourth variable
* @param j5 key of the fifth variable
*/
NonlinearFactor5(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4, const KEY5& j5) :
Base(noiseModel, make_tuple(j1,j2,j3,j4,j5)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5) {}
NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) :
Base(noiseModel) {
keys_.resize(5);
keys_[0] = j1;
keys_[1] = j2;
keys_[2] = j3;
keys_[3] = j4;
keys_[4] = j5;
}
virtual ~NonlinearFactor5() {}
virtual ~NoiseModelFactor5() {}
/** methods to retrieve keys */
inline const KEY1& key1() const { return key1_; }
inline const KEY2& key2() const { return key2_; }
inline const KEY3& key3() const { return key3_; }
inline const KEY4& key4() const { return key4_; }
inline const KEY5& key5() const { return key5_; }
inline Key key1() const { return keys_[0]; }
inline Key key2() const { return keys_[1]; }
inline Key key3() const { return keys_[2]; }
inline Key key4() const { return keys_[3]; }
inline Key key5() const { return keys_[4]; }
/** Calls the 5-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4]);
else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_]);
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]));
} else {
return zero(this->dim());
}
}
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor5("
<< (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ","
<< (std::string) this->key3_ << ","
<< (std::string) this->key4_ << ","
<< (std::string) this->key5_ << ")\n";
this->noiseModel_->print(" noise model: ");
}
/**
* Override this method to finish implementing a 5-way factor.
* If any of the optional Matrix reference arguments are specified, it should compute
@ -778,49 +692,36 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
ar & BOOST_SERIALIZATION_NVP(key4_);
ar & BOOST_SERIALIZATION_NVP(key5_);
}
}; // \class NonlinearFactor5
}; // \class NoiseModelFactor5
/* ************************************************************************* */
/** A convenient base class for creating your own NoiseModelFactor with 6
* variables. To derive from this class, implement evaluateError(). */
template<class VALUES, class KEY1, class KEY2, class KEY3, class KEY4, class KEY5, class KEY6>
class NonlinearFactor6: public NoiseModelFactor<VALUES> {
template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5, class VALUE6>
class NoiseModelFactor6: public NoiseModelFactor {
public:
// typedefs for value types pulled from keys
typedef typename KEY1::Value X1;
typedef typename KEY2::Value X2;
typedef typename KEY3::Value X3;
typedef typename KEY4::Value X4;
typedef typename KEY5::Value X5;
typedef typename KEY6::Value X6;
typedef VALUE1 X1;
typedef VALUE2 X2;
typedef VALUE3 X3;
typedef VALUE4 X4;
typedef VALUE5 X5;
typedef VALUE6 X6;
protected:
// The values of the keys. Not const to allow serialization
KEY1 key1_;
KEY2 key2_;
KEY3 key3_;
KEY4 key4_;
KEY5 key5_;
KEY6 key6_;
typedef NoiseModelFactor<VALUES> Base;
typedef NonlinearFactor6<VALUES, KEY1, KEY2, KEY3, KEY4, KEY5, KEY6> This;
typedef NoiseModelFactor Base;
typedef NoiseModelFactor6<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6> This;
public:
/**
* Default Constructor for I/O
*/
NonlinearFactor6() {}
NoiseModelFactor6() {}
/**
* Constructor
@ -831,44 +732,40 @@ public:
* @param j5 key of the fifth variable
* @param j6 key of the fifth variable
*/
NonlinearFactor6(const SharedNoiseModel& noiseModel, const KEY1& j1, const KEY2& j2, const KEY3& j3, const KEY4& j4, const KEY5& j5, const KEY6& j6) :
Base(noiseModel, make_tuple(j1,j2,j3,j4,j5,j6)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5), key6_(j6) {}
NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) :
Base(noiseModel) {
keys_.resize(6);
keys_[0] = j1;
keys_[1] = j2;
keys_[2] = j3;
keys_[3] = j4;
keys_[4] = j5;
keys_[5] = j6;
}
virtual ~NonlinearFactor6() {}
virtual ~NoiseModelFactor6() {}
/** methods to retrieve keys */
inline const KEY1& key1() const { return key1_; }
inline const KEY2& key2() const { return key2_; }
inline const KEY3& key3() const { return key3_; }
inline const KEY4& key4() const { return key4_; }
inline const KEY5& key5() const { return key5_; }
inline const KEY6& key6() const { return key6_; }
inline Key key1() const { return keys_[0]; }
inline Key key2() const { return keys_[1]; }
inline Key key3() const { return keys_[2]; }
inline Key key4() const { return keys_[3]; }
inline Key key5() const { return keys_[4]; }
inline Key key6() const { return keys_[5]; }
/** Calls the 6-key specific version of evaluateError, which is pure virtual
* so must be implemented in the derived class. */
virtual Vector unwhitenedError(const VALUES& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
virtual Vector unwhitenedError(const Values& x, boost::optional<std::vector<Matrix>&> H = boost::none) const {
if(this->active(x)) {
if(H)
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_], (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]), (*H)[0], (*H)[1], (*H)[2], (*H)[3], (*H)[4], (*H)[5]);
else
return evaluateError(x[key1_], x[key2_], x[key3_], x[key4_], x[key5_], x[key6_]);
return evaluateError(x.at<X1>(keys_[0]), x.at<X2>(keys_[1]), x.at<X3>(keys_[2]), x.at<X4>(keys_[3]), x.at<X5>(keys_[4]), x.at<X6>(keys_[5]));
} else {
return zero(this->dim());
}
}
/** Print */
virtual void print(const std::string& s = "") const {
std::cout << s << ": NonlinearFactor6("
<< (std::string) this->key1_ << ","
<< (std::string) this->key2_ << ","
<< (std::string) this->key3_ << ","
<< (std::string) this->key4_ << ","
<< (std::string) this->key5_ << ","
<< (std::string) this->key6_ << ")\n";
this->noiseModel_->print(" noise model: ");
}
/**
* Override this method to finish implementing a 6-way factor.
* If any of the optional Matrix reference arguments are specified, it should compute
@ -891,14 +788,8 @@ private:
void serialize(ARCHIVE & ar, const unsigned int version) {
ar & boost::serialization::make_nvp("NoiseModelFactor",
boost::serialization::base_object<Base>(*this));
ar & BOOST_SERIALIZATION_NVP(key1_);
ar & BOOST_SERIALIZATION_NVP(key2_);
ar & BOOST_SERIALIZATION_NVP(key3_);
ar & BOOST_SERIALIZATION_NVP(key4_);
ar & BOOST_SERIALIZATION_NVP(key5_);
ar & BOOST_SERIALIZATION_NVP(key6_);
}
}; // \class NonlinearFactor6
}; // \class NoiseModelFactor6
/* ************************************************************************* */

50
gtsam/nonlinear/NonlinearFactorGraph-inl.h → gtsam/nonlinear/NonlinearFactorGraph.cpp
View File

@ -10,39 +10,40 @@
* -------------------------------------------------------------------------- */
/**
* @file NonlinearFactorGraph-inl.h
* @file NonlinearFactorGraph.cpp
* @brief Factor Graph Consisting of non-linear factors
* @author Frank Dellaert
* @author Carlos Nieto
* @author Christian Potthast
*/
#pragma once
#include <cmath>
#include <boost/foreach.hpp>
#include <gtsam/inference/FactorGraph.h>
#include <gtsam/inference/inference.h>
#include <boost/foreach.hpp>
#include <cmath>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class VALUES>
double NonlinearFactorGraph<VALUES>::probPrime(const VALUES& c) const {
double NonlinearFactorGraph::probPrime(const Values& c) const {
return exp(-0.5 * error(c));
}
/* ************************************************************************* */
template<class VALUES>
void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
Base::print(str);
void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const {
cout << str << "size: " << size() << endl;
for (size_t i = 0; i < factors_.size(); i++) {
stringstream ss;
ss << "factor " << i << ": ";
if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter);
}
}
/* ************************************************************************* */
template<class VALUES>
double NonlinearFactorGraph<VALUES>::error(const VALUES& c) const {
double NonlinearFactorGraph::error(const Values& c) const {
double total_error = 0.;
// iterate over all the factors_ to accumulate the log probabilities
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
@ -53,9 +54,8 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class VALUES>
std::set<Symbol> NonlinearFactorGraph<VALUES>::keys() const {
std::set<Symbol> keys;
std::set<Key> NonlinearFactorGraph::keys() const {
std::set<Key> keys;
BOOST_FOREACH(const sharedFactor& factor, this->factors_) {
if(factor)
keys.insert(factor->begin(), factor->end());
@ -64,9 +64,8 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class VALUES>
Ordering::shared_ptr NonlinearFactorGraph<VALUES>::orderingCOLAMD(
const VALUES& config) const {
Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD(
const Values& config) const {
// Create symbolic graph and initial (iterator) ordering
SymbolicFactorGraph::shared_ptr symbolic;
@ -91,8 +90,7 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class VALUES>
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::symbolic(const Ordering& ordering) const {
SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const {
// Generate the symbolic factor graph
SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph);
symbolicfg->reserve(this->size());
@ -108,21 +106,19 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class VALUES>
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph<
VALUES>::symbolic(const VALUES& config) const {
pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic(
const Values& config) const {
// Generate an initial key ordering in iterator order
Ordering::shared_ptr ordering(config.orderingArbitrary());
return make_pair(symbolic(*ordering), ordering);
}
/* ************************************************************************* */
template<class VALUES>
typename GaussianFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::linearize(
const VALUES& config, const Ordering& ordering) const {
GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(
const Values& config, const Ordering& ordering) const {
// create an empty linear FG
typename GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph);
linearFG->reserve(this->size());
// linearize all factors

53
gtsam/nonlinear/NonlinearFactorGraph.h
View File

@ -28,42 +28,37 @@
namespace gtsam {
/**
* A non-linear factor graph is templated on a values structure, but the factor type
* is fixed as a NonlinearFactor. The values structures are typically (in SAM) more general
* A non-linear factor graph is a graph of non-Gaussian, i.e. non-linear factors,
* which derive from NonlinearFactor. The values structures are typically (in SAM) more general
* than just vectors, e.g., Rot3 or Pose3, which are objects in non-linear manifolds.
* Linearizing the non-linear factor graph creates a linear factor graph on the
* tangent vector space at the linearization point. Because the tangent space is a true
* vector space, the config type will be an VectorValues in that linearized factor graph.
* \nosubgrouping
*/
template<class VALUES>
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<VALUES> > {
class NonlinearFactorGraph: public FactorGraph<NonlinearFactor> {
public:
typedef VALUES Values;
typedef FactorGraph<NonlinearFactor<VALUES> > Base;
typedef boost::shared_ptr<NonlinearFactorGraph<VALUES> > shared_ptr;
typedef boost::shared_ptr<NonlinearFactor<VALUES> > sharedFactor;
/// @name Testable
/// @{
typedef FactorGraph<NonlinearFactor> Base;
typedef boost::shared_ptr<NonlinearFactorGraph> shared_ptr;
typedef boost::shared_ptr<NonlinearFactor> sharedFactor;
/** print just calls base class */
void print(const std::string& str = "NonlinearFactorGraph: ") const;
/// @}
/// @name Standard Interface
/// @{
void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
/** return keys in some random order */
std::set<Symbol> keys() const;
std::set<Key> keys() const;
/** unnormalized error */
double error(const VALUES& c) const;
double error(const Values& c) const;
/** Unnormalized probability. O(n) */
double probPrime(const VALUES& c) const;
double probPrime(const Values& c) const;
template<class F>
void add(const F& factor) {
this->push_back(boost::shared_ptr<F>(new F(factor)));
}
/**
* Create a symbolic factor graph using an existing ordering
@ -77,31 +72,20 @@ namespace gtsam {
* ordering is found.
*/
std::pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr>
symbolic(const VALUES& config) const;
symbolic(const Values& config) const;
/**
* Compute a fill-reducing ordering using COLAMD. This returns the
* ordering and a VariableIndex, which can later be re-used to save
* computation.
*/
Ordering::shared_ptr orderingCOLAMD(const VALUES& config) const;
Ordering::shared_ptr orderingCOLAMD(const Values& config) const;
/**
* linearize a nonlinear factor graph
*/
boost::shared_ptr<GaussianFactorGraph >
linearize(const VALUES& config, const Ordering& ordering) const;
/// @}
/// @name Advanced Interface
/// @{
template<class F>
void add(const F& factor) {
this->push_back(boost::shared_ptr<F>(new F(factor)));
}
/// @}
linearize(const Values& config, const Ordering& ordering) const;
private:
@ -116,4 +100,3 @@ namespace gtsam {
} // namespace
#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>

22
gtsam/nonlinear/NonlinearISAM-inl.h
View File

@ -29,8 +29,8 @@ using namespace std;
using namespace gtsam;
/* ************************************************************************* */
template<class VALUES, class GRAPH>
void NonlinearISAM<VALUES,GRAPH>::update(const Factors& newFactors,
template<class GRAPH>
void NonlinearISAM<GRAPH>::update(const Factors& newFactors,
const Values& initialValues) {
if(newFactors.size() > 0) {
@ -50,7 +50,7 @@ void NonlinearISAM<VALUES,GRAPH>::update(const Factors& newFactors,
// Augment ordering
// FIXME: should just loop over new values
BOOST_FOREACH(const typename Factors::sharedFactor& factor, newFactors)
BOOST_FOREACH(const Symbol& key, factor->keys())
BOOST_FOREACH(Key key, factor->keys())
ordering_.tryInsert(key, ordering_.nVars()); // will do nothing if already present
boost::shared_ptr<GaussianFactorGraph> linearizedNewFactors(
@ -62,8 +62,8 @@ void NonlinearISAM<VALUES,GRAPH>::update(const Factors& newFactors,
}
/* ************************************************************************* */
template<class VALUES, class GRAPH>
void NonlinearISAM<VALUES,GRAPH>::reorder_relinearize() {
template<class GRAPH>
void NonlinearISAM<GRAPH>::reorder_relinearize() {
// cout << "Reordering, relinearizing..." << endl;
@ -89,8 +89,8 @@ void NonlinearISAM<VALUES,GRAPH>::reorder_relinearize() {
}
/* ************************************************************************* */
template<class VALUES, class GRAPH>
VALUES NonlinearISAM<VALUES,GRAPH>::estimate() const {
template<class GRAPH>
Values NonlinearISAM<GRAPH>::estimate() const {
if(isam_.size() > 0)
return linPoint_.retract(optimize(isam_), ordering_);
else
@ -98,14 +98,14 @@ VALUES NonlinearISAM<VALUES,GRAPH>::estimate() const {
}
/* ************************************************************************* */
template<class VALUES, class GRAPH>
Matrix NonlinearISAM<VALUES,GRAPH>::marginalCovariance(const Symbol& key) const {
template<class GRAPH>
Matrix NonlinearISAM<GRAPH>::marginalCovariance(Key key) const {
return isam_.marginalCovariance(ordering_[key]);
}
/* ************************************************************************* */
template<class VALUES, class GRAPH>
void NonlinearISAM<VALUES,GRAPH>::print(const std::string& s) const {
template<class GRAPH>
void NonlinearISAM<GRAPH>::print(const std::string& s) const {
cout << "ISAM - " << s << ":" << endl;
cout << " ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl;
isam_.print("GaussianISAM");

7
gtsam/nonlinear/NonlinearISAM.h
View File

@ -24,11 +24,10 @@ namespace gtsam {
/**
* Wrapper class to manage ISAM in a nonlinear context
*/
template<class VALUES, class GRAPH = gtsam::NonlinearFactorGraph<VALUES> >
template<class GRAPH = gtsam::NonlinearFactorGraph >
class NonlinearISAM {
public:
typedef VALUES Values;
typedef GRAPH Factors;
protected:
@ -71,7 +70,7 @@ public:
Values estimate() const;
/** find the marginal covariance for a single variable */
Matrix marginalCovariance(const Symbol& key) const;
Matrix marginalCovariance(Key key) const;
// access
@ -105,7 +104,7 @@ public:
void reorder_relinearize();
/** manually add a key to the end of the ordering */
void addKey(const Symbol& key) { ordering_.push_back(key); }
void addKey(Key key) { ordering_.push_back(key); }
/** replace the current ordering */
void setOrdering(const Ordering& new_ordering) { ordering_ = new_ordering; }

36
gtsam/nonlinear/NonlinearOptimization-inl.h
View File

@ -34,17 +34,17 @@ namespace gtsam {
/**
* The Elimination solver
*/
template<class G, class T>
T optimizeSequential(const G& graph, const T& initialEstimate,
template<class G>
Values optimizeSequential(const G& graph, const Values& initialEstimate,
const NonlinearOptimizationParameters& parameters, bool useLM) {
// Use a variable ordering from COLAMD
Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
// Create an nonlinear Optimizer that uses a Sequential Solver
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
typedef NonlinearOptimizer<G, GaussianFactorGraph, GaussianSequentialSolver> Optimizer;
Optimizer optimizer(boost::make_shared<const G>(graph),
boost::make_shared<const T>(initialEstimate), ordering,
boost::make_shared<const Values>(initialEstimate), ordering,
boost::make_shared<NonlinearOptimizationParameters>(parameters));
// Now optimize using either LM or GN methods.
@ -57,17 +57,17 @@ namespace gtsam {
/**
* The multifrontal solver
*/
template<class G, class T>
T optimizeMultiFrontal(const G& graph, const T& initialEstimate,
template<class G>
Values optimizeMultiFrontal(const G& graph, const Values& initialEstimate,
const NonlinearOptimizationParameters& parameters, bool useLM) {
// Use a variable ordering from COLAMD
Ordering::shared_ptr ordering = graph.orderingCOLAMD(initialEstimate);
// Create an nonlinear Optimizer that uses a Multifrontal Solver
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
typedef NonlinearOptimizer<G, GaussianFactorGraph, GaussianMultifrontalSolver> Optimizer;
Optimizer optimizer(boost::make_shared<const G>(graph),
boost::make_shared<const T>(initialEstimate), ordering,
boost::make_shared<const Values>(initialEstimate), ordering,
boost::make_shared<NonlinearOptimizationParameters>(parameters));
// now optimize using either LM or GN methods
@ -81,20 +81,20 @@ namespace gtsam {
/**
* The sparse preconditioned conjugate gradient solver
*/
template<class G, class T>
T optimizeSPCG(const G& graph, const T& initialEstimate,
template<class G>
Values optimizeSPCG(const G& graph, const Values& initialEstimate,
const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
bool useLM = true) {
// initial optimization state is the same in both cases tested
typedef SubgraphSolver<G,GaussianFactorGraph,T> Solver;
typedef SubgraphSolver<G,GaussianFactorGraph,Values> Solver;
typedef boost::shared_ptr<Solver> shared_Solver;
typedef NonlinearOptimizer<G, T, GaussianFactorGraph, Solver> SPCGOptimizer;
typedef NonlinearOptimizer<G, GaussianFactorGraph, Solver> SPCGOptimizer;
shared_Solver solver = boost::make_shared<Solver>(
graph, initialEstimate, IterativeOptimizationParameters());
SPCGOptimizer optimizer(
boost::make_shared<const G>(graph),
boost::make_shared<const T>(initialEstimate),
boost::make_shared<const Values>(initialEstimate),
solver->ordering(),
solver,
boost::make_shared<NonlinearOptimizationParameters>(parameters));
@ -110,20 +110,20 @@ namespace gtsam {
/**
* optimization that returns the values
*/
template<class G, class T>
T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters,
template<class G>
Values optimize(const G& graph, const Values& initialEstimate, const NonlinearOptimizationParameters& parameters,
const LinearSolver& solver,
const NonlinearOptimizationMethod& nonlinear_method) {
switch (solver) {
case SEQUENTIAL:
return optimizeSequential<G,T>(graph, initialEstimate, parameters,
return optimizeSequential<G>(graph, initialEstimate, parameters,
nonlinear_method == LM);
case MULTIFRONTAL:
return optimizeMultiFrontal<G,T>(graph, initialEstimate, parameters,
return optimizeMultiFrontal<G>(graph, initialEstimate, parameters,
nonlinear_method == LM);
#if ENABLE_SPCG
case SPCG:
// return optimizeSPCG<G,T>(graph, initialEstimate, parameters,
// return optimizeSPCG<G,Values>(graph, initialEstimate, parameters,
// nonlinear_method == LM);
throw runtime_error("optimize: SPCG not supported yet due to the specific pose constraint");
#endif

4
gtsam/nonlinear/NonlinearOptimization.h
View File

@ -44,8 +44,8 @@ namespace gtsam {
/**
* optimization that returns the values
*/
template<class G, class T>
T optimize(const G& graph, const T& initialEstimate,
template<class G>
Values optimize(const G& graph, const Values& initialEstimate,
const NonlinearOptimizationParameters& parameters = NonlinearOptimizationParameters(),
const LinearSolver& solver = MULTIFRONTAL,
const NonlinearOptimizationMethod& nonlinear_method = LM);

2
gtsam/nonlinear/NonlinearOptimizationParameters.cpp
View File

@ -102,7 +102,7 @@ NonlinearOptimizationParameters::newLambda(double lambda) {
/* ************************************************************************* */
NonlinearOptimizationParameters::shared_ptr
NonlinearOptimizationParameters::newDrecreaseThresholds(double absDecrease,
NonlinearOptimizationParameters::newDecreaseThresholds(double absDecrease,
double relDecrease) {
shared_ptr ptr(boost::make_shared<NonlinearOptimizationParameters>());
ptr->absDecrease_ = absDecrease;

2
gtsam/nonlinear/NonlinearOptimizationParameters.h
View File

@ -110,7 +110,7 @@ namespace gtsam {
static shared_ptr newLambda(double lambda);
/// a copy of old instance except new thresholds
static shared_ptr newDrecreaseThresholds(double absDecrease,
static shared_ptr newDecreaseThresholds(double absDecrease,
double relDecrease);
/// a copy of old instance except new QR flag

42
gtsam/nonlinear/NonlinearOptimizer-inl.h
View File

@ -30,8 +30,8 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W>::NonlinearOptimizer(shared_graph graph,
shared_values values, shared_ordering ordering, shared_parameters parameters) :
graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering),
parameters_(parameters), iterations_(0),
@ -47,8 +47,8 @@ namespace gtsam {
/* ************************************************************************* */
// FIXME: remove this constructor
template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W>::NonlinearOptimizer(shared_graph graph,
shared_values values, shared_ordering ordering,
shared_solver spcg_solver, shared_parameters parameters) :
graph_(graph), values_(values), error_(graph->error(*values)), ordering_(ordering),
@ -66,8 +66,8 @@ namespace gtsam {
/* ************************************************************************* */
// One iteration of Gauss Newton
/* ************************************************************************* */
template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterate() const {
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterate() const {
Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
@ -86,7 +86,7 @@ namespace gtsam {
if (verbosity >= Parameters::DELTA) delta.print("delta");
// take old values and update it
shared_values newValues(new C(values_->retract(delta, *ordering_)));
shared_values newValues(new Values(values_->retract(delta, *ordering_)));
// maybe show output
if (verbosity >= Parameters::VALUES) newValues->print("newValues");
@ -99,8 +99,8 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class G, class C, class L, class S, class W>
NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::gaussNewton() const {
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::gaussNewton() const {
static W writer(error_);
if (error_ < parameters_->sumError_ ) {
@ -130,8 +130,8 @@ namespace gtsam {
// TODO: in theory we can't infinitely recurse, but maybe we should put a max.
// Reminder: the parameters are Graph type $G$, Values class type $T$,
// linear system class $L$, the non linear solver type $S$, and the writer type $W$
template<class G, class T, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::try_lambda(const L& linearSystem) {
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::try_lambda(const L& linearSystem) {
const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
const Parameters::LambdaMode lambdaMode = parameters_->lambdaMode_ ;
@ -178,7 +178,7 @@ namespace gtsam {
if (verbosity >= Parameters::TRYDELTA) delta.print("delta");
// update values
shared_values newValues(new T(values_->retract(delta, *ordering_)));
shared_values newValues(new Values(values_->retract(delta, *ordering_)));
// create new optimization state with more adventurous lambda
double error = graph_->error(*newValues);
@ -228,8 +228,8 @@ namespace gtsam {
// One iteration of Levenberg Marquardt
// Reminder: the parameters are Graph type $G$, Values class type $T$,
// linear system class $L$, the non linear solver type $S$, and the writer type $W$
template<class G, class T, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::iterateLM() {
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterateLM() {
const Parameters::verbosityLevel verbosity = parameters_->verbosity_ ;
const double lambda = parameters_->lambda_ ;
@ -253,8 +253,8 @@ namespace gtsam {
/* ************************************************************************* */
// Reminder: the parameters are Graph type $G$, Values class type $T$,
// linear system class $L$, the non linear solver type $S$, and the writer type $W$
template<class G, class T, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::levenbergMarquardt() {
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::levenbergMarquardt() {
// Initialize
bool converged = false;
@ -299,20 +299,20 @@ namespace gtsam {
}
/* ************************************************************************* */
template<class G, class T, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::iterateDogLeg() {
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::iterateDogLeg() {
S solver(*graph_->linearize(*values_, *ordering_), parameters_->useQR_);
DoglegOptimizerImpl::IterationResult result = DoglegOptimizerImpl::Iterate(
parameters_->lambda_, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *solver.eliminate(),
*graph_, *values_, *ordering_, error_, parameters_->verbosity_ > Parameters::ERROR);
shared_values newValues(new T(values_->retract(result.dx_d, *ordering_)));
shared_values newValues(new Values(values_->retract(result.dx_d, *ordering_)));
return newValuesErrorLambda_(newValues, result.f_error, result.Delta);
}
/* ************************************************************************* */
template<class G, class T, class L, class S, class W>
NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::dogLeg() {
template<class G, class L, class S, class W>
NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::dogLeg() {
static W writer(error_);
// check if we're already close enough

46
gtsam/nonlinear/NonlinearOptimizer.h
View File

@ -37,20 +37,20 @@ public:
* and then one of the optimization routines is called. These iterate
* until convergence. All methods are functional and return a new state.
*
* The class is parameterized by the Graph type $G$, Values class type $T$,
* The class is parameterized by the Graph type $G$, Values class type $Values$,
* linear system class $L$, the non linear solver type $S$, and the writer type $W$
*
* The values class type $T$ is in order to be able to optimize over non-vector values structures.
* The values class type $Values$ is in order to be able to optimize over non-vector values structures.
*
* A nonlinear system solver $S$
* Concept NonLinearSolver<G,T,L> implements
* linearize: G * T -> L
* solve : L -> T
* Concept NonLinearSolver<G,Values,L> implements
* linearize: G * Values -> L
* solve : L -> Values
*
* The writer $W$ generates output to disk or the screen.
*
* For example, in a 2D case, $G$ can be Pose2Graph, $T$ can be Pose2Values,
* $L$ can be GaussianFactorGraph and $S$ can be Factorization<Pose2Graph, Pose2Values>.
* For example, in a 2D case, $G$ can be pose2SLAM::Graph, $Values$ can be Pose2Values,
* $L$ can be GaussianFactorGraph and $S$ can be Factorization<pose2SLAM::Graph, Pose2Values>.
* The solver class has two main functions: linearize and optimize. The first one
* linearizes the nonlinear cost function around the current estimate, and the second
* one optimizes the linearized system using various methods.
@ -58,12 +58,12 @@ public:
* To use the optimizer in code, include <gtsam/NonlinearOptimizer-inl.h> in your cpp file
* \nosubgrouping
*/
template<class G, class T, class L = GaussianFactorGraph, class GS = GaussianMultifrontalSolver, class W = NullOptimizerWriter>
template<class G, class L = GaussianFactorGraph, class GS = GaussianMultifrontalSolver, class W = NullOptimizerWriter>
class NonlinearOptimizer {
public:
// For performance reasons in recursion, we store values in a shared_ptr
typedef boost::shared_ptr<const T> shared_values; ///Prevent memory leaks in Values
typedef boost::shared_ptr<const Values> shared_values; ///Prevent memory leaks in Values
typedef boost::shared_ptr<const G> shared_graph; /// Prevent memory leaks in Graph
typedef boost::shared_ptr<L> shared_linear; /// Not sure
typedef boost::shared_ptr<const Ordering> shared_ordering; ///ordering parameters
@ -74,7 +74,7 @@ public:
private:
typedef NonlinearOptimizer<G, T, L, GS> This;
typedef NonlinearOptimizer<G, L, GS> This;
typedef boost::shared_ptr<const std::vector<size_t> > shared_dimensions;
/// keep a reference to const version of the graph
@ -182,7 +182,7 @@ public:
/**
* Copy constructor
*/
NonlinearOptimizer(const NonlinearOptimizer<G, T, L, GS> &optimizer) :
NonlinearOptimizer(const NonlinearOptimizer<G, L, GS> &optimizer) :
graph_(optimizer.graph_), values_(optimizer.values_), error_(optimizer.error_),
ordering_(optimizer.ordering_), parameters_(optimizer.parameters_),
iterations_(optimizer.iterations_), dimensions_(optimizer.dimensions_), structure_(optimizer.structure_) {}
@ -242,13 +242,13 @@ public:
/**
* Return mean and covariance on a single variable
*/
Matrix marginalCovariance(Symbol j) const {
Matrix marginalCovariance(Key j) const {
return createSolver()->marginalCovariance((*ordering_)[j]);
}
/**
* linearize and optimize
* This returns an VectorValues, i.e., vectors in tangent space of T
* This returns an VectorValues, i.e., vectors in tangent space of Values
*/
VectorValues linearizeAndOptimizeForDelta() const {
return *createSolver()->optimize();
@ -340,19 +340,19 @@ public:
* Static interface to LM optimization (no shared_ptr arguments) - see above
*/
static shared_values optimizeLM(const G& graph,
const T& values,
const Values& values,
const Parameters parameters = Parameters()) {
return optimizeLM(boost::make_shared<const G>(graph),
boost::make_shared<const T>(values),
boost::make_shared<const Values>(values),
boost::make_shared<Parameters>(parameters));
}
///TODO: comment
static shared_values optimizeLM(const G& graph,
const T& values,
const Values& values,
Parameters::verbosityLevel verbosity) {
return optimizeLM(boost::make_shared<const G>(graph),
boost::make_shared<const T>(values),
boost::make_shared<const Values>(values),
verbosity);
}
@ -392,19 +392,19 @@ public:
* Static interface to Dogleg optimization (no shared_ptr arguments) - see above
*/
static shared_values optimizeDogLeg(const G& graph,
const T& values,
const Values& values,
const Parameters parameters = Parameters()) {
return optimizeDogLeg(boost::make_shared<const G>(graph),
boost::make_shared<const T>(values),
boost::make_shared<const Values>(values),
boost::make_shared<Parameters>(parameters));
}
///TODO: comment
static shared_values optimizeDogLeg(const G& graph,
const T& values,
const Values& values,
Parameters::verbosityLevel verbosity) {
return optimizeDogLeg(boost::make_shared<const G>(graph),
boost::make_shared<const T>(values),
boost::make_shared<const Values>(values),
verbosity);
}
@ -431,9 +431,9 @@ public:
/**
* Static interface to GN optimization (no shared_ptr arguments) - see above
*/
static shared_values optimizeGN(const G& graph, const T& values, const Parameters parameters = Parameters()) {
static shared_values optimizeGN(const G& graph, const Values& values, const Parameters parameters = Parameters()) {
return optimizeGN(boost::make_shared<const G>(graph),
boost::make_shared<const T>(values),
boost::make_shared<const Values>(values),
boost::make_shared<Parameters>(parameters));
}
};

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