Merge remote-tracking branch 'svn/trunk' into 2.1.0

Conflicts:
	gtsam/inference/FactorGraph-inl.h
	gtsam/inference/tests/testFactorGraph.cpp
	matlab/CMakeLists.txt

.cproject

@@ -309,14 +309,6 @@
 				<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>
@@ -343,6 +335,7 @@
 			</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>
@@ -350,6 +343,7 @@
 			</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>
@@ -397,6 +391,7 @@
 			</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>
@@ -404,6 +399,7 @@
 			</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>
@@ -411,6 +407,7 @@
 			</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>
@@ -426,11 +423,20 @@
 			</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="testPoseRTV.run" path="build/gtsam_unstable/dynamics" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -519,22 +525,6 @@
 				<useDefaultCommand>false</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>all</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<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>
@@ -551,6 +541,22 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>all</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</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>
@@ -575,26 +581,26 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
+				<buildArguments>-j2</buildArguments>
-				<buildTarget>testValues.run</buildTarget>
+				<buildTarget>all</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
+				<buildArguments>-j2</buildArguments>
-				<buildTarget>testOrdering.run</buildTarget>
+				<buildTarget>check</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
+				<buildArguments>-j2</buildArguments>
-				<buildTarget>testKey.run</buildTarget>
+				<buildTarget>clean</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -679,26 +685,26 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
+				<buildArguments>-j5</buildArguments>
-				<buildTarget>all</buildTarget>
+				<buildTarget>testValues.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
+				<buildArguments>-j5</buildArguments>
-				<buildTarget>check</buildTarget>
+				<buildTarget>testOrdering.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
+				<buildArguments>-j5</buildArguments>
-				<buildTarget>clean</buildTarget>
+				<buildTarget>testKey.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -985,6 +991,7 @@
 			</target>
 			<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testGraph.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -992,6 +999,7 @@
 			</target>
 			<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testJunctionTree.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -999,6 +1007,7 @@
 			</target>
 			<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
+				<buildArguments/>
 				<buildTarget>testSymbolicBayesNetB.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -1028,6 +1037,30 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testIterative.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testIterative.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSubgraphSolver.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testSubgraphSolver.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGaussianFactorGraphB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testGaussianFactorGraphB.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="clean" path="build/wrap/gtsam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -1134,6 +1167,7 @@
 			</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>
@@ -1179,10 +1213,10 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testLinearContainerFactor.run" path="build/gtsam_unstable/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
+				<buildArguments>-j5</buildArguments>
-				<buildTarget>testGaussianFactor.run</buildTarget>
+				<buildTarget>testLinearContainerFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -1267,10 +1301,10 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testLinearContainerFactor.run" path="build/gtsam_unstable/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
+				<buildArguments>-j2</buildArguments>
-				<buildTarget>testLinearContainerFactor.run</buildTarget>
+				<buildTarget>testGaussianFactor.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -1605,7 +1639,6 @@
 			</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>
@@ -1645,7 +1678,6 @@
 			</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>
@@ -1653,7 +1685,6 @@
 			</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>
@@ -1845,7 +1876,6 @@
 			</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>
@@ -1867,102 +1897,6 @@
 				<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>
-				<buildTarget>testRot3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testRot2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>timeRot3.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPose2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testCal3_S2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testSimpleCamera.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testHomography2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testCalibratedCamera.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>check</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>clean</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j2</buildArguments>
-				<buildTarget>testPoint2.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j1</buildArguments>
@@ -2164,6 +2098,7 @@
 			</target>
 			<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>-G DEB</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2171,6 +2106,7 @@
 			</target>
 			<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>-G RPM</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2178,6 +2114,7 @@
 			</target>
 			<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>-G TGZ</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2185,6 +2122,7 @@
 			</target>
 			<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>cpack</buildCommand>
+				<buildArguments/>
 				<buildTarget>--config CPackSourceConfig.cmake</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>false</useDefaultCommand>
@@ -2318,34 +2256,98 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
+				<buildArguments>-j2</buildArguments>
-				<buildTarget>testSpirit.run</buildTarget>
+				<buildTarget>testRot3.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
+				<buildArguments>-j2</buildArguments>
-				<buildTarget>testWrap.run</buildTarget>
+				<buildTarget>testRot2.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
+				<buildArguments>-j2</buildArguments>
-				<buildTarget>check.wrap</buildTarget>
+				<buildTarget>testPose3.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+			<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
+				<buildArguments>-j2</buildArguments>
-				<buildTarget>wrap</buildTarget>
+				<buildTarget>timeRot3.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPose2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testCal3_S2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testSimpleCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testHomography2.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testCalibratedCamera.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>check</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>clean</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j2</buildArguments>
+				<buildTarget>testPoint2.run</buildTarget>
 				<stopOnError>true</stopOnError>
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
@@ -2389,6 +2391,38 @@
 				<useDefaultCommand>false</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testSpirit.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testWrap.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>check.wrap</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>wrap</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 		</buildTargets>
 	</storageModule>
 </cproject>

CMakeLists.txt

@@ -161,7 +161,7 @@ if (DOXYGEN_FOUND)
     add_subdirectory(doc)
 endif()
 
-# Set up CPack
+## Set up CPack
 set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "GTSAM")
 set(CPACK_PACKAGE_VENDOR "Frank Dellaert, Georgia Institute of Technology")
 set(CPACK_PACKAGE_CONTACT "Frank Dellaert, dellaert@cc.gatech.edu")
@@ -171,12 +171,18 @@ set(CPACK_PACKAGE_VERSION_MAJOR ${GTSAM_VERSION_MAJOR})
 set(CPACK_PACKAGE_VERSION_MINOR ${GTSAM_VERSION_MINOR})
 set(CPACK_PACKAGE_VERSION_PATCH ${GTSAM_VERSION_PATCH})
 set(CPACK_PACKAGE_INSTALL_DIRECTORY "CMake ${CMake_VERSION_MAJOR}.${CMake_VERSION_MINOR}")
-set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory
+#set(CPACK_INSTALLED_DIRECTORIES "doc;.") # Include doc directory
-set(CPACK_SOURCE_IGNORE_FILES "/build;/\\\\.;/makestats.sh$")
+#set(CPACK_INSTALLED_DIRECTORIES ".") # FIXME: throws error
+set(CPACK_SOURCE_IGNORE_FILES "/build*;/\\\\.;/makestats.sh$")
 set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/gtsam_unstable/")
+set(CPACK_SOURCE_IGNORE_FILES "${CPACK_SOURCE_IGNORE_FILES}" "/package_scripts/")
 set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
 #set(CPACK_SOURCE_PACKAGE_FILE_NAME "gtsam-aspn${GTSAM_VERSION_PATCH}") # Used for creating ASPN tarballs
 
+# Deb-package specific cpack
+set(CPACK_DEBIAN_PACKAGE_NAME "libgtsam-dev")
+set(CPACK_DEBIAN_PACKAGE_DEPENDS "libboost-dev (>= 1.43)") #Example: "libc6 (>= 2.3.1-6), libgcc1 (>= 1:3.4.2-12)")
+
 # print configuration variables
 message(STATUS "===============================================================")
 message(STATUS "================  Configuration Options  ======================")

README

@@ -59,10 +59,12 @@ Tested compilers
  - GCC 4.2-4.7
  - Clang 2.9-3.2
  - OSX GCC 4.2
+ - MSVC 2010, 2012
  
 Tested systems:
  - Ubuntu 11.04, 11.10, 12.04
  - MacOS 10.6, 10.7
+ - Windows 7
 
 2)
 GTSAM makes extensive use of debug assertions, and we highly recommend you work
@@ -72,8 +74,8 @@ will run up to 10x faster in Release mode! See the end of this document for
 additional debugging tips.
 
 3)
-GTSAM has Doxygen documentation. To generate, run the the script 
+GTSAM has Doxygen documentation. To generate, run 'make doc' from your
-makedoc.sh.  
+build directory.
 
 4)
 The instructions below install the library to the default system install path and

examples/Pose2SLAMwSPCG.cpp

@@ -54,13 +54,9 @@
 // for each variable, held in a Values container.
 #include <gtsam/nonlinear/Values.h>
 
-// ???
-#include <gtsam/linear/SimpleSPCGSolver.h>
 #include <gtsam/linear/SubgraphSolver.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
 
-
-
 using namespace std;
 using namespace gtsam;
 
@@ -110,14 +106,6 @@ int main(int argc, char** argv) {
   parameters.verbosityLM = LevenbergMarquardtParams::LAMBDA;
   parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT;
 
-  {
-    parameters.iterativeParams = boost::make_shared<SimpleSPCGSolverParameters>();
-    LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
-    Values result = optimizer.optimize();
-    result.print("Final Result:\n");
-    cout << "simple spcg solver final error = " << graph.error(result) << endl;
-  }
-
   {
     parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();
     LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);

gtsam.h

@@ -1022,8 +1022,10 @@ class GaussianFactorGraph {
 
 	// Conversion to matrices
 	Matrix sparseJacobian_() const;
-	Matrix denseJacobian() const;
+	Matrix augmentedJacobian() const;
-	Matrix denseHessian() const;
+	pair<Matrix,Vector> jacobian() const;
+	Matrix augmentedHessian() const;
+	pair<Matrix,Vector> hessian() const;
 };
 
 class GaussianISAM {
@@ -1045,6 +1047,46 @@ class GaussianSequentialSolver {
 	Matrix marginalCovariance(size_t j) const;
 };
 
+#include <gtsam/linear/IterativeSolver.h>
+virtual class IterativeOptimizationParameters {
+  string getKernel() const ;
+  string getVerbosity() const;
+  void setKernel(string s) ;
+  void setVerbosity(string s) ;
+};
+
+//virtual class IterativeSolver {
+//  IterativeSolver();
+//  gtsam::VectorValues optimize ();
+//};
+
+#include <gtsam/linear/ConjugateGradientSolver.h>
+virtual class ConjugateGradientParameters : gtsam::IterativeOptimizationParameters {
+  ConjugateGradientParameters();
+  size_t getMinIterations() const ;
+  size_t getMaxIterations() const ;
+  size_t getReset() const;
+  double getEpsilon_rel() const;
+  double getEpsilon_abs() const;
+
+  void setMinIterations(size_t value);
+  void setMaxIterations(size_t value);
+  void setReset(size_t value);
+  void setEpsilon_rel(double value);
+  void setEpsilon_abs(double value);
+};
+
+#include <gtsam/linear/SubgraphSolver.h>
+virtual class SubgraphSolverParameters : gtsam::ConjugateGradientParameters {
+  SubgraphSolverParameters();
+  void print(string s) const;
+};
+
+class SubgraphSolver  {
+  SubgraphSolver(const gtsam::GaussianFactorGraph &A, const gtsam::SubgraphSolverParameters &parameters);
+  gtsam::VectorValues optimize() const;
+};
+
 #include <gtsam/linear/KalmanFilter.h>
 class KalmanFilter {
 	KalmanFilter(size_t n);
@@ -1288,6 +1330,7 @@ virtual class SuccessiveLinearizationParams : gtsam::NonlinearOptimizerParams {
 	
 	void setLinearSolverType(string solver);
 	void setOrdering(const gtsam::Ordering& ordering);
+	void setIterativeParams(const gtsam::SubgraphSolverParameters &params);
 
   bool isMultifrontal() const;
   bool isSequential() const;
@@ -1443,6 +1486,8 @@ class ISAM2 {
 
   bool equals(const gtsam::ISAM2& other, double tol) const;
   void print(string s) const;
+  void printStats() const;
+  void saveGraph(string s) const;
 
   gtsam::ISAM2Result update();
   gtsam::ISAM2Result update(const gtsam::NonlinearFactorGraph& newFactors, const gtsam::Values& newTheta);

gtsam/base/DSFVector.cpp

@@ -18,7 +18,7 @@
 
 #include <boost/make_shared.hpp>
 #include <boost/foreach.hpp>
-#include <gtsam_unstable/base/DSFVector.h>
+#include <gtsam/base/DSFVector.h>
 
 using namespace std;
 

gtsam/base/Matrix.h

@@ -67,11 +67,30 @@ inline Matrix Matrix_(const Vector& v) { return Matrix_(v.size(),1,v);}
 */
 Matrix Matrix_(size_t m, size_t n, ...);
 
+// Matlab-like syntax
+
 /**
- * MATLAB like constructors
+ * Creates an zeros matrix, with matlab-like syntax
+ *
+ * Note: if assigning a block (created from an Eigen block() function) of a matrix to zeros,
+ * don't use this function, instead use ".setZero(m,n)" to avoid an Eigen error.
  */
 Matrix zeros(size_t m, size_t n);
+
+/**
+ * Creates an identity matrix, with matlab-like syntax
+ *
+ * Note: if assigning a block (created from an Eigen block() function) of a matrix to identity,
+ * don't use this function, instead use ".setIdentity(m,n)" to avoid an Eigen error.
+ */
 Matrix eye(size_t m, size_t n);
+
+/**
+ * Creates a square identity matrix, with matlab-like syntax
+ *
+ * Note: if assigning a block (created from an Eigen block() function) of a matrix to identity,
+ * don't use this function, instead use ".setIdentity(m)" to avoid an Eigen error.
+ */
 inline Matrix eye( size_t m ) { return eye(m,m); }
 Matrix diag(const Vector& v);
 

gtsam/base/tests/testDSFVector.cpp

@@ -24,7 +24,7 @@
 using namespace boost::assign;
 #include <CppUnitLite/TestHarness.h>
 
-#include <gtsam_unstable/base/DSFVector.h>
+#include <gtsam/base/DSFVector.h>
 
 using namespace std;
 using namespace gtsam;

gtsam/inference/FactorGraph-inl.h

@@ -100,7 +100,7 @@ namespace gtsam {
 			++ firstIndex;
 
 		// Check that number of variables is in bounds
-		if(firstIndex + nFrontals >= variableIndex.size())
+		if(firstIndex + nFrontals > variableIndex.size())
 			throw std::invalid_argument("Requested to eliminate more frontal variables than exist in the factor graph.");
 
 		// Get set of involved factors

gtsam/inference/tests/testFactorGraph.cpp

@@ -60,65 +60,6 @@ TEST(FactorGraph, eliminateFrontals) {
 	EXPECT(assert_equal(expectedCond, *actualCond));
 }
 
-///* ************************************************************************* */
-// SL-FIX TEST( FactorGraph, splitMinimumSpanningTree )
-//{
-//	SymbolicFactorGraph G;
-//	G.push_factor("x1", "x2");
-//	G.push_factor("x1", "x3");
-//	G.push_factor("x1", "x4");
-//	G.push_factor("x2", "x3");
-//	G.push_factor("x2", "x4");
-//	G.push_factor("x3", "x4");
-//
-//	SymbolicFactorGraph T, C;
-//	boost::tie(T, C) = G.splitMinimumSpanningTree();
-//
-//	SymbolicFactorGraph expectedT, expectedC;
-//	expectedT.push_factor("x1", "x2");
-//	expectedT.push_factor("x1", "x3");
-//	expectedT.push_factor("x1", "x4");
-//	expectedC.push_factor("x2", "x3");
-//	expectedC.push_factor("x2", "x4");
-//	expectedC.push_factor("x3", "x4");
-//	CHECK(assert_equal(expectedT,T));
-//	CHECK(assert_equal(expectedC,C));
-//}
-
-///* ************************************************************************* */
-///**
-// *  x1 - x2 - x3 - x4 - x5
-// *       |    |  / |
-// *       l1   l2   l3
-// */
-// SL-FIX TEST( FactorGraph, removeSingletons )
-//{
-//	SymbolicFactorGraph G;
-//	G.push_factor("x1", "x2");
-//	G.push_factor("x2", "x3");
-//	G.push_factor("x3", "x4");
-//	G.push_factor("x4", "x5");
-//	G.push_factor("x2", "l1");
-//	G.push_factor("x3", "l2");
-//	G.push_factor("x4", "l2");
-//	G.push_factor("x4", "l3");
-//
-//	SymbolicFactorGraph singletonGraph;
-//	set<Symbol> singletons;
-//	boost::tie(singletonGraph, singletons) = G.removeSingletons();
-//
-//	set<Symbol> singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3";
-//	CHECK(singletons_excepted == singletons);
-//
-//	SymbolicFactorGraph singletonGraph_excepted;
-//	singletonGraph_excepted.push_factor("x2", "l1");
-//	singletonGraph_excepted.push_factor("x4", "l3");
-//	singletonGraph_excepted.push_factor("x1", "x2");
-//	singletonGraph_excepted.push_factor("x4", "x5");
-//	singletonGraph_excepted.push_factor("x2", "x3");
-//	CHECK(singletonGraph_excepted.equals(singletonGraph));
-//}
-
 /* ************************************************************************* */
 int main() {	TestResult tr; return TestRegistry::runAllTests(tr); }
 /* ************************************************************************* */

gtsam/linear/ConjugateGradientSolver.h

@@ -20,9 +20,7 @@ namespace gtsam {
  */
 
 class ConjugateGradientParameters : public IterativeOptimizationParameters {
-
 public:
-
   typedef IterativeOptimizationParameters Base;
   typedef boost::shared_ptr<ConjugateGradientParameters> shared_ptr;
 
@@ -49,7 +47,21 @@ public:
   inline double epsilon_rel() const { return epsilon_rel_; }
   inline double epsilon_abs() const { return epsilon_abs_; }
 
-  void print() const {
+  inline size_t getMinIterations() const { return minIterations_; }
+  inline size_t getMaxIterations() const { return maxIterations_; }
+  inline size_t getReset() const { return reset_; }
+  inline double getEpsilon() const { return epsilon_rel_; }
+  inline double getEpsilon_rel() const { return epsilon_rel_; }
+  inline double getEpsilon_abs() const { return epsilon_abs_; }
+
+  inline void setMinIterations(size_t value) { minIterations_ = value; }
+  inline void setMaxIterations(size_t value) { maxIterations_ = value; }
+  inline void setReset(size_t value) { reset_ = value; }
+  inline void setEpsilon(double value) { epsilon_rel_ = value; }
+  inline void setEpsilon_rel(double value) { epsilon_rel_ = value; }
+  inline void setEpsilon_abs(double value) { epsilon_abs_ = value; }
+
+  virtual void print(const std::string &s="") const {
     Base::print();
     std::cout << "ConjugateGradientParameters: "
               << "minIter = " << minIterations_
@@ -61,18 +73,4 @@ public:
   }
 };
 
-//class ConjugateGradientSolver : public IterativeSolver {
-//
-//public:
-//
-//  typedef ConjugateGradientParameters Parameters;
-//
-//  Parameters parameters_;
-//
-//  ConjugateGradientSolver(const ConjugateGradientParameters &parameters) : parameters_(parameters) {}
-//  virtual VectorValues::shared_ptr optimize () = 0;
-//  virtual const IterativeOptimizationParameters& _params() const = 0;
-//};
-
-
 }

gtsam/linear/GaussianBayesNet.cpp

@@ -16,7 +16,7 @@
  */
 
 #include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/linear/JacobianFactorGraph.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/inference/BayesNet-inl.h>
 
@@ -242,12 +242,12 @@ double determinant(const GaussianBayesNet& bayesNet) {
 
 /* ************************************************************************* */
 VectorValues gradient(const GaussianBayesNet& bayesNet, const VectorValues& x0) {
-  return gradient(FactorGraph<JacobianFactor>(bayesNet), x0);
+  return gradient(GaussianFactorGraph(bayesNet), x0);
 }
 
 /* ************************************************************************* */
 void gradientAtZero(const GaussianBayesNet& bayesNet, VectorValues& g) {
-  gradientAtZero(FactorGraph<JacobianFactor>(bayesNet), g);
+  gradientAtZero(GaussianFactorGraph(bayesNet), g);
 }
 
 /* ************************************************************************* */

gtsam/linear/GaussianBayesTree.cpp

@@ -18,7 +18,7 @@
  */
 
 #include <gtsam/linear/GaussianBayesTree.h>
-#include <gtsam/linear/JacobianFactorGraph.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 
 namespace gtsam {
 

gtsam/linear/GaussianFactorGraph.cpp

@@ -166,7 +166,7 @@ namespace gtsam {
 	}
 
 	/* ************************************************************************* */
-	Matrix GaussianFactorGraph::denseJacobian() const {
+	Matrix GaussianFactorGraph::augmentedJacobian() const {
     // Convert to Jacobians
     FactorGraph<JacobianFactor> jfg;
     jfg.reserve(this->size());
@@ -182,6 +182,14 @@ namespace gtsam {
 		return combined.matrix_augmented();
 	}
 
+	/* ************************************************************************* */
+	std::pair<Matrix,Vector> GaussianFactorGraph::jacobian() const {
+		Matrix augmented = augmentedJacobian();
+		return make_pair(
+			augmented.leftCols(augmented.cols()-1),
+			augmented.col(augmented.cols()-1));
+	}
+
 	/* ************************************************************************* */
 	// Helper functions for Combine
 	static boost::tuple<vector<size_t>, size_t, size_t> countDims(const FactorGraph<JacobianFactor>& factors, const VariableSlots& variableSlots) {
@@ -317,9 +325,7 @@ break;
 	}
 
 	/* ************************************************************************* */
-	static
+	static FastMap<Index, SlotEntry> findScatterAndDims(const FactorGraph<GaussianFactor>& factors) {
-	FastMap<Index, SlotEntry> findScatterAndDims
-	(const FactorGraph<GaussianFactor>& factors) {
 
 		const bool debug = ISDEBUG("findScatterAndDims");
 
@@ -349,7 +355,7 @@ break;
 	}
 
 	/* ************************************************************************* */
-	Matrix GaussianFactorGraph::denseHessian() const {
+	Matrix GaussianFactorGraph::augmentedHessian() const {
 
 		Scatter scatter = findScatterAndDims(*this);
 
@@ -367,6 +373,14 @@ break;
 		return result;
 	}
 
+	/* ************************************************************************* */
+	std::pair<Matrix,Vector> GaussianFactorGraph::hessian() const {
+		Matrix augmented = augmentedHessian();
+		return make_pair(
+			augmented.topLeftCorner(augmented.rows()-1, augmented.rows()-1),
+			augmented.col(augmented.rows()-1).head(augmented.rows()-1));
+	}
+
 	/* ************************************************************************* */
 	GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
 			GaussianFactor>& factors, size_t nrFrontals) {
@@ -507,4 +521,126 @@ break;
 
 	} // \EliminatePreferCholesky
 
+
+
+	/* ************************************************************************* */
+	static JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) {
+		JacobianFactor::shared_ptr result = boost::dynamic_pointer_cast<JacobianFactor>(gf);
+		if( !result ) {
+			result = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
+		}
+		return result;
+	}
+
+	/* ************************************************************************* */
+	Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x) {
+		Errors e;
+		BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
+		  JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
+			e.push_back((*Ai)*x);
+		}
+		return e;
+	}
+
+	/* ************************************************************************* */
+	void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e) {
+		multiplyInPlace(fg,x,e.begin());
+	}
+
+	/* ************************************************************************* */
+	void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) {
+		Errors::iterator ei = e;
+		BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
+      JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
+      *ei = (*Ai)*x;
+			ei++;
+		}
+	}
+
+	/* ************************************************************************* */
+	// x += alpha*A'*e
+	void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x) {
+		// For each factor add the gradient contribution
+		Errors::const_iterator ei = e.begin();
+		BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
+      JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
+      Ai->transposeMultiplyAdd(alpha,*(ei++),x);
+		}
+	}
+
+	/* ************************************************************************* */
+	VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0) {
+		// It is crucial for performance to make a zero-valued clone of x
+		VectorValues g = VectorValues::Zero(x0);
+		Errors e;
+		BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
+      JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
+      e.push_back(Ai->error_vector(x0));
+		}
+		transposeMultiplyAdd(fg, 1.0, e, g);
+		return g;
+	}
+
+	/* ************************************************************************* */
+	void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g) {
+		// Zero-out the gradient
+		g.setZero();
+		Errors e;
+		BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
+      JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
+      e.push_back(-Ai->getb());
+		}
+		transposeMultiplyAdd(fg, 1.0, e, g);
+	}
+
+	/* ************************************************************************* */
+	void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
+		Index i = 0 ;
+		BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
+      JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
+      r[i] = Ai->getb();
+			i++;
+		}
+		VectorValues Ax = VectorValues::SameStructure(r);
+		multiply(fg,x,Ax);
+		axpy(-1.0,Ax,r);
+	}
+
+	/* ************************************************************************* */
+	void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
+		r.vector() = Vector::Zero(r.dim());
+		Index i = 0;
+		BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
+      JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
+      SubVector &y = r[i];
+			for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
+				y += Ai->getA(j) * x[*j];
+			}
+			++i;
+		}
+	}
+
+	/* ************************************************************************* */
+	void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x) {
+		x.vector() = Vector::Zero(x.dim());
+		Index i = 0;
+		BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
+      JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
+      for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) {
+				x[*j] += Ai->getA(j).transpose() * r[i];
+			}
+			++i;
+		}
+	}
+
+	/* ************************************************************************* */
+	boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x) {
+		boost::shared_ptr<Errors> e(new Errors);
+		BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) {
+      JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G);
+      e->push_back(Ai->error_vector(x));
+		}
+		return e;
+	}
+
 } // namespace gtsam

gtsam/linear/GaussianFactorGraph.h

@@ -31,24 +31,6 @@
 
 namespace gtsam {
 
-  /** return A*x-b
-   * \todo Make this a member function - affects SubgraphPreconditioner */
-  template<class FACTOR>
-  Errors gaussianErrors(const FactorGraph<FACTOR>& fg, const VectorValues& x) {
-    return *gaussianErrors_(fg, x);
-  }
-
-  /** shared pointer version
-   * \todo Make this a member function - affects SubgraphPreconditioner */
-  template<class FACTOR>
-  boost::shared_ptr<Errors> gaussianErrors_(const FactorGraph<FACTOR>& fg, const VectorValues& x) {
-    boost::shared_ptr<Errors> e(new Errors);
-    BOOST_FOREACH(const typename boost::shared_ptr<FACTOR>& factor, fg) {
-      e->push_back(factor->error_vector(x));
-    }
-    return e;
-  }
-
   /**
    * A Linear Factor Graph is a factor graph where all factors are Gaussian, i.e.
    *   Factor == GaussianFactor
@@ -188,15 +170,43 @@ namespace gtsam {
 		Matrix sparseJacobian_() const;
 
     /**
-     * Return a dense \f$ m \times n \f$ Jacobian matrix, augmented with b
+     * Return a dense \f$ [ \;A\;b\; ] \in \mathbb{R}^{m \times n+1} \f$
-     * with standard deviations are baked into A and b
+		 * Jacobian matrix, augmented with b with the noise models baked
+		 * into A and b.  The negative log-likelihood is
+		 * \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$.  See also
+		 * GaussianFactorGraph::jacobian and GaussianFactorGraph::sparseJacobian.
      */
-    Matrix denseJacobian() const;
+    Matrix augmentedJacobian() const;
+
+		/**
+		 * Return the dense Jacobian \f$ A \f$ and right-hand-side \f$ b \f$,
+		 * with the noise models baked into A and b. The negative log-likelihood
+		 * is \f$ \frac{1}{2} \Vert Ax-b \Vert^2 \f$.  See also
+		 * GaussianFactorGraph::augmentedJacobian and
+		 * GaussianFactorGraph::sparseJacobian.
+		 */
+		std::pair<Matrix,Vector> jacobian() const;
 
     /**
-     * Return a dense \f$ n \times n \f$ Hessian matrix, augmented with \f$ A^T b \f$
+     * Return a dense \f$ \Lambda \in \mathbb{R}^{n+1 \times n+1} \f$ Hessian
+		 * matrix, augmented with the information vector \f$ \eta \f$.  The
+		 * augmented Hessian is
+		 \f[ \left[ \begin{array}{ccc}
+		 \Lambda & \eta \\
+		 \eta^T & c
+		 \end{array} \right] \f]
+		 and the negative log-likelihood is
+		 \f$ \frac{1}{2} x^T \Lambda x + \eta^T x + c \f$.
      */
-    Matrix denseHessian() const;
+    Matrix augmentedHessian() const;
+
+		/**
+		 * Return the dense Hessian \f$ \Lambda \f$ and information vector
+		 * \f$ \eta \f$, with the noise models baked in. The negative log-likelihood
+		 * is \frac{1}{2} x^T \Lambda x + \eta^T x + c.  See also
+		 * GaussianFactorGraph::augmentedHessian.
+		 */
+		std::pair<Matrix,Vector> hessian() const;
 
   private:
     /** Serialization function */
@@ -294,4 +304,54 @@ namespace gtsam {
   GaussianFactorGraph::EliminationResult EliminateCholesky(const FactorGraph<
 			GaussianFactor>& factors, size_t nrFrontals = 1);
 
+  /****** Linear Algebra Opeations ******/
+
+  /** return A*x */
+  Errors operator*(const GaussianFactorGraph& fg, const VectorValues& x);
+
+  /** In-place version e <- A*x that overwrites e. */
+  void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, Errors& e);
+
+  /** In-place version e <- A*x that takes an iterator. */
+  void multiplyInPlace(const GaussianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e);
+
+  /** x += alpha*A'*e */
+  void transposeMultiplyAdd(const GaussianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x);
+
+  /**
+   * Compute the gradient of the energy function,
+   * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
+   * centered around \f$ x = x_0 \f$.
+   * The gradient is \f$ A^T(Ax-b) \f$.
+   * @param fg The Jacobian factor graph $(A,b)$
+   * @param x0 The center about which to compute the gradient
+   * @return The gradient as a VectorValues
+   */
+  VectorValues gradient(const GaussianFactorGraph& fg, const VectorValues& x0);
+
+  /**
+   * Compute the gradient of the energy function,
+   * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
+   * centered around zero.
+   * The gradient is \f$ A^T(Ax-b) \f$.
+   * @param fg The Jacobian factor graph $(A,b)$
+   * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
+   * @return The gradient as a VectorValues
+   */
+  void gradientAtZero(const GaussianFactorGraph& fg, VectorValues& g);
+
+  /* matrix-vector operations */
+  void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
+  void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r);
+  void transposeMultiply(const GaussianFactorGraph& fg, const VectorValues &r, VectorValues &x);
+
+  /** shared pointer version
+   * \todo Make this a member function - affects SubgraphPreconditioner */
+  boost::shared_ptr<Errors> gaussianErrors_(const GaussianFactorGraph& fg, const VectorValues& x);
+
+	/** return A*x-b
+   * \todo Make this a member function - affects SubgraphPreconditioner */
+  inline Errors gaussianErrors(const GaussianFactorGraph& fg, const VectorValues& x) {
+    return *gaussianErrors_(fg, x); }
+
 } // namespace gtsam

gtsam/linear/HessianFactor.cpp

@@ -261,12 +261,10 @@ HessianFactor::HessianFactor(const FactorGraph<GaussianFactor>& factors,
 	if (debug) cout << "Combining " << factors.size() << " factors" << endl;
 	BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, factors)
 	{
-		shared_ptr hessian(boost::dynamic_pointer_cast<HessianFactor>(factor));
+		if(factor) {
-		if (hessian)
+			if(shared_ptr hessian = boost::dynamic_pointer_cast<HessianFactor>(factor))
-			updateATA(*hessian, scatter);
+				updateATA(*hessian, scatter);
-		else {
+			else if(JacobianFactor::shared_ptr jacobianFactor = boost::dynamic_pointer_cast<JacobianFactor>(factor))
-			JacobianFactor::shared_ptr jacobianFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
-			if (jacobianFactor)
 				updateATA(*jacobianFactor, scatter);
 			else
 				throw invalid_argument("GaussianFactor is neither Hessian nor Jacobian");
@@ -309,16 +307,6 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
 	}
 }
 
-/* ************************************************************************* */
-double HessianFactor::constantTerm() const {
-	return info_(this->size(), this->size())(0,0);
-}
-
-/* ************************************************************************* */
-HessianFactor::constColumn HessianFactor::linearTerm() const {
-	return info_.rangeColumn(0, this->size(), this->size(), 0);
-}
-
 /* ************************************************************************* */
 Matrix HessianFactor::computeInformation() const {
   return info_.full().selfadjointView<Eigen::Upper>();
@@ -360,7 +348,6 @@ void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatte
 
 	// Apply updates to the upper triangle
 	tic(3, "update");
-	assert(this->info_.nBlocks() - 1 == scatter.size());
 	for(size_t j2=0; j2<update.info_.nBlocks(); ++j2) {
 		size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
 		for(size_t j1=0; j1<=j2; ++j1) {
@@ -437,7 +424,6 @@ void HessianFactor::updateATA(const JacobianFactor& update, const Scatter& scatt
 
 	// Apply updates to the upper triangle
 	tic(3, "update");
-	assert(this->info_.nBlocks() - 1 == scatter.size());
 	for(size_t j2=0; j2<update.Ab_.nBlocks(); ++j2) {
 		size_t slot2 = (j2 == update.size()) ? this->info_.nBlocks()-1 : slots[j2];
 		for(size_t j1=0; j1<=j2; ++j1) {

gtsam/linear/HessianFactor.h

@@ -120,20 +120,6 @@ namespace gtsam {
     InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1]
     BlockInfo info_;    ///< The block view of the full information matrix.
 
-    /** Update the factor by adding the information from the JacobianFactor
-     * (used internally during elimination).
-     * @param update The JacobianFactor containing the new information to add
-     * @param scatter A mapping from variable index to slot index in this HessianFactor
-     */
-    void updateATA(const JacobianFactor& update, const Scatter& scatter);
-
-    /** Update the factor by adding the information from the HessianFactor
-     * (used internally during elimination).
-     * @param update The HessianFactor containing the new information to add
-     * @param scatter A mapping from variable index to slot index in this HessianFactor
-     */
-    void updateATA(const HessianFactor& update, const Scatter& scatter);
-
   public:
 
     typedef boost::shared_ptr<HessianFactor> shared_ptr; ///< A shared_ptr to this
@@ -256,16 +242,39 @@ namespace gtsam {
      */
     constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); }
 
+    /** Return a view of the block at (j1,j2) of the <em>upper-triangular part</em> of the
+     * information matrix \f$ H \f$, no data is copied.  See HessianFactor class documentation
+     * above to explain that only the upper-triangular part of the information matrix is stored
+     * and returned by this function.
+     * @param j1 Which block row to get, as an iterator pointing to the slot in this factor.  You can
+     * use, for example, begin() + 2 to get the 3rd variable in this factor.
+     * @param j2 Which block column to get, as an iterator pointing to the slot in this factor.  You can
+     * use, for example, begin() + 2 to get the 3rd variable in this factor.
+     * @return A view of the requested block, not a copy.
+     */
+    Block info(iterator j1, iterator j2) { return info_(j1-begin(), j2-begin()); }
+
     /** Return the <em>upper-triangular part</em> of the full *augmented* information matrix,
      * as described above.  See HessianFactor class documentation above to explain that only the
      * upper-triangular part of the information matrix is stored and returned by this function.
      */
     constBlock info() const { return info_.full(); }
 
+    /** Return the <em>upper-triangular part</em> of the full *augmented* information matrix,
+     * as described above.  See HessianFactor class documentation above to explain that only the
+     * upper-triangular part of the information matrix is stored and returned by this function.
+     */
+    Block info() { return info_.full(); }
+
     /** Return the constant term \f$ f \f$ as described above
      * @return The constant term \f$ f \f$
      */
-    double constantTerm() const;
+    double constantTerm() const { return info_(this->size(), this->size())(0,0); }
+
+    /** Return the constant term \f$ f \f$ as described above
+     * @return The constant term \f$ f \f$
+     */
+    double& constantTerm() { return info_(this->size(), this->size())(0,0); }
 
     /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable.
      * @param j Which block row to get, as an iterator pointing to the slot in this factor.  You can
@@ -273,9 +282,19 @@ namespace gtsam {
      * @return The linear term \f$ g \f$ */
     constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); }
 
+    /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable.
+     * @param j Which block row to get, as an iterator pointing to the slot in this factor.  You can
+     * use, for example, begin() + 2 to get the 3rd variable in this factor.
+     * @return The linear term \f$ g \f$ */
+    Column linearTerm(iterator j) { return info_.column(j-begin(), size(), 0); }
+
     /** Return the complete linear term \f$ g \f$ as described above.
      * @return The linear term \f$ g \f$ */
-    constColumn linearTerm() const;
+    constColumn linearTerm() const { return info_.rangeColumn(0, this->size(), this->size(), 0); }
+
+    /** Return the complete linear term \f$ g \f$ as described above.
+     * @return The linear term \f$ g \f$ */
+    Column linearTerm() { return info_.rangeColumn(0, this->size(), this->size(), 0); }
     
     /** Return the augmented information matrix represented by this GaussianFactor.
      * The augmented information matrix contains the information matrix with an
@@ -315,6 +334,20 @@ namespace gtsam {
     /** split partially eliminated factor */
     boost::shared_ptr<GaussianConditional> splitEliminatedFactor(size_t nrFrontals);
 
+    /** Update the factor by adding the information from the JacobianFactor
+     * (used internally during elimination).
+     * @param update The JacobianFactor containing the new information to add
+     * @param scatter A mapping from variable index to slot index in this HessianFactor
+     */
+    void updateATA(const JacobianFactor& update, const Scatter& scatter);
+
+    /** Update the factor by adding the information from the HessianFactor
+     * (used internally during elimination).
+     * @param update The HessianFactor containing the new information to add
+     * @param scatter A mapping from variable index to slot index in this HessianFactor
+     */
+    void updateATA(const HessianFactor& update, const Scatter& scatter);
+
     /** assert invariants */
     void assertInvariants() const;
 

gtsam/linear/IterativeSolver.cpp

@@ -0,0 +1,50 @@
+/**
+ * @file   IterativeSolver.cpp
+ * @brief  
+ * @date   Sep 3, 2012
+ * @author Yong-Dian Jian
+ */
+
+#include <gtsam/linear/IterativeSolver.h>
+#include <boost/algorithm/string.hpp>
+#include <string>
+
+namespace gtsam {
+
+std::string IterativeOptimizationParameters::getKernel() const { return kernelTranslator(kernel_); }
+std::string IterativeOptimizationParameters::getVerbosity() const { return verbosityTranslator(verbosity_); }
+void IterativeOptimizationParameters::setKernel(const std::string &src) { kernel_ = kernelTranslator(src); }
+void IterativeOptimizationParameters::setVerbosity(const std::string &src) { verbosity_ = verbosityTranslator(src); }
+
+IterativeOptimizationParameters::Kernel IterativeOptimizationParameters::kernelTranslator(const std::string &src) {
+  std::string s = src;  boost::algorithm::to_upper(s);
+  if (s == "CG") return IterativeOptimizationParameters::CG;
+  /* default is cg */
+  else return IterativeOptimizationParameters::CG;
+}
+
+IterativeOptimizationParameters::Verbosity IterativeOptimizationParameters::verbosityTranslator(const std::string &src)  {
+  std::string s = src;  boost::algorithm::to_upper(s);
+  if (s == "SILENT") return IterativeOptimizationParameters::SILENT;
+  else if (s == "COMPLEXITY") return IterativeOptimizationParameters::COMPLEXITY;
+  else if (s == "ERROR") return IterativeOptimizationParameters::ERROR;
+  /* default is default */
+  else return IterativeOptimizationParameters::SILENT;
+}
+
+std::string IterativeOptimizationParameters::kernelTranslator(IterativeOptimizationParameters::Kernel k)  {
+  if ( k == CG ) return "CG";
+  else return "UNKNOWN";
+}
+
+std::string IterativeOptimizationParameters::verbosityTranslator(IterativeOptimizationParameters::Verbosity verbosity)  {
+  if (verbosity == SILENT) return "SILENT";
+  else if (verbosity == COMPLEXITY) return "COMPLEXITY";
+  else if (verbosity == ERROR) return "ERROR";
+  else return "UNKNOWN";
+}
+
+
+}
+
+

gtsam/linear/IterativeSolver.h

@@ -13,6 +13,7 @@
 
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
+#include <string>
 
 namespace gtsam {
 
@@ -41,23 +42,34 @@ namespace gtsam {
     inline Kernel kernel() const { return kernel_; }
     inline Verbosity verbosity() const { return verbosity_; }
 
+    /* matlab interface */
+    std::string getKernel() const ;
+    std::string getVerbosity() const;
+    void setKernel(const std::string &s) ;
+    void setVerbosity(const std::string &s) ;
+
     void print() const {
-      const std::string kernelStr[1] = {"cg"};
       std::cout << "IterativeOptimizationParameters: "
-                << "kernel = " << kernelStr[kernel_]
+                << "kernel = " << kernelTranslator(kernel_)
-                << ", verbosity = " << verbosity_ << std::endl;
+                << ", verbosity = " << verbosityTranslator(verbosity_) << std::endl;
     }
+
+    static Kernel kernelTranslator(const std::string &s);
+    static Verbosity verbosityTranslator(const std::string &s);
+    static std::string kernelTranslator(Kernel k);
+    static std::string verbosityTranslator(Verbosity v);
   };
 
   class IterativeSolver {
   public:
     typedef boost::shared_ptr<IterativeSolver> shared_ptr;
-    IterativeSolver(){}
+    IterativeSolver() {}
     virtual ~IterativeSolver() {}
 
+    /* interface to the nonlinear optimizer  */
+    virtual VectorValues optimize () = 0;
     /* update interface to the nonlinear optimizer  */
     virtual void replaceFactors(const GaussianFactorGraph::shared_ptr &factorGraph, const double lambda) {}
-    virtual VectorValues optimize () = 0;
   };
 
 }

gtsam/linear/JacobianFactor.h

@@ -322,7 +322,6 @@ namespace gtsam {
     	ar & BOOST_SERIALIZATION_NVP(model_);
     	ar & BOOST_SERIALIZATION_NVP(matrix_);
     }
-
   }; // JacobianFactor
 
 } // gtsam

gtsam/linear/JacobianFactorGraph.cpp

@@ -1,122 +0,0 @@
-/**
- * @file   JacobianFactorGraph.h
- * @date   Jun 6, 2012
- * @author Yong Dian Jian
- */
-
-#include <gtsam/linear/JacobianFactorGraph.h>
-#include <boost/foreach.hpp>
-
-namespace gtsam {
-
-/* ************************************************************************* */
-Errors operator*(const JacobianFactorGraph& fg, const VectorValues& x) {
-  Errors e;
-  BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
-    e.push_back((*Ai)*x);
-  }
-  return e;
-}
-
-/* ************************************************************************* */
-void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, Errors& e) {
-  multiplyInPlace(fg,x,e.begin());
-}
-
-/* ************************************************************************* */
-void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e) {
-  Errors::iterator ei = e;
-  BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
-    *ei = (*Ai)*x;
-    ei++;
-  }
-}
-
-
-/* ************************************************************************* */
-// x += alpha*A'*e
-void transposeMultiplyAdd(const JacobianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x) {
-  // For each factor add the gradient contribution
-  Errors::const_iterator ei = e.begin();
-  BOOST_FOREACH(const JacobianFactor::shared_ptr& Ai, fg) {
-    Ai->transposeMultiplyAdd(alpha,*(ei++),x);
-  }
-}
-
-/* ************************************************************************* */
-VectorValues gradient(const JacobianFactorGraph& fg, const VectorValues& x0) {
-  // It is crucial for performance to make a zero-valued clone of x
-  VectorValues g = VectorValues::Zero(x0);
-  Errors e;
-  BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
-    e.push_back(factor->error_vector(x0));
-  }
-  transposeMultiplyAdd(fg, 1.0, e, g);
-  return g;
-}
-
-/* ************************************************************************* */
-void gradientAtZero(const JacobianFactorGraph& fg, VectorValues& g) {
-  // Zero-out the gradient
-  g.setZero();
-  Errors e;
-  BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
-    e.push_back(-factor->getb());
-  }
-  transposeMultiplyAdd(fg, 1.0, e, g);
-}
-
-/* ************************************************************************* */
-void residual(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
-  Index i = 0 ;
-  BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
-    r[i] = factor->getb();
-    i++;
-  }
-  VectorValues Ax = VectorValues::SameStructure(r);
-  multiply(fg,x,Ax);
-  axpy(-1.0,Ax,r);
-}
-
-/* ************************************************************************* */
-void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r) {
-  r.vector() = Vector::Zero(r.dim());
-  Index i = 0;
-  BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
-    SubVector &y = r[i];
-    for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
-      y += factor->getA(j) * x[*j];
-    }
-    ++i;
-  }
-}
-
-/* ************************************************************************* */
-void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x) {
-  x.vector() = Vector::Zero(x.dim());
-  Index i = 0;
-  BOOST_FOREACH(const JacobianFactor::shared_ptr& factor, fg) {
-    for(JacobianFactor::const_iterator j = factor->begin(); j != factor->end(); ++j) {
-      x[*j] += factor->getA(j).transpose() * r[i];
-    }
-    ++i;
-  }
-}
-
-/* ************************************************************************* */
-JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg) {
-  JacobianFactorGraph::shared_ptr jfg(new JacobianFactorGraph());
-  jfg->reserve(gfg.size());
-  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, gfg) {
-    JacobianFactor::shared_ptr castedFactor(boost::dynamic_pointer_cast<JacobianFactor>(factor));
-    if(castedFactor) jfg->push_back(castedFactor);
-    else throw std::invalid_argument("dynamicCastFactors(), dynamic_cast failed, meaning an invalid cast was requested.");
-  }
-  return jfg;
-}
-
-
-
-} // namespace
-
-

gtsam/linear/JacobianFactorGraph.h

@@ -1,73 +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   JacobianFactorGraph.cpp
- * @date   Jun 6, 2012
- * @brief  Linear Algebra Operations for a JacobianFactorGraph
- * @author Yong Dian Jian
- */
-#pragma once
-
-#include <gtsam/linear/Errors.h>
-#include <gtsam/inference/FactorGraph.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/JacobianFactor.h>
-#include <gtsam/linear/VectorValues.h>
-#include <boost/shared_ptr.hpp>
-
-namespace gtsam {
-
-  typedef FactorGraph<JacobianFactor> JacobianFactorGraph;
-
-  /** return A*x */
-  Errors operator*(const JacobianFactorGraph& fg, const VectorValues& x);
-
-  /** In-place version e <- A*x that overwrites e. */
-  void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, Errors& e);
-
-  /** In-place version e <- A*x that takes an iterator. */
-  void multiplyInPlace(const JacobianFactorGraph& fg, const VectorValues& x, const Errors::iterator& e);
-
-  /** x += alpha*A'*e */
-  void transposeMultiplyAdd(const JacobianFactorGraph& fg, double alpha, const Errors& e, VectorValues& x);
-
-  /**
-   * Compute the gradient of the energy function,
-   * \f$ \nabla_{x=x_0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
-   * centered around \f$ x = x_0 \f$.
-   * The gradient is \f$ A^T(Ax-b) \f$.
-   * @param fg The Jacobian factor graph $(A,b)$
-   * @param x0 The center about which to compute the gradient
-   * @return The gradient as a VectorValues
-   */
-  VectorValues gradient(const JacobianFactorGraph& fg, const VectorValues& x0);
-
-  /**
-   * Compute the gradient of the energy function,
-   * \f$ \nabla_{x=0} \left\Vert \Sigma^{-1} A x - b \right\Vert^2 \f$,
-   * centered around zero.
-   * The gradient is \f$ A^T(Ax-b) \f$.
-   * @param fg The Jacobian factor graph $(A,b)$
-   * @param [output] g A VectorValues to store the gradient, which must be preallocated, see allocateVectorValues
-   * @return The gradient as a VectorValues
-   */
-  void gradientAtZero(const JacobianFactorGraph& fg, VectorValues& g);
-
-  /* matrix-vector operations */
-  void residual(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r);
-  void multiply(const JacobianFactorGraph& fg, const VectorValues &x, VectorValues &r);
-  void transposeMultiply(const JacobianFactorGraph& fg, const VectorValues &r, VectorValues &x);
-
-  /** dynamic_cast the gaussian factors down to jacobian factors */
-  JacobianFactorGraph::shared_ptr dynamicCastFactors(const GaussianFactorGraph &gfg);
-
-}

gtsam/linear/SimpleSPCGSolver.cpp

@@ -1,226 +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
-
- * -------------------------------------------------------------------------- */
-
-#include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/linear/GaussianConditional.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/JacobianFactorGraph.h>
-#include <gtsam/linear/SimpleSPCGSolver.h>
-#include <gtsam/linear/VectorValues.h>
-#include <gtsam/inference/EliminationTree.h>
-#include <boost/foreach.hpp>
-#include <boost/shared_ptr.hpp>
-
-namespace gtsam {
-
-/* utility function */
-std::vector<size_t> extractRowSpec_(const FactorGraph<JacobianFactor>& jfg) {
-  std::vector<size_t> spec; spec.reserve(jfg.size());
-  BOOST_FOREACH ( const JacobianFactor::shared_ptr &jf, jfg ) {
-    spec.push_back(jf->rows());
-  }
-  return spec;
-}
-
-std::vector<size_t> extractColSpec_(const FactorGraph<GaussianFactor>& gfg, const VariableIndex &index) {
-  const size_t n = index.size();
-  std::vector<size_t> spec(n, 0);
-  for ( Index i = 0 ; i < n ; ++i ) {
-    const GaussianFactor &gf = *(gfg[index[i].front()]);
-    for ( GaussianFactor::const_iterator it = gf.begin() ; it != gf.end() ; ++it ) {
-      spec[*it] = gf.getDim(it);
-    }
-  }
-  return spec;
-}
-
-SimpleSPCGSolver::SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters &parameters)
-  : Base(), parameters_(parameters)
-{
-  std::vector<size_t> colSpec = extractColSpec_(gfg, VariableIndex(gfg));
-
-  nVar_ = colSpec.size();
-
-  /* Split the factor graph into At (tree) and Ac (constraints)
-   * Note: This part has to be refined for your graph/application */
-  GaussianFactorGraph::shared_ptr At;
-  boost::tie(At, Ac_) = this->splitGraph(gfg);
-
-  /* construct row vector spec of the new system */
-  nAc_ = Ac_->size();
-  std::vector<size_t> rowSpec = extractRowSpec_(*Ac_);
-  for ( Index i = 0 ; i < nVar_ ; ++i ) {
-    rowSpec.push_back(colSpec[i]);
-  }
-
-  /* solve the tree with direct solver. get preconditioner */
-  Rt_ = EliminationTree<GaussianFactor>::Create(*At)->eliminate(EliminateQR);
-  xt_ = boost::make_shared<VectorValues>(gtsam::optimize(*Rt_));
-
-  /* initial value for the lspcg method */
-  y0_ = boost::make_shared<VectorValues>(VectorValues::Zero(colSpec));
-
-  /* the right hand side of the new system */
-  by_ = boost::make_shared<VectorValues>(VectorValues::Zero(rowSpec));
-  for ( Index i = 0 ; i < nAc_ ; ++i ) {
-    JacobianFactor::shared_ptr jf = (*Ac_)[i];
-    Vector xi = internal::extractVectorValuesSlices(*xt_, jf->begin(), jf->end());
-    (*by_)[i] = jf->getb() - jf->getA()*xi;
-  }
-
-  /* allocate buffer for row and column vectors */
-  tmpY_ = boost::make_shared<VectorValues>(VectorValues::Zero(colSpec));
-  tmpB_ = boost::make_shared<VectorValues>(VectorValues::Zero(rowSpec));
-}
-
-/* implements the CGLS method in Section 7.4 of Bjork's book */
-VectorValues SimpleSPCGSolver::optimize (const VectorValues &initial) {
-
-  VectorValues::shared_ptr x(new VectorValues(initial));
-  VectorValues r = VectorValues::Zero(*by_),
-               q = VectorValues::Zero(*by_),
-               p = VectorValues::Zero(*y0_),
-               s = VectorValues::Zero(*y0_);
-
-  residual(*x, r);
-  transposeMultiply(r, s) ;
-  p.vector() = s.vector() ;
-
-  double gamma = s.vector().squaredNorm(), new_gamma = 0.0, alpha = 0.0, beta = 0.0 ;
-
-  const double threshold = std::max(parameters_.epsilon_abs(), parameters_.epsilon() * parameters_.epsilon() * gamma);
-  const size_t iMaxIterations = parameters_.maxIterations();
-  const Parameters::Verbosity verbosity = parameters_.verbosity();
-
-  if ( verbosity >= ConjugateGradientParameters::ERROR )
-    std::cout << "[SimpleSPCGSolver] epsilon = " << parameters_.epsilon()
-         << ", max = " << parameters_.maxIterations()
-         << ", ||r0|| = " << std::sqrt(gamma)
-         << ", threshold = " << threshold << std::endl;
-
-  size_t k ;
-  for ( k = 1 ; k < iMaxIterations ; ++k ) {
-
-    multiply(p, q);
-    alpha = gamma / q.vector().squaredNorm() ;
-    x->vector() += (alpha * p.vector());
-    r.vector() -= (alpha * q.vector());
-    transposeMultiply(r, s);
-    new_gamma = s.vector().squaredNorm();
-    beta = new_gamma / gamma ;
-    p.vector() = s.vector() + beta * p.vector();
-    gamma = new_gamma ;
-
-    if ( verbosity >= ConjugateGradientParameters::ERROR) {
-      std::cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << std::endl;
-    }
-
-    if ( gamma < threshold ) break ;
-  } // k
-
-  if ( verbosity >= ConjugateGradientParameters::ERROR )
-    std::cout << "[SimpleSPCGSolver] iteration " << k << ": a = " << alpha << ": b = " << beta << ", ||r|| = " << std::sqrt(gamma) << std::endl;
-
-  /* transform y back to x */
-  return this->transform(*x);
-}
-
-void SimpleSPCGSolver::residual(const VectorValues &input, VectorValues &output) {
-  output.vector() = by_->vector();
-  this->multiply(input, *tmpB_);
-  axpy(-1.0, *tmpB_, output);
-}
-
-void SimpleSPCGSolver::multiply(const VectorValues &input, VectorValues &output) {
-  this->backSubstitute(input, *tmpY_);
-  gtsam::multiply(*Ac_, *tmpY_, output);
-  for ( Index i = 0 ; i < nVar_ ; ++i ) {
-    output[i + nAc_] = input[i];
-  }
-}
-
-void SimpleSPCGSolver::transposeMultiply(const VectorValues &input, VectorValues &output) {
-  gtsam::transposeMultiply(*Ac_, input, *tmpY_);
-  this->transposeBackSubstitute(*tmpY_, output);
-  for ( Index i = 0 ; i < nVar_ ; ++i ) {
-    output[i] += input[nAc_+i];
-  }
-}
-
-void SimpleSPCGSolver::backSubstitute(const VectorValues &input, VectorValues &output) {
-  for ( Index i = 0 ; i < input.size() ; ++i ) {
-    output[i] = input[i] ;
-  }
-  BOOST_REVERSE_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, *Rt_) {
-    const Index key = *(cg->beginFrontals());
-    Vector xS = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents());
-    xS = input[key] - cg->get_S() * xS;
-    output[key] = cg->get_R().triangularView<Eigen::Upper>().solve(xS);
-  }
-}
-
-void SimpleSPCGSolver::transposeBackSubstitute(const VectorValues &input, VectorValues &output) {
-  for ( Index i = 0 ; i < input.size() ; ++i ) {
-    output[i] = input[i] ;
-  }
-  BOOST_FOREACH(const boost::shared_ptr<const GaussianConditional> cg, *Rt_) {
-    const Index key = *(cg->beginFrontals());
-    output[key] = gtsam::backSubstituteUpper(output[key], Matrix(cg->get_R()));
-    const Vector d = internal::extractVectorValuesSlices(output, cg->beginParents(), cg->endParents())
-                   - cg->get_S().transpose() * output[key];
-    internal::writeVectorValuesSlices(d, output, cg->beginParents(), cg->endParents());
-  }
-}
-
-VectorValues SimpleSPCGSolver::transform(const VectorValues &y) {
-  VectorValues x = VectorValues::Zero(y);
-  this->backSubstitute(y, x);
-  axpy(1.0, *xt_, x);
-  return x;
-}
-
-boost::tuple<GaussianFactorGraph::shared_ptr, FactorGraph<JacobianFactor>::shared_ptr>
-SimpleSPCGSolver::splitGraph(const GaussianFactorGraph &gfg) {
-
-  VariableIndex index(gfg);
-  size_t n = index.size();
-  std::vector<bool> connected(n, false);
-
-  GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph());
-  FactorGraph<JacobianFactor>::shared_ptr Ac( new FactorGraph<JacobianFactor>());
-
-  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
-    bool augment = false ;
-
-    /* check whether this factor should be augmented to the "tree" graph */
-    if ( gf->keys().size() == 1 ) augment = true;
-    else {
-      BOOST_FOREACH ( const Index key, *gf ) {
-        if ( connected[key] == false ) {
-          augment = true ;
-        }
-        connected[key] = true;
-      }
-    }
-
-    if ( augment ) At->push_back(gf);
-    else Ac->push_back(boost::dynamic_pointer_cast<JacobianFactor>(gf));
-  }
-
-  return boost::tie(At, Ac);
-}
-
-
-
-
-
-
-}

gtsam/linear/SimpleSPCGSolver.h

@@ -1,98 +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
-
- * -------------------------------------------------------------------------- */
-
-#pragma once
-
-#include <gtsam/linear/ConjugateGradientSolver.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/GaussianBayesNet.h>
-#include <gtsam/linear/IterativeSolver.h>
-#include <gtsam/linear/JacobianFactor.h>
-
-namespace gtsam {
-
-struct SimpleSPCGSolverParameters : public ConjugateGradientParameters {
-  typedef ConjugateGradientParameters Base;
-  SimpleSPCGSolverParameters() : Base() {}
-};
-
-/**
- * This class gives a simple implementation to the SPCG solver presented in Dellaert et al in IROS'10.
- *
- * Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the problem into
- * \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part.
- * \f$ A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly.
- * Then we solve a reparametrized problem \f$ f(y) = |y|^2 + |A_c R_t^{-1} y - \bar{b_y}|^2 \f$, where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$
- *
- * In the matrix form, it is equivalent to solving \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. We can solve it
- * with the least-squares variation of the conjugate gradient method.
- *
- * Note: A full SPCG implementation will come up soon in the next release.
- *
- * \nosubgrouping
- */
-
-class SimpleSPCGSolver : public IterativeSolver {
-
-public:
-
-  typedef IterativeSolver Base;
-  typedef SimpleSPCGSolverParameters Parameters;
-  typedef boost::shared_ptr<IterativeSolver> shared_ptr;
-
-protected:
-
-  size_t nVar_ ;                                ///< number of variables \f$ x \f$
-  size_t nAc_ ;                                 ///< number of factors in \f$ A_c \f$
-  FactorGraph<JacobianFactor>::shared_ptr Ac_;  ///< the constrained part of the graph
-  GaussianBayesNet::shared_ptr Rt_;             ///< the gaussian bayes net of the tree part of the graph
-  VectorValues::shared_ptr xt_;                 ///< the solution of the \f$ A_t^{-1} b_t \f$
-  VectorValues::shared_ptr y0_;                 ///< a column zero vector
-  VectorValues::shared_ptr by_;                 ///< \f$ [\bar{b_y} ; 0 ] \f$
-  VectorValues::shared_ptr tmpY_;               ///< buffer for the column vectors
-  VectorValues::shared_ptr tmpB_;               ///< buffer for the row vectors
-  Parameters parameters_;                       ///< Parameters for iterative method
-
-public:
-
-  SimpleSPCGSolver(const GaussianFactorGraph &gfg, const Parameters &parameters);
-  virtual ~SimpleSPCGSolver() {}
-  virtual VectorValues optimize () {return optimize(*y0_);}
-
-protected:
-
-  VectorValues optimize (const VectorValues &initial);
-
-  /** output = \f$ [\bar{b_y} ; 0 ] - [A_c R_t^{-1} ; I] \f$ input */
-  void residual(const VectorValues &input, VectorValues &output);
-
-  /** output = \f$ [A_c R_t^{-1} ; I] \f$ input */
-  void multiply(const VectorValues &input, VectorValues &output);
-
-  /** output = \f$ [R_t^{-T} A_c^T,  I] \f$ input */
-  void transposeMultiply(const VectorValues &input, VectorValues &output);
-
-  /** output = \f$ R_t^{-1} \f$ input */
-  void backSubstitute(const VectorValues &rhs, VectorValues &sol) ;
-
-  /** output = \f$ R_t^{-T} \f$ input */
-  void transposeBackSubstitute(const VectorValues &rhs, VectorValues &sol) ;
-
-  /** return \f$ R_t^{-1} y + x_t \f$ */
-  VectorValues transform(const VectorValues &y);
-
-  /** naively split a gaussian factor graph into tree and constraint parts
-   * Note: This function has to be refined for your graph/application */
-  boost::tuple<GaussianFactorGraph::shared_ptr, FactorGraph<JacobianFactor>::shared_ptr>
-  splitGraph(const GaussianFactorGraph &gfg);
-};
-
-}

gtsam/linear/SubgraphPreconditioner.cpp

@@ -17,18 +17,29 @@
 
 #include <gtsam/linear/SubgraphPreconditioner.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/JacobianFactorGraph.h>
-
 #include <boost/foreach.hpp>
 
 using namespace std;
 
 namespace gtsam {
+
+	/* ************************************************************************* */
+	static GaussianFactorGraph::shared_ptr convertToJacobianFactors(const GaussianFactorGraph &gfg) {
+		GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
+		BOOST_FOREACH(const GaussianFactor::shared_ptr &gf, gfg) {
+			JacobianFactor::shared_ptr jf = boost::dynamic_pointer_cast<JacobianFactor>(gf);
+			if( !jf ) {
+				jf = boost::make_shared<JacobianFactor>(*gf); // Convert any non-Jacobian factors to Jacobians (e.g. Hessian -> Jacobian with Cholesky)
+			}
+			result->push_back(jf);
+		}
+		return result;
+	}
 
 	/* ************************************************************************* */
-	SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2,
+	SubgraphPreconditioner::SubgraphPreconditioner(const sharedFG& Ab2,
 			const sharedBayesNet& Rc1, const sharedValues& xbar) :
-		Ab1_(Ab1), Ab2_(Ab2), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) {
+		Ab2_(convertToJacobianFactors(*Ab2)), Rc1_(Rc1), xbar_(xbar), b2bar_(new Errors(-gaussianErrors(*Ab2_,*xbar))) {
 	}
 
 	/* ************************************************************************* */

gtsam/linear/SubgraphPreconditioner.h

@@ -18,6 +18,7 @@
 #pragma once
 
 #include <gtsam/linear/JacobianFactor.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 
 namespace gtsam {
@@ -32,15 +33,14 @@ namespace gtsam {
 	class SubgraphPreconditioner {
 
 	public:
-
 	  typedef boost::shared_ptr<SubgraphPreconditioner> shared_ptr;
 		typedef boost::shared_ptr<const GaussianBayesNet> sharedBayesNet;
-		typedef boost::shared_ptr<const FactorGraph<JacobianFactor> > sharedFG;
+		typedef boost::shared_ptr<const GaussianFactorGraph> sharedFG;
 		typedef boost::shared_ptr<const VectorValues> sharedValues;
 		typedef boost::shared_ptr<const Errors> sharedErrors;
 
 	private:
-		sharedFG Ab1_, Ab2_;
+		sharedFG Ab2_;
 		sharedBayesNet Rc1_;
 		sharedValues xbar_;  ///< A1 \ b1
 		sharedErrors b2bar_; ///< A2*xbar - b2
@@ -48,17 +48,14 @@ namespace gtsam {
 	public:
 
 		SubgraphPreconditioner();
+
 		/**
 		 * Constructor
-		 * @param Ab1: the Graph A1*x=b1
 		 * @param Ab2: the Graph A2*x=b2
 		 * @param Rc1: the Bayes Net R1*x=c1
 		 * @param xbar: the solution to R1*x=c1
 		 */
-		SubgraphPreconditioner(const sharedFG& Ab1, const sharedFG& Ab2, const sharedBayesNet& Rc1,	const sharedValues& xbar);
+    SubgraphPreconditioner(const sharedFG& Ab2, const sharedBayesNet& Rc1, const sharedValues& xbar);
-
-		/** Access Ab1 */
-		const sharedFG& Ab1() const { return Ab1_; }
 
 		/** Access Ab2 */
 		const sharedFG& Ab2() const { return Ab2_; }
@@ -69,23 +66,23 @@ namespace gtsam {
 		/** Access b2bar */
 		const sharedErrors b2bar() const { return b2bar_; }
 
-	    /**
+    /**
-	     * Add zero-mean i.i.d. Gaussian prior terms to each variable
+     * Add zero-mean i.i.d. Gaussian prior terms to each variable
-	     * @param sigma Standard deviation of Gaussian
+     * @param sigma Standard deviation of Gaussian
-	     */
+     */
-//	    SubgraphPreconditioner add_priors(double sigma) const;
+//  SubgraphPreconditioner add_priors(double sigma) const;
 
 		/* x = xbar + inv(R1)*y */
 		VectorValues x(const VectorValues& y) const;
 
 		/* A zero VectorValues with the structure of xbar */
 		VectorValues zero() const {
-			VectorValues V(VectorValues::Zero(*xbar_)) ;
+			VectorValues V(VectorValues::Zero(*xbar_));
 			return V ;
 		}
 
 		/**
-		 * Add constraint part of the error only, used in both calls above
+		 * Add constraint part of the error only
 		 * y += alpha*inv(R1')*A2'*e2
 		 * Takes a range indicating e2 !!!!
 		 */

gtsam/linear/SubgraphSolver-inl.h

@@ -1,80 +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
-//
-// * -------------------------------------------------------------------------- */
-//
-//#pragma once
-//
-//#include <boost/foreach.hpp>
-//
-//#include <gtsam_unstable/linear/iterative-inl.h>
-//#include <gtsam/inference/graph-inl.h>
-//#include <gtsam/inference/EliminationTree.h>
-//
-//namespace gtsam {
-//
-//template<class GRAPH, class LINEAR, class KEY>
-//void SubgraphSolver<GRAPH,LINEAR,KEY>::replaceFactors(const typename LINEAR::shared_ptr &graph) {
-//
-//	GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>();
-//	GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared<GaussianFactorGraph>();
-//
-//	if (parameters_->verbosity()) cout << "split the graph ...";
-//	split(pairs_, *graph, *Ab1, *Ab2) ;
-//	if (parameters_->verbosity()) cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl;
-//
-//	//	// Add a HardConstraint to the root, otherwise the root will be singular
-//	//	Key root = keys.back();
-//	//	T_.addHardConstraint(root, theta0[root]);
-//	//
-//	//	// compose the approximate solution
-//	//	theta_bar_ = composePoses<GRAPH, Constraint, Pose, Values> (T_, tree, theta0[root]);
-//
-//	LINEAR sacrificialAb1 = *Ab1; // duplicate !!!!!
-//	SubgraphPreconditioner::sharedBayesNet Rc1 =
-//			EliminationTree<GaussianFactor>::Create(sacrificialAb1)->eliminate(&EliminateQR);
-//	SubgraphPreconditioner::sharedValues xbar = gtsam::optimize_(*Rc1);
-//
-//	pc_ = boost::make_shared<SubgraphPreconditioner>(
-//			Ab1->dynamicCastFactors<FactorGraph<JacobianFactor> >(), Ab2->dynamicCastFactors<FactorGraph<JacobianFactor> >(),Rc1,xbar);
-//}
-//
-//template<class GRAPH, class LINEAR, class KEY>
-//VectorValues::shared_ptr SubgraphSolver<GRAPH,LINEAR,KEY>::optimize() const {
-//
-//	// preconditioned conjugate gradient
-//	VectorValues zeros = pc_->zero();
-//	VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, Errors>
-//	(*pc_, zeros, *parameters_);
-//
-//	boost::shared_ptr<VectorValues> xbar = boost::make_shared<VectorValues>() ;
-//	*xbar = pc_->x(ybar);
-//	return xbar;
-//}
-//
-//template<class GRAPH, class LINEAR, class KEY>
-//void SubgraphSolver<GRAPH,LINEAR,KEY>::initialize(const GRAPH& G, const Values& theta0) {
-//	// generate spanning tree
-//	PredecessorMap<KEY> tree_ = gtsam::findMinimumSpanningTree<GRAPH, KEY, Constraint>(G);
-//
-//	// make the ordering
-//	list<KEY> keys = predecessorMap2Keys(tree_);
-//	ordering_ = boost::make_shared<Ordering>(list<Key>(keys.begin(), keys.end()));
-//
-//	// build factor pairs
-//	pairs_.clear();
-//	typedef pair<KEY,KEY> EG ;
-//	BOOST_FOREACH( const EG &eg, tree_ ) {
-//		Key key1 = Key(eg.first),
-//				key2 = Key(eg.second) ;
-//		pairs_.insert(pair<Index, Index>((*ordering_)[key1], (*ordering_)[key2])) ;
-//	}
-//}
-//
-//} // \namespace gtsam

gtsam/linear/SubgraphSolver.cpp

@@ -14,100 +14,119 @@
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/iterative-inl.h>
-#include <gtsam/linear/JacobianFactorGraph.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/SubgraphSolver.h>
 #include <gtsam/linear/VectorValues.h>
 #include <gtsam/inference/graph-inl.h>
 #include <gtsam/inference/EliminationTree.h>
-
+#include <gtsam/base/DSFVector.h>
 #include <boost/foreach.hpp>
 #include <boost/shared_ptr.hpp>
-
 #include <list>
 
 using namespace std;
 
 namespace gtsam {
 
+/**************************************************************************************************/
 SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters &parameters)
   : parameters_(parameters)
 {
+  initialize(gfg);
+}
 
+/**************************************************************************************************/
+SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &jfg, const Parameters &parameters)
+  : parameters_(parameters)
+{
+  initialize(*jfg);
+}
 
-  GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>();
+/**************************************************************************************************/
-  GaussianFactorGraph::shared_ptr Ab2 = boost::make_shared<GaussianFactorGraph>();
+SubgraphSolver::SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters)
+  : parameters_(parameters) {
 
-  boost::tie(Ab1, Ab2) = splitGraph(gfg) ;
+  GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(Ab1)->eliminate(&EliminateQR);
+  initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
+}
 
-  if (parameters_.verbosity())
+/**************************************************************************************************/
-    cout << ",with " << Ab1->size() << " and " << Ab2->size() << " factors" << endl;
+SubgraphSolver::SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1,
-
+    const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters)
-  //  // Add a HardConstraint to the root, otherwise the root will be singular
+  : parameters_(parameters) {
-  //  Key root = keys.back();
-  //  T_.addHardConstraint(root, theta0[root]);
-  //
-  //  // compose the approximate solution
-  //  theta_bar_ = composePoses<GRAPH, Constraint, Pose, Values> (T_, tree, theta0[root]);
 
   GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR);
-  VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
+  initialize(Rc1, Ab2);
+}
 
-  // Convert or cast Ab1 to JacobianFactors
+/**************************************************************************************************/
-  boost::shared_ptr<FactorGraph<JacobianFactor> > Ab1Jacobians = boost::make_shared<FactorGraph<JacobianFactor> >();
+SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2,
-  Ab1Jacobians->reserve(Ab1->size());
+    const Parameters &parameters) : parameters_(parameters)
-  BOOST_FOREACH(const boost::shared_ptr<GaussianFactor>& factor, *Ab1) {
+{
-    if(boost::shared_ptr<JacobianFactor> jf =
+  initialize(Rc1, boost::make_shared<GaussianFactorGraph>(Ab2));
-      boost::dynamic_pointer_cast<JacobianFactor>(factor))
+}
-      Ab1Jacobians->push_back(jf);
-    else
-      Ab1Jacobians->push_back(boost::make_shared<JacobianFactor>(*factor));
-  }
-
-  // Convert or cast Ab2 to JacobianFactors
-  boost::shared_ptr<FactorGraph<JacobianFactor> > Ab2Jacobians = boost::make_shared<FactorGraph<JacobianFactor> >();
-  Ab1Jacobians->reserve(Ab2->size());
-  BOOST_FOREACH(const boost::shared_ptr<GaussianFactor>& factor, *Ab2) {
-    if(boost::shared_ptr<JacobianFactor> jf =
-      boost::dynamic_pointer_cast<JacobianFactor>(factor))
-      Ab2Jacobians->push_back(jf);
-    else
-      Ab2Jacobians->push_back(boost::make_shared<JacobianFactor>(*factor));
-  }
 
-  pc_ = boost::make_shared<SubgraphPreconditioner>(Ab1Jacobians, Ab2Jacobians, Rc1, xbar);
+/**************************************************************************************************/
+SubgraphSolver::SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1,
+    const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters) : parameters_(parameters)
+{
+  initialize(Rc1, Ab2);
 }
 
 VectorValues SubgraphSolver::optimize() {
-
   VectorValues ybar = conjugateGradients<SubgraphPreconditioner, VectorValues, Errors>(*pc_, pc_->zero(), parameters_);
   return pc_->x(ybar);
 }
 
-boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>
+void SubgraphSolver::initialize(const GaussianFactorGraph &jfg)
-SubgraphSolver::splitGraph(const GaussianFactorGraph &gfg) {
+{
+  GaussianFactorGraph::shared_ptr Ab1 = boost::make_shared<GaussianFactorGraph>(),
+                                  Ab2 = boost::make_shared<GaussianFactorGraph>();
 
-  VariableIndex index(gfg);
+  boost::tie(Ab1, Ab2) = splitGraph(jfg) ;
-  size_t n = index.size();
+  if (parameters_.verbosity())
-  std::vector<bool> connected(n, false);
+    cout << "Split A into (A1) " << Ab1->size() << " and (A2) " << Ab2->size() << " factors" << endl;
+
+  GaussianBayesNet::shared_ptr Rc1 = EliminationTree<GaussianFactor>::Create(*Ab1)->eliminate(&EliminateQR);
+  VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
+  pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
+}
+
+void SubgraphSolver::initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2)
+{
+  VectorValues::shared_ptr xbar(new VectorValues(gtsam::optimize(*Rc1)));
+  pc_ = boost::make_shared<SubgraphPreconditioner>(Ab2, Rc1, xbar);
+}
+
+boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>
+SubgraphSolver::splitGraph(const GaussianFactorGraph &jfg) {
+
+  const VariableIndex index(jfg);
+  const size_t n = index.size();
+  DSFVector D(n);
 
   GaussianFactorGraph::shared_ptr At(new GaussianFactorGraph());
   GaussianFactorGraph::shared_ptr Ac( new GaussianFactorGraph());
 
-  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, gfg ) {
+  size_t t = 0;
+  BOOST_FOREACH ( const GaussianFactor::shared_ptr &gf, jfg ) {
+
+    if ( gf->keys().size() > 2 ) {
+      throw runtime_error("SubgraphSolver::splitGraph the graph is not simple, sanity check failed ");
+    }
 
     bool augment = false ;
 
     /* check whether this factor should be augmented to the "tree" graph */
     if ( gf->keys().size() == 1 ) augment = true;
     else {
-      BOOST_FOREACH ( const Index key, *gf ) {
+      const Index u = gf->keys()[0], v = gf->keys()[1],
-        if ( connected[key] == false ) {
+                  u_root = D.findSet(u), v_root = D.findSet(v);
-          augment = true ;
+      if ( u_root != v_root ) {
-          connected[key] = true;
+        t++; augment = true ;
-        }
+        D.makeUnionInPlace(u_root, v_root);
       }
     }
-
     if ( augment ) At->push_back(gf);
     else Ac->push_back(gf);
   }
@@ -115,7 +134,4 @@ SubgraphSolver::splitGraph(const GaussianFactorGraph &gfg) {
   return boost::tie(At, Ac);
 }
 
-
-
-
 } // \namespace gtsam

gtsam/linear/SubgraphSolver.h

@@ -13,7 +13,9 @@
 
 #include <gtsam/linear/ConjugateGradientSolver.h>
 #include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
+#include <gtsam/linear/GaussianBayesNet.h>
 
 #include <boost/make_shared.hpp>
 
@@ -23,31 +25,61 @@ class SubgraphSolverParameters : public ConjugateGradientParameters {
 public:
   typedef ConjugateGradientParameters Base;
   SubgraphSolverParameters() : Base() {}
+  virtual void print(const std::string &s="") const { Base::print(s); }
 };
 
 /**
- * A linear system solver using subgraph preconditioning conjugate gradient
+ * This class implements the SPCG solver presented in Dellaert et al in IROS'10.
+ *
+ * Given a linear least-squares problem \f$ f(x) = |A x - b|^2 \f$. We split the problem into
+ * \f$ f(x) = |A_t - b_t|^2 + |A_c - b_c|^2 \f$ where \f$ A_t \f$ denotes the "tree" part, and \f$ A_c \f$ denotes the "constraint" part.
+ * \f$ A_t \f$ is factorized into \f$ Q_t R_t \f$, and we compute \f$ c_t = Q_t^{-1} b_t \f$, and \f$ x_t = R_t^{-1} c_t \f$ accordingly.
+ * Then we solve a reparametrized problem \f$ f(y) = |y|^2 + |A_c R_t^{-1} y - \bar{b_y}|^2 \f$, where \f$ y = R_t(x - x_t) \f$, and \f$ \bar{b_y} = (b_c - A_c x_t) \f$
+ *
+ * In the matrix form, it is equivalent to solving \f$ [A_c R_t^{-1} ; I ] y = [\bar{b_y} ; 0] \f$. We can solve it
+ * with the least-squares variation of the conjugate gradient method.
+ *
+ * To use it in nonlinear optimization, please see the following example
+ *
+ *  LevenbergMarquardtParams parameters;
+ *  parameters.linearSolverType = SuccessiveLinearizationParams::CONJUGATE_GRADIENT;
+ *  parameters.iterativeParams = boost::make_shared<SubgraphSolverParameters>();
+ *  LevenbergMarquardtOptimizer optimizer(graph, initialEstimate, parameters);
+ *  Values result = optimizer.optimize();
+ *
+ * \nosubgrouping
  */
 
 class SubgraphSolver : public IterativeSolver {
 
 public:
-
   typedef SubgraphSolverParameters Parameters;
 
 protected:
-
 	Parameters parameters_;
 	SubgraphPreconditioner::shared_ptr pc_;  ///< preconditioner object
 
 public:
+	/* Given a gaussian factor graph, split it into a spanning tree (A1) + others (A2) for SPCG */
+	SubgraphSolver(const GaussianFactorGraph &A, const Parameters &parameters);
+  SubgraphSolver(const GaussianFactorGraph::shared_ptr &A, const Parameters &parameters);
+
+	/* The user specify the subgraph part and the constraint part, may throw exception if A1 is underdetermined */
+	SubgraphSolver(const GaussianFactorGraph &Ab1, const GaussianFactorGraph &Ab2, const Parameters &parameters);
+  SubgraphSolver(const GaussianFactorGraph::shared_ptr &Ab1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters);
+
+	/* The same as above, but the A1 is solved before */
+	SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph &Ab2, const Parameters &parameters);
+	SubgraphSolver(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2, const Parameters &parameters);
 
-	SubgraphSolver(const GaussianFactorGraph &gfg, const Parameters &parameters);
   virtual ~SubgraphSolver() {}
   virtual VectorValues optimize () ;
 
 protected:
 
+  void initialize(const GaussianFactorGraph &jfg);
+  void initialize(const GaussianBayesNet::shared_ptr &Rc1, const GaussianFactorGraph::shared_ptr &Ab2);
+
   boost::tuple<GaussianFactorGraph::shared_ptr, GaussianFactorGraph::shared_ptr>
   splitGraph(const GaussianFactorGraph &gfg) ;
 };

gtsam/linear/iterative-inl.h

@@ -122,7 +122,7 @@ namespace gtsam {
 	// conjugate gradient method.
 	// S: linear system, V: step vector, E: errors
 	template<class S, class V, class E>
-	V conjugateGradients(const S& Ab,	V x, const ConjugateGradientParameters &parameters, bool steepest = false) {
+	V conjugateGradients(const S& Ab,	V x, const ConjugateGradientParameters &parameters, bool steepest) {
 
 		CGState<S, V, E> state(Ab, x, parameters, steepest);
 

gtsam/linear/iterative.cpp

@@ -19,53 +19,60 @@
 #include <gtsam/linear/iterative-inl.h>
 #include <gtsam/base/Vector.h>
 #include <gtsam/base/Matrix.h>
-#include <gtsam/linear/JacobianFactorGraph.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/IterativeSolver.h>
 
-
 #include <iostream>
 
 using namespace std;
 
 namespace gtsam {
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	void System::print (const string& s) const {
+  void System::print(const string& s) const {
-		cout << s << endl;
+    cout << s << endl;
-		gtsam::print(A_, "A");
+    gtsam::print(A_, "A");
-		gtsam::print(b_, "b");
+    gtsam::print(b_, "b");
-	}
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
 
-	Vector steepestDescent(const System& Ab, const Vector& x, const ConjugateGradientParameters & parameters) {
+  Vector steepestDescent(const System& Ab, const Vector& x,
-		return conjugateGradients<System, Vector, Vector> (Ab, x, parameters, true);
+      const ConjugateGradientParameters & parameters) {
-	}
+    return conjugateGradients<System, Vector, Vector>(Ab, x, parameters, true);
+  }
 
-	Vector conjugateGradientDescent(const System& Ab, const Vector& x, const ConjugateGradientParameters & parameters) {
+  Vector conjugateGradientDescent(const System& Ab, const Vector& x,
-		return conjugateGradients<System, Vector, Vector> (Ab, x, parameters);
+      const ConjugateGradientParameters & parameters) {
-	}
+    return conjugateGradients<System, Vector, Vector>(Ab, x, parameters);
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x, const ConjugateGradientParameters & parameters) {
+  Vector steepestDescent(const Matrix& A, const Vector& b, const Vector& x,
-		System Ab(A, b);
+      const ConjugateGradientParameters & parameters) {
-		return conjugateGradients<System, Vector, Vector> (Ab, x, parameters, true);
+    System Ab(A, b);
-	}
+    return conjugateGradients<System, Vector, Vector>(Ab, x, parameters, true);
+  }
 
-	Vector conjugateGradientDescent(const Matrix& A, const Vector& b, const Vector& x, const ConjugateGradientParameters & parameters) {
+  Vector conjugateGradientDescent(const Matrix& A, const Vector& b,
-		System Ab(A, b);
+      const Vector& x, const ConjugateGradientParameters & parameters) {
-		return conjugateGradients<System, Vector, Vector> (Ab, x, parameters);
+    System Ab(A, b);
-	}
+    return conjugateGradients<System, Vector, Vector>(Ab, x, parameters);
+  }
 
-	/* ************************************************************************* */
+  /* ************************************************************************* */
-	VectorValues steepestDescent(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const ConjugateGradientParameters & parameters) {
+  VectorValues steepestDescent(const GaussianFactorGraph& fg,
-		return conjugateGradients<FactorGraph<JacobianFactor>, VectorValues, Errors> (fg, x, parameters, true);
+      const VectorValues& x, const ConjugateGradientParameters & parameters) {
-	}
+    return conjugateGradients<GaussianFactorGraph, VectorValues, Errors>(
+        fg, x, parameters, true);
+  }
 
-	VectorValues conjugateGradientDescent(const FactorGraph<JacobianFactor>& fg, const VectorValues& x, const ConjugateGradientParameters & parameters) {
+  VectorValues conjugateGradientDescent(const GaussianFactorGraph& fg,
-		return conjugateGradients<FactorGraph<JacobianFactor>, VectorValues, Errors> (fg, x, parameters);
+      const VectorValues& x, const ConjugateGradientParameters & parameters) {
-	}
+    return conjugateGradients<GaussianFactorGraph, VectorValues, Errors>(
+        fg, x, parameters);
+  }
 
-	/* ************************************************************************* */
+/* ************************************************************************* */
 
 } // namespace gtsam

gtsam/linear/iterative.h

@@ -31,12 +31,11 @@ namespace gtsam {
 	 * "Vector" class E needs dot(v,v)
 	 * @param Ab, the "system" that needs to be solved, examples below
 	 * @param x is the initial estimate
-	 * @param epsilon determines the convergence criterion: norm(g)<epsilon*norm(g0)
-	 * @param maxIterations, if 0 will be set to |x|
 	 * @param steepest flag, if true does steepest descent, not CG
 	 * */
-	template<class S, class V, class E>
+  template<class S, class V, class E>
-	V conjugateGradients(const S& Ab, V x, bool verbose, double epsilon, size_t maxIterations, bool steepest = false);
+  V conjugateGradients(const S& Ab, V x,
+      const ConjugateGradientParameters &parameters, bool steepest = false);
 
 	/**
 	 * Helper class encapsulating the combined system |Ax-b_|^2
@@ -127,11 +126,9 @@ namespace gtsam {
 			const Vector& x,
 			const ConjugateGradientParameters & parameters);
 
-	class GaussianFactorGraph;
-
 	/**
 	 * Method of steepest gradients, Gaussian Factor Graph version
-	 * */
+	 */
 	VectorValues steepestDescent(
 			const GaussianFactorGraph& fg,
 			const VectorValues& x,
@@ -139,7 +136,7 @@ namespace gtsam {
 
 	/**
 	 * Method of conjugate gradients (CG), Gaussian Factor Graph version
-	 * */
+	 */
 	VectorValues conjugateGradientDescent(
 			const GaussianFactorGraph& fg,
 			const VectorValues& x,

gtsam/linear/tests/testGaussianFactorGraph.cpp

@@ -410,7 +410,7 @@ TEST(GaussianFactor, eliminateFrontals)
   factors.push_back(factor4);
 
   // extract the dense matrix for the graph
-  Matrix actualDense = factors.denseJacobian();
+  Matrix actualDense = factors.augmentedJacobian();
   EXPECT(assert_equal(2.0 * Ab, actualDense));
 
   // Convert to Jacobians, inefficient copy of all factors instead of selectively converting only Hessians
@@ -619,7 +619,7 @@ TEST(GaussianFactorGraph, sparseJacobian) {
 }
 
 /* ************************************************************************* */
-TEST(GaussianFactorGraph, denseHessian) {
+TEST(GaussianFactorGraph, matrices) {
   // Create factor graph:
   // x1 x2 x3 x4 x5  b
   //  1  2  3  0  0  4
@@ -639,9 +639,24 @@ TEST(GaussianFactorGraph, denseHessian) {
       9,10, 0,11,12,13,
       0, 0, 0,14,15,16;
 
+	Matrix expectedJacobian = jacobian;
   Matrix expectedHessian = jacobian.transpose() * jacobian;
-  Matrix actualHessian = gfg.denseHessian();
+	Matrix expectedA = jacobian.leftCols(jacobian.cols()-1);
+	Vector expectedb = jacobian.col(jacobian.cols()-1);
+	Matrix expectedL = expectedA.transpose() * expectedA;
+	Vector expectedeta = expectedA.transpose() * expectedb;
+
+	Matrix actualJacobian = gfg.augmentedJacobian();
+	Matrix actualHessian = gfg.augmentedHessian();
+	Matrix actualA; Vector actualb; boost::tie(actualA,actualb) = gfg.jacobian();
+	Matrix actualL; Vector actualeta; boost::tie(actualL,actualeta) = gfg.hessian();
+
+	EXPECT(assert_equal(expectedJacobian, actualJacobian));
   EXPECT(assert_equal(expectedHessian, actualHessian));
+	EXPECT(assert_equal(expectedA, actualA));
+	EXPECT(assert_equal(expectedb, actualb));
+	EXPECT(assert_equal(expectedL, actualL));
+	EXPECT(assert_equal(expectedeta, actualeta));
 }
 
 /* ************************************************************************* */

gtsam/linear/tests/testJacobianFactorGraph.cpp

@@ -0,0 +1,92 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testJacobianFactorGraph.cpp
+ *  @brief  Unit tests for GaussianFactorGraph
+ *  @author Yong Dian Jian
+ **/
+
+#include <gtsam/base/TestableAssertions.h>
+#include <CppUnitLite/TestHarness.h>
+
+
+///* ************************************************************************* */
+//TEST( GaussianFactorGraph, gradient )
+//{
+//	GaussianFactorGraph fg = createGaussianFactorGraph();
+//
+//	// Construct expected gradient
+//	VectorValues expected;
+//
+//  // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2
+//	// worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
+//  expected.insert(L(1),Vector_(2,  5.0,-12.5));
+//  expected.insert(X(1),Vector_(2, 30.0,  5.0));
+//  expected.insert(X(2),Vector_(2,-25.0, 17.5));
+//
+//	// Check the gradient at delta=0
+//  VectorValues zero = createZeroDelta();
+//	VectorValues actual = fg.gradient(zero);
+//	EXPECT(assert_equal(expected,actual));
+//
+//	// Check it numerically for good measure
+//	Vector numerical_g = numericalGradient<VectorValues>(error,zero,0.001);
+//	EXPECT(assert_equal(Vector_(6,5.0,-12.5,30.0,5.0,-25.0,17.5),numerical_g));
+//
+//	// Check the gradient at the solution (should be zero)
+//	Ordering ord;
+//  ord += X(2),L(1),X(1);
+//	GaussianFactorGraph fg2 = createGaussianFactorGraph();
+//  VectorValues solution = fg2.optimize(ord); // destructive
+//	VectorValues actual2 = fg.gradient(solution);
+//	EXPECT(assert_equal(zero,actual2));
+//}
+
+///* ************************************************************************* */
+//TEST( GaussianFactorGraph, transposeMultiplication )
+//{
+//  // create an ordering
+//  Ordering ord; ord += X(2),L(1),X(1);
+//
+//	GaussianFactorGraph A = createGaussianFactorGraph(ord);
+//  Errors e;
+//  e += Vector_(2, 0.0, 0.0);
+//  e += Vector_(2,15.0, 0.0);
+//  e += Vector_(2, 0.0,-5.0);
+//  e += Vector_(2,-7.5,-5.0);
+//
+//  VectorValues expected = createZeroDelta(ord), actual = A ^ e;
+//  expected[ord[L(1)]] = Vector_(2, -37.5,-50.0);
+//  expected[ord[X(1)]] = Vector_(2,-150.0, 25.0);
+//  expected[ord[X(2)]] = Vector_(2, 187.5, 25.0);
+//	EXPECT(assert_equal(expected,actual));
+//}
+
+///* ************************************************************************* */
+//TEST( GaussianFactorGraph, rhs )
+//{
+//  // create an ordering
+//  Ordering ord; ord += X(2),L(1),X(1);
+//
+//	GaussianFactorGraph Ab = createGaussianFactorGraph(ord);
+//	Errors expected = createZeroDelta(ord), actual = Ab.rhs();
+//  expected.push_back(Vector_(2,-1.0,-1.0));
+//  expected.push_back(Vector_(2, 2.0,-1.0));
+//  expected.push_back(Vector_(2, 0.0, 1.0));
+//  expected.push_back(Vector_(2,-1.0, 1.5));
+//	EXPECT(assert_equal(expected,actual));
+//}
+
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
+/* ************************************************************************* */

gtsam/nonlinear/ISAM2.cpp

@@ -27,7 +27,7 @@ using namespace boost::assign;
 #include <gtsam/linear/GaussianJunctionTree.h>
 #include <gtsam/inference/BayesTree-inl.h>
 #include <gtsam/linear/HessianFactor.h>
-#include <gtsam/linear/JacobianFactorGraph.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 
 #include <gtsam/nonlinear/ISAM2.h>
 #include <gtsam/nonlinear/DoglegOptimizerImpl.h>

gtsam/nonlinear/ISAM2.h

@@ -598,6 +598,9 @@ public:
 
   const ISAM2Params& params() const { return params_; }
 
+  /** prints out clique statistics */
+  void printStats() const { getCliqueData().getStats().print(); }
+
   //@}
 
 private:

gtsam/nonlinear/Marginals.cpp

@@ -152,7 +152,7 @@ JointMarginal Marginals::jointMarginalInformation(const std::vector<Key>& variab
 		}
 
     // Get information matrix
-    Matrix augmentedInfo = jointFG.denseHessian();
+    Matrix augmentedInfo = jointFG.augmentedHessian();
     Matrix info = augmentedInfo.topLeftCorner(augmentedInfo.rows()-1, augmentedInfo.cols()-1);
 
     return JointMarginal(info, dims, variableConversion);

gtsam/nonlinear/NonlinearConjugateGradientOptimizer.cpp

@@ -8,7 +8,7 @@
 #include <gtsam/nonlinear/NonlinearConjugateGradientOptimizer.h>
 #include <gtsam/nonlinear/Ordering.h>
 #include <gtsam/nonlinear/Values.h>
-#include <gtsam/linear/JacobianFactorGraph.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/VectorValues.h>
 
 #include <cmath>

gtsam/nonlinear/Ordering.cpp

@@ -44,10 +44,23 @@ void Ordering::print(const string& str, const KeyFormatter& keyFormatter) const
   cout << str;
 	// Print ordering in index order
 	Ordering::InvertedMap inverted = this->invert();
-	cout << "key (zero-based order)\n";
+	// Print the ordering with varsPerLine ordering entries printed on each line,
+	// for compactness.
+	static const size_t varsPerLine = 10;
+	bool endedOnNewline = false;
 	BOOST_FOREACH(const Ordering::InvertedMap::value_type& index_key, inverted) {
-		cout << keyFormatter(index_key.second) << " (" << index_key.first << ")\n";
+		if(index_key.first % varsPerLine != 0)
+			cout << ", ";
+		cout << index_key.first << ":" << keyFormatter(index_key.second);
+		if(index_key.first % varsPerLine == varsPerLine - 1) {
+			cout << "\n";
+			endedOnNewline = true;
+		} else {
+			endedOnNewline = false;
+		}
 	}
+	if(!endedOnNewline)
+		cout << "\n";
 	cout.flush();
 }
 

gtsam/nonlinear/SuccessiveLinearizationOptimizer.cpp

@@ -8,7 +8,6 @@
 #include <gtsam/nonlinear/SuccessiveLinearizationOptimizer.h>
 #include <gtsam/inference/EliminationTree.h>
 #include <gtsam/linear/GaussianJunctionTree.h>
-#include <gtsam/linear/SimpleSPCGSolver.h>
 #include <gtsam/linear/SubgraphSolver.h>
 #include <gtsam/linear/VectorValues.h>
 #include <boost/shared_ptr.hpp>
@@ -16,6 +15,10 @@
 
 namespace gtsam {
 
+void SuccessiveLinearizationParams::setIterativeParams(const SubgraphSolverParameters &params) {
+  iterativeParams = boost::make_shared<SubgraphSolverParameters>(params);
+}
+
 void SuccessiveLinearizationParams::print(const std::string& str) const {
 	NonlinearOptimizerParams::print(str);
 	switch ( linearSolverType ) {
@@ -60,11 +63,7 @@ VectorValues solveGaussianFactorGraph(const GaussianFactorGraph &gfg, const Succ
   }
   else if ( params.isCG() ) {
     if ( !params.iterativeParams ) throw std::runtime_error("solveGaussianFactorGraph: cg parameter has to be assigned ...");
-    if ( boost::dynamic_pointer_cast<SimpleSPCGSolverParameters>(params.iterativeParams)) {
+    if ( boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams) ) {
-      SimpleSPCGSolver solver (gfg, *boost::dynamic_pointer_cast<SimpleSPCGSolverParameters>(params.iterativeParams));
-      delta = solver.optimize();
-    }
-    else if ( boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams) ) {
       SubgraphSolver solver (gfg, *boost::dynamic_pointer_cast<SubgraphSolverParameters>(params.iterativeParams));
       delta = solver.optimize();
     }

gtsam/nonlinear/SuccessiveLinearizationOptimizer.h

@@ -19,7 +19,7 @@
 #pragma once
 
 #include <gtsam/nonlinear/NonlinearOptimizer.h>
-#include <gtsam/linear/IterativeSolver.h>
+#include <gtsam/linear/SubgraphSolver.h>
 
 namespace gtsam {
 
@@ -74,6 +74,7 @@ public:
 	std::string getLinearSolverType() const { return linearSolverTranslator(linearSolverType); }
 
 	void setLinearSolverType(const std::string& solver) { linearSolverType = linearSolverTranslator(solver); }
+  void setIterativeParams(const SubgraphSolverParameters &params);
 	void setOrdering(const Ordering& ordering) { this->ordering = ordering; }
 
 private:

gtsam/slam/tests/testDataset.cpp

@@ -26,7 +26,7 @@ using namespace std;
 using namespace gtsam;
 
 TEST(dataSet, findExampleDataFile) {
-	const string expected_end = "gtsam/examples/Data/example.graph";
+	const string expected_end = "examples/Data/example.graph";
 	const string actual = findExampleDataFile("example");
 	string actual_end = actual.substr(actual.size() - expected_end.size(), expected_end.size());
 	boost::replace_all(actual_end, "\\", "/"); // Convert directory separators to forward-slash

gtsam_unstable/base/Dummy.cpp

@@ -0,0 +1,43 @@
+/* ----------------------------------------------------------------------------
+
+ * 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 Dummy.h
+ * @brief Dummy class for testing MATLAB memory allocation
+ * @author Andrew Melim
+ * @author Frank Dellaert
+ * @date June 14, 2012
+ */
+
+#include <gtsam_unstable/base/Dummy.h>
+#include <iostream>
+
+namespace gtsam {
+
+static size_t gDummyCount = 0;
+
+Dummy::Dummy():id(++gDummyCount) {
+  std::cout << "Dummy constructor " << id << std::endl;
+}
+
+Dummy::~Dummy() {
+  std::cout << "Dummy destructor " << id << std::endl;
+}
+
+void Dummy::print(const std::string& s) const {
+  std::cout << s << "Dummy " << id << std::endl;
+}
+
+unsigned char Dummy::dummyTwoVar(unsigned char a) const {
+  return a;
+}
+
+}

gtsam_unstable/base/Dummy.h

@@ -17,26 +17,16 @@
  * @date June 14, 2012
  */
 
-namespace gtsam {
+#include <string>
 
-  static size_t gDummyCount;
+namespace gtsam {
 
   struct Dummy {
     size_t id;
-    Dummy():id(++gDummyCount) {
+    Dummy();
-      std::cout << "Dummy constructor " << id << std::endl;
+    ~Dummy();
-    }
+    void print(const std::string& s="") const ;
-    ~Dummy() {
+    unsigned char dummyTwoVar(unsigned char a) const ;
-      std::cout << "Dummy destructor " << id << std::endl;
-    }
-    void print(const std::string& s="") const {
-      std::cout << s << "Dummy " << id << std::endl;
-    }
-
-    unsigned char dummyTwoVar(unsigned char a) const {
-        return a;
-    }
-
   };
 
 } // namespace gtsam

gtsam_unstable/nonlinear/LinearContainerFactor.cpp

@@ -38,6 +38,15 @@ void LinearContainerFactor::initializeLinearizationPoint(const Values& lineariza
 	}
 }
 
+/* ************************************************************************* */
+LinearContainerFactor::LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
+		const boost::optional<Values>& linearizationPoint)
+: factor_(factor), linearizationPoint_(linearizationPoint) {
+	// Extract keys stashed in linear factor
+	BOOST_FOREACH(const Index& idx, factor_->keys())
+		keys_.push_back(idx);
+}
+
 /* ************************************************************************* */
 LinearContainerFactor::LinearContainerFactor(
 		const JacobianFactor& factor, const Ordering& ordering,
@@ -205,7 +214,7 @@ GaussianFactor::shared_ptr LinearContainerFactor::negate(const Ordering& orderin
 /* ************************************************************************* */
 NonlinearFactor::shared_ptr LinearContainerFactor::negate() const {
 	GaussianFactor::shared_ptr antifactor = factor_->negate(); // already has keys in place
-	return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor));
+	return NonlinearFactor::shared_ptr(new LinearContainerFactor(antifactor,linearizationPoint_));
 }
 
 /* ************************************************************************* */

gtsam_unstable/nonlinear/LinearContainerFactor.h

@@ -23,6 +23,10 @@ protected:
 	GaussianFactor::shared_ptr factor_;
 	boost::optional<Values> linearizationPoint_;
 
+	/** direct copy constructor */
+	LinearContainerFactor(const GaussianFactor::shared_ptr& factor,
+			const boost::optional<Values>& linearizationPoint);
+
 public:
 
 	/** Primary constructor: store a linear factor and decode the ordering */
@@ -105,7 +109,7 @@ public:
    * Clones the underlying linear factor
    */
   NonlinearFactor::shared_ptr clone() const {
-  	return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_));
+  	return NonlinearFactor::shared_ptr(new LinearContainerFactor(factor_,linearizationPoint_));
   }
 
   // casting syntactic sugar

gtsam_unstable/slam/PoseRotationPrior.h

@@ -67,9 +67,9 @@ public:
 		if (H) {
 			*H = gtsam::zeros(rDim, xDim);
 			if (pose_traits::isRotFirst<Pose>())
-				(*H).leftCols(rDim) = eye(rDim);
+				(*H).leftCols(rDim).setIdentity(rDim, rDim);
 			else
-				(*H).rightCols(rDim) = eye(rDim);
+				(*H).rightCols(rDim).setIdentity(rDim, rDim);
 		}
 
 		return Rotation::Logmap(newR) - Rotation::Logmap(measured_);

matlab/+gtsam/VisualISAMInitialize.m

@@ -50,7 +50,7 @@ for i=1:2
             initialEstimates.insert(jj, truth.points{j});
         end
         if options.pointPriors % add point priors
-            newFactors.add(priorFactorPoint3(jj, truth.points{j}, noiseModels.point));
+            newFactors.add(PriorFactorPoint3(jj, truth.points{j}, noiseModels.point));
         end
     end
 end

matlab/+gtsam/VisualISAMPlot.m

@@ -39,11 +39,12 @@ drawnow
 %% do various optional things
 
 if options.saveFigures
-    fig2 = figure('visible','off');
+    figToSave = figure('visible','off');
-    newax = copyobj(h,fig2);
+    newax = copyobj(h,figToSave);
-    colormap(fig2,'hot');
+    colormap(figToSave,'hot');
     set(newax, 'units', 'normalized', 'position', [0.13 0.11 0.775 0.815]);
-    print(fig2,'-dpng',sprintf('VisualiSAM%03d.png',M));
+    print(figToSave,'-dpng',sprintf('VisualiSAM%03d.png',M));
+    axes(h);
 end
 
 if options.saveDotFiles

matlab/CMakeLists.txt

@@ -2,17 +2,16 @@
 
 # Tests
 message(STATUS "Installing Matlab Toolbox Tests")
-install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_tests" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m")
+install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_tests" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m" PATTERN ".svn" EXCLUDE)
 
 # Examples
 message(STATUS "Installing Matlab Toolbox Examples")
 # Matlab files: *.m and *.fig
-install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m")
+install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m" PATTERN "*.fig" PATTERN ".svn" EXCLUDE)
-install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/gtsam_examples" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.fig")
 
 # Utilities
 message(STATUS "Installing Matlab Toolbox Utilities")
-install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m")
+install(DIRECTORY "${GTSAM_SOURCE_ROOT_DIR}/matlab/+gtsam" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}" FILES_MATCHING PATTERN "*.m" PATTERN ".svn" EXCLUDE)
 install(FILES "${GTSAM_SOURCE_ROOT_DIR}/matlab/README-gtsam-toolbox.txt" DESTINATION "${GTSAM_TOOLBOX_INSTALL_PATH}")
 
 message(STATUS "Installing Matlab Toolbox Examples (Data)")

matlab/gtsam_examples/PlanarSLAMExample_graph.m

@@ -15,7 +15,7 @@ clear
 import gtsam.*
 
 %% Find data file
-datafile = '/Users/dellaert/borg/gtsam/examples/Data/example.graph';
+datafile = findExampleDataFile('example.graph');
 
 %% Initialize graph, initial estimate, and odometry noise
 model = noiseModel.Diagonal.Sigmas([0.05; 0.05; 2*pi/180]);

matlab/gtsam_examples/Pose2SLAMwSPCG.m

@@ -51,7 +51,14 @@ initialEstimate.insert(4, Pose2(4.0, 2.0, pi  ));
 initialEstimate.insert(5, Pose2(2.1, 2.1,-pi/2));
 initialEstimate.print(sprintf('\nInitial estimate:\n'));
 
-%% Optimize using Levenberg-Marquardt optimization with an ordering from colamd
+%% Optimize using Levenberg-Marquardt optimization with SubgraphSolver
-optimizer = DoglegOptimizer(graph, initialEstimate);
+params = gtsam.LevenbergMarquardtParams;
-result = optimizer.optimizeSafely();
+subgraphParams = gtsam.SubgraphSolverParameters;
-result.print(sprintf('\nFinal result:\n'));
+params.setLinearSolverType('CONJUGATE_GRADIENT');
+params.setIterativeParams(subgraphParams);
+optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate);
+result = optimizer.optimize();
+result.print(sprintf('\nFinal result:\n'));
+
+
+

matlab/gtsam_examples/VisualISAM_gui.m

@@ -82,7 +82,7 @@ options.showImages = get(handles.showImagesCB,'Value');
 options.hardConstraint = get(handles.hardConstraintCB,'Value');
 options.pointPriors = get(handles.pointPriorsCB,'Value');
 options.batchInitialization = get(handles.batchInitCB,'Value');
-options.reorderInterval = str2num(get(handles.reorderIntervalEdit,'String'));
+%options.reorderInterval = str2num(get(handles.reorderIntervalEdit,'String'));
 options.alwaysRelinearize = get(handles.alwaysRelinearizeCB,'Value');
 
 % Display Options

matlab/gtsam_examples/gtsamExamples.m

@@ -22,7 +22,7 @@ function varargout = gtsamExamples(varargin)
 
 % Edit the above text to modify the response to help gtsamExamples
 
-% Last Modified by GUIDE v2.5 24-Jul-2012 10:18:05
+% Last Modified by GUIDE v2.5 03-Sep-2012 13:34:13
 
 % Begin initialization code - DO NOT EDIT
 gui_Singleton = 1;
@@ -142,6 +142,13 @@ function PlanarSLAMSampling_Callback(hObject, eventdata, handles)
 axes(handles.axes3);
 PlanarSLAMExample_sampling
 
+% --- Executes on button press in PlanarSLAMGraph.
+function PlanarSLAMGraph_Callback(hObject, eventdata, handles)
+axes(handles.axes3);
+echo on
+PlanarSLAMExample_graph
+echo off
+
 % --- Executes on button press in SFM.
 function SFM_Callback(hObject, eventdata, handles)
 axes(handles.axes3);

package_scripts/toolbox_package_unix.sh

@@ -0,0 +1,43 @@
+#!/bin/sh
+
+# Detect platform
+os=`uname -s`
+arch=`uname -m`
+if [ "$os" = "Linux" -a "$arch" = "x86_64" ]; then
+	platform=lin64
+elif [ "$os" = "Linux" -a "$arch" = "i686" ]; then
+	platform=lin32
+else
+	echo "Unrecognized platform"
+	exit 1
+fi
+
+echo "Platform is ${platform}"
+
+# Check for empty directory
+if [ ! -z "`ls`" ]; then
+	echo "Please run this script from an empty build directory"
+	exit 1
+fi
+
+# Check for boost
+if [ -z "$1" -o -z "$2" ]; then
+	echo "Usage: $0 BOOSTTREE MEX"
+	echo "BOOSTTREE should be a boost source tree compiled with toolbox_build_boost."
+	echo "MEX should be the full path of the mex compiler"
+	exit 1
+fi
+
+# Run cmake
+cmake -DCMAKE_BUILD_TYPE=Release -DCMAKE_INSTALL_PREFIX="$PWD/stage" -DBoost_NO_SYSTEM_PATHS:bool=true -DBoost_USE_STATIC_LIBS:bool=true -DBOOST_ROOT="$1" -DGTSAM_BUILD_UNSTABLE:bool=false -DGTSAM_DISABLE_EXAMPLES_ON_INSTALL:bool=true -DGTSAM_DISABLE_TESTS_ON_INSTALL:bool=true -DGTSAM_MEX_BUILD_STATIC_MODULE:bool=true -DMEX_COMMAND="$2" ..
+
+if [ ! $? ]; then
+	echo "CMake failed"
+	exit 1
+fi
+
+# Compile
+make install
+
+# Create package
+tar czf gtsam-toolbox-2.1.0-$platform.tgz -C stage/borg toolbox

tests/smallExample.cpp

@@ -138,9 +138,9 @@ namespace example {
 	}
 
 	/* ************************************************************************* */
-	JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering) {
+	GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering) {
 		// Create empty graph
-		JacobianFactorGraph fg;
+	  GaussianFactorGraph fg;
 
 		SharedDiagonal unit2 = noiseModel::Unit::Create(2);
 
@@ -273,7 +273,7 @@ namespace example {
 	}
 
 	/* ************************************************************************* */
-	JacobianFactorGraph createSimpleConstraintGraph() {
+	GaussianFactorGraph createSimpleConstraintGraph() {
 		// create unary factor
 		// prior on _x_, mean = [1,-1], sigma=0.1
 		Matrix Ax = eye(2);
@@ -293,7 +293,7 @@ namespace example {
 				constraintModel));
 
 		// construct the graph
-		JacobianFactorGraph fg;
+		GaussianFactorGraph fg;
 		fg.push_back(f1);
 		fg.push_back(f2);
 
@@ -310,7 +310,7 @@ namespace example {
 	}
 
 	/* ************************************************************************* */
-	JacobianFactorGraph createSingleConstraintGraph() {
+	GaussianFactorGraph createSingleConstraintGraph() {
 		// create unary factor
 		// prior on _x_, mean = [1,-1], sigma=0.1
 		Matrix Ax = eye(2);
@@ -335,7 +335,7 @@ namespace example {
 				constraintModel));
 
 		// construct the graph
-		JacobianFactorGraph fg;
+		GaussianFactorGraph fg;
 		fg.push_back(f1);
 		fg.push_back(f2);
 
@@ -351,7 +351,7 @@ namespace example {
 	}
 
 	/* ************************************************************************* */
-	JacobianFactorGraph createMultiConstraintGraph() {
+	GaussianFactorGraph createMultiConstraintGraph() {
 		// unary factor 1
 		Matrix A = eye(2);
 		Vector b = Vector_(2, -2.0, 2.0);
@@ -396,7 +396,7 @@ namespace example {
 				constraintModel));
 
 		// construct the graph
-		JacobianFactorGraph fg;
+		GaussianFactorGraph fg;
 		fg.push_back(lf1);
 		fg.push_back(lc1);
 		fg.push_back(lc2);
@@ -458,7 +458,8 @@ namespace example {
 				xtrue[ordering[key(x, y)]] = Point2(x,y).vector();
 
 		// linearize around zero
-		return boost::make_tuple(*nlfg.linearize(zeros, ordering), xtrue);
+		boost::shared_ptr<GaussianFactorGraph> gfg = nlfg.linearize(zeros, ordering);
+		return boost::make_tuple(*gfg, xtrue);
 	}
 
 	/* ************************************************************************* */
@@ -471,9 +472,9 @@ namespace example {
 	}
 
 	/* ************************************************************************* */
-	pair<JacobianFactorGraph, JacobianFactorGraph > splitOffPlanarTree(size_t N,
+	pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(size_t N,
-			const JacobianFactorGraph& original) {
+			const GaussianFactorGraph& original) {
-		JacobianFactorGraph T, C;
+		GaussianFactorGraph T, C;
 
 		// Add the x11 constraint to the tree
 		T.push_back(original[0]);

tests/smallExample.h

@@ -23,7 +23,6 @@
 
 #include <tests/simulated2D.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/linear/JacobianFactorGraph.h>
 #include <boost/tuple/tuple.hpp>
 
 namespace gtsam {
@@ -66,7 +65,7 @@ namespace gtsam {
 		 * create a linear factor graph
 		 * The non-linear graph above evaluated at NoisyValues
 		 */
-		JacobianFactorGraph createGaussianFactorGraph(const Ordering& ordering);
+		GaussianFactorGraph createGaussianFactorGraph(const Ordering& ordering);
 
 		/**
 		 * create small Chordal Bayes Net x <- y
@@ -100,21 +99,21 @@ namespace gtsam {
 		 * Creates a simple constrained graph with one linear factor and
 		 * one binary equality constraint that sets x = y
 		 */
-		JacobianFactorGraph createSimpleConstraintGraph();
+		GaussianFactorGraph createSimpleConstraintGraph();
 		VectorValues createSimpleConstraintValues();
 
 		/**
 		 * Creates a simple constrained graph with one linear factor and
 		 * one binary constraint.
 		 */
-		JacobianFactorGraph createSingleConstraintGraph();
+		GaussianFactorGraph createSingleConstraintGraph();
 		VectorValues createSingleConstraintValues();
 
 		/**
 		 * Creates a constrained graph with a linear factor and two
 		 * binary constraints that share a node
 		 */
-		JacobianFactorGraph createMultiConstraintGraph();
+		GaussianFactorGraph createMultiConstraintGraph();
 		VectorValues createMultiConstraintValues();
 
 		/* ******************************************************* */
@@ -147,8 +146,8 @@ namespace gtsam {
 		 *   |
 		 * -x11-x21-x31
 		 */
-		std::pair<JacobianFactorGraph, JacobianFactorGraph > splitOffPlanarTree(
+		std::pair<GaussianFactorGraph, GaussianFactorGraph > splitOffPlanarTree(
-				size_t N, const JacobianFactorGraph& original);
+				size_t N, const GaussianFactorGraph& original);
 
 	} // example
 } // gtsam

tests/testDoglegOptimizer.cpp

@@ -96,7 +96,7 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPoint) {
   gradientValues.vector() = gradient;
 
   // Compute the gradient using dense matrices
-  Matrix augmentedHessian = GaussianFactorGraph(gbn).denseHessian();
+  Matrix augmentedHessian = GaussianFactorGraph(gbn).augmentedHessian();
   LONGS_EQUAL(11, augmentedHessian.cols());
   VectorValues denseMatrixGradient = *allocateVectorValues(gbn);
   denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10);
@@ -200,7 +200,7 @@ TEST(DoglegOptimizer, BT_BN_equivalency) {
   GaussianFactorGraph expected(gbn);
   GaussianFactorGraph actual(bt);
 
-  EXPECT(assert_equal(expected.denseHessian(), actual.denseHessian()));
+  EXPECT(assert_equal(expected.augmentedHessian(), actual.augmentedHessian()));
 }
 
 /* ************************************************************************* */
@@ -276,7 +276,7 @@ TEST(DoglegOptimizer, ComputeSteepestDescentPointBT) {
   gradientValues.vector() = gradient;
 
   // Compute the gradient using dense matrices
-  Matrix augmentedHessian = GaussianFactorGraph(bt).denseHessian();
+  Matrix augmentedHessian = GaussianFactorGraph(bt).augmentedHessian();
   LONGS_EQUAL(11, augmentedHessian.cols());
   VectorValues denseMatrixGradient = *allocateVectorValues(bt);
   denseMatrixGradient.vector() = -augmentedHessian.col(10).segment(0,10);

tests/testGaussianFactor.cpp

@@ -57,42 +57,16 @@ TEST( GaussianFactor, linearFactor )
 	JacobianFactor expected(ordering[kx1], -10*I,ordering[kx2], 10*I, b, noiseModel::Unit::Create(2));
 
 	// create a small linear factor graph
-	FactorGraph<JacobianFactor> fg = example::createGaussianFactorGraph(ordering);
+	GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);
 
 	// get the factor kf2 from the factor graph
-	JacobianFactor::shared_ptr lf = fg[1];
+	JacobianFactor::shared_ptr lf =
+	    boost::dynamic_pointer_cast<JacobianFactor>(fg[1]);
 
 	// check if the two factors are the same
 	EXPECT(assert_equal(expected,*lf));
 }
 
-///* ************************************************************************* */
-// SL-FIX TEST( GaussianFactor, keys )
-//{
-//	// get the factor kf2 from the small linear factor graph
-//  Ordering ordering; ordering += kx1,kx2,kl1;
-//  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-//	GaussianFactor::shared_ptr lf = fg[1];
-//	list<Symbol> expected;
-//	expected.push_back(kx1);
-//	expected.push_back(kx2);
-//	EXPECT(lf->keys() == expected);
-//}
-
-///* ************************************************************************* */
-// SL-FIX TEST( GaussianFactor, dimensions )
-//{
-//  // get the factor kf2 from the small linear factor graph
-//  Ordering ordering; ordering += kx1,kx2,kl1;
-//  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-//
-//  // Check a single factor
-//  Dimensions expected;
-//  insert(expected)(kx1, 2)(kx2, 2);
-//  Dimensions actual = fg[1]->dimensions();
-//  EXPECT(expected==actual);
-//}
-
 /* ************************************************************************* */
 TEST( GaussianFactor, getDim )
 {
@@ -110,62 +84,6 @@ TEST( GaussianFactor, getDim )
 	EXPECT_LONGS_EQUAL(expected, actual);
 }
 
-///* ************************************************************************* */
-// SL-FIX TEST( GaussianFactor, combine )
-//{
-//	// create a small linear factor graph
-//  Ordering ordering; ordering += kx1,kx2,kl1;
-//  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-//
-//	// get two factors from it and insert the factors into a vector
-//	vector<GaussianFactor::shared_ptr> lfg;
-//	lfg.push_back(fg[4 - 1]);
-//	lfg.push_back(fg[2 - 1]);
-//
-//	// combine in a factor
-//	GaussianFactor combined(lfg);
-//
-//	// sigmas
-//	double sigma2 = 0.1;
-//	double sigma4 = 0.2;
-//	Vector sigmas = Vector_(4, sigma4, sigma4, sigma2, sigma2);
-//
-//	// the expected combined linear factor
-//	Matrix Ax2 = Matrix_(4, 2, // x2
-//			-5., 0.,
-//			+0., -5.,
-//			10., 0.,
-//			+0., 10.);
-//
-//	Matrix Al1 = Matrix_(4, 2,	// l1
-//			5., 0.,
-//			0., 5.,
-//			0., 0.,
-//			0., 0.);
-//
-//	Matrix Ax1 = Matrix_(4, 2,	// x1
-//			0.00, 0., // f4
-//			0.00, 0., // f4
-//			-10., 0., // f2
-//			0.00, -10. // f2
-//	);
-//
-//	// the RHS
-//	Vector b2(4);
-//	b2(0) = -1.0;
-//	b2(1) =  1.5;
-//	b2(2) =  2.0;
-//	b2(3) = -1.0;
-//
-//	// use general constructor for making arbitrary factors
-//	vector<pair<Symbol, Matrix> > meas;
-//	meas.push_back(make_pair(kx2, Ax2));
-//	meas.push_back(make_pair(kl1, Al1));
-//	meas.push_back(make_pair(kx1, Ax1));
-//	GaussianFactor expected(meas, b2, noiseModel::Diagonal::Sigmas(ones(4)));
-//	EXPECT(assert_equal(expected,combined));
-//}
-
 /* ************************************************************************* */
 TEST( GaussianFactor, error )
 {
@@ -186,54 +104,13 @@ TEST( GaussianFactor, error )
 	DOUBLES_EQUAL( 1.0, actual, 0.00000001 );
 }
 
-///* ************************************************************************* */
-// SL-FIX TEST( GaussianFactor, eliminate )
-//{
-//	// create a small linear factor graph
-//  Ordering ordering; ordering += kx1,kx2,kl1;
-//  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-//
-//	// get two factors from it and insert the factors into a vector
-//	vector<GaussianFactor::shared_ptr> lfg;
-//	lfg.push_back(fg[4 - 1]);
-//	lfg.push_back(fg[2 - 1]);
-//
-//	// combine in a factor
-//	GaussianFactor combined(lfg);
-//
-//	// eliminate the combined factor
-//	GaussianConditional::shared_ptr actualCG;
-//	GaussianFactor::shared_ptr actualLF;
-//	boost::tie(actualCG,actualLF) = combined.eliminate(kx2);
-//
-//	// create expected Conditional Gaussian
-//	Matrix I = eye(2)*sqrt(125.0);
-//	Matrix R11 = I, S12 = -0.2*I, S13 = -0.8*I;
-//	Vector d = I*Vector_(2,0.2,-0.14);
-//
-//	// Check the conditional Gaussian
-//	GaussianConditional
-//	expectedCG(kx2, d, R11, kl1, S12, kx1, S13, repeat(2, 1.0));
-//
-//	// the expected linear factor
-//	I = eye(2)/0.2236;
-//	Matrix Bl1 = I, Bx1 = -I;
-//	Vector b1 = I*Vector_(2,0.0,0.2);
-//
-//	GaussianFactor expectedLF(kl1, Bl1, kx1, Bx1, b1, repeat(2,1.0));
-//
-//	// check if the result matches
-//	EXPECT(assert_equal(expectedCG,*actualCG,1e-3));
-//	EXPECT(assert_equal(expectedLF,*actualLF,1e-3));
-//}
-
 /* ************************************************************************* */
 TEST( GaussianFactor, matrix )
 {
 	const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
 	// create a small linear factor graph
   Ordering ordering; ordering += kx1,kx2,kl1;
-  FactorGraph<JacobianFactor> fg = example::createGaussianFactorGraph(ordering);
+  GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);
 
 	// get the factor kf2 from the factor graph
 	//GaussianFactor::shared_ptr lf = fg[1]; // NOTE: using the older version
@@ -282,7 +159,7 @@ TEST( GaussianFactor, matrix_aug )
 	const Key kx1 = X(1), kx2 = X(2), kl1 = L(1);
 	// create a small linear factor graph
   Ordering ordering; ordering += kx1,kx2,kl1;
-  FactorGraph<JacobianFactor> fg = example::createGaussianFactorGraph(ordering);
+  GaussianFactorGraph fg = example::createGaussianFactorGraph(ordering);
 
 	// get the factor kf2 from the factor graph
 	//GaussianFactor::shared_ptr lf = fg[1];
@@ -328,66 +205,6 @@ void print(const list<T>& i) {
 	cout << endl;
 }
 
-///* ************************************************************************* */
-// SL-FIX TEST( GaussianFactor, sparse )
-//{
-//	// create a small linear factor graph
-//  Ordering ordering; ordering += kx1,kx2,kl1;
-//  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-//
-//	// get the factor kf2 from the factor graph
-//	GaussianFactor::shared_ptr lf = fg[1];
-//
-//	// render with a given ordering
-//	Ordering ord;
-//	ord += kx1,kx2;
-//
-//	list<int> i,j;
-//	list<double> s;
-//	boost::tie(i,j,s) = lf->sparse(fg.columnIndices(ord));
-//
-//	list<int> i1,j1;
-//	i1 += 1,2,1,2;
-//	j1 += 1,2,3,4;
-//
-//	list<double> s1;
-//	s1 += -10,-10,10,10;
-//
-//	EXPECT(i==i1);
-//	EXPECT(j==j1);
-//	EXPECT(s==s1);
-//}
-
-///* ************************************************************************* */
-// SL-FIX TEST( GaussianFactor, sparse2 )
-//{
-//	// create a small linear factor graph
-//  Ordering ordering; ordering += kx1,kx2,kl1;
-//  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-//
-//	// get the factor kf2 from the factor graph
-//	GaussianFactor::shared_ptr lf = fg[1];
-//
-//	// render with a given ordering
-//	Ordering ord;
-//	ord += kx2,kl1,kx1;
-//
-//	list<int> i,j;
-//	list<double> s;
-//	boost::tie(i,j,s) = lf->sparse(fg.columnIndices(ord));
-//
-//	list<int> i1,j1;
-//	i1 += 1,2,1,2;
-//	j1 += 5,6,1,2;
-//
-//	list<double> s1;
-//	s1 += -10,-10,10,10;
-//
-//	EXPECT(i==i1);
-//	EXPECT(j==j1);
-//	EXPECT(s==s1);
-//}
-
 /* ************************************************************************* */
 TEST( GaussianFactor, size )
 {

tests/testGaussianFactorGraphB.cpp

@@ -19,7 +19,7 @@
 #include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianSequentialSolver.h>
-#include <gtsam/linear/JacobianFactorGraph.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/inference/SymbolicFactorGraph.h>
 #include <gtsam/base/numericalDerivative.h>
 #include <gtsam/base/Matrix.h>
@@ -56,143 +56,17 @@ TEST( GaussianFactorGraph, equals ) {
 }
 
 /* ************************************************************************* */
-//TEST( GaussianFactorGraph, error ) {
+TEST( GaussianFactorGraph, error ) {
-//  Ordering ordering; ordering += X(1),X(2),L(1);
+  Ordering ordering; ordering += X(1),X(2),L(1);
-//  FactorGraph<JacobianFactor> fg = createGaussianFactorGraph(ordering);
+  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-//  VectorValues cfg = createZeroDelta(ordering);
+  VectorValues cfg = createZeroDelta(ordering);
-//
-//  // note the error is the same as in testNonlinearFactorGraph as a
-//  // zero delta config in the linear graph is equivalent to noisy in
-//  // non-linear, which is really linear under the hood
-//  double actual = fg.error(cfg);
-//  DOUBLES_EQUAL( 5.625, actual, 1e-9 );
-//}
 
-/* ************************************************************************* */
+  // note the error is the same as in testNonlinearFactorGraph as a
-/* unit test for find seperator                                              */
+  // zero delta config in the linear graph is equivalent to noisy in
-/* ************************************************************************* */
+  // non-linear, which is really linear under the hood
-// SL-NEEDED? TEST( GaussianFactorGraph, find_separator )
+  double actual = fg.error(cfg);
-//{
+  DOUBLES_EQUAL( 5.625, actual, 1e-9 );
-//  GaussianFactorGraph fg = createGaussianFactorGraph();
+}
-//
-//  set<Symbol> separator = fg.find_separator(X(2));
-//  set<Symbol> expected;
-//  expected.insert(X(1));
-//  expected.insert(L(1));
-//
-//  EXPECT(separator.size()==expected.size());
-//  set<Symbol>::iterator it1 = separator.begin(), it2 = expected.begin();
-//  for(; it1!=separator.end(); it1++, it2++)
-//    EXPECT(*it1 == *it2);
-//}
-
-///* ************************************************************************* */
-// SL-FIX TEST( GaussianFactorGraph, combine_factors_x1 )
-//{
-//  // create a small example for a linear factor graph
-//  GaussianFactorGraph fg = createGaussianFactorGraph();
-//
-//  // combine all factors
-//  GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,X(1));
-//
-//  // the expected linear factor
-//  Matrix Al1 = Matrix_(6,2,
-//			 0., 0.,
-//			 0., 0.,
-//			 0., 0.,
-//			 0., 0.,
-//			 5., 0.,
-//			 0., 5.
-//			 );
-//
-//  Matrix Ax1 = Matrix_(6,2,
-//			 10., 0.,
-//			 0., 10.,
-//			-10., 0.,
-//			 0.,-10.,
-//			-5., 0.,
-//			 0.,-5.
-//			 );
-//
-//  Matrix Ax2 = Matrix_(6,2,
-//			 0., 0.,
-//			 0., 0.,
-//			 10., 0.,
-//			 0., 10.,
-//			 0., 0.,
-//			 0., 0.
-//			 );
-//
-//  // the expected RHS vector
-//  Vector b(6);
-//  b(0) = -1;
-//  b(1) = -1;
-//  b(2) =  2;
-//  b(3) = -1;
-//  b(4) =  0;
-//  b(5) =  1;
-//
-//  vector<pair<Symbol, Matrix> > meas;
-//  meas.push_back(make_pair(L(1), Al1));
-//  meas.push_back(make_pair(X(1), Ax1));
-//  meas.push_back(make_pair(X(2), Ax2));
-//  GaussianFactor expected(meas, b, ones(6));
-//  //GaussianFactor expected(L(1), Al1, X(1), Ax1, X(2), Ax2, b);
-//
-//  // check if the two factors are the same
-//  EXPECT(assert_equal(expected,*actual));
-//}
-//
-///* ************************************************************************* */
-// SL-FIX TEST( GaussianFactorGraph, combine_factors_x2 )
-//{
-// // create a small example for a linear factor graph
-//  GaussianFactorGraph fg = createGaussianFactorGraph();
-//
-//  // combine all factors
-//  GaussianFactor::shared_ptr actual = removeAndCombineFactors(fg,X(2));
-//
-//  // the expected linear factor
-//  Matrix Al1 = Matrix_(4,2,
-//			 // l1
-//			 0., 0.,
-//			 0., 0.,
-//			 5., 0.,
-//			 0., 5.
-//			 );
-//
-//  Matrix Ax1 = Matrix_(4,2,
-//                         // x1
-//			-10., 0., // f2
-//			 0.,-10., // f2
-//			 0., 0., // f4
-//			 0., 0.  // f4
-//			 );
-//
-//  Matrix Ax2 = Matrix_(4,2,
-//			 // x2
-//			 10., 0.,
-//			 0., 10.,
-//			-5., 0.,
-//			 0.,-5.
-//			 );
-//
-//  // the expected RHS vector
-//  Vector b(4);
-//  b(0) =  2;
-//  b(1) = -1;
-//  b(2) = -1;
-//  b(3) =  1.5;
-//
-//  vector<pair<Symbol, Matrix> > meas;
-//  meas.push_back(make_pair(L(1), Al1));
-//  meas.push_back(make_pair(X(1), Ax1));
-//  meas.push_back(make_pair(X(2), Ax2));
-//  GaussianFactor expected(meas, b, ones(4));
-//
-//  // check if the two factors are the same
-//  EXPECT(assert_equal(expected,*actual));
-//}
 
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, eliminateOne_x1 )
@@ -336,47 +210,6 @@ TEST( GaussianFactorGraph, eliminateAll )
   EXPECT(assert_equal(expected,actualQR,tol));
 }
 
-///* ************************************************************************* */
-//TEST( GaussianFactorGraph, eliminateAll_fast )
-//{
-//	// create expected Chordal bayes Net
-//	Matrix I = eye(2);
-//
-//	Vector d1 = Vector_(2, -0.1,-0.1);
-//	GaussianBayesNet expected = simpleGaussian(X(1),d1,0.1);
-//
-//	double sig1 = 0.149071;
-//	Vector d2 = Vector_(2, 0.0, 0.2)/sig1, sigma2 = ones(2);
-//	push_front(expected,L(1),d2, I/sig1,X(1), (-1)*I/sig1,sigma2);
-//
-//	double sig2 = 0.0894427;
-//	Vector d3 = Vector_(2, 0.2, -0.14)/sig2, sigma3 = ones(2);
-//	push_front(expected,X(2),d3, I/sig2,L(1), (-0.2)*I/sig2, X(1), (-0.8)*I/sig2, sigma3);
-//
-//	// Check one ordering
-//	GaussianFactorGraph fg1 = createGaussianFactorGraph();
-//	Ordering ordering;
-//	ordering += X(2),L(1),X(1);
-//	GaussianBayesNet actual = fg1.eliminate(ordering, false);
-//	EXPECT(assert_equal(expected,actual,tol));
-//}
-
-///* ************************************************************************* */
-//TEST( GaussianFactorGraph, add_priors )
-//{
-//  Ordering ordering; ordering += L(1),X(1),X(2);
-//  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-//  GaussianFactorGraph actual = fg.add_priors(3, vector<size_t>(3,2));
-//  GaussianFactorGraph expected = createGaussianFactorGraph(ordering);
-//  Matrix A = eye(2);
-//  Vector b = zero(2);
-//  SharedDiagonal sigma = noiseModel::Isotropic::Sigma(2,3.0);
-//  expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[L(1)],A,b,sigma)));
-//  expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(1)],A,b,sigma)));
-//  expected.push_back(GaussianFactor::shared_ptr(new JacobianFactor(ordering[X(2)],A,b,sigma)));
-//  EXPECT(assert_equal(expected,actual));
-//}
-
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, copying )
 {
@@ -397,46 +230,6 @@ TEST( GaussianFactorGraph, copying )
   EXPECT(assert_equal(expected,actual));
 }
 
-///* ************************************************************************* */
-// SL-FIX TEST( GaussianFactorGraph, matrix )
-//{
-//  // render with a given ordering
-//  Ordering ord;
-//  ord += X(2),L(1),X(1);
-//
-//  // Create a graph
-//  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
-//
-//  Matrix A; Vector b;
-//  boost::tie(A,b) = fg.matrix();
-//
-//  Matrix A1 = Matrix_(2*4,3*2,
-//		     +0.,  0.,  0.,  0., 10.,  0., // unary factor on x1 (prior)
-//		     +0.,  0.,  0.,  0.,  0., 10.,
-//		     10.,  0.,  0.,  0.,-10.,  0., // binary factor on x2,x1 (odometry)
-//		     +0., 10.,  0.,  0.,  0.,-10.,
-//		     +0.,  0.,  5.,  0., -5.,  0., // binary factor on l1,x1 (z1)
-//		     +0.,  0.,  0.,  5.,  0., -5.,
-//		     -5.,  0.,  5.,  0.,  0.,  0., // binary factor on x2,l1 (z2)
-//		     +0., -5.,  0.,  5.,  0.,  0.
-//    );
-//  Vector b1 = Vector_(8,-1., -1., 2., -1., 0., 1., -1., 1.5);
-//
-//  EQUALITY(A,A1);
-//  EXPECT(b==b1);
-//}
-
-///* ************************************************************************* */
-// SL-FIX TEST( GaussianFactorGraph, sizeOfA )
-//{
-//	// create a small linear factor graph
-//	GaussianFactorGraph fg = createGaussianFactorGraph();
-//
-//  pair<size_t, size_t> mn = fg.sizeOfA();
-//  EXPECT(8 == mn.first);
-//  EXPECT(6 == mn.second);
-//}
-
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
 {
@@ -451,11 +244,6 @@ TEST( GaussianFactorGraph, CONSTRUCTOR_GaussianBayesNet )
   GaussianFactorGraph fg2(CBN);
   GaussianBayesNet CBN2 = *GaussianSequentialSolver(fg2).eliminate();
   EXPECT(assert_equal(CBN,CBN2));
-
-  // Base FactorGraph only
-//  FactorGraph<GaussianFactor> fg3(CBN);
-//  GaussianBayesNet CBN3 = gtsam::eliminate<GaussianFactor,GaussianConditional>(fg3,ord);
-//  EXPECT(assert_equal(CBN,CBN3));
 }
 
 /* ************************************************************************* */
@@ -469,16 +257,6 @@ TEST( GaussianFactorGraph, getOrdering)
   EXPECT(assert_equal(expected,actual));
 }
 
-// SL-FIX TEST( GaussianFactorGraph, getOrdering2)
-//{
-//  Ordering expected;
-//  expected += L(1),X(1);
-//  GaussianFactorGraph fg = createGaussianFactorGraph();
-//  set<Symbol> interested; interested += L(1),X(1);
-//  Ordering actual = fg.getOrdering(interested);
-//  EXPECT(assert_equal(expected,actual));
-//}
-
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, optimize_Cholesky )
 {
@@ -515,24 +293,6 @@ TEST( GaussianFactorGraph, optimize_QR )
   EXPECT(assert_equal(expected,actual));
 }
 
-///* ************************************************************************* */
-// SL-FIX TEST( GaussianFactorGraph, optimizeMultiFrontlas )
-//{
-//  // create an ordering
-//  Ordering ord; ord += X(2),L(1),X(1);
-//
-//	// create a graph
-//	GaussianFactorGraph fg = createGaussianFactorGraph(ord);
-//
-//	// optimize the graph
-//	VectorValues actual = fg.optimizeMultiFrontals(ord);
-//
-//	// verify
-//	VectorValues expected = createCorrectDelta();
-//
-//  EXPECT(assert_equal(expected,actual));
-//}
-
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, combine)
 {
@@ -585,48 +345,6 @@ void print(vector<int> v) {
 	cout << endl;
 }
 
-///* ************************************************************************* */
-// SL-NEEDED? TEST( GaussianFactorGraph, factor_lookup)
-//{
-//	// create a test graph
-//	GaussianFactorGraph fg = createGaussianFactorGraph();
-//
-//	// ask for all factor indices connected to x1
-//	list<size_t> x1_factors = fg.factors(X(1));
-//	size_t x1_indices[] = { 0, 1, 2 };
-//	list<size_t> x1_expected(x1_indices, x1_indices + 3);
-//	EXPECT(x1_factors==x1_expected);
-//
-//	// ask for all factor indices connected to x2
-//	list<size_t> x2_factors = fg.factors(X(2));
-//	size_t x2_indices[] = { 1, 3 };
-//	list<size_t> x2_expected(x2_indices, x2_indices + 2);
-//	EXPECT(x2_factors==x2_expected);
-//}
-
-///* ************************************************************************* */
-// SL-NEEDED? TEST( GaussianFactorGraph, findAndRemoveFactors )
-//{
-//	// create the graph
-//	GaussianFactorGraph fg = createGaussianFactorGraph();
-//
-//  // We expect to remove these three factors: 0, 1, 2
-//  GaussianFactor::shared_ptr f0 = fg[0];
-//  GaussianFactor::shared_ptr f1 = fg[1];
-//  GaussianFactor::shared_ptr f2 = fg[2];
-//
-//  // call the function
-//  vector<GaussianFactor::shared_ptr> factors = fg.findAndRemoveFactors(X(1));
-//
-//  // Check the factors
-//  EXPECT(f0==factors[0]);
-//  EXPECT(f1==factors[1]);
-//  EXPECT(f2==factors[2]);
-//
-//  // EXPECT if the factors are deleted from the factor graph
-//  LONGS_EQUAL(1,fg.nrFactors());
-//}
-
 /* ************************************************************************* */
 TEST(GaussianFactorGraph, createSmoother)
 {
@@ -636,35 +354,6 @@ TEST(GaussianFactorGraph, createSmoother)
 	LONGS_EQUAL(5,fg2.size());
 }
 
-///* ************************************************************************* */
-// SL-NEEDED? TEST( GaussianFactorGraph, variables )
-//{
-//  GaussianFactorGraph fg = createGaussianFactorGraph();
-//  Dimensions expected;
-//  insert(expected)(L(1), 2)(X(1), 2)(X(2), 2);
-//  Dimensions actual = fg.dimensions();
-//  EXPECT(expected==actual);
-//}
-
-///* ************************************************************************* */
-// SL-NEEDED? TEST( GaussianFactorGraph, keys )
-//{
-//  GaussianFactorGraph fg = createGaussianFactorGraph();
-//  Ordering expected;
-//  expected += L(1),X(1),X(2);
-//  EXPECT(assert_equal(expected,fg.keys()));
-//}
-
-///* ************************************************************************* */
-// SL-NEEDED? TEST( GaussianFactorGraph, involves )
-//{
-//  GaussianFactorGraph fg = createGaussianFactorGraph();
-//  EXPECT(fg.involves(L(1)));
-//  EXPECT(fg.involves(X(1)));
-//  EXPECT(fg.involves(X(2)));
-//  EXPECT(!fg.involves(X(3)));
-//}
-
 /* ************************************************************************* */
 double error(const VectorValues& x) {
   // create an ordering
@@ -674,45 +363,13 @@ double error(const VectorValues& x) {
 	return fg.error(x);
 }
 
-///* ************************************************************************* */
-// SL-NEEDED? TEST( GaussianFactorGraph, gradient )
-//{
-//	GaussianFactorGraph fg = createGaussianFactorGraph();
-//
-//	// Construct expected gradient
-//	VectorValues expected;
-//
-//  // 2*f(x) = 100*(x1+c[X(1)])^2 + 100*(x2-x1-[0.2;-0.1])^2 + 25*(l1-x1-[0.0;0.2])^2 + 25*(l1-x2-[-0.2;0.3])^2
-//	// worked out: df/dx1 = 100*[0.1;0.1] + 100*[0.2;-0.1]) + 25*[0.0;0.2] = [10+20;10-10+5] = [30;5]
-//  expected.insert(L(1),Vector_(2,  5.0,-12.5));
-//  expected.insert(X(1),Vector_(2, 30.0,  5.0));
-//  expected.insert(X(2),Vector_(2,-25.0, 17.5));
-//
-//	// Check the gradient at delta=0
-//  VectorValues zero = createZeroDelta();
-//	VectorValues actual = fg.gradient(zero);
-//	EXPECT(assert_equal(expected,actual));
-//
-//	// Check it numerically for good measure
-//	Vector numerical_g = numericalGradient<VectorValues>(error,zero,0.001);
-//	EXPECT(assert_equal(Vector_(6,5.0,-12.5,30.0,5.0,-25.0,17.5),numerical_g));
-//
-//	// Check the gradient at the solution (should be zero)
-//	Ordering ord;
-//  ord += X(2),L(1),X(1);
-//	GaussianFactorGraph fg2 = createGaussianFactorGraph();
-//  VectorValues solution = fg2.optimize(ord); // destructive
-//	VectorValues actual2 = fg.gradient(solution);
-//	EXPECT(assert_equal(zero,actual2));
-//}
-
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, multiplication )
 {
   // create an ordering
   Ordering ord; ord += X(2),L(1),X(1);
 
-	FactorGraph<JacobianFactor> A = createGaussianFactorGraph(ord);
+  GaussianFactorGraph A = createGaussianFactorGraph(ord);
   VectorValues x = createCorrectDelta(ord);
   Errors actual = A * x;
   Errors expected;
@@ -723,41 +380,6 @@ TEST( GaussianFactorGraph, multiplication )
 	EXPECT(assert_equal(expected,actual));
 }
 
-///* ************************************************************************* */
-// SL-NEEDED? TEST( GaussianFactorGraph, transposeMultiplication )
-//{
-//  // create an ordering
-//  Ordering ord; ord += X(2),L(1),X(1);
-//
-//	GaussianFactorGraph A = createGaussianFactorGraph(ord);
-//  Errors e;
-//  e += Vector_(2, 0.0, 0.0);
-//  e += Vector_(2,15.0, 0.0);
-//  e += Vector_(2, 0.0,-5.0);
-//  e += Vector_(2,-7.5,-5.0);
-//
-//  VectorValues expected = createZeroDelta(ord), actual = A ^ e;
-//  expected[ord[L(1)]] = Vector_(2, -37.5,-50.0);
-//  expected[ord[X(1)]] = Vector_(2,-150.0, 25.0);
-//  expected[ord[X(2)]] = Vector_(2, 187.5, 25.0);
-//	EXPECT(assert_equal(expected,actual));
-//}
-
-///* ************************************************************************* */
-// SL-NEEDED? TEST( GaussianFactorGraph, rhs )
-//{
-//  // create an ordering
-//  Ordering ord; ord += X(2),L(1),X(1);
-//
-//	GaussianFactorGraph Ab = createGaussianFactorGraph(ord);
-//	Errors expected = createZeroDelta(ord), actual = Ab.rhs();
-//  expected.push_back(Vector_(2,-1.0,-1.0));
-//  expected.push_back(Vector_(2, 2.0,-1.0));
-//  expected.push_back(Vector_(2, 0.0, 1.0));
-//  expected.push_back(Vector_(2,-1.0, 1.5));
-//	EXPECT(assert_equal(expected,actual));
-//}
-
 /* ************************************************************************* */
 // Extra test on elimination prompted by Michael's email to Frank 1/4/2010
 TEST( GaussianFactorGraph, elimination )
@@ -824,22 +446,6 @@ TEST( GaussianFactorGraph, constrained_single )
 	EXPECT(assert_equal(expected, actual));
 }
 
-///* ************************************************************************* */
-//SL-FIX TEST( GaussianFactorGraph, constrained_single2 )
-//{
-//	// get a graph with a constraint in it
-//	GaussianFactorGraph fg = createSingleConstraintGraph();
-//
-//	// eliminate and solve
-//	Ordering ord;
-//	ord += "yk, x";
-//	VectorValues actual = fg.optimize(ord);
-//
-//	// verify
-//	VectorValues expected = createSingleConstraintValues();
-//	EXPECT(assert_equal(expected, actual));
-//}
-
 /* ************************************************************************* */
 TEST( GaussianFactorGraph, constrained_multi1 )
 {
@@ -855,68 +461,9 @@ TEST( GaussianFactorGraph, constrained_multi1 )
 	EXPECT(assert_equal(expected, actual));
 }
 
-///* ************************************************************************* */
-//SL-FIX TEST( GaussianFactorGraph, constrained_multi2 )
-//{
-//	// get a graph with a constraint in it
-//	GaussianFactorGraph fg = createMultiConstraintGraph();
-//
-//	// eliminate and solve
-//	Ordering ord;
-//	ord += "zk, xk, y";
-//	VectorValues actual = fg.optimize(ord);
-//
-//	// verify
-//	VectorValues expected = createMultiConstraintValues();
-//	EXPECT(assert_equal(expected, actual));
-//}
-
 /* ************************************************************************* */
 
-SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1);
+static SharedDiagonal model = noiseModel::Isotropic::Sigma(2,1);
-
-// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree )
-//{
-//	GaussianFactorGraph g;
-//	Matrix I = eye(2);
-//	Vector b = Vector_(0, 0, 0);
-//	g.add(X(1), I, X(2), I, b, model);
-//	g.add(X(1), I, X(3), I, b, model);
-//	g.add(X(1), I, X(4), I, b, model);
-//	g.add(X(2), I, X(3), I, b, model);
-//	g.add(X(2), I, X(4), I, b, model);
-//	g.add(X(3), I, X(4), I, b, model);
-//
-//	map<string, string> tree = g.findMinimumSpanningTree<string, GaussianFactor>();
-//	EXPECT(tree[X(1)].compare(X(1))==0);
-//	EXPECT(tree[X(2)].compare(X(1))==0);
-//	EXPECT(tree[X(3)].compare(X(1))==0);
-//	EXPECT(tree[X(4)].compare(X(1))==0);
-//}
-
-///* ************************************************************************* */
-// SL-FIX TEST( GaussianFactorGraph, split )
-//{
-//	GaussianFactorGraph g;
-//	Matrix I = eye(2);
-//	Vector b = Vector_(0, 0, 0);
-//	g.add(X(1), I, X(2), I, b, model);
-//	g.add(X(1), I, X(3), I, b, model);
-//	g.add(X(1), I, X(4), I, b, model);
-//	g.add(X(2), I, X(3), I, b, model);
-//	g.add(X(2), I, X(4), I, b, model);
-//
-//	PredecessorMap<string> tree;
-//	tree[X(1)] = X(1);
-//	tree[X(2)] = X(1);
-//	tree[X(3)] = X(1);
-//	tree[X(4)] = X(1);
-//
-//	GaussianFactorGraph Ab1, Ab2;
-//  g.split<string, GaussianFactor>(tree, Ab1, Ab2);
-//	LONGS_EQUAL(3, Ab1.size());
-//	LONGS_EQUAL(2, Ab2.size());
-//}
 
 /* ************************************************************************* */
 TEST(GaussianFactorGraph, replace)
@@ -935,91 +482,18 @@ TEST(GaussianFactorGraph, replace)
 
 	GaussianFactorGraph actual;
 	actual.push_back(f1);
-//	actual.checkGraphConsistency();
 	actual.push_back(f2);
-//	actual.checkGraphConsistency();
 	actual.push_back(f3);
-//	actual.checkGraphConsistency();
 	actual.replace(0, f4);
-//	actual.checkGraphConsistency();
 
 	GaussianFactorGraph expected;
 	expected.push_back(f4);
-//	actual.checkGraphConsistency();
 	expected.push_back(f2);
-//	actual.checkGraphConsistency();
 	expected.push_back(f3);
-//	actual.checkGraphConsistency();
 
 	EXPECT(assert_equal(expected, actual));
 }
 
-///* ************************************************************************* */
-//TEST ( GaussianFactorGraph, combine_matrix ) {
-//	// create a small linear factor graph
-//	GaussianFactorGraph fg = createGaussianFactorGraph();
-//	Dimensions dimensions = fg.dimensions();
-//
-//	// get two factors from it and insert the factors into a vector
-//	vector<GaussianFactor::shared_ptr> lfg;
-//	lfg.push_back(fg[4 - 1]);
-//	lfg.push_back(fg[2 - 1]);
-//
-//	// combine in a factor
-//	Matrix Ab; SharedDiagonal noise;
-//	Ordering order; order += X(2), L(1), X(1);
-//	boost::tie(Ab, noise) = combineFactorsAndCreateMatrix(lfg, order, dimensions);
-//
-//	// the expected augmented matrix
-//	Matrix expAb = Matrix_(4, 7,
-//			-5.,  0., 5., 0.,  0.,  0.,-1.0,
-//			+0., -5., 0., 5.,  0.,  0., 1.5,
-//			10.,  0., 0., 0.,-10.,  0., 2.0,
-//			+0., 10., 0., 0.,  0.,-10.,-1.0);
-//
-//	// expected noise model
-//	SharedDiagonal expModel = noiseModel::Unit::Create(4);
-//
-//	EXPECT(assert_equal(expAb, Ab));
-//	EXPECT(assert_equal(*expModel, *noise));
-//}
-
-/* ************************************************************************* */
-/*
- *   x2 x1 x3 b
- *    1  1    1       1  1  0  1
- *    1    1  1  ->      1  1  1
- *         1  1             1  1
- */
-// SL-NEEDED? TEST ( GaussianFactorGraph, eliminateFrontals ) {
-//	typedef GaussianFactorGraph::sharedFactor Factor;
-//	SharedDiagonal model(Vector_(1, 0.5));
-//	GaussianFactorGraph fg;
-//	Factor factor1(new JacobianFactor(X(1), Matrix_(1,1,1.), X(2), Matrix_(1,1,1.), Vector_(1,1.),  model));
-//	Factor factor2(new JacobianFactor(X(2), Matrix_(1,1,1.), X(3), Matrix_(1,1,1.), Vector_(1,1.),  model));
-//	Factor factor3(new JacobianFactor(X(3), Matrix_(1,1,1.), X(3), Matrix_(1,1,1.), Vector_(1,1.),  model));
-//	fg.push_back(factor1);
-//	fg.push_back(factor2);
-//	fg.push_back(factor3);
-//
-//	Ordering frontals; frontals += X(2), X(1);
-//	GaussianBayesNet bn = fg.eliminateFrontals(frontals);
-//
-//	GaussianBayesNet bn_expected;
-//	GaussianBayesNet::sharedConditional conditional1(new GaussianConditional(X(2), Vector_(1, 2.), Matrix_(1, 1, 2.),
-//			X(1), Matrix_(1, 1, 1.), X(3), Matrix_(1, 1, 1.), Vector_(1, 1.)));
-//	GaussianBayesNet::sharedConditional conditional2(new GaussianConditional(X(1), Vector_(1, 0.), Matrix_(1, 1, -1.),
-//			X(3), Matrix_(1, 1, 1.), Vector_(1, 1.)));
-//	bn_expected.push_back(conditional1);
-//	bn_expected.push_back(conditional2);
-//	EXPECT(assert_equal(bn_expected, bn));
-//
-//	GaussianFactorGraph::sharedFactor factor_expected(new JacobianFactor(X(3), Matrix_(1, 1, 2.), Vector_(1, 2.), SharedDiagonal(Vector_(1, 1.))));
-//	GaussianFactorGraph fg_expected;
-//	fg_expected.push_back(factor_expected);
-//	EXPECT(assert_equal(fg_expected, fg));
-//}
-
 /* ************************************************************************* */
 TEST(GaussianFactorGraph, createSmoother2)
 {
@@ -1049,7 +523,7 @@ TEST(GaussianFactorGraph, hasConstraints)
 	EXPECT(hasConstraints(fgc2));
 
 	Ordering ordering; ordering += X(1), X(2), L(1);
-	FactorGraph<GaussianFactor> fg = createGaussianFactorGraph(ordering);
+	GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
 	EXPECT(!hasConstraints(fg));
 }
 

tests/testGaussianISAM.cpp

@@ -42,12 +42,12 @@ using symbol_shorthand::L;
 // Some numbers that should be consistent among all smoother tests
 
 static double sigmax1 = 0.786153, sigmax2 = 1.0/1.47292, sigmax3 = 0.671512, sigmax4 =
-		0.669534, sigmax5 = sigmax3, sigmax6 = sigmax2, sigmax7 = sigmax1;
+		0.669534 /*, sigmax5 = sigmax3, sigmax6 = sigmax2*/, sigmax7 = sigmax1;
 
 static const double tol = 1e-4;
 
 /* ************************************************************************* */
-TEST_UNSAFE( ISAM, iSAM_smoother )
+TEST( ISAM, iSAM_smoother )
 {
   Ordering ordering;
   for (int t = 1; t <= 7; t++) ordering += X(t);
@@ -76,31 +76,6 @@ TEST_UNSAFE( ISAM, iSAM_smoother )
 	EXPECT(assert_equal(e, optimized));
 }
 
-/* ************************************************************************* */
-// SL-FIX TEST( ISAM, iSAM_smoother2 )
-//{
-//	// Create smoother with 7 nodes
-//	GaussianFactorGraph smoother = createSmoother(7);
-//
-//	// Create initial tree from first 4 timestamps in reverse order !
-//	Ordering ord; ord += X(4),X(3),X(2),X(1);
-//	GaussianFactorGraph factors1;
-//	for (int i=0;i<7;i++) factors1.push_back(smoother[i]);
-//	GaussianISAM actual(*inference::Eliminate(factors1));
-//
-//	// run iSAM with remaining factors
-//	GaussianFactorGraph factors2;
-//	for (int i=7;i<13;i++) factors2.push_back(smoother[i]);
-//	actual.update(factors2);
-//
-//	// Create expected Bayes Tree by solving smoother with "natural" ordering
-//	Ordering ordering;
-//	for (int t = 1; t <= 7; t++) ordering += symbol('x', t);
-//	GaussianISAM expected(smoother.eliminate(ordering));
-//
-//	EXPECT(assert_equal(expected, actual));
-//}
-
 /* ************************************************************************* *
  Bayes tree for smoother with "natural" ordering:
 C1 x6 x7

tests/testGaussianISAM2.cpp

@@ -12,7 +12,7 @@
 #include <gtsam/linear/GaussianBayesNet.h>
 #include <gtsam/linear/GaussianSequentialSolver.h>
 #include <gtsam/linear/GaussianBayesTree.h>
-#include <gtsam/linear/JacobianFactorGraph.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/nonlinear/Ordering.h>
 #include <gtsam/nonlinear/Values.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>

tests/testGraph.cpp

@@ -105,6 +105,108 @@ TEST( Graph, composePoses )
 	CHECK(assert_equal(expected, *actual));
 }
 
+// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree )
+//{
+//	GaussianFactorGraph g;
+//	Matrix I = eye(2);
+//	Vector b = Vector_(0, 0, 0);
+//	g.add(X(1), I, X(2), I, b, model);
+//	g.add(X(1), I, X(3), I, b, model);
+//	g.add(X(1), I, X(4), I, b, model);
+//	g.add(X(2), I, X(3), I, b, model);
+//	g.add(X(2), I, X(4), I, b, model);
+//	g.add(X(3), I, X(4), I, b, model);
+//
+//	map<string, string> tree = g.findMinimumSpanningTree<string, GaussianFactor>();
+//	EXPECT(tree[X(1)].compare(X(1))==0);
+//	EXPECT(tree[X(2)].compare(X(1))==0);
+//	EXPECT(tree[X(3)].compare(X(1))==0);
+//	EXPECT(tree[X(4)].compare(X(1))==0);
+//}
+
+///* ************************************************************************* */
+// SL-FIX TEST( GaussianFactorGraph, split )
+//{
+//	GaussianFactorGraph g;
+//	Matrix I = eye(2);
+//	Vector b = Vector_(0, 0, 0);
+//	g.add(X(1), I, X(2), I, b, model);
+//	g.add(X(1), I, X(3), I, b, model);
+//	g.add(X(1), I, X(4), I, b, model);
+//	g.add(X(2), I, X(3), I, b, model);
+//	g.add(X(2), I, X(4), I, b, model);
+//
+//	PredecessorMap<string> tree;
+//	tree[X(1)] = X(1);
+//	tree[X(2)] = X(1);
+//	tree[X(3)] = X(1);
+//	tree[X(4)] = X(1);
+//
+//	GaussianFactorGraph Ab1, Ab2;
+//  g.split<string, GaussianFactor>(tree, Ab1, Ab2);
+//	LONGS_EQUAL(3, Ab1.size());
+//	LONGS_EQUAL(2, Ab2.size());
+//}
+
+///* ************************************************************************* */
+// SL-FIX TEST( FactorGraph, splitMinimumSpanningTree )
+//{
+//	SymbolicFactorGraph G;
+//	G.push_factor("x1", "x2");
+//	G.push_factor("x1", "x3");
+//	G.push_factor("x1", "x4");
+//	G.push_factor("x2", "x3");
+//	G.push_factor("x2", "x4");
+//	G.push_factor("x3", "x4");
+//
+//	SymbolicFactorGraph T, C;
+//	boost::tie(T, C) = G.splitMinimumSpanningTree();
+//
+//	SymbolicFactorGraph expectedT, expectedC;
+//	expectedT.push_factor("x1", "x2");
+//	expectedT.push_factor("x1", "x3");
+//	expectedT.push_factor("x1", "x4");
+//	expectedC.push_factor("x2", "x3");
+//	expectedC.push_factor("x2", "x4");
+//	expectedC.push_factor("x3", "x4");
+//	CHECK(assert_equal(expectedT,T));
+//	CHECK(assert_equal(expectedC,C));
+//}
+
+///* ************************************************************************* */
+///**
+// *  x1 - x2 - x3 - x4 - x5
+// *       |    |  / |
+// *       l1   l2   l3
+// */
+// SL-FIX TEST( FactorGraph, removeSingletons )
+//{
+//	SymbolicFactorGraph G;
+//	G.push_factor("x1", "x2");
+//	G.push_factor("x2", "x3");
+//	G.push_factor("x3", "x4");
+//	G.push_factor("x4", "x5");
+//	G.push_factor("x2", "l1");
+//	G.push_factor("x3", "l2");
+//	G.push_factor("x4", "l2");
+//	G.push_factor("x4", "l3");
+//
+//	SymbolicFactorGraph singletonGraph;
+//	set<Symbol> singletons;
+//	boost::tie(singletonGraph, singletons) = G.removeSingletons();
+//
+//	set<Symbol> singletons_excepted; singletons_excepted += "x1", "x2", "x5", "l1", "l3";
+//	CHECK(singletons_excepted == singletons);
+//
+//	SymbolicFactorGraph singletonGraph_excepted;
+//	singletonGraph_excepted.push_factor("x2", "l1");
+//	singletonGraph_excepted.push_factor("x4", "l3");
+//	singletonGraph_excepted.push_factor("x1", "x2");
+//	singletonGraph_excepted.push_factor("x4", "x5");
+//	singletonGraph_excepted.push_factor("x2", "x3");
+//	CHECK(singletonGraph_excepted.equals(singletonGraph));
+//}
+
 /* ************************************************************************* */
 int main() {
 	TestResult tr;

tests/testIterative.cpp

@@ -0,0 +1,154 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testIterative.cpp
+ *  @brief  Unit tests for iterative methods
+ *  @author Frank Dellaert
+ **/
+
+#include <tests/smallExample.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/nonlinear/NonlinearEquality.h>
+#include <gtsam/nonlinear/Ordering.h>
+#include <gtsam/nonlinear/Symbol.h>
+#include <gtsam/linear/GaussianSequentialSolver.h>
+#include <gtsam/linear/iterative.h>
+#include <gtsam/geometry/Pose2.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+using namespace std;
+using namespace gtsam;
+using namespace example;
+using symbol_shorthand::X; // to create pose keys
+using symbol_shorthand::L; // to create landmark keys
+
+static ConjugateGradientParameters parameters;
+// add following below to add printing:
+// parameters.verbosity_ = ConjugateGradientParameters::COMPLEXITY;
+
+/* ************************************************************************* */
+TEST( Iterative, steepestDescent )
+{
+	// Create factor graph
+	Ordering ordering;
+	ordering += L(1), X(1), X(2);
+	GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
+
+  // eliminate and solve
+  VectorValues expected = *GaussianSequentialSolver(fg).optimize();
+
+	// Do gradient descent
+	VectorValues zero = VectorValues::Zero(expected); // TODO, how do we do this normally?
+	VectorValues actual = steepestDescent(fg, zero, parameters);
+	CHECK(assert_equal(expected,actual,1e-2));
+}
+
+/* ************************************************************************* */
+TEST( Iterative, conjugateGradientDescent )
+{
+  // Create factor graph
+  Ordering ordering;
+  ordering += L(1), X(1), X(2);
+  GaussianFactorGraph fg = createGaussianFactorGraph(ordering);
+
+  // eliminate and solve
+  VectorValues expected = *GaussianSequentialSolver(fg).optimize();
+
+  // get matrices
+	Matrix A;
+	Vector b;
+	Vector x0 = gtsam::zero(6);
+	boost::tie(A, b) = fg.jacobian();
+	Vector expectedX = Vector_(6, -0.1, 0.1, -0.1, -0.1, 0.1, -0.2);
+
+	// Do conjugate gradient descent, System version
+	System Ab(A, b);
+	Vector actualX = conjugateGradientDescent(Ab, x0, parameters);
+	CHECK(assert_equal(expectedX,actualX,1e-9));
+
+	// Do conjugate gradient descent, Matrix version
+	Vector actualX2 = conjugateGradientDescent(A, b, x0, parameters);
+	CHECK(assert_equal(expectedX,actualX2,1e-9));
+
+	// Do conjugate gradient descent on factor graph
+	VectorValues zero = VectorValues::Zero(expected);
+	VectorValues actual = conjugateGradientDescent(fg, zero, parameters);
+	CHECK(assert_equal(expected,actual,1e-2));
+}
+
+/* ************************************************************************* */
+TEST( Iterative, conjugateGradientDescent_hard_constraint )
+{
+  Values config;
+  Pose2 pose1 = Pose2(0.,0.,0.);
+  config.insert(X(1), pose1);
+  config.insert(X(2), Pose2(1.5,0.,0.));
+
+	NonlinearFactorGraph graph;
+  graph.add(NonlinearEquality<Pose2>(X(1), pose1));
+  graph.add(BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)));
+
+  Ordering ordering;
+  ordering += X(1), X(2);
+  boost::shared_ptr<GaussianFactorGraph> fg = graph.linearize(config,ordering);
+
+  VectorValues zeros = VectorValues::Zero(2, 3);
+
+  ConjugateGradientParameters parameters;
+  parameters.setEpsilon_abs(1e-3);
+  parameters.setEpsilon_rel(1e-5);
+  parameters.setMaxIterations(100);
+  VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters);
+
+  VectorValues expected;
+  expected.insert(0, zero(3));
+  expected.insert(1, Vector_(3,-0.5,0.,0.));
+  CHECK(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+TEST( Iterative, conjugateGradientDescent_soft_constraint )
+{
+	Values config;
+	config.insert(X(1), Pose2(0.,0.,0.));
+	config.insert(X(2), Pose2(1.5,0.,0.));
+
+	NonlinearFactorGraph graph;
+	graph.add(PriorFactor<Pose2>(X(1), Pose2(0.,0.,0.), noiseModel::Isotropic::Sigma(3, 1e-10)));
+	graph.add(BetweenFactor<Pose2>(X(1),X(2), Pose2(1.,0.,0.), noiseModel::Isotropic::Sigma(3, 1)));
+
+  Ordering ordering;
+  ordering += X(1), X(2);
+  boost::shared_ptr<GaussianFactorGraph> fg = graph.linearize(config,ordering);
+
+  VectorValues zeros = VectorValues::Zero(2, 3);
+
+	ConjugateGradientParameters parameters;
+	parameters.setEpsilon_abs(1e-3);
+	parameters.setEpsilon_rel(1e-5);
+	parameters.setMaxIterations(100);
+	VectorValues actual = conjugateGradientDescent(*fg, zeros, parameters);
+
+	VectorValues expected;
+	expected.insert(0, zero(3));
+	expected.insert(1, Vector_(3,-0.5,0.,0.));
+	CHECK(assert_equal(expected, actual));
+}
+
+/* ************************************************************************* */
+int main() {
+	TestResult tr;
+	return TestRegistry::runAllTests(tr);
+}
+/* ************************************************************************* */

tests/testNonlinearFactor.cpp

@@ -73,8 +73,8 @@ TEST( NonlinearFactor, equals2 )
   Graph::sharedFactor f0 = fg[0], f1 = fg[1];
 
   CHECK(f0->equals(*f0));
-// SL-FIX  CHECK(!f0->equals(*f1));
+	CHECK(!f0->equals(*f1));
-// SL-FIX  CHECK(!f1->equals(*f0));
+	CHECK(!f1->equals(*f0));
 }
 
 /* ************************************************************************* */

tests/testSubgraphPreconditioner.cpp

@@ -17,8 +17,9 @@
 
 #include <tests/smallExample.h>
 #include <gtsam/nonlinear/Ordering.h>
+#include <gtsam/nonlinear/Symbol.h>
 #include <gtsam/linear/iterative.h>
-#include <gtsam/linear/JacobianFactorGraph.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
 #include <gtsam/linear/GaussianSequentialSolver.h>
 #include <gtsam/linear/SubgraphPreconditioner.h>
 #include <gtsam/base/numericalDerivative.h>
@@ -35,19 +36,30 @@ using namespace gtsam;
 using namespace example;
 
 // define keys
-Key i3003 = 3003, i2003 = 2003, i1003 = 1003;
+// Create key for simulated planar graph
-Key i3002 = 3002, i2002 = 2002, i1002 = 1002;
+Symbol key(int x, int y) {
-Key i3001 = 3001, i2001 = 2001, i1001 = 1001;
+  return symbol_shorthand::X(1000*x+y);
+}
 
-// TODO fix Ordering::equals, because the ordering *is* correct !
 /* ************************************************************************* */
-//TEST( SubgraphPreconditioner, planarOrdering )
+TEST( SubgraphPreconditioner, planarOrdering ) {
-//{
+  // Check canonical ordering
-//  // Check canonical ordering
+  Ordering expected, ordering = planarOrdering(3);
-//  Ordering expected, ordering = planarOrdering(3);
+  expected +=
-//  expected += i3003, i2003, i1003, i3002, i2002, i1002, i3001, i2001, i1001;
+      key(3, 3), key(2, 3), key(1, 3),
-//  CHECK(assert_equal(expected,ordering));
+      key(3, 2), key(2, 2), key(1, 2),
-//}
+      key(3, 1), key(2, 1), key(1, 1);
+  CHECK(assert_equal(expected,ordering));
+}
+
+/* ************************************************************************* */
+/** unnormalized error */
+static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
+  double total_error = 0.;
+  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg)
+    total_error += factor->error(x);
+  return total_error;
+}
 
 /* ************************************************************************* */
 TEST( SubgraphPreconditioner, planarGraph )
@@ -58,7 +70,7 @@ TEST( SubgraphPreconditioner, planarGraph )
   boost::tie(A, xtrue) = planarGraph(3);
   LONGS_EQUAL(13,A.size());
   LONGS_EQUAL(9,xtrue.size());
-  DOUBLES_EQUAL(0,A.error(xtrue),1e-9); // check zero error for xtrue
+  DOUBLES_EQUAL(0,error(A,xtrue),1e-9); // check zero error for xtrue
 
   // Check that xtrue is optimal
   GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(A).eliminate();
@@ -67,143 +79,143 @@ TEST( SubgraphPreconditioner, planarGraph )
 }
 
 /* ************************************************************************* */
-//TEST( SubgraphPreconditioner, splitOffPlanarTree )
+TEST( SubgraphPreconditioner, splitOffPlanarTree )
-//{
+{
-//  // Build a planar graph
+  // Build a planar graph
-//  GaussianFactorGraph A;
+  GaussianFactorGraph A;
-//  VectorValues xtrue;
+  VectorValues xtrue;
-//  boost::tie(A, xtrue) = planarGraph(3);
+  boost::tie(A, xtrue) = planarGraph(3);
-//
+
-//  // Get the spanning tree and constraints, and check their sizes
+  // Get the spanning tree and constraints, and check their sizes
-//  JacobianFactorGraph T, C;
+  GaussianFactorGraph T, C;
-//  // TODO big mess: GFG and JFG mess !!!
+  boost::tie(T, C) = splitOffPlanarTree(3, A);
-//  boost::tie(T, C) = splitOffPlanarTree(3, A);
+  LONGS_EQUAL(9,T.size());
-//  LONGS_EQUAL(9,T.size());
+  LONGS_EQUAL(4,C.size());
-//  LONGS_EQUAL(4,C.size());
+
-//
+  // Check that the tree can be solved to give the ground xtrue
-//  // Check that the tree can be solved to give the ground xtrue
+  GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate();
-//  GaussianBayesNet::shared_ptr R1 = GaussianSequentialSolver(T).eliminate();
+  VectorValues xbar = optimize(*R1);
-//  VectorValues xbar = optimize(*R1);
+  CHECK(assert_equal(xtrue,xbar));
-//  CHECK(assert_equal(xtrue,xbar));
+}
-//}
 
 /* ************************************************************************* */
-//TEST( SubgraphPreconditioner, system )
+
-//{
+TEST( SubgraphPreconditioner, system )
-//  // Build a planar graph
+{
-//  JacobianFactorGraph Ab;
+  // Build a planar graph
-//  VectorValues xtrue;
+  GaussianFactorGraph Ab;
-//  size_t N = 3;
+  VectorValues xtrue;
-//  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
+  size_t N = 3;
-//
+  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
-//  // Get the spanning tree and corresponding ordering
+
-//  GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
+  // Get the spanning tree and corresponding ordering
-//  boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
+  GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
-//  SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
+  boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
-//  SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
+  SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
-//
+  SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
-//  // Eliminate the spanning tree to build a prior
+
-//  SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
+  // Eliminate the spanning tree to build a prior
-//  VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1
+  SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
-//
+  VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1
-//  // Create Subgraph-preconditioned system
+
-//  VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
+  // Create Subgraph-preconditioned system
-//  SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared);
+  VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
-//
+  SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
-//  // Create zero config
+
-//  VectorValues zeros = VectorValues::Zero(xbar);
+  // Create zero config
-//
+  VectorValues zeros = VectorValues::Zero(xbar);
-//  // Set up y0 as all zeros
+
-//  VectorValues y0 = zeros;
+  // Set up y0 as all zeros
-//
+  VectorValues y0 = zeros;
-//  // y1 = perturbed y0
+
-//  VectorValues y1 = zeros;
+  // y1 = perturbed y0
-//  y1[i2003] = Vector_(2, 1.0, -1.0);
+  VectorValues y1 = zeros;
-//
+  y1[1] = Vector_(2, 1.0, -1.0);
-//  // Check corresponding x  values
+
-//  VectorValues expected_x1 = xtrue, x1 = system.x(y1);
+  // Check corresponding x  values
-//  expected_x1[i2003] = Vector_(2, 2.01, 2.99);
+  VectorValues expected_x1 = xtrue, x1 = system.x(y1);
-//  expected_x1[i3003] = Vector_(2, 3.01, 2.99);
+  expected_x1[1] = Vector_(2, 2.01, 2.99);
-//  CHECK(assert_equal(xtrue, system.x(y0)));
+  expected_x1[0] = Vector_(2, 3.01, 2.99);
-//  CHECK(assert_equal(expected_x1,system.x(y1)));
+  CHECK(assert_equal(xtrue, system.x(y0)));
-//
+  CHECK(assert_equal(expected_x1,system.x(y1)));
-//  // Check errors
+
-////  DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9); // TODO !
+  // Check errors
-////  DOUBLES_EQUAL(3,error(Ab,x1),1e-9); // TODO !
+  DOUBLES_EQUAL(0,error(Ab,xtrue),1e-9);
-//  DOUBLES_EQUAL(0,error(system,y0),1e-9);
+  DOUBLES_EQUAL(3,error(Ab,x1),1e-9);
-//  DOUBLES_EQUAL(3,error(system,y1),1e-9);
+  DOUBLES_EQUAL(0,error(system,y0),1e-9);
-//
+  DOUBLES_EQUAL(3,error(system,y1),1e-9);
-//  // Test gradient in x
+
-//  VectorValues expected_gx0 = zeros;
+  // Test gradient in x
-//  VectorValues expected_gx1 = zeros;
+  VectorValues expected_gx0 = zeros;
-//  CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue)));
+  VectorValues expected_gx1 = zeros;
-//  expected_gx1[i1003] = Vector_(2, -100., 100.);
+  CHECK(assert_equal(expected_gx0,gradient(Ab,xtrue)));
-//  expected_gx1[i2002] = Vector_(2, -100., 100.);
+  expected_gx1[2] = Vector_(2, -100., 100.);
-//  expected_gx1[i2003] = Vector_(2, 200., -200.);
+  expected_gx1[4] = Vector_(2, -100., 100.);
-//  expected_gx1[i3002] = Vector_(2, -100., 100.);
+  expected_gx1[1] = Vector_(2, 200., -200.);
-//  expected_gx1[i3003] = Vector_(2, 100., -100.);
+  expected_gx1[3] = Vector_(2, -100., 100.);
-//  CHECK(assert_equal(expected_gx1,gradient(Ab,x1)));
+  expected_gx1[0] = Vector_(2, 100., -100.);
-//
+  CHECK(assert_equal(expected_gx1,gradient(Ab,x1)));
-//  // Test gradient in y
+
-//  VectorValues expected_gy0 = zeros;
+  // Test gradient in y
-//  VectorValues expected_gy1 = zeros;
+  VectorValues expected_gy0 = zeros;
-//  expected_gy1[i1003] = Vector_(2, 2., -2.);
+  VectorValues expected_gy1 = zeros;
-//  expected_gy1[i2002] = Vector_(2, -2., 2.);
+  expected_gy1[2] = Vector_(2, 2., -2.);
-//  expected_gy1[i2003] = Vector_(2, 3., -3.);
+  expected_gy1[4] = Vector_(2, -2., 2.);
-//  expected_gy1[i3002] = Vector_(2, -1., 1.);
+  expected_gy1[1] = Vector_(2, 3., -3.);
-//  expected_gy1[i3003] = Vector_(2, 1., -1.);
+  expected_gy1[3] = Vector_(2, -1., 1.);
-//  CHECK(assert_equal(expected_gy0,gradient(system,y0)));
+  expected_gy1[0] = Vector_(2, 1., -1.);
-//  CHECK(assert_equal(expected_gy1,gradient(system,y1)));
+  CHECK(assert_equal(expected_gy0,gradient(system,y0)));
-//
+  CHECK(assert_equal(expected_gy1,gradient(system,y1)));
-//  // Check it numerically for good measure
+
-//  // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1)
+  // Check it numerically for good measure
-//  //	Vector numerical_g1 = numericalGradient<VectorValues> (error, y1, 0.001);
+  // TODO use boost::bind(&SubgraphPreconditioner::error,&system,_1)
-//  //	Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2.,
+  //	Vector numerical_g1 = numericalGradient<VectorValues> (error, y1, 0.001);
-//  //			3., -3., 0., 0., -1., 1., 1., -1.);
+  //	Vector expected_g1 = Vector_(18, 0., 0., 0., 0., 2., -2., 0., 0., -2., 2.,
-//  //	CHECK(assert_equal(expected_g1,numerical_g1));
+  //			3., -3., 0., 0., -1., 1., 1., -1.);
-//}
+  //	CHECK(assert_equal(expected_g1,numerical_g1));
+}
 
 /* ************************************************************************* */
-//TEST( SubgraphPreconditioner, conjugateGradients )
+TEST( SubgraphPreconditioner, conjugateGradients )
-//{
+{
-//  // Build a planar graph
+  // Build a planar graph
-//  GaussianFactorGraph Ab;
+  GaussianFactorGraph Ab;
-//  VectorValues xtrue;
+  VectorValues xtrue;
-//  size_t N = 3;
+  size_t N = 3;
-//  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
+  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
-//
+
-//  // Get the spanning tree and corresponding ordering
+  // Get the spanning tree and corresponding ordering
-//  GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
+  GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
-//  boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
+  boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
-//  SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
+  SubgraphPreconditioner::sharedFG Ab1(new GaussianFactorGraph(Ab1_));
-//  SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
+  SubgraphPreconditioner::sharedFG Ab2(new GaussianFactorGraph(Ab2_));
-//
+
-//  // Eliminate the spanning tree to build a prior
+  // Eliminate the spanning tree to build a prior
-//  Ordering ordering = planarOrdering(N);
+  Ordering ordering = planarOrdering(N);
-//  SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
+  SubgraphPreconditioner::sharedBayesNet Rc1 = GaussianSequentialSolver(Ab1_).eliminate(); // R1*x-c1
-//  VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1
+  VectorValues xbar = optimize(*Rc1); // xbar = inv(R1)*c1
-//
+
-//  // Create Subgraph-preconditioned system
+  // Create Subgraph-preconditioned system
-//  VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
+  VectorValues::shared_ptr xbarShared(new VectorValues(xbar)); // TODO: horrible
-//  SubgraphPreconditioner system(Ab1, Ab2, Rc1, xbarShared);
+  SubgraphPreconditioner system(Ab2, Rc1, xbarShared);
-//
+
-//  // Create zero config y0 and perturbed config y1
+  // Create zero config y0 and perturbed config y1
-//  VectorValues y0 = VectorValues::Zero(xbar);
+  VectorValues y0 = VectorValues::Zero(xbar);
-//
+
-//  VectorValues y1 = y0;
+  VectorValues y1 = y0;
-//  y1[i2003] = Vector_(2, 1.0, -1.0);
+  y1[1] = Vector_(2, 1.0, -1.0);
-//  VectorValues x1 = system.x(y1);
+  VectorValues x1 = system.x(y1);
-//
+
-//  // Solve for the remaining constraints using PCG
+  // Solve for the remaining constraints using PCG
-//  ConjugateGradientParameters parameters;
+  ConjugateGradientParameters parameters;
-////  VectorValues actual = gtsam::conjugateGradients<SubgraphPreconditioner,
+  VectorValues actual = conjugateGradients<SubgraphPreconditioner,
-////      VectorValues, Errors>(system, y1, verbose, epsilon, epsilon, maxIterations);
+      VectorValues, Errors>(system, y1, parameters);
-////  CHECK(assert_equal(y0,actual));
+  CHECK(assert_equal(y0,actual));
-//
+
-//  // Compare with non preconditioned version:
+  // Compare with non preconditioned version:
-//  VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters);
+  VectorValues actual2 = conjugateGradientDescent(Ab, x1, parameters);
-//  CHECK(assert_equal(xtrue,actual2,1e-4));
+  CHECK(assert_equal(xtrue,actual2,1e-4));
-//}
+}
 
 /* ************************************************************************* */
 int main() { TestResult tr; return TestRegistry::runAllTests(tr); }

tests/testSubgraphSolver.cpp

@@ -0,0 +1,114 @@
+/* ----------------------------------------------------------------------------
+
+ * 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   testSubgraphSolver.cpp
+ *  @brief  Unit tests for SubgraphSolver
+ *  @author Yong-Dian Jian
+ **/
+
+#include <tests/smallExample.h>
+#include <gtsam/nonlinear/Ordering.h>
+#include <gtsam/nonlinear/Symbol.h>
+#include <gtsam/linear/GaussianBayesNet.h>
+#include <gtsam/linear/iterative.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/SubgraphSolver.h>
+#include <gtsam/inference/EliminationTree-inl.h>
+#include <gtsam/base/numericalDerivative.h>
+
+#include <CppUnitLite/TestHarness.h>
+
+#include <boost/foreach.hpp>
+#include <boost/tuple/tuple.hpp>
+#include <boost/assign/std/list.hpp>
+using namespace boost::assign;
+
+using namespace std;
+using namespace gtsam;
+using namespace example;
+
+/* ************************************************************************* */
+/** unnormalized error */
+static double error(const GaussianFactorGraph& fg, const VectorValues& x) {
+  double total_error = 0.;
+  BOOST_FOREACH(const GaussianFactor::shared_ptr& factor, fg)
+    total_error += factor->error(x);
+  return total_error;
+}
+
+
+/* ************************************************************************* */
+TEST( SubgraphSolver, constructor1 )
+{
+  // Build a planar graph
+  GaussianFactorGraph Ab;
+  VectorValues xtrue;
+  size_t N = 3;
+  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
+
+  // The first constructor just takes a factor graph (and parameters)
+  // and it will split the graph into A1 and A2, where A1 is a spanning tree
+  SubgraphSolverParameters parameters;
+  SubgraphSolver solver(Ab, parameters);
+  VectorValues optimized = solver.optimize(); // does PCG optimization
+  DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
+}
+
+/* ************************************************************************* */
+TEST( SubgraphSolver, constructor2 )
+{
+  // Build a planar graph
+  GaussianFactorGraph Ab;
+  VectorValues xtrue;
+  size_t N = 3;
+  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
+
+  // Get the spanning tree and corresponding ordering
+  GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
+  boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
+
+  // The second constructor takes two factor graphs,
+  // so the caller can specify the preconditioner (Ab1) and the constraints that are left out (Ab2)
+  SubgraphSolverParameters parameters;
+  SubgraphSolver solver(Ab1_, Ab2_, parameters);
+  VectorValues optimized = solver.optimize();
+  DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
+}
+
+/* ************************************************************************* */
+TEST( SubgraphSolver, constructor3 )
+{
+  // Build a planar graph
+  GaussianFactorGraph Ab;
+  VectorValues xtrue;
+  size_t N = 3;
+  boost::tie(Ab, xtrue) = planarGraph(N); // A*x-b
+
+  // Get the spanning tree and corresponding ordering
+  GaussianFactorGraph Ab1_, Ab2_; // A1*x-b1 and A2*x-b2
+  boost::tie(Ab1_, Ab2_) = splitOffPlanarTree(N, Ab);
+
+  // The caller solves |A1*x-b1|^2 == |R1*x-c1|^2 via QR factorization, where R1 is square UT
+  GaussianBayesNet::shared_ptr Rc1 = //
+      EliminationTree<GaussianFactor>::Create(Ab1_)->eliminate(&EliminateQR);
+
+  // The third constructor allows the caller to pass an already solved preconditioner Rc1_
+  // as a Bayes net, in addition to the "loop closing constraints" Ab2, as before
+  SubgraphSolverParameters parameters;
+  SubgraphSolver solver(Rc1, Ab2_, parameters);
+  VectorValues optimized = solver.optimize();
+  DOUBLES_EQUAL(0.0, error(Ab, optimized), 1e-5);
+}
+
+/* ************************************************************************* */
+int main() { TestResult tr; return TestRegistry::runAllTests(tr); }
+/* ************************************************************************* */

wrap/Class.cpp

@@ -332,7 +332,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const
     { 
 
 	    string up_name  = boost::to_upper_copy(name);
-        proxyFile.oss << "%" << up_name << "(";
+        proxyFile.oss << "%" << name << "(";
         unsigned int i = 0;
 		BOOST_FOREACH(const Argument& arg, argList) 
 		{
@@ -352,7 +352,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const
         BOOST_FOREACH(ArgumentList argList, m.argLists) 
         { 
 	        string up_name  = boost::to_upper_copy(m.name);
-            proxyFile.oss << "%" << up_name << "(";
+            proxyFile.oss << "%" << m.name << "(";
             unsigned int i = 0;
 		    BOOST_FOREACH(const Argument& arg, argList) 
 		    {
@@ -373,7 +373,7 @@ void Class::comment_fragment(FileWriter& proxyFile) const
         BOOST_FOREACH(ArgumentList argList, m.argLists) 
         { 
 	        string up_name  = boost::to_upper_copy(m.name);
-            proxyFile.oss << "%" << up_name << "(";
+            proxyFile.oss << "%" << m.name << "(";
             unsigned int i = 0;
 		    BOOST_FOREACH(const Argument& arg, argList) 
 		    {

wrap/tests/expected/Point2.m

@@ -1,15 +1,15 @@
 %-------Constructors-------
-%POINT2()
+%Point2()
-%POINT2(double x, double y)
+%Point2(double x, double y)
 % 
 %-------Methods-------
-%ARGCHAR(char a) : returns void
+%argChar(char a) : returns void
-%ARGUCHAR(unsigned char a) : returns void
+%argUChar(unsigned char a) : returns void
-%DIM() : returns int
+%dim() : returns int
-%RETURNCHAR() : returns char
+%returnChar() : returns char
-%VECTORCONFUSION() : returns VectorNotEigen
+%vectorConfusion() : returns VectorNotEigen
-%X() : returns double
+%x() : returns double
-%Y() : returns double
+%y() : returns double
 % 
 %-------Static Methods-------
 %

wrap/tests/expected/Point3.m

@@ -1,12 +1,12 @@
 %-------Constructors-------
-%POINT3(double x, double y, double z)
+%Point3(double x, double y, double z)
 % 
 %-------Methods-------
-%NORM() : returns double
+%norm() : returns double
 % 
 %-------Static Methods-------
-%STATICFUNCTIONRET(double z) : returns Point3
+%StaticFunctionRet(double z) : returns Point3
-%STATICFUNCTION() : returns double
+%staticFunction() : returns double
 %
 %For more detailed documentation on GTSAM go to our Doxygen page, which can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
 classdef Point3 < handle

wrap/tests/expected/Test.m

@@ -1,27 +1,27 @@
 %-------Constructors-------
-%TEST()
+%Test()
-%TEST(double a, Matrix b)
+%Test(double a, Matrix b)
 % 
 %-------Methods-------
-%ARG_EIGENCONSTREF(Matrix value) : returns void
+%arg_EigenConstRef(Matrix value) : returns void
-%CREATE_MIXEDPTRS() : returns pair< Test, SharedTest >
+%create_MixedPtrs() : returns pair< Test, SharedTest >
-%CREATE_PTRS() : returns pair< SharedTest, SharedTest >
+%create_ptrs() : returns pair< SharedTest, SharedTest >
-%PRINT() : returns void
+%print() : returns void
-%RETURN_POINT2PTR(bool value) : returns Point2
+%return_Point2Ptr(bool value) : returns Point2
-%RETURN_TEST(Test value) : returns Test
+%return_Test(Test value) : returns Test
-%RETURN_TESTPTR(Test value) : returns Test
+%return_TestPtr(Test value) : returns Test
-%RETURN_BOOL(bool value) : returns bool
+%return_bool(bool value) : returns bool
-%RETURN_DOUBLE(double value) : returns double
+%return_double(double value) : returns double
-%RETURN_FIELD(Test t) : returns bool
+%return_field(Test t) : returns bool
-%RETURN_INT(int value) : returns int
+%return_int(int value) : returns int
-%RETURN_MATRIX1(Matrix value) : returns Matrix
+%return_matrix1(Matrix value) : returns Matrix
-%RETURN_MATRIX2(Matrix value) : returns Matrix
+%return_matrix2(Matrix value) : returns Matrix
-%RETURN_PAIR(Vector v, Matrix A) : returns pair< Vector, Matrix >
+%return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix >
-%RETURN_PTRS(Test p1, Test p2) : returns pair< SharedTest, SharedTest >
+%return_ptrs(Test p1, Test p2) : returns pair< SharedTest, SharedTest >
-%RETURN_SIZE_T(size_t value) : returns size_t
+%return_size_t(size_t value) : returns size_t
-%RETURN_STRING(string value) : returns string
+%return_string(string value) : returns string
-%RETURN_VECTOR1(Vector value) : returns Vector
+%return_vector1(Vector value) : returns Vector
-%RETURN_VECTOR2(Vector value) : returns Vector
+%return_vector2(Vector value) : returns Vector
 % 
 %-------Static Methods-------
 %

wrap/tests/expected_namespaces/+ns1/ClassA.m

@@ -1,5 +1,5 @@
 %-------Constructors-------
-%CLASSA()
+%ClassA()
 % 
 %-------Methods-------
 % 

wrap/tests/expected_namespaces/+ns1/ClassB.m

@@ -1,5 +1,5 @@
 %-------Constructors-------
-%CLASSB()
+%ClassB()
 % 
 %-------Methods-------
 % 

wrap/tests/expected_namespaces/+ns2/+ns3/ClassB.m

@@ -1,5 +1,5 @@
 %-------Constructors-------
-%CLASSB()
+%ClassB()
 % 
 %-------Methods-------
 % 

wrap/tests/expected_namespaces/+ns2/ClassA.m

@@ -1,13 +1,13 @@
 %-------Constructors-------
-%CLASSA()
+%ClassA()
 % 
 %-------Methods-------
-%MEMBERFUNCTION() : returns double
+%memberFunction() : returns double
-%NSARG(ClassB arg) : returns int
+%nsArg(ClassB arg) : returns int
-%NSRETURN(double q) : returns ns2::ns3::ClassB
+%nsReturn(double q) : returns ns2::ns3::ClassB
 % 
 %-------Static Methods-------
-%AFUNCTION() : returns double
+%afunction() : returns double
 %
 %For more detailed documentation on GTSAM go to our Doxygen page, which can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
 classdef ClassA < handle

wrap/tests/expected_namespaces/+ns2/ClassC.m

@@ -1,5 +1,5 @@
 %-------Constructors-------
-%CLASSC()
+%ClassC()
 % 
 %-------Methods-------
 % 

wrap/tests/expected_namespaces/ClassD.m

@@ -1,5 +1,5 @@
 %-------Constructors-------
-%CLASSD()
+%ClassD()
 % 
 %-------Methods-------
 %