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

.cproject

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

.project

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

CppUnitLite/Makefile.am

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

Makefile.am

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

aminclude.am

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

autogen.sh

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

configure.ac

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

examples/CameraResectioning.cpp

@@ -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> {
+class ResectioningFactor: public NoiseModelFactor1<Pose3> {
-  typedef NonlinearFactor1<PoseValues, PoseKey> Base;
+  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 =
+  virtual ~ResectioningFactor() {}
-      boost::none) const {
+
+  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> (
+  Values result = optimize<NonlinearFactorGraph> (graph, initial);
-      graph, initial);
   result.print("Final result: ");
 
   return 0;

examples/Makefile.am

@@ -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 ./%
-#----------------------------------------------------------------------------------------------------

examples/PlanarSLAMExample_easy.cpp

@@ -36,9 +36,6 @@ 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
 	Graph graph;
@@ -47,14 +44,14 @@ 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
 
 	/* 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(1, 2, odom_measurement, odom_model);
-	graph.addOdometry(x2, x3, 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(1, 1, bearing11, range11, meas_model);
-	graph.addBearingRange(x2, l1, bearing21, range21, meas_model);
+	graph.addBearingRange(2, 1, bearing21, range21, meas_model);
-	graph.addBearingRange(x3, l2, bearing32, range32, 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.insertPose(1, Pose2(0.5, 0.0, 0.2));
-	initialEstimate.insert(x2, Pose2(2.3, 0.1,-0.2));
+	initialEstimate.insertPose(2, Pose2(2.3, 0.1,-0.2));
-	initialEstimate.insert(x3, Pose2(4.1, 0.1, 0.1));
+	initialEstimate.insertPose(3, Pose2(4.1, 0.1, 0.1));
-	initialEstimate.insert(l1, Point2(1.8, 2.1));
+	initialEstimate.insertPoint(1, Point2(1.8, 2.1));
-	initialEstimate.insert(l2, Point2(4.1, 1.8));
+	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;

examples/PlanarSLAMSelfContained_advanced.cpp

@@ -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::NonlinearOptimizer<gtsam::NonlinearFactorGraph,gtsam::GaussianFactorGraph,gtsam::GaussianSequentialSolver> OptimizerSeqential;   // optimization engine for this domain
-typedef gtsam::TypedSymbol<gtsam::Point2,'l'> PointKey;      // Key for points, with type included
+typedef gtsam::NonlinearOptimizer<gtsam::NonlinearFactorGraph,gtsam::GaussianFactorGraph,gtsam::GaussianMultifrontalSolver> OptimizerMultifrontal;   // optimization engine for this domain
-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
 
 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);
+	Symbol x1('x',1), x2('x',2), x3('x',3);
-	PointKey l1(1), l2(2);
+	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<Pose2> odom12(x1, x2, odom_measurement, odom_model);
-	BetweenFactor<PlanarValues,PoseKey> odom23(x2, x3, 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<Pose2, Point2> meas11(x1, l1, bearing11, range11, meas_model);
-	BearingRangeFactor<PlanarValues, PoseKey, PointKey> meas21(x2, l1, bearing21, range21, meas_model);
+	BearingRangeFactor<Pose2, Point2> meas21(x2, l1, bearing21, range21, meas_model);
-	BearingRangeFactor<PlanarValues, PoseKey, PointKey> meas32(x3, l2, bearing32, range32, 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));

examples/Pose2SLAMExample_advanced.cpp

@@ -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(1, 2, odom_measurement, odom_model);
-	graph->addOdometry(x2, x3, 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->insertPose(1, Pose2(0.5, 0.0, 0.2));
-	initial->insert(x2, Pose2(2.3, 0.1,-0.2));
+	initial->insertPose(2, Pose2(2.3, 0.1,-0.2));
-	initial->insert(x3, Pose2(4.1, 0.1, 0.1));
+	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 covariance1  = optimizer_result.marginalCovariance(PoseKey(1));
-	Matrix covariance2  = optimizer_result.marginalCovariance(x2);
+	Matrix covariance2  = optimizer_result.marginalCovariance(PoseKey(1));
 
 	print(covariance1, "Covariance1");
 	print(covariance2, "Covariance2");

examples/Pose2SLAMExample_easy.cpp

@@ -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(1, 2, odom_measurement, odom_model);
-	graph.addOdometry(x2, x3, 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.insertPose(1, Pose2(0.5, 0.0, 0.2));
-	initial.insert(x2, Pose2(2.3, 0.1,-0.2));
+	initial.insertPose(2, Pose2(2.3, 0.1,-0.2));
-	initial.insert(x3, Pose2(4.1, 0.1, 0.1));
+	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");
 
 

examples/Pose2SLAMwSPCG_advanced.cpp

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

examples/Pose2SLAMwSPCG_easy.cpp

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

examples/SimpleRotation.cpp

@@ -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/Symbol.h>
-#include <gtsam/nonlinear/Values.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);
+	Symbol key('x',1);
-	PriorFactor<RotValues, Key> factor(key, prior, model);
+	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;

examples/easyPoint2KalmanFilter.cpp

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

examples/elaboratePoint2KalmanFilter.cpp

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

examples/matlab/PlanarSLAMExample_easy.m

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

examples/matlab/Pose2SLAMExample_advanced.m

@@ -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 = pose2SLAMOptimizer(graph,initialEstimate,ord,params);                      
+%result.print('final result');
 
-result.print('final result');        
 % %     
 % % disp('\\\');
 % % 
 % % %% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
-% % result = graph.optimize(initialEstimate);
+result = graph.optimize(initialEstimate);
-% % result.print('final result');
+result.print('final result');
 
 %% Get the corresponding dense matrix
 ord = graph.orderingCOLAMD(result);

examples/matlab/Pose2SLAMExample_easy.m

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

examples/vSLAMexample/Makefile.am

@@ -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 ./$^
-#----------------------------------------------------------------------------------------------------

examples/vSLAMexample/vISAMexample.cpp

@@ -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 = shared_ptr<Values> (new Values());
-  initialValues->insert(pose_id, pose);
+  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: ");
   }

examples/vSLAMexample/vSFMexample.cpp

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

gtsam.h

@@ -251,13 +251,13 @@ class SharedDiagonal {
 };
 
 class SharedNoiseModel {
-	static gtsam::SharedNoiseModel sharedSigmas(Vector sigmas);
+	static gtsam::SharedNoiseModel Sigmas(Vector sigmas);
-	static gtsam::SharedNoiseModel sharedSigma(size_t dim, double sigma);
+	static gtsam::SharedNoiseModel Sigma(size_t dim, double sigma);
-	static gtsam::SharedNoiseModel sharedPrecisions(Vector precisions);
+	static gtsam::SharedNoiseModel Precisions(Vector precisions);
-	static gtsam::SharedNoiseModel sharedPrecision(size_t dim, double precision);
+	static gtsam::SharedNoiseModel Precision(size_t dim, double precision);
-	static gtsam::SharedNoiseModel sharedUnit(size_t dim);
+	static gtsam::SharedNoiseModel Unit(size_t dim);
-	static gtsam::SharedNoiseModel sharedSqrtInformation(Matrix R);
+	static gtsam::SharedNoiseModel SqrtInformation(Matrix R);
-	static gtsam::SharedNoiseModel sharedCovariance(Matrix covariance);
+	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{
+//class gtsam::pose2SLAM::Graph{
-//	Pose2Graph();
+//	pose2SLAM::Graph();
 //	void print(string s) const;
 //	GaussianFactorGraph* linearize_(const gtsam::Pose2Values& config) const;
 //	void push_back(Pose2Factor* factor);

gtsam/3rdparty/Makefile.am

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

gtsam/Makefile.am

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

gtsam/base/DerivedValue.h

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

gtsam/base/FastList.h

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

gtsam/base/FastMap.h

@@ -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 {
 
@@ -57,7 +58,7 @@ public:
   friend class boost::serialization::access;
   template<class ARCHIVE>
   void serialize(ARCHIVE & ar, const unsigned int version) {
-    ar & boost::serialization::base_object<Base>(*this);
+    ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(Base);
   }
 };
 

gtsam/base/FastSet.h

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

gtsam/base/FastVector.h

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

gtsam/base/Lie.h

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

gtsam/base/LieVector.h

@@ -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() {}

gtsam/base/Makefile.am

@@ -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 ./$^
-#----------------------------------------------------------------------------------------------------

gtsam/base/Testable.h

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

gtsam/base/Value.h

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

gtsam/base/Vector.cpp

@@ -43,14 +43,13 @@ namespace gtsam {
 /* ************************************************************************* */
 void odprintf_(const char *format, ostream& stream, ...) {
 	char buf[4096], *p = buf;
-	int     n;
 
 	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, ...)
+void odprintf(const char *format, ...) {
-{
 	char buf[4096], *p = buf;
-	int     n;
 
 	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;
 }
 
 /* ************************************************************************* */

gtsam/base/serializationTestHelpers.h

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

gtsam/base/tests/testSerializationBase.cpp

@@ -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); }
+/* ************************************************************************* */

gtsam/geometry/Cal3Bundler.h

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

gtsam/geometry/Cal3DS2.h

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

gtsam/geometry/Cal3_S2.h

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

gtsam/geometry/Cal3_S2Stereo.h

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

gtsam/geometry/CalibratedCamera.h

@@ -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 {
+		virtual void print(const std::string& s = "") const {
-            pose_.print(); 
+			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_);
 	    }
 

gtsam/geometry/Makefile.am

@@ -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 ./$^
-#----------------------------------------------------------------------------------------------------

gtsam/geometry/PinholeCamera.h

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

gtsam/geometry/Point2.h

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

gtsam/geometry/Point3.h

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

gtsam/geometry/Pose2.h

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

gtsam/geometry/Pose3.h

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

gtsam/geometry/Rot2.h

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

gtsam/geometry/Rot3.h

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

gtsam/geometry/StereoCamera.h

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

gtsam/geometry/StereoPoint2.h

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

gtsam/geometry/tests/testSerializationGeometry.cpp

@@ -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); }
+/* ************************************************************************* */

gtsam/inference/ClusterTree.h

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

gtsam/inference/Conditional.h

@@ -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 KEY KeyType;
-  typedef Conditional<Key> This;
+  typedef Conditional<KeyType> This;
-  typedef Factor<Key> Base;
+  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(); }
+	KeyType firstFrontalKey() const { assert(nrFrontals_>0); return FactorType::front(); }
-	Key lastFrontalKey() const { assert(nrFrontals_>0); return *(endFrontals()-1); }
+	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;
 }
 

gtsam/inference/Factor-inl.h

@@ -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::multiset < KeyType > nonunique(keys_.begin(), keys_.end());
-		std::set < Key > unique(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(

gtsam/inference/Factor.h

@@ -55,28 +55,28 @@ class Factor {
 
 public:
 
-  typedef KEY Key; ///< The KEY template parameter
+  typedef KEY KeyType; ///< The KEY template parameter
-  typedef Factor<Key> This; ///< This class
+  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) {
+	Factor(const std::set<KeyType>& keys) {
-		BOOST_FOREACH(const Key& key, keys) keys_.push_back(key);
+		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

gtsam/inference/IndexConditional.cpp

@@ -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()) {
+    BOOST_FOREACH(KeyType& parent, parents()) {
-      Key newParent = inversePermutation[parent];
+      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 KeyType frontal, this->frontals()) {
-      BOOST_FOREACH(const Key separator, this->parents()) {
+      BOOST_FOREACH(const KeyType separator, this->parents()) {
         assert(inversePermutation[frontal] < inversePermutation[separator]);
       }
     }

gtsam/inference/Makefile.am

@@ -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 ./$^
-

gtsam/inference/graph-inl.h

@@ -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));
+		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_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>
+template<class G, class Factor, class POSE, class KEY>
-boost::shared_ptr<VALUES> composePoses(const G& graph, const PredecessorMap<typename VALUES::Key>& tree,
+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();
+		KEY key1 = factor->key1();
-		typename VALUES::Key key2 = factor->key2();
+		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);
+	boost::shared_ptr<Values> config(new Values);
-	typename VALUES::Key rootKey = boost::get(boost::vertex_name, g, root);
+	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;

gtsam/inference/graph.h

@@ -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>
+	template<class G, class Factor, class POSE, class KEY>
-	boost::shared_ptr<VALUES>
+	boost::shared_ptr<Values>
-		composePoses(const G& graph, const PredecessorMap<typename VALUES::Key>& tree, const POSE& rootPose);
+		composePoses(const G& graph, const PredecessorMap<KEY>& tree, const POSE& rootPose);
 
 
 	/**

gtsam/inference/tests/testFactorGraph.cpp

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

gtsam/inference/tests/testISAM.cpp

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

gtsam/inference/tests/testJunctionTree.cpp

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

gtsam/inference/tests/testSerializationInference.cpp

@@ -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); }
+/* ************************************************************************* */

gtsam/inference/tests/timeSymbolMaps.cpp

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

gtsam/linear/GaussianFactorGraph.cpp

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

gtsam/linear/Makefile.am

@@ -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 ./$^
-

gtsam/linear/SharedNoiseModel.h

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

gtsam/linear/SubgraphSolver-inl.h

@@ -21,8 +21,8 @@ using namespace std;
 
 namespace gtsam {
 
-template<class GRAPH, class LINEAR, class VALUES>
+template<class GRAPH, class LINEAR, class KEY>
-void SubgraphSolver<GRAPH,LINEAR,VALUES>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
+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>
+template<class GRAPH, class LINEAR, class KEY>
-VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,VALUES>::optimize() {
+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>
+template<class GRAPH, class LINEAR, class KEY>
-void SubgraphSolver<GRAPH,LINEAR,VALUES>::initialize(const GRAPH& G, const VALUES& theta0) {
+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_);
+	list<KEY> keys = predecessorMap2Keys(tree_);
-	ordering_ = boost::make_shared<Ordering>(list<Symbol>(keys.begin(), keys.end()));
+	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),
+		Key key1 = Key(eg.first),
-				key2 = Symbol(eg.second) ;
+				key2 = Key(eg.second) ;
 		pairs_.insert(pair<Index, Index>((*ordering_)[key1], (*ordering_)[key2])) ;
 	}
 }

gtsam/linear/VectorValues.h

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

gtsam/linear/tests/testSerializationLinear.cpp

@@ -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); }
+/* ************************************************************************* */

gtsam/linear/tests/timeGaussianFactor.cpp

@@ -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++*/

gtsam/nonlinear/DoglegOptimizerImpl.h

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

gtsam/nonlinear/ExtendedKalmanFilter-inl.h

@@ -27,10 +27,10 @@
 namespace gtsam {
 
 	/* ************************************************************************* */
-	template<class VALUES, class KEY>
+	template<class VALUE>
-	typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::solve_(
+	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>
+	template<class VALUE>
-	ExtendedKalmanFilter<VALUES, KEY>::ExtendedKalmanFilter(T x_initial,
+	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>
+	template<class VALUE>
-	typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::predict(
+	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 x0 = motionFactor.key1();
-		KEY x1 = motionFactor.key2();
+		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>
+	template<class VALUE>
-	typename ExtendedKalmanFilter<VALUES, KEY>::T ExtendedKalmanFilter<VALUES, KEY>::update(
+	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

gtsam/nonlinear/ExtendedKalmanFilter.h

@@ -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 boost::shared_ptr<ExtendedKalmanFilter<VALUE> > shared_ptr;
-		typedef typename KEY::Value T;
+		typedef VALUE T;
-		typedef NonlinearFactor2<VALUES, KEY, KEY> MotionFactor;
+		typedef NoiseModelFactor2<VALUE, VALUE> MotionFactor;
-		typedef NonlinearFactor1<VALUES, KEY> MeasurementFactor;
+		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 Ordering& ordering, const Values& linearizationPoints,
-				const KEY& x, JacobianFactor::shared_ptr& newPrior) const;
+				Key x, JacobianFactor::shared_ptr& newPrior) const;
 
 	public:
 

gtsam/nonlinear/GaussianISAM2-inl.h

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

gtsam/nonlinear/GaussianISAM2.h

@@ -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> >
+template <class GRAPH = NonlinearFactorGraph>
-class GaussianISAM2 : public ISAM2<GaussianConditional, VALUES, GRAPH> {
+class GaussianISAM2 : public ISAM2<GaussianConditional, GRAPH> {
-  typedef ISAM2<GaussianConditional, VALUES, GRAPH> Base;
+  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

gtsam/nonlinear/ISAM2-impl-inl.h

@@ -19,10 +19,10 @@ namespace gtsam {
 
 using namespace std;
 
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-struct ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl {
+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 {
+template<class CONDITIONAL, class GRAPH>
-  Ordering& ordering;
+void ISAM2<CONDITIONAL,GRAPH>::Impl::AddVariables(
-  Permuted<VectorValues>& vconfig;
+    const Values& newTheta, Values& theta, Permuted<VectorValues>& delta, Ordering& ordering, typename Base::Nodes& nodes, const KeyFormatter& keyFormatter) {
-  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) {
   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);
+    Index nextVar = originalnVars;
-    newTheta.apply(vadder);
+    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>
+template<class CONDITIONAL, class GRAPH>
-FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::IndicesFromFactors(const Ordering& ordering, const GRAPH& factors) {
+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 typename NonlinearFactor::shared_ptr& factor, factors) {
-    BOOST_FOREACH(const Symbol& key, factor->keys()) {
+    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>
+template<class CONDITIONAL, class GRAPH>
-FastSet<Index> ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::CheckRelinearization(const Permuted<VectorValues>& delta, const Ordering& ordering, const ISAM2Params::RelinearizationThreshold& relinearizeThreshold) {
+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>
+template<class CONDITIONAL, class GRAPH>
-void ISAM2<CONDITIONAL,VALUES,GRAPH>::Impl::FindAll(typename ISAM2Clique<CONDITIONAL>::shared_ptr clique, FastSet<Index>& keys, const vector<bool>& markedMask) {
+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()
+template<class CONDITIONAL, class GRAPH>
-// This debug version sets delta entries that are applied to "Inf".  The
+void ISAM2<CONDITIONAL, GRAPH>::Impl::ExpmapMasked(Values& values, const Permuted<VectorValues>& delta, const Ordering& ordering,
-// idea is that if a delta is applied, the variable is being relinearized,
+    const vector<bool>& mask, boost::optional<Permuted<VectorValues>&> invalidateIfDebug, const KeyFormatter& keyFormatter) {
-// 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) {
   // 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);
+  assert(values.size() == ordering.size());
-  values.apply(selectiveExpmapper);
+  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>
+template<class CONDITIONAL, class GRAPH>
-typename ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolveResult
+typename ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolveResult
-ISAM2<CONDITIONAL, VALUES, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
+ISAM2<CONDITIONAL, GRAPH>::Impl::PartialSolve(GaussianFactorGraph& factors,
     const FastSet<Index>& keys, const ReorderingMode& reorderingMode) {
 
   static const bool debug = ISDEBUG("ISAM2 recalculate");

gtsam/nonlinear/ISAM2-inl.h

@@ -39,8 +39,8 @@ static const bool disableReordering = false;
 static const double batchThreshold = 0.65;
 
 /* ************************************************************************* */
-template<class CONDITIONAL, class VALUES, class GRAPH>
+template<class CONDITIONAL, class GRAPH>
-ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2(const ISAM2Params& params):
+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>
+template<class CONDITIONAL, class GRAPH>
-ISAM2<CONDITIONAL, VALUES, GRAPH>::ISAM2():
+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>
+template<class CONDITIONAL, class GRAPH>
-FastList<size_t> ISAM2<CONDITIONAL, VALUES, GRAPH>::getAffectedFactors(const FastList<Index>& keys) const {
+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>
+template<class CONDITIONAL, class GRAPH>
-FactorGraph<typename ISAM2<CONDITIONAL, VALUES, GRAPH>::CacheFactor>
+FactorGraph<typename ISAM2<CONDITIONAL, GRAPH>::CacheFactor>
-ISAM2<CONDITIONAL, VALUES, GRAPH>::getCachedBoundaryFactors(Cliques& orphans) {
+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>
+template<class CONDITIONAL, class GRAPH>
-boost::shared_ptr<FastSet<Index> > ISAM2<CONDITIONAL, VALUES, GRAPH>::recalculate(
+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>
+template<class CONDITIONAL, class GRAPH>
-ISAM2Result ISAM2<CONDITIONAL, VALUES, GRAPH>::update(
+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>
+template<class CONDITIONAL, class GRAPH>
-VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateEstimate() const {
+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>
+template<class CONDITIONAL, class GRAPH>
-VALUES ISAM2<CONDITIONAL, VALUES, GRAPH>::calculateBestEstimate() const {
+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>
+template<class CONDITIONAL, class GRAPH>
-VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam) {
+VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam) {
   VectorValues delta = *allocateVectorValues(isam);
   optimize2(isam.root(), delta);
   return delta;

gtsam/nonlinear/ISAM2.h

@@ -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
+      OptimizationParams _optimizationParams = ISAM2GaussNewtonParams(), ///< see ISAM2Params::optimizationParams
-      RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params public variables, ISAM2Params::relinearizeThreshold
+      RelinearizationThreshold _relinearizeThreshold = 0.1, ///< see ISAM2Params::relinearizeThreshold
-      int _relinearizeSkip = 10, ///< see ISAM2Params public variables, ISAM2Params::relinearizeSkip
+      int _relinearizeSkip = 10, ///< see ISAM2Params::relinearizeSkip
-      bool _enableRelinearization = true, ///< see ISAM2Params public variables, ISAM2Params::enableRelinearization
+      bool _enableRelinearization = true, ///< see ISAM2Params::enableRelinearization
-      bool _evaluateNonlinearError = false ///< see ISAM2Params public variables, ISAM2Params::evaluateNonlinearError
+      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 ISAM2<CONDITIONAL> This; ///< This class
-  typedef VALUES Values;
   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>
+template<class CONDITIONAL, class GRAPH>
-VectorValues optimize(const ISAM2<CONDITIONAL,VALUES,GRAPH>& isam);
+VectorValues optimize(const ISAM2<CONDITIONAL, GRAPH>& isam);
 
 } /// namespace gtsam
 

gtsam/nonlinear/Key.cpp

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

gtsam/nonlinear/Key.h

@@ -11,348 +11,30 @@
 
 /**
  * @file Key.h
- * @date Jan 12, 2010
+ * @brief 
- * @author: Frank Dellaert
+ * @author Richard Roberts
- * @author: Richard Roberts
+ * @date Feb 20, 2012
  */
-
 #pragma once
 
-#include <list>
+#include <boost/function.hpp>
-#include <iostream>
+#include <string>
-#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'
 
 namespace gtsam {
 
-/**
+  /// Integer nonlinear key type
- * TypedSymbol key class is templated on
+  typedef size_t Key;
- * 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 {
 
-protected:
+  /// Typedef for a function to format a key, i.e. to convert it to a string
-  size_t j_;
+  typedef boost::function<std::string(Key)> KeyFormatter;
 
-public:
+  // Helper function for DefaultKeyFormatter
+  std::string _defaultKeyFormatter(Key key);
 
-  // typedefs
+  /// The default KeyFormatter, which is used if no KeyFormatter is passed to
-  typedef T Value;
+  /// a nonlinear 'print' function.  Automatically detects plain integer keys
-  typedef boost::mpl::char_<C> Chr;  // to reconstruct the type: use Chr::value
+  /// 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
-

gtsam/nonlinear/Makefile.am

@@ -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 ./$^
-

gtsam/nonlinear/NonlinearEquality.h

@@ -47,11 +47,11 @@ namespace gtsam {
 	 *
 	 * \nosubgrouping
 	 */
-	template<class VALUES, class KEY>
+	template<class VALUE>
-	class NonlinearEquality: public NonlinearFactor1<VALUES, KEY> {
+	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 {
+		virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-			std::cout << "Constraint: " << s << " on [" << (std::string)(this->key_) << "]\n";
+			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 {
+		virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
-			if (!Base::equals(f)) return false;
+		  const This* e = dynamic_cast<const This*>(&f);
-			return feasible_.equals(f.feasible_, tol) &&
+		  return e && Base::equals(f) && feasible_.equals(e->feasible_, tol) &&
-					fabs(error_gain_ - f.error_gain_) < 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 {
+		virtual double error(const Values& c) const {
-			const T& xj = c[this->key_];
+			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 {
+		virtual GaussianFactor::shared_ptr linearize(const Values& x, const Ordering& ordering) const {
-			const T& xj = x[this->key_];
+			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>
+	template<class VALUE>
-	class NonlinearEquality1 : public NonlinearFactor1<VALUES, KEY> {
+	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>
+	template<class VALUE>
-	class NonlinearEquality2 : public NonlinearFactor2<VALUES, KEY, KEY> {
+	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

gtsam/nonlinear/NonlinearFactor.h

@@ -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<Key> {
-class NonlinearFactor: public Factor<Symbol> {
 
 protected:
 
   // Some handy typedefs
-  typedef Factor<Symbol> Base;
+  typedef Factor<Key> Base;
-  typedef NonlinearFactor<VALUES> This;
+  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 {
+  virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-    std::cout << s << ": NonlinearFactor\n";
+    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 {
-class NoiseModelFactor: public NonlinearFactor<VALUES> {
 
 protected:
 
   // handy typedefs
-  typedef NonlinearFactor<VALUES> Base;
+  typedef NonlinearFactor Base;
-  typedef NoiseModelFactor<VALUES> This;
+  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 {
+  virtual void print(const std::string& s = "", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
-    std::cout << s << ": NoiseModelFactor\n";
+    Base::print(s, keyFormatter);
-    std::cout << "  ";
-    BOOST_FOREACH(const Symbol& key, this->keys()) { std::cout << (std::string)key << " "; }
-    std::cout << "\n";
     this->noiseModel_->print("  noise model: ");
   }
 
   /** Check if two factors are equal */
-  virtual bool equals(const NoiseModelFactor<VALUES>& f, double tol = 1e-9) const {
+  virtual bool equals(const NonlinearFactor& f, double tol = 1e-9) const {
-    return noiseModel_->equals(*f.noiseModel_, tol) && Base::equals(f, tol);
+    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>
+template<class VALUE>
-class NonlinearFactor1: public NoiseModelFactor<VALUES> {
+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
+  typedef NoiseModelFactor Base;
-  KEY key_;
+  typedef NoiseModelFactor1<VALUE> This;
-
-  typedef NoiseModelFactor<VALUES> Base;
-  typedef NonlinearFactor1<VALUES, KEY> 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) :
+  NoiseModelFactor1(const SharedNoiseModel& noiseModel, Key key1) :
-    Base(noiseModel, make_tuple(key1)), 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>
+template<class VALUE1, class VALUE2>
-class NonlinearFactor2: public NoiseModelFactor<VALUES> {
+class NoiseModelFactor2: public NoiseModelFactor {
 
 public:
 
   // typedefs for value types pulled from keys
-  typedef typename KEY1::Value X1;
+  typedef VALUE1 X1;
-  typedef typename KEY2::Value X2;
+  typedef VALUE2 X2;
 
 protected:
 
-  // The values of the keys. Not const to allow serialization
+  typedef NoiseModelFactor Base;
-  KEY1 key1_;
+  typedef NoiseModelFactor2<VALUE1, VALUE2> This;
-  KEY2 key2_;
-
-  typedef NoiseModelFactor<VALUES> Base;
-  typedef NonlinearFactor2<VALUES, KEY1, KEY2> 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) :
+  NoiseModelFactor2(const SharedNoiseModel& noiseModel, Key j1, Key j2) :
-    Base(noiseModel, make_tuple(j1,j2)), key1_(j1), key2_(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 Key key1() const { return keys_[0];	}
-  inline const KEY2& key2() const {	return key2_;	}
+  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 X1& x1 = x.at<X1>(keys_[0]);
-      const X2& x2 = x[key2_];
+      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>
+template<class VALUE1, class VALUE2, class VALUE3>
-class NonlinearFactor3: public NoiseModelFactor<VALUES> {
+class NoiseModelFactor3: public NoiseModelFactor {
 
 public:
 
   // typedefs for value types pulled from keys
-  typedef typename KEY1::Value X1;
+  typedef VALUE1 X1;
-  typedef typename KEY2::Value X2;
+  typedef VALUE2 X2;
-  typedef typename KEY3::Value X3;
+  typedef VALUE3 X3;
 
 protected:
 
-  // The values of the keys. Not const to allow serialization
+  typedef NoiseModelFactor Base;
-  KEY1 key1_;
+  typedef NoiseModelFactor3<VALUE1, VALUE2, VALUE3> This;
-  KEY2 key2_;
-  KEY3 key3_;
-
-  typedef NoiseModelFactor<VALUES> Base;
-  typedef NonlinearFactor3<VALUES, KEY1, KEY2, KEY3> 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) :
+  NoiseModelFactor3(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3) :
-    Base(noiseModel, make_tuple(j1,j2,j3)), key1_(j1), key2_(j2), key3_(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 Key key1() const { return keys_[0]; }
-  inline const KEY2& key2() const { return key2_; }
+  inline Key key2() const { return keys_[1]; }
-  inline const KEY3& key3() const { return key3_; }
+  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>
+template<class VALUE1, class VALUE2, class VALUE3, class VALUE4>
-class NonlinearFactor4: public NoiseModelFactor<VALUES> {
+class NoiseModelFactor4: public NoiseModelFactor {
 
 public:
 
   // typedefs for value types pulled from keys
-  typedef typename KEY1::Value X1;
+  typedef VALUE1 X1;
-  typedef typename KEY2::Value X2;
+  typedef VALUE2 X2;
-  typedef typename KEY3::Value X3;
+  typedef VALUE3 X3;
-  typedef typename KEY4::Value X4;
+  typedef VALUE4 X4;
 
 protected:
 
-  // The values of the keys. Not const to allow serialization
+  typedef NoiseModelFactor Base;
-  KEY1 key1_;
+  typedef NoiseModelFactor4<VALUE1, VALUE2, VALUE3, VALUE4> This;
-  KEY2 key2_;
-  KEY3 key3_;
-  KEY4 key4_;
-
-  typedef NoiseModelFactor<VALUES> Base;
-  typedef NonlinearFactor4<VALUES, KEY1, KEY2, KEY3, KEY4> 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) :
+  NoiseModelFactor4(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4) :
-    Base(noiseModel, make_tuple(j1,j2,j3,j4)), key1_(j1), key2_(j2), key3_(j3), key4_(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 Key key1() const { return keys_[0]; }
-  inline const KEY2& key2() const { return key2_; }
+  inline Key key2() const { return keys_[1]; }
-  inline const KEY3& key3() const { return key3_; }
+  inline Key key3() const { return keys_[2]; }
-  inline const KEY4& key4() const { return key4_; }
+  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>
+template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5>
-class NonlinearFactor5: public NoiseModelFactor<VALUES> {
+class NoiseModelFactor5: public NoiseModelFactor {
 
 public:
 
   // typedefs for value types pulled from keys
-  typedef typename KEY1::Value X1;
+  typedef VALUE1 X1;
-  typedef typename KEY2::Value X2;
+  typedef VALUE2 X2;
-  typedef typename KEY3::Value X3;
+  typedef VALUE3 X3;
-  typedef typename KEY4::Value X4;
+  typedef VALUE4 X4;
-  typedef typename KEY5::Value X5;
+  typedef VALUE5 X5;
 
 protected:
 
-  // The values of the keys. Not const to allow serialization
+  typedef NoiseModelFactor Base;
-  KEY1 key1_;
+  typedef NoiseModelFactor5<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5> This;
-  KEY2 key2_;
-  KEY3 key3_;
-  KEY4 key4_;
-  KEY5 key5_;
-
-  typedef NoiseModelFactor<VALUES> Base;
-  typedef NonlinearFactor5<VALUES, KEY1, KEY2, KEY3, KEY4, KEY5> 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) :
+  NoiseModelFactor5(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5) :
-    Base(noiseModel, make_tuple(j1,j2,j3,j4,j5)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(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 Key key1() const { return keys_[0]; }
-  inline const KEY2& key2() const { return key2_; }
+  inline Key key2() const { return keys_[1]; }
-  inline const KEY3& key3() const { return key3_; }
+  inline Key key3() const { return keys_[2]; }
-  inline const KEY4& key4() const { return key4_; }
+  inline Key key4() const { return keys_[3]; }
-  inline const KEY5& key5() const { return key5_; }
+  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>
+template<class VALUE1, class VALUE2, class VALUE3, class VALUE4, class VALUE5, class VALUE6>
-class NonlinearFactor6: public NoiseModelFactor<VALUES> {
+class NoiseModelFactor6: public NoiseModelFactor {
 
 public:
 
   // typedefs for value types pulled from keys
-  typedef typename KEY1::Value X1;
+  typedef VALUE1 X1;
-  typedef typename KEY2::Value X2;
+  typedef VALUE2 X2;
-  typedef typename KEY3::Value X3;
+  typedef VALUE3 X3;
-  typedef typename KEY4::Value X4;
+  typedef VALUE4 X4;
-  typedef typename KEY5::Value X5;
+  typedef VALUE5 X5;
-  typedef typename KEY6::Value X6;
+  typedef VALUE6 X6;
 
 protected:
 
-  // The values of the keys. Not const to allow serialization
+  typedef NoiseModelFactor Base;
-  KEY1 key1_;
+  typedef NoiseModelFactor6<VALUE1, VALUE2, VALUE3, VALUE4, VALUE5, VALUE6> This;
-  KEY2 key2_;
-  KEY3 key3_;
-  KEY4 key4_;
-  KEY5 key5_;
-  KEY6 key6_;
-
-  typedef NoiseModelFactor<VALUES> Base;
-  typedef NonlinearFactor6<VALUES, KEY1, KEY2, KEY3, KEY4, KEY5, KEY6> 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) :
+  NoiseModelFactor6(const SharedNoiseModel& noiseModel, Key j1, Key j2, Key j3, Key j4, Key j5, Key j6) :
-    Base(noiseModel, make_tuple(j1,j2,j3,j4,j5,j6)), key1_(j1), key2_(j2), key3_(j3), key4_(j4), key5_(j5), key6_(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 Key key1() const { return keys_[0]; }
-  inline const KEY2& key2() const { return key2_; }
+  inline Key key2() const { return keys_[1]; }
-  inline const KEY3& key3() const { return key3_; }
+  inline Key key3() const { return keys_[2]; }
-  inline const KEY4& key4() const { return key4_; }
+  inline Key key4() const { return keys_[3]; }
-  inline const KEY5& key5() const { return key5_; }
+  inline Key key5() const { return keys_[4]; }
-  inline const KEY6& key6() const { return key6_; }
+  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
 
 /* ************************************************************************* */
 

gtsam/nonlinear/NonlinearFactorGraph.cpp

@@ -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 <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <cmath>
 
 using namespace std;
 
 namespace gtsam {
 
 	/* ************************************************************************* */
-	template<class VALUES>
+	double NonlinearFactorGraph::probPrime(const Values& c) const {
-	double NonlinearFactorGraph<VALUES>::probPrime(const VALUES& c) const {
 		return exp(-0.5 * error(c));
 	}
 
 	/* ************************************************************************* */
-	template<class VALUES>
+	void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const {
-	void NonlinearFactorGraph<VALUES>::print(const std::string& str) const {
+    cout << str << "size: " << size() << endl;
-		Base::print(str);
+    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::error(const Values& c) const {
-	double NonlinearFactorGraph<VALUES>::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<Key> NonlinearFactorGraph::keys() const {
-	std::set<Symbol> NonlinearFactorGraph<VALUES>::keys() const {
+		std::set<Key> keys;
-		std::set<Symbol> 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::orderingCOLAMD(
-	Ordering::shared_ptr NonlinearFactorGraph<VALUES>::orderingCOLAMD(
+			const Values& config) const {
-			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::symbolic(const Ordering& ordering) const {
-	SymbolicFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::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::symbolic(
-	pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph<
+		const Values& config) const {
-			VALUES>::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>
+	GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize(
-	typename GaussianFactorGraph::shared_ptr NonlinearFactorGraph<VALUES>::linearize(
+			const Values& config, const Ordering& ordering) const {
-			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

gtsam/nonlinear/NonlinearFactorGraph.h

@@ -28,42 +28,37 @@
 namespace gtsam {
 
 	/**
-	 * A non-linear factor graph is templated on a values structure, but the factor type
+	 * A non-linear factor graph is a graph of non-Gaussian, i.e. non-linear factors,
-	 * is fixed as a NonlinearFactor. The values structures are typically (in SAM) more general
+	 * 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> {
-	class NonlinearFactorGraph: public FactorGraph<NonlinearFactor<VALUES> > {
 
 	public:
 
-		typedef VALUES Values;
+	  typedef FactorGraph<NonlinearFactor> Base;
-	  typedef FactorGraph<NonlinearFactor<VALUES> > Base;
+	  typedef boost::shared_ptr<NonlinearFactorGraph> shared_ptr;
-	  typedef boost::shared_ptr<NonlinearFactorGraph<VALUES> > shared_ptr;
+		typedef boost::shared_ptr<NonlinearFactor> sharedFactor;
-		typedef boost::shared_ptr<NonlinearFactor<VALUES> > sharedFactor;
-
-		/// @name Testable
-		/// @{
 
     /** print just calls base class */
-    void print(const std::string& str = "NonlinearFactorGraph: ") const;
+    void print(const std::string& str = "NonlinearFactorGraph: ", const KeyFormatter& keyFormatter = DefaultKeyFormatter) const;
-
-  	/// @}
-  	/// @name Standard Interface
-  	/// @{
 
     /** 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;
+				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)));
-		}
-
-		/// @}
 
 	private:
 
@@ -116,4 +100,3 @@ namespace gtsam {
 
 } // namespace
 
-#include <gtsam/nonlinear/NonlinearFactorGraph-inl.h>

gtsam/nonlinear/NonlinearISAM-inl.h

@@ -29,8 +29,8 @@ using namespace std;
 using namespace gtsam;
 
 /* ************************************************************************* */
-template<class VALUES, class GRAPH>
+template<class GRAPH>
-void NonlinearISAM<VALUES,GRAPH>::update(const Factors& newFactors,
+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>
+template<class GRAPH>
-void NonlinearISAM<VALUES,GRAPH>::reorder_relinearize() {
+void NonlinearISAM<GRAPH>::reorder_relinearize() {
 
 //  cout << "Reordering, relinearizing..." << endl;
 
@@ -89,8 +89,8 @@ void NonlinearISAM<VALUES,GRAPH>::reorder_relinearize() {
 }
 
 /* ************************************************************************* */
-template<class VALUES, class GRAPH>
+template<class GRAPH>
-VALUES NonlinearISAM<VALUES,GRAPH>::estimate() const {
+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>
+template<class GRAPH>
-Matrix NonlinearISAM<VALUES,GRAPH>::marginalCovariance(const Symbol& key) const {
+Matrix NonlinearISAM<GRAPH>::marginalCovariance(Key key) const {
 	return isam_.marginalCovariance(ordering_[key]);
 }
 
 /* ************************************************************************* */
-template<class VALUES, class GRAPH>
+template<class GRAPH>
-void NonlinearISAM<VALUES,GRAPH>::print(const std::string& s) const {
+void NonlinearISAM<GRAPH>::print(const std::string& s) const {
 	cout << "ISAM - " << s << ":" << endl;
 	cout << "  ReorderInterval: " << reorderInterval_ << " Current Count: " << reorderCounter_ << endl;
 	isam_.print("GaussianISAM");

gtsam/nonlinear/NonlinearISAM.h

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

gtsam/nonlinear/NonlinearOptimization-inl.h

@@ -34,17 +34,17 @@ namespace gtsam {
 	/**
 	 * The Elimination solver
 	 */
-	template<class G, class T>
+	template<class G>
-	T	optimizeSequential(const G& graph, const T& initialEstimate,
+	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>
+	template<class G>
-	T	optimizeMultiFrontal(const G& graph, const T& initialEstimate,
+	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>
+	template<class G>
-	T	optimizeSPCG(const G& graph, const T& initialEstimate,
+	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>
+	template<class G>
-	T optimize(const G& graph, const T& initialEstimate, const NonlinearOptimizationParameters& parameters,
+	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

gtsam/nonlinear/NonlinearOptimization.h

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

gtsam/nonlinear/NonlinearOptimizationParameters.cpp

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

gtsam/nonlinear/NonlinearOptimizationParameters.h

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

gtsam/nonlinear/NonlinearOptimizer-inl.h

@@ -30,8 +30,8 @@ using namespace std;
 namespace gtsam {
 
 	/* ************************************************************************* */
-	template<class G, class C, class L, class S, class W>
+	template<class G, class L, class S, class W>
-	NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
+	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>
+	template<class G, class L, class S, class W>
-	NonlinearOptimizer<G, C, L, S, W>::NonlinearOptimizer(shared_graph graph,
+	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>
+	template<class G, class L, class S, class W>
-	NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::iterate() const {
+	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>
+	template<class G, class L, class S, class W>
-	NonlinearOptimizer<G, C, L, S, W> NonlinearOptimizer<G, C, L, S, W>::gaussNewton() const {
+	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>
+	template<class G, class L, class S, class W>
-	NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::try_lambda(const L& linearSystem) {
+	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>
+	template<class G, class L, class S, class W>
-	NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::iterateLM() {
+	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>
+	template<class G, class L, class S, class W>
-	NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::levenbergMarquardt() {
+	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>
+  template<class G, class L, class S, class W>
-  NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::iterateDogLeg() {
+  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>
+  template<class G, class L, class S, class W>
-  NonlinearOptimizer<G, T, L, S, W> NonlinearOptimizer<G, T, L, S, W>::dogLeg() {
+  NonlinearOptimizer<G, L, S, W> NonlinearOptimizer<G, L, S, W>::dogLeg() {
     static W writer(error_);
 
     // check if we're already close enough

gtsam/nonlinear/NonlinearOptimizer.h

@@ -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
+ * Concept NonLinearSolver<G,Values,L> implements
- *   linearize: G * T -> L
+ *   linearize: G * Values -> L
- *   solve : L -> T
+ *   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,
+ * For example, in a 2D case, $G$ can be pose2SLAM::Graph, $Values$ can be Pose2Values,
- * $L$ can be GaussianFactorGraph and $S$ can be Factorization<Pose2Graph, 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));
 	}
 };