Merged gtborg/gtsam into develop

.cproject

@@ -568,6 +568,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>
@@ -575,6 +576,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>
@@ -622,6 +624,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>
@@ -629,6 +632,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>
@@ -636,6 +640,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>
@@ -651,6 +656,7 @@
 			</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>
@@ -728,46 +734,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testValues.run</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">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testOrdering.run</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">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testKey.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testLinearContainerFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testLinearContainerFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testWhiteNoiseFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j6 -j8</buildArguments>
-				<buildTarget>testWhiteNoiseFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1114,6 +1080,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>
@@ -1159,6 +1126,14 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testParticleFactor.run" path="build/gtsam_unstable/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testParticleFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1239,14 +1214,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testParticleFactor.run" path="build/gtsam_unstable/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testParticleFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="check" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1351,6 +1318,22 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testImuFactor.run" path="build/gtsam/navigation" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testImuFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testCombinedImuFactor.run" path="build/gtsam/navigation" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testCombinedImuFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="all" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -1433,7 +1416,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>
@@ -1473,7 +1455,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>
@@ -1481,7 +1462,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>
@@ -1495,22 +1475,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testImuFactor.run" path="build/gtsam/navigation" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testImuFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testCombinedImuFactor.run" path="build/gtsam/navigation" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testCombinedImuFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j5</buildArguments>
@@ -1768,6 +1732,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>
@@ -1775,6 +1740,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>
@@ -1782,6 +1748,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>
@@ -1789,6 +1756,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>
@@ -2217,70 +2185,6 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
-			<target name="testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testGeneralSFMFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testProjectionFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testProjectionFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testGeneralSFMFactor_Cal3Bundler.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testGeneralSFMFactor_Cal3Bundler.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testAntiFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j6 -j8</buildArguments>
-				<buildTarget>testAntiFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testBetweenFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j6 -j8</buildArguments>
-				<buildTarget>testBetweenFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testDataset.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testDataset.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testEssentialMatrixFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testEssentialMatrixFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
-			<target name="testRotateFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
-				<buildCommand>make</buildCommand>
-				<buildArguments>-j5</buildArguments>
-				<buildTarget>testRotateFactor.run</buildTarget>
-				<stopOnError>true</stopOnError>
-				<useDefaultCommand>true</useDefaultCommand>
-				<runAllBuilders>true</runAllBuilders>
-			</target>
 			<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -2547,6 +2451,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>
@@ -2554,6 +2459,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>
@@ -2561,6 +2467,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>
@@ -2678,6 +2585,70 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="testAntiFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testAntiFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testBetweenFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testBetweenFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testDataset.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testDataset.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testEssentialMatrixFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testEssentialMatrixFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGeneralSFMFactor_Cal3Bundler.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testGeneralSFMFactor_Cal3Bundler.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testGeneralSFMFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testGeneralSFMFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testProjectionFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testProjectionFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testRotateFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testRotateFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j2</buildArguments>
@@ -2830,6 +2801,70 @@
 				<useDefaultCommand>true</useDefaultCommand>
 				<runAllBuilders>true</runAllBuilders>
 			</target>
+			<target name="Pose2SLAMExample_lago.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>Pose2SLAMExample_lago.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="Pose2SLAMExample_g2o.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>Pose2SLAMExample_g2o.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testLago.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testLago.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testLinearContainerFactor.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testLinearContainerFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testOrdering.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testOrdering.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testValues.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testValues.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="testWhiteNoiseFactor.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>testWhiteNoiseFactor.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
+			<target name="timeLago.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
+				<buildCommand>make</buildCommand>
+				<buildArguments>-j5</buildArguments>
+				<buildTarget>timeLago.run</buildTarget>
+				<stopOnError>true</stopOnError>
+				<useDefaultCommand>true</useDefaultCommand>
+				<runAllBuilders>true</runAllBuilders>
+			</target>
 			<target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
 				<buildCommand>make</buildCommand>
 				<buildArguments>-j4</buildArguments>
@@ -2848,7 +2883,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>

.gitignore

@@ -2,3 +2,5 @@
 /doc*
 *.pyc
 *.DS_Store
+/examples/Data/dubrovnik-3-7-pre-rewritten.txt
+/examples/Data/pose2example-rewritten.txt

CMakeLists.txt

@@ -4,7 +4,7 @@ cmake_minimum_required(VERSION 2.6)
 
 # Set the version number for the library
 set (GTSAM_VERSION_MAJOR 3)
-set (GTSAM_VERSION_MINOR 0)
+set (GTSAM_VERSION_MINOR 1)
 set (GTSAM_VERSION_PATCH 0)
 math (EXPR GTSAM_VERSION_NUMERIC "10000 * ${GTSAM_VERSION_MAJOR} + 100 * ${GTSAM_VERSION_MINOR} + ${GTSAM_VERSION_PATCH}")
 set (GTSAM_VERSION_STRING "${GTSAM_VERSION_MAJOR}.${GTSAM_VERSION_MINOR}.${GTSAM_VERSION_PATCH}")
@@ -273,6 +273,13 @@ if(MSVC)
 	add_definitions(/wd4251 /wd4275 /wd4251 /wd4661 /wd4344) # Disable non-DLL-exported base class and other warnings
 endif()
 
+# GCC 4.8+ complains about local typedefs which we use for shared_ptr etc.
+if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU")
+  if (NOT CMAKE_CXX_COMPILER_VERSION VERSION_LESS 4.8)
+    add_definitions(-Wno-unused-local-typedefs)
+  endif()
+endif()
+
 if(GTSAM_ENABLE_CONSISTENCY_CHECKS)
   add_definitions(-DGTSAM_EXTRA_CONSISTENCY_CHECKS)
 endif()

examples/Data/dubrovnik-3-7-pre-rewritten.txt

@@ -1,80 +0,0 @@
-3 7 19
-
-0 0 -385.989990234375 387.1199951171875
-1 0 -38.439998626708984375 492.1199951171875
-2 0 -667.91998291015625 123.1100006103515625
-0 1 383.8800048828125 -15.299989700317382812
-1 1 559.75 -106.15000152587890625
-0 2 591.54998779296875 136.44000244140625
-1 2 863.8599853515625 -23.469970703125
-2 2 494.720001220703125 112.51999664306640625
-0 3 592.5 125.75
-1 3 861.08001708984375 -35.219970703125
-2 3 498.540008544921875 101.55999755859375
-0 4 348.720001220703125 558.3800048828125
-1 4 776.030029296875 483.529998779296875
-2 4 7.7800288200378417969 326.350006103515625
-0 5 14.010009765625 96.420013427734375
-1 5 207.1300048828125 118.3600006103515625
-0 6 202.7599945068359375 340.989990234375
-1 6 543.18011474609375 294.80999755859375
-2 6 -58.419979095458984375 110.8300018310546875
-
-  0.29656188120312942935
--0.035318354384285870207
-  0.31252101755032046793
-0.47230274932665988752
--0.3572340863744113415
--2.0517704282499575896
-1430.031982421875
--7.5572756941255647689e-08
-3.2377570134516087119e-14
-
- 0.28532097381985194184
--0.27699838370789808817
-0.048601169984112867206
-  -1.2598695987143850861
--0.049063798188844320869
-  -1.9586867140445654023
-1432.137451171875
--7.3171918302250560373e-08
-3.1759419042137054801e-14
-
-0.057491325683772541433
- 0.34853090049579965592
- 0.47985129303736057116
- 8.1963904289063389541
- 6.5146840788718787252
--3.8392804395897406344
-1572.047119140625
--1.5962623223231275915e-08
--1.6507904730136101212e-14
-
--11.317351620610928364
-3.3594874875767186673
--42.755222607849105998
-
-4.2648515634753199066
--8.4629358700849355301
--22.252086323427270997
-
-10.996977688149536689
--9.2123370180278048025
--29.206739014051372294
-
-10.935342607054865383
--9.4338917557810741954
--29.112263909175499776
-
-15.714024935401759819
-1.3745079651566265433
--59.286834979937104606
-
--1.3624227800805182031
--4.1979357415396094666
--21.034430148188398846
-
-6.7690173115899296974
--4.7352452433700786827
--53.605307875695892506
-

examples/Data/pose2example-rewritten.txt

@@ -1,23 +0,0 @@
-VERTEX_SE2 0 0 0 0
-VERTEX_SE2 1 1.03039 0.01135 -0.081596
-VERTEX_SE2 2 2.03614 -0.129733 -0.301887
-VERTEX_SE2 3 3.0151 -0.442395 -0.345514
-VERTEX_SE2 4 3.34395 0.506678 1.21471
-VERTEX_SE2 5 3.68449 1.46405 1.18379
-VERTEX_SE2 6 4.06463 2.41478 1.17633
-VERTEX_SE2 7 4.42978 3.30018 1.25917
-VERTEX_SE2 8 4.12888 2.32148 -1.82539
-VERTEX_SE2 9 3.88465 1.32751 -1.95302
-VERTEX_SE2 10 3.53107 0.388263 -2.14893
-EDGE_SE2 0 1 1.03039 0.01135 -0.081596 44.7214 0 0 44.7214 0 30.9017
-EDGE_SE2 1 2 1.0139 -0.058639 -0.220291 44.7214 0 0 44.7214 0 30.9017
-EDGE_SE2 2 3 1.02765 -0.007456 -0.043627 44.7214 0 0 44.7214 0 30.9017
-EDGE_SE2 3 4 -0.012016 1.00436 1.56023 44.7214 0 0 44.7214 0 30.9017
-EDGE_SE2 4 5 1.01603 0.014565 -0.03093 44.7214 0 0 44.7214 0 30.9017
-EDGE_SE2 5 6 1.02389 0.006808 -0.007452 44.7214 0 0 44.7214 0 30.9017
-EDGE_SE2 6 7 0.957734 0.003159 0.082836 44.7214 0 0 44.7214 0 30.9017
-EDGE_SE2 7 8 -1.02382 -0.013668 -3.08456 44.7214 0 0 44.7214 0 30.9017
-EDGE_SE2 8 9 1.02344 0.013984 -0.127624 44.7214 0 0 44.7214 0 30.9017
-EDGE_SE2 9 10 1.00335 0.02225 -0.195918 44.7214 0 0 44.7214 0 30.9017
-EDGE_SE2 5 9 0.033943 0.032439 3.07364 44.7214 0 0 44.7214 0 30.9017
-EDGE_SE2 3 10 0.04402 0.988477 -1.55351 44.7214 0 0 44.7214 0 30.9017

examples/Pose2SLAMExample_g2o.cpp

@@ -18,46 +18,45 @@
  * @author Luca Carlone
  */
 
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/inference/Key.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/dataset.h>
-#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/GaussNewtonOptimizer.h>
-#include <gtsam/nonlinear/Marginals.h>
-#include <gtsam/nonlinear/Values.h>
 #include <fstream>
-#include <sstream>
 
 using namespace std;
 using namespace gtsam;
 
+int main(const int argc, const char *argv[]) {
 
-int main(const int argc, const char *argv[]){
+  // Read graph from file
-
+  string g2oFile;
   if (argc < 2)
-    std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl;
+    g2oFile = "../../examples/Data/noisyToyGraph.txt";
-  const string g2oFile = argv[1];
+  else
+    g2oFile = argv[1];
 
-  NonlinearFactorGraph graph;
+  NonlinearFactorGraph::shared_ptr graph;
-  Values initial;
+  Values::shared_ptr initial;
-  readG2o(g2oFile, graph, initial);
+  boost::tie(graph, initial) = readG2o(g2oFile);
 
   // Add prior on the pose having index (key) = 0
-  NonlinearFactorGraph graphWithPrior = graph;
+  NonlinearFactorGraph graphWithPrior = *graph;
-  noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
+  noiseModel::Diagonal::shared_ptr priorModel = //
+      noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
 
   std::cout << "Optimizing the factor graph" << std::endl;
-  GaussNewtonOptimizer optimizer(graphWithPrior, initial); // , parameters);
+  GaussNewtonOptimizer optimizer(graphWithPrior, *initial);
   Values result = optimizer.optimize();
   std::cout << "Optimization complete" << std::endl;
 
+  if (argc < 3) {
+    result.print("result");
+  } else {
     const string outputFile = argv[2];
     std::cout << "Writing results to file: " << outputFile << std::endl;
-  writeG2o(outputFile, graph, result);
+    writeG2o(*graph, result, outputFile);
     std::cout << "done! " << std::endl;
-
+  }
   return 0;
 }

examples/Pose2SLAMExample_lago.cpp

@@ -12,51 +12,53 @@
 /**
  * @file Pose2SLAMExample_lago.cpp
  * @brief A 2D Pose SLAM example that reads input from g2o, and solve the Pose2 problem
- * using LAGO (Linear Approximation for Graph Optimization). See class LagoInitializer.h
+ * using LAGO (Linear Approximation for Graph Optimization). See class lago.h
  * Output is written on a file, in g2o format
  * Syntax for the script is ./Pose2SLAMExample_lago input.g2o output.g2o
  * @date May 15, 2014
  * @author Luca Carlone
  */
 
-#include <gtsam/geometry/Pose2.h>
+#include <gtsam/slam/lago.h>
-#include <gtsam/inference/Key.h>
-#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/slam/dataset.h>
-#include <gtsam/nonlinear/LagoInitializer.h>
+#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
-#include <gtsam/nonlinear/Values.h>
 #include <fstream>
-#include <sstream>
 
 using namespace std;
 using namespace gtsam;
 
+int main(const int argc, const char *argv[]) {
 
-int main(const int argc, const char *argv[]){
+  // Read graph from file
-
+  string g2oFile;
   if (argc < 2)
-    std::cout << "Please specify: 1st argument: input file (in g2o format) and 2nd argument: output file" << std::endl;
+    g2oFile = "../../examples/Data/noisyToyGraph.txt";
-  const string g2oFile = argv[1];
+  else
+    g2oFile = argv[1];
 
-  NonlinearFactorGraph graph;
+  NonlinearFactorGraph::shared_ptr graph;
-  Values initial;
+  Values::shared_ptr initial;
-  readG2o(g2oFile, graph, initial);
+  boost::tie(graph, initial) = readG2o(g2oFile);
 
   // Add prior on the pose having index (key) = 0
-  NonlinearFactorGraph graphWithPrior = graph;
+  NonlinearFactorGraph graphWithPrior = *graph;
-  noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
+  noiseModel::Diagonal::shared_ptr priorModel = //
+      noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+  graphWithPrior.print();
 
   std::cout << "Computing LAGO estimate" << std::endl;
-  Values estimateLago = initializeLago(graphWithPrior);
+  Values estimateLago = lago::initialize(graphWithPrior);
   std::cout << "done!" << std::endl;
 
+  if (argc < 3) {
+    estimateLago.print("estimateLago");
+  } else {
     const string outputFile = argv[2];
     std::cout << "Writing results to file: " << outputFile << std::endl;
-  writeG2o(outputFile, graph, estimateLago);
+    writeG2o(*graph, estimateLago, outputFile);
     std::cout << "done! " << std::endl;
+  }
 
   return 0;
 }

examples/SFMExample_SmartFactor.cpp

@@ -0,0 +1,168 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    SFMExample_SmartFactor.cpp
+ * @brief   A structure-from-motion problem on a simulated dataset, using smart projection factor
+ * @author  Duy-Nguyen Ta
+ * @author  Jing Dong
+ */
+
+/**
+ * A structure-from-motion example with landmarks
+ *  - The landmarks form a 10 meter cube
+ *  - The robot rotates around the landmarks, always facing towards the cube
+ */
+
+// For loading the data
+#include "SFMdata.h"
+
+// Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
+#include <gtsam/geometry/Point2.h>
+
+// Each variable in the system (poses and landmarks) must be identified with a unique key.
+// We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
+// Here we will use Symbols
+#include <gtsam/inference/Symbol.h>
+
+// In GTSAM, measurement functions are represented as 'factors'.
+// The factor we used here is SmartProjectionPoseFactor. Every smart factor represent a single landmark,
+// The SmartProjectionPoseFactor only optimize the pose of camera, not the calibration,
+// The calibration should be known.
+#include <gtsam/slam/SmartProjectionPoseFactor.h>
+
+// Also, we will initialize the robot at some location using a Prior factor.
+#include <gtsam/slam/PriorFactor.h>
+
+// When the factors are created, we will add them to a Factor Graph. As the factors we are using
+// are nonlinear factors, we will need a Nonlinear Factor Graph.
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+
+// Finally, once all of the factors have been added to our factor graph, we will want to
+// solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
+// GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
+// trust-region method known as Powell's Degleg
+#include <gtsam/nonlinear/DoglegOptimizer.h>
+
+// The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
+// nonlinear functions around an initial linearization point, then solve the linear system
+// to update the linearization point. This happens repeatedly until the solver converges
+// to a consistent set of variable values. This requires us to specify an initial guess
+// for each variable, held in a Values container.
+#include <gtsam/nonlinear/Values.h>
+
+#include <vector>
+
+using namespace std;
+using namespace gtsam;
+
+// Make the typename short so it looks much cleaner
+typedef gtsam::SmartProjectionPoseFactor<gtsam::Pose3, gtsam::Point3, gtsam::Cal3_S2>
+  SmartFactor;
+
+/* ************************************************************************* */
+int main(int argc, char* argv[]) {
+
+  // Define the camera calibration parameters
+  Cal3_S2::shared_ptr K(new Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0));
+
+  // Define the camera observation noise model
+  noiseModel::Isotropic::shared_ptr measurementNoise = noiseModel::Isotropic::Sigma(2, 1.0); // one pixel in u and v
+
+  // Create the set of ground-truth landmarks
+  vector<Point3> points = createPoints();
+
+  // Create the set of ground-truth poses
+  vector<Pose3> poses = createPoses();
+
+  // Create a factor graph
+  NonlinearFactorGraph graph;
+
+  // A vector saved all Smart factors (for get landmark position after optimization)
+  vector<SmartFactor::shared_ptr> smartfactors_ptr;
+
+  // Simulated measurements from each camera pose, adding them to the factor graph
+  for (size_t i = 0; i < points.size(); ++i) {
+
+    // every landmark represent a single landmark, we use shared pointer to init the factor, and then insert measurements.
+    SmartFactor::shared_ptr smartfactor(new SmartFactor());
+
+    for (size_t j = 0; j < poses.size(); ++j) {
+
+      // generate the 2D measurement
+      SimpleCamera camera(poses[j], *K);
+      Point2 measurement = camera.project(points[i]);
+
+      // call add() function to add measurment into a single factor, here we need to add:
+      //    1. the 2D measurement
+      //    2. the corresponding camera's key
+      //    3. camera noise model
+      //    4. camera calibration
+      smartfactor->add(measurement, Symbol('x', j), measurementNoise, K);
+    }
+
+    // save smartfactors to get landmark position
+    smartfactors_ptr.push_back(smartfactor);
+
+    // insert the smart factor in the graph
+    graph.push_back(smartfactor);
+  }
+
+  // Add a prior on pose x0. This indirectly specifies where the origin is.
+  noiseModel::Diagonal::shared_ptr poseNoise = noiseModel::Diagonal::Sigmas((Vector(6) << Vector3::Constant(0.3), Vector3::Constant(0.1))); // 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
+  graph.push_back(PriorFactor<Pose3>(Symbol('x', 0), poses[0], poseNoise)); // add directly to graph
+
+  // Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
+  // Here we add a prior on the second pose x1, so this will fix the scale by indicating the distance between x0 and x1.
+  // Because these two are fixed, the rest poses will be alse fixed.
+  graph.push_back(PriorFactor<Pose3>(Symbol('x', 1), poses[1], poseNoise)); // add directly to graph
+
+  graph.print("Factor Graph:\n");
+
+  // Create the data structure to hold the initial estimate to the solution
+  // Intentionally initialize the variables off from the ground truth
+  Values initialEstimate;
+  for (size_t i = 0; i < poses.size(); ++i)
+    initialEstimate.insert(Symbol('x', i), poses[i].compose(Pose3(Rot3::rodriguez(-0.1, 0.2, 0.25), Point3(0.05, -0.10, 0.20))));
+  initialEstimate.print("Initial Estimates:\n");
+
+  // Optimize the graph and print results
+  Values result = DoglegOptimizer(graph, initialEstimate).optimize();
+  result.print("Final results:\n");
+
+
+  // Notice: Smart factor represent the 3D point as a factor, not a variable.
+  // The 3D position of the landmark is not explicitly calculated by the optimizer.
+  // If you do want to output the landmark's 3D position, you should use the internal function point()
+  // of the smart factor to get the 3D point.
+  Values landmark_result;
+  for (size_t i = 0; i < points.size(); ++i) {
+
+    // The output of point() is in boost::optional<gtsam::Point3>, since sometimes
+    // the triangulation opterations inside smart factor will encounter degeneracy.
+    // Check the manual of boost::optional for more details for the usages.
+    boost::optional<Point3> point;
+
+    // here we use the saved smart factors to call, pass in all optimized pose to calculate landmark positions
+    point = smartfactors_ptr.at(i)->point(result);
+
+    // ignore if boost::optional return NULL
+    if (point)
+      landmark_result.insert(Symbol('l', i), *point);
+  }
+
+  landmark_result.print("Landmark results:\n");
+
+
+  return 0;
+}
+/* ************************************************************************* */
+

examples/StereoVOExample_large.cpp

@@ -25,47 +25,43 @@
  */
 
 #include <gtsam/geometry/Pose3.h>
-#include <gtsam/inference/Key.h>
+#include <gtsam/geometry/Cal3_S2Stereo.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
-#include <gtsam/nonlinear/Marginals.h>
-#include <gtsam/nonlinear/Values.h>
-#include <gtsam/geometry/Cal3_S2Stereo.h>
-
-#include <gtsam/slam/StereoFactor.h>
-#include <gtsam/nonlinear/NonlinearEquality.h>
 #include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/StereoFactor.h>
+#include <gtsam/slam/dataset.h>
 
+#include <string>
 #include <fstream>
 #include <iostream>
-#include <sstream>
-#include <string>
 
 using namespace std;
 using namespace gtsam;
 
 int main(int argc, char** argv){
 
+  Values initial_estimate;
   NonlinearFactorGraph graph;
   const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
-  Values initial_estimate = Values();
-  vector<double> read_vector;
-  string read_string, parse_string;
 
-  string data_folder = "C:/Users/Stephen/Documents/Borg/gtsam/Examples/Data/";
+  string calibration_loc = findExampleDataFile("VO_calibration.txt");
-  string calibration_loc = data_folder + "VO_calibration.txt";
+  string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
-  string pose_loc = data_folder + "VO_camera_poses_large.txt";
+  string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
-  string factor_loc = data_folder + "VO_stereo_factors_large.txt";
   
   //read camera calibration info from file
-  double fx,fy,s,u,v,b;
+  // focal lengths fx, fy, skew s, principal point u0, v0, baseline b
-  ifstream calibration_file(calibration_loc);
+  double fx, fy, s, u0, v0, b;
+  ifstream calibration_file(calibration_loc.c_str());
   cout << "Reading calibration info" << endl;
-  calibration_file >> fx >> fy >> s >> u >> v >> b;
+  calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
-  //create stereo camera calibration object
-  const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u,v,b));
 
-  ifstream pose_file(pose_loc);
+  //create stereo camera calibration object
+  const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(fx,fy,s,u0,v0,b));
+  
+  ifstream pose_file(pose_loc.c_str());
   cout << "Reading camera poses" << endl;
   int pose_id;
   MatrixRowMajor m(4,4);
@@ -77,30 +73,36 @@ int main(int argc, char** argv){
     initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
   }
   
-  double x, l, uL, uR, v, X, Y, Z;
+  // camera and landmark keys
-  ifstream factor_file(factor_loc);
+  size_t x, l;
+
+  // pixel coordinates uL, uR, v (same for left/right images due to rectification)
+  // landmark coordinates X, Y, Z in camera frame, resulting from triangulation
+  double uL, uR, v, X, Y, Z;
+  ifstream factor_file(factor_loc.c_str());
   cout << "Reading stereo factors" << endl;
   //read stereo measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
   while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
     graph.push_back(
-          GenericStereoFactor<Pose3,Point3>(StereoPoint2(uL, uR, v), model,
+        GenericStereoFactor<Pose3, Point3>(StereoPoint2(uL, uR, v), model,
             Symbol('x', x), Symbol('l', l), K));
     //if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
-      if(!initial_estimate.exists(Symbol('l',l))){
+    if (!initial_estimate.exists(Symbol('l', l))) {
       Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
       //transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space
-        Point3 worldPoint = camPose.transform_from(Point3(X,Y,Z));
+      Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z));
-        initial_estimate.insert(Symbol('l',l),worldPoint);
+      initial_estimate.insert(Symbol('l', l), worldPoint);
     }
   }
 
   Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
-  first_pose.print("Check estimate poses:\n");
   //constrain the first pose such that it cannot change from its original value during optimization
+  // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
+  // QR is much slower than Cholesky, but numerically more stable
   graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
 
   cout << "Optimizing" << endl;
-  //create Levenberg-Marquardt optimizer to solve the initial factor graph estimate
+  //create Levenberg-Marquardt optimizer to optimize the factor graph
   LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
   Values result = optimizer.optimize();
 

gtsam.h

@@ -2249,6 +2249,13 @@ pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename,
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D(string filename);
 pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> load2D_robust(string filename,
     gtsam::noiseModel::Base* model);
+void save2D(const gtsam::NonlinearFactorGraph& graph,
+    const gtsam::Values& config, gtsam::noiseModel::Diagonal* model,
+    string filename);
+
+pair<gtsam::NonlinearFactorGraph*, gtsam::Values*> readG2o(string filename);
+void writeG2o(const gtsam::NonlinearFactorGraph& graph,
+    const gtsam::Values& estimate, string filename);
 
 //*************************************************************************
 // Navigation

gtsam/3rdparty/CCOLAMD/Source/ccolamd.c

@@ -617,11 +617,12 @@
 
 #include "ccolamd.h"
 
-#include <stdlib.h>
 #include <math.h>
 #include <limits.h>
 
 #ifdef MATLAB_MEX_FILE
+#include <stdint.h>
+typedef uint16_t char16_t;
 #include "mex.h"
 #include "matrix.h"
 #endif

gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c

@@ -13,6 +13,9 @@
 
 #ifndef NPRINT
 #ifdef MATLAB_MEX_FILE
+#include <stdlib.h>
+#include <stdint.h>
+typedef uint16_t char16_t;
 #include "mex.h"
 int (*ccolamd_printf) (const char *, ...) = mexPrintf ;
 #else

gtsam/geometry/tests/testTriangulation.cpp

@@ -16,7 +16,7 @@
  *      Author: cbeall3
  */
 
-#include <gtsam_unstable/geometry/triangulation.h>
+#include <gtsam/geometry/triangulation.h>
 #include <gtsam/geometry/Cal3Bundler.h>
 #include <CppUnitLite/TestHarness.h>
 

gtsam/geometry/triangulation.cpp

@@ -16,7 +16,7 @@
  * @author Chris Beall
  */
 
-#include <gtsam_unstable/geometry/triangulation.h>
+#include <gtsam/geometry/triangulation.h>
 
 #include <gtsam/geometry/PinholeCamera.h>
 #include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>

gtsam/geometry/triangulation.h

@@ -18,8 +18,8 @@
 
 #pragma once
 
-#include <gtsam_unstable/base/dllexport.h>
+
-#include <gtsam_unstable/geometry/TriangulationFactor.h>
+#include <gtsam/geometry/TriangulationFactor.h>
 #include <gtsam/nonlinear/NonlinearFactorGraph.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/PriorFactor.h>
@@ -52,7 +52,7 @@ public:
  * @param rank_tol SVD rank tolerance
  * @return Triangulated Point3
  */
-GTSAM_UNSTABLE_EXPORT Point3 triangulateDLT(
+GTSAM_EXPORT Point3 triangulateDLT(
     const std::vector<Matrix>& projection_matrices,
     const std::vector<Point2>& measurements, double rank_tol);
 
@@ -120,7 +120,7 @@ std::pair<NonlinearFactorGraph, Values> triangulationGraph(
  * @param landmarkKey to refer to landmark
  * @return refined Point3
  */
-GTSAM_UNSTABLE_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
+GTSAM_EXPORT Point3 optimize(const NonlinearFactorGraph& graph,
     const Values& values, Key landmarkKey);
 
 /**

gtsam/inference/VariableIndex-inl.h

@@ -23,43 +23,35 @@ namespace gtsam {
 
 /* ************************************************************************* */
 template<class FG>
-void VariableIndex::augment(const FG& factors, boost::optional<const FastVector<size_t>&> newFactorIndices)
+void VariableIndex::augment(const FG& factors,
-{
+    boost::optional<const FastVector<size_t>&> newFactorIndices) {
   gttic(VariableIndex_augment);
 
   // Augment index for each factor
-  for(size_t i = 0; i < factors.size(); ++i)
+  for (size_t i = 0; i < factors.size(); ++i) {
-  {
+    if (factors[i]) {
-    if(factors[i])
-    {
       const size_t globalI =
-        newFactorIndices ?
+          newFactorIndices ? (*newFactorIndices)[i] : nFactors_;
-        (*newFactorIndices)[i] :
+      BOOST_FOREACH(const Key key, *factors[i]) {
-        nFactors_;
-      BOOST_FOREACH(const Key key, *factors[i])
-      {
         index_[key].push_back(globalI);
-        ++ nEntries_;
+        ++nEntries_;
       }
     }
 
     // Increment factor count even if factors are null, to keep indices consistent
-    if(newFactorIndices)
+    if (newFactorIndices) {
-    {
+      if ((*newFactorIndices)[i] >= nFactors_)
-      if((*newFactorIndices)[i] >= nFactors_)
         nFactors_ = (*newFactorIndices)[i] + 1;
-    }
+    } else {
-    else
+      ++nFactors_;
-    {
-      ++ nFactors_;
     }
   }
 }
 
 /* ************************************************************************* */
 template<typename ITERATOR, class FG>
-void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG& factors)
+void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor,
-{
+    const FG& factors) {
   gttic(VariableIndex_remove);
 
   // NOTE: We intentionally do not decrement nFactors_ because the factor
@@ -68,17 +60,20 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG&
   // one greater than the highest-numbered factor referenced in a VariableIndex.
   ITERATOR factorIndex = firstFactor;
   size_t i = 0;
-  for( ; factorIndex != lastFactor; ++factorIndex, ++i) {
+  for (; factorIndex != lastFactor; ++factorIndex, ++i) {
-    if(i >= factors.size())
+    if (i >= factors.size())
-      throw std::invalid_argument("Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
+      throw std::invalid_argument(
-    if(factors[i]) {
+          "Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
+    if (factors[i]) {
       BOOST_FOREACH(Key j, *factors[i]) {
         Factors& factorEntries = internalAt(j);
-        Factors::iterator entry = std::find(factorEntries.begin(), factorEntries.end(), *factorIndex);
+        Factors::iterator entry = std::find(factorEntries.begin(),
-        if(entry == factorEntries.end())
+            factorEntries.end(), *factorIndex);
-          throw std::invalid_argument("Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
+        if (entry == factorEntries.end())
+          throw std::invalid_argument(
+              "Internal error, indices and factors passed into VariableIndex::remove are not consistent with the existing variable index");
         factorEntries.erase(entry);
-        -- nEntries_;
+        --nEntries_;
       }
     }
   }
@@ -87,10 +82,11 @@ void VariableIndex::remove(ITERATOR firstFactor, ITERATOR lastFactor, const FG&
 /* ************************************************************************* */
 template<typename ITERATOR>
 void VariableIndex::removeUnusedVariables(ITERATOR firstKey, ITERATOR lastKey) {
-  for(ITERATOR key = firstKey; key != lastKey; ++key) {
+  for (ITERATOR key = firstKey; key != lastKey; ++key) {
     KeyMap::iterator entry = index_.find(*key);
-    if(!entry->second.empty())
+    if (!entry->second.empty())
-      throw std::invalid_argument("Asking to remove variables from the variable index that are not unused");
+      throw std::invalid_argument(
+          "Asking to remove variables from the variable index that are not unused");
     index_.erase(entry);
   }
 }

gtsam/linear/GaussianFactorGraph.cpp

@@ -70,7 +70,7 @@ namespace gtsam {
       vector<size_t> dims_accumulated;
       dims_accumulated.resize(dims.size()+1,0);
       dims_accumulated[0]=0;
-      for (int i=1; i<dims_accumulated.size(); i++)
+      for (size_t i=1; i<dims_accumulated.size(); i++)
     	  dims_accumulated[i] = dims_accumulated[i-1]+dims[i-1];
       return dims_accumulated;
     }

gtsam/linear/NoiseModel.cpp

@@ -49,7 +49,7 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) {
 
 /* ************************************************************************* */
 // check *above the diagonal* for non-zero entries
-static boost::optional<Vector> checkIfDiagonal(const Matrix M) {
+boost::optional<Vector> checkIfDiagonal(const Matrix M) {
   size_t m = M.rows(), n = M.cols();
   // check all non-diagonal entries
   bool full = false;
@@ -74,23 +74,46 @@ static boost::optional<Vector> checkIfDiagonal(const Matrix M) {
 /* ************************************************************************* */
 Gaussian::shared_ptr Gaussian::SqrtInformation(const Matrix& R, bool smart) {
   size_t m = R.rows(), n = R.cols();
-  if (m != n) throw invalid_argument("Gaussian::SqrtInformation: R not square");
+  if (m != n)
+    throw invalid_argument("Gaussian::SqrtInformation: R not square");
   boost::optional<Vector> diagonal = boost::none;
   if (smart)
     diagonal = checkIfDiagonal(R);
-  if (diagonal) return Diagonal::Sigmas(reciprocal(*diagonal),true);
+  if (diagonal)
-  else return shared_ptr(new Gaussian(R.rows(),R));
+    return Diagonal::Sigmas(reciprocal(*diagonal), true);
+  else
+    return shared_ptr(new Gaussian(R.rows(), R));
 }
 
 /* ************************************************************************* */
-Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) {
+Gaussian::shared_ptr Gaussian::Information(const Matrix& M, bool smart) {
+  size_t m = M.rows(), n = M.cols();
+  if (m != n)
+    throw invalid_argument("Gaussian::Information: R not square");
+  boost::optional<Vector> diagonal = boost::none;
+  if (smart)
+    diagonal = checkIfDiagonal(M);
+  if (diagonal)
+    return Diagonal::Precisions(*diagonal, true);
+  else {
+    Matrix R = RtR(M);
+    return shared_ptr(new Gaussian(R.rows(), R));
+  }
+}
+
+/* ************************************************************************* */
+Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance,
+    bool smart) {
   size_t m = covariance.rows(), n = covariance.cols();
-  if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square");
+  if (m != n)
+    throw invalid_argument("Gaussian::Covariance: covariance not square");
   boost::optional<Vector> variances = boost::none;
   if (smart)
     variances = checkIfDiagonal(covariance);
-  if (variances) return Diagonal::Variances(*variances,true);
+  if (variances)
-  else return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
+    return Diagonal::Variances(*variances, true);
+  else
+    return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
 }
 
 /* ************************************************************************* */

gtsam/linear/NoiseModel.h

@@ -164,6 +164,13 @@ namespace gtsam {
        */
       static shared_ptr SqrtInformation(const Matrix& R, bool smart = true);
 
+      /**
+       * A Gaussian noise model created by specifying an information matrix.
+       * @param M The information matrix
+       * @param smart check if can be simplified to derived class
+       */
+      static shared_ptr Information(const Matrix& M, bool smart = true);
+
       /**
        * A Gaussian noise model created by specifying a covariance matrix.
        * @param covariance The square covariance Matrix
@@ -865,6 +872,9 @@ namespace gtsam {
       }
     };
     
+    // Helper function
+    GTSAM_EXPORT boost::optional<Vector> checkIfDiagonal(const Matrix M);
+
   } // namespace noiseModel
 
   /** Note, deliberately not in noiseModel namespace.

gtsam/linear/tests/testNoiseModel.cpp

@@ -285,6 +285,17 @@ TEST(NoiseModel, SmartSqrtInformation2 )
   EXPECT(assert_equal(*expected,*actual));
 }
 
+/* ************************************************************************* */
+TEST(NoiseModel, SmartInformation )
+{
+  bool smart = true;
+  gtsam::SharedGaussian expected = Unit::Isotropic::Variance(3,2);
+  Matrix M = 0.5*eye(3);
+  EXPECT(checkIfDiagonal(M));
+  gtsam::SharedGaussian actual = Gaussian::Information(M, smart);
+  EXPECT(assert_equal(*expected,*actual));
+}
+
 /* ************************************************************************* */
 TEST(NoiseModel, SmartCovariance )
 {

gtsam/navigation/MagFactor.h

@@ -54,8 +54,11 @@ public:
   static Point3 unrotate(const Rot2& R, const Point3& p,
       boost::optional<Matrix&> HR = boost::none) {
     Point3 q = Rot3::yaw(R.theta()).unrotate(p, HR);
-    if (HR)
+    if (HR) {
-      *HR = HR->col(2);
+      // assign to temporary first to avoid error in Win-Debug mode
+      Matrix H = HR->col(2);
+      *HR = H;
+    }
     return q;
   }
 

gtsam/nonlinear/LagoInitializer.h

@@ -1,100 +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  LagoInitializer.h
- *  @brief Initialize Pose2 in a factor graph using LAGO
- *  (Linear Approximation for Graph Optimization). see papers:
- *
- *  L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate
- *  approximation for planar pose graph optimization, IJRR, 2014.
- *
- *  L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation
- *  for graph-based simultaneous localization and mapping, RSS, 2011.
- *
- *  @param graph: nonlinear factor graph (can include arbitrary factors but we assume
- *  that there is a subgraph involving Pose2 and betweenFactors). Also in the current
- *  version we assume that there is an odometric spanning path (x0->x1, x1->x2, etc)
- *  and a prior on x0. This assumption can be relaxed by using the extra argument
- *  useOdometricPath = false, although this part of code is not stable yet.
- *  @return Values: initial guess from LAGO (only pose2 are initialized)
- *
- *  @author Luca Carlone
- *  @author Frank Dellaert
- *  @date   May 14, 2014
- */
-
-#pragma once
-
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/inference/Symbol.h>
-#include <gtsam/linear/GaussianFactorGraph.h>
-#include <gtsam/linear/VectorValues.h>
-#include <gtsam/inference/graph.h>
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
-#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/slam/BetweenFactor.h>
-
-namespace gtsam {
-
-typedef std::map<Key,double> key2doubleMap;
-const Key keyAnchor = symbol('Z',9999999);
-noiseModel::Diagonal::shared_ptr priorOrientationNoise = noiseModel::Diagonal::Variances((Vector(1) << 1e-8));
-noiseModel::Diagonal::shared_ptr priorPose2Noise = noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
-
-/*  This function computes the cumulative orientation (without wrapping) wrt the root of a spanning tree (tree)
- *  for a node (nodeKey). The function starts at the nodes and moves towards the root
- *  summing up the (directed) rotation measurements. Relative measurements are encoded in "deltaThetaMap"
- *  The root is assumed to have orientation zero.
- */
-GTSAM_EXPORT double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree,
-    const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap);
-
-/*  This function computes the cumulative orientations (without wrapping)
- *  for all node wrt the root (root has zero orientation)
- */
-GTSAM_EXPORT key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
-    const PredecessorMap<Key>& tree);
-
-/*  Given a factor graph "g", and a spanning tree "tree", the function selects the nodes belonging to the tree and to g,
- *  and stores the factor slots corresponding to edges in the tree and to chordsIds wrt this tree
- *  Also it computes deltaThetaMap which is a fast way to encode relative orientations along the tree:
- *  for a node key2, s.t. tree[key2]=key1, the values deltaThetaMap[key2] is the relative orientation theta[key2]-theta[key1]
- */
-GTSAM_EXPORT void getSymbolicGraph(
-    /*OUTPUTS*/ std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds, key2doubleMap& deltaThetaMap,
-    /*INPUTS*/ const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g);
-
-/*  Retrieves the deltaTheta and the corresponding noise model from a BetweenFactor<Pose2> */
-GTSAM_EXPORT void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
-    Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta);
-
-/*  Linear factor graph with regularized orientation measurements */
-GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(const std::vector<size_t>& spanningTreeIds, const std::vector<size_t>& chordsIds,
-    const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
-
-/* Selects the subgraph of betweenFactors and transforms priors into between wrt a fictitious node */
-GTSAM_EXPORT NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph);
-
-/* Returns the orientations of a graph including only BetweenFactors<Pose2> */
-GTSAM_EXPORT VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath = true);
-
-/*  LAGO: Returns the orientations of the Pose2 in a generic factor graph */
-GTSAM_EXPORT VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true);
-
-/*  Returns the values for the Pose2 in a generic factor graph */
-GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath = true);
-
-/*  Only corrects the orientation part in initialGuess */
-GTSAM_EXPORT Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess);
-
-} // end of namespace gtsam

gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp

@@ -244,7 +244,7 @@ void LevenbergMarquardtOptimizer::iterate() {
     try {
       delta = solve(dampedSystem, state_.values, params_);
       systemSolvedSuccessfully = true;
-    } catch (IndeterminantLinearSystemException& e) {
+    } catch (IndeterminantLinearSystemException) {
       systemSolvedSuccessfully = false;
     }
 

gtsam/slam/JacobianFactorQR.h

@@ -6,7 +6,7 @@
  */
 
 #pragma once
-#include <gtsam_unstable/slam/JacobianSchurFactor.h>
+#include <gtsam/slam/JacobianSchurFactor.h>
 
 namespace gtsam {
 /**

gtsam/slam/JacobianFactorSVD.h

@@ -5,7 +5,7 @@
  */
 
 #pragma once
-#include "gtsam_unstable/slam/JacobianSchurFactor.h"
+#include "gtsam/slam/JacobianSchurFactor.h"
 
 namespace gtsam {
 /**

gtsam/slam/SmartFactorBase.h

@@ -325,7 +325,7 @@ public:
       const Cameras& cameras, const Point3& point,
       const double lambda = 0.0) const {
 
-    int numKeys = this->keys_.size();
+    size_t numKeys = this->keys_.size();
     std::vector<KeyMatrix2D> Fblocks;
     double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
         lambda);
@@ -352,7 +352,7 @@ public:
     Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
     Vector s = svd.singularValues();
     // Enull = zeros(2 * numKeys, 2 * numKeys - 3);
-    int numKeys = this->keys_.size();
+    size_t numKeys = this->keys_.size();
     Enull = svd.matrixU().block(0, 3, 2 * numKeys, 2 * numKeys - 3); // last 2m-3 columns
 
     return f;

gtsam/slam/SmartProjectionFactor.h

@@ -21,11 +21,10 @@
 
 #include "SmartFactorBase.h"
 
-#include <gtsam_unstable/geometry/triangulation.h>
+#include <gtsam/geometry/triangulation.h>
 #include <gtsam/geometry/Pose3.h>
 #include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/dataset.h>
-#include <gtsam_unstable/geometry/triangulation.h>
 
 #include <boost/optional.hpp>
 #include <boost/make_shared.hpp>
@@ -54,7 +53,7 @@ public:
   double f;
 };
 
-enum linearizationType {
+enum LinearizationMode {
   HESSIAN, JACOBIAN_SVD, JACOBIAN_Q
 };
 
@@ -263,7 +262,7 @@ public:
           try {
             Point2 reprojectionError(camera.project(point_) - zi);
             totalReprojError += reprojectionError.vector().norm();
-          } catch (CheiralityException& e) {
+          } catch (CheiralityException) {
             cheiralityException_ = true;
           }
           i += 1;

gtsam/slam/SmartProjectionPoseFactor.h

@@ -41,9 +41,8 @@ template<class POSE, class LANDMARK, class CALIBRATION>
 class SmartProjectionPoseFactor: public SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> {
 protected:
 
-  linearizationType linearizeTo_;
+  LinearizationMode linearizeTo_;  ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
 
-  // Known calibration
   std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
 
 public:
@@ -69,7 +68,7 @@ public:
   SmartProjectionPoseFactor(const double rankTol = 1,
       const double linThreshold = -1, const bool manageDegeneracy = false,
       const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none,
-      linearizationType linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
+      LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
       double dynamicOutlierRejectionThreshold = -1) :
         Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
         landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}
@@ -80,7 +79,7 @@ public:
   /**
    * add a new measurement and pose key
    * @param measured is the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
-   * @param poseKey is the index corresponding to the camera observing the same landmark
+   * @param poseKey is key corresponding to the camera observing the same landmark
    * @param noise_i is the measurement noise
    * @param K_i is the (known) camera calibration
    */
@@ -92,8 +91,11 @@ public:
   }
 
   /**
-   * add a new measurements and pose keys
    *  Variant of the previous one in which we include a set of measurements
+   * @param measurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
+   * @param poseKeys vector of keys corresponding to the camera observing the same landmark
+   * @param noises vector of measurement noises
+   * @param Ks vector of calibration objects
    */
   void add(std::vector<Point2> measurements, std::vector<Key> poseKeys,
       std::vector<SharedNoiseModel> noises,
@@ -105,8 +107,11 @@ public:
   }
 
   /**
-   * add a new measurements and pose keys
    * Variant of the previous one in which we include a set of measurements with the same noise and calibration
+   * @param mmeasurements vector of the 2m dimensional location of the projection of a single landmark in the m view (the measurement)
+   * @param poseKeys vector of keys corresponding to the camera observing the same landmark
+   * @param noise measurement noise (same for all measurements)
+   * @param K the (known) camera calibration (same for all measurements)
    */
   void add(std::vector<Point2> measurements, std::vector<Key> poseKeys,
       const SharedNoiseModel noise, const boost::shared_ptr<CALIBRATION> K) {
@@ -141,7 +146,12 @@ public:
     return 6 * this->keys_.size();
   }
 
-  // Collect all cameras
+  /**
+   * Collect all cameras involved in this factor
+   * @param values Values structure which must contain camera poses corresponding
+   * to keys involved in this factor
+   * @return vector of Values
+   */
   typename Base::Cameras cameras(const Values& values) const {
     typename Base::Cameras cameras;
     size_t i=0;
@@ -154,7 +164,9 @@ public:
   }
 
   /**
-   * linear factor on the poses
+   * Linearize to Gaussian Factor
+   * @param values Values structure which must contain camera poses for this factor
+   * @return
    */
   virtual boost::shared_ptr<GaussianFactor> linearize(
       const Values& values) const {
@@ -184,7 +196,7 @@ public:
   }
 
   /** return the calibration object */
-  inline const boost::shared_ptr<CALIBRATION> calibration() const {
+  inline const std::vector<boost::shared_ptr<CALIBRATION> > calibration() const {
     return K_all_;
   }
 

gtsam/slam/dataset.cpp

@@ -16,18 +16,18 @@
  * @brief utility functions for loading datasets
  */
 
-#include <fstream>
-#include <sstream>
-#include <cstdlib>
-
-#include <boost/filesystem.hpp>
-
-#include <gtsam/geometry/Pose2.h>
-#include <gtsam/linear/Sampler.h>
-#include <gtsam/inference/Symbol.h>
 #include <gtsam/slam/dataset.h>
 #include <gtsam/slam/BetweenFactor.h>
 #include <gtsam/slam/BearingRangeFactor.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/linear/Sampler.h>
+#include <gtsam/inference/Symbol.h>
+
+#include <boost/filesystem.hpp>
+
+#include <fstream>
+#include <sstream>
+#include <cstdlib>
 
 using namespace std;
 namespace fs = boost::filesystem;
@@ -55,35 +55,122 @@ string findExampleDataFile(const string& name) {
   // Find first name that exists
   BOOST_FOREACH(const fs::path& root, rootsToSearch) {
     BOOST_FOREACH(const fs::path& name, namesToSearch) {
-      if(fs::is_regular_file(root / name))
+      if (fs::is_regular_file(root / name))
         return (root / name).string();
     }
   }
 
   // If we did not return already, then we did not find the file
-  throw std::invalid_argument(
+  throw
+invalid_argument(
     "gtsam::findExampleDataFile could not find a matching file in\n"
     SOURCE_TREE_DATASET_DIR " or\n"
     INSTALLED_DATASET_DIR " named\n" +
     name + ", " + name + ".graph, or " + name + ".txt");
 }
+
+/* ************************************************************************* */
+string createRewrittenFileName(const string& name) {
+  // Search source tree and installed location
+  if (!exists(fs::path(name))) {
+    throw invalid_argument(
+        "gtsam::createRewrittenFileName could not find a matching file in\n"
+            + name);
+  }
+
+  fs::path p(name);
+  fs::path newpath = fs::path(p.parent_path().string())
+      / fs::path(p.stem().string() + "-rewritten.txt");
+
+  return newpath.string();
+}
+/* ************************************************************************* */
+
 #endif
 
 /* ************************************************************************* */
-pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
+GraphAndValues load2D(pair<string, SharedNoiseModel> dataset, int maxID,
-    pair<string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset,
+    bool addNoise, bool smart, NoiseFormat noiseFormat,
-    int maxID, bool addNoise, bool smart) {
+    KernelFunctionType kernelFunctionType) {
-  return load2D(dataset.first, dataset.second, maxID, addNoise, smart);
+  return load2D(dataset.first, dataset.second, maxID, addNoise, smart,
+      noiseFormat, kernelFunctionType);
 }
 
 /* ************************************************************************* */
-pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
+// Read noise parameters and interpret them according to flags
-    const string& filename, boost::optional<noiseModel::Diagonal::shared_ptr> model, int maxID,
+static SharedNoiseModel readNoiseModel(ifstream& is, bool smart,
-    bool addNoise, bool smart) {
+    NoiseFormat noiseFormat, KernelFunctionType kernelFunctionType) {
-  cout << "Will try to read " << filename << endl;
+  double v1, v2, v3, v4, v5, v6;
+  is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;
+
+  // Read matrix and check that diagonal entries are non-zero
+  Matrix M(3, 3);
+  switch (noiseFormat) {
+  case NoiseFormatG2O:
+  case NoiseFormatCOV:
+    // i.e., [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ]
+    if (v1 == 0.0 || v4 == 0.0 || v6 == 0.0)
+      throw runtime_error(
+          "load2D::readNoiseModel looks like this is not G2O matrix order");
+    M << v1, v2, v3, v2, v4, v5, v3, v5, v6;
+    break;
+  case NoiseFormatTORO:
+  case NoiseFormatGRAPH:
+    // http://www.openslam.org/toro.html
+    // inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr
+    // i.e., [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ]
+    if (v1 == 0.0 || v3 == 0.0 || v4 == 0.0)
+      throw invalid_argument(
+          "load2D::readNoiseModel looks like this is not TORO matrix order");
+    M << v1, v2, v5, v2, v3, v6, v5, v6, v4;
+    break;
+  default:
+    throw runtime_error("load2D: invalid noise format");
+  }
+
+  // Now, create a Gaussian noise model
+  // The smart flag will try to detect a simpler model, e.g., unit
+  SharedNoiseModel model;
+  switch (noiseFormat) {
+  case NoiseFormatG2O:
+  case NoiseFormatTORO:
+    // In both cases, what is stored in file is the information matrix
+    model = noiseModel::Gaussian::Information(M, smart);
+    break;
+  case NoiseFormatGRAPH:
+  case NoiseFormatCOV:
+    // These cases expect covariance matrix
+    model = noiseModel::Gaussian::Covariance(M, smart);
+    break;
+  default:
+    throw invalid_argument("load2D: invalid noise format");
+  }
+
+  switch (kernelFunctionType) {
+  case KernelFunctionTypeNONE:
+    return model;
+    break;
+  case KernelFunctionTypeHUBER:
+    return noiseModel::Robust::Create(
+        noiseModel::mEstimator::Huber::Create(1.345), model);
+    break;
+  case KernelFunctionTypeTUKEY:
+    return noiseModel::Robust::Create(
+        noiseModel::mEstimator::Tukey::Create(4.6851), model);
+    break;
+  default:
+    throw invalid_argument("load2D: invalid kernel function type");
+  }
+}
+
+/* ************************************************************************* */
+GraphAndValues load2D(const string& filename, SharedNoiseModel model, int maxID,
+    bool addNoise, bool smart, NoiseFormat noiseFormat,
+    KernelFunctionType kernelFunctionType) {
+
   ifstream is(filename.c_str());
   if (!is)
-    throw std::invalid_argument("load2D: can not find the file!");
+    throw invalid_argument("load2D: can not find file " + filename);
 
   Values::shared_ptr initial(new Values);
   NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
@@ -92,16 +179,18 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
 
   // load the poses
   while (is) {
-    if(! (is >> tag))
+    if (!(is >> tag))
       break;
 
-    if ((tag == "VERTEX2") || (tag == "VERTEX")) {
+    if ((tag == "VERTEX2") || (tag == "VERTEX_SE2") || (tag == "VERTEX")) {
       int id;
       double x, y, yaw;
       is >> id >> x >> y >> yaw;
+
       // optional filter
       if (maxID && id >= maxID)
         continue;
+
       initial->insert(id, Pose2(x, y, yaw));
     }
     is.ignore(LINESIZE, '\n');
@@ -109,54 +198,47 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
   is.clear(); /* clears the end-of-file and error flags */
   is.seekg(0, ios::beg);
 
-  // Create a sampler with random number generator
+  // If asked, create a sampler with random number generator
-  Sampler sampler(42u);
+  Sampler sampler;
+  if (addNoise) {
+    noiseModel::Diagonal::shared_ptr noise;
+    if (model)
+      noise = boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
+    if (!noise)
+      throw invalid_argument(
+          "gtsam::load2D: invalid noise model for adding noise"
+              "(current version assumes diagonal noise model)!");
+    sampler = Sampler(noise);
+  }
 
   // Parse the pose constraints
   int id1, id2;
   bool haveLandmark = false;
   while (is) {
-    if(! (is >> tag))
+    if (!(is >> tag))
       break;
 
-    if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) {
+    if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "EDGE_SE2")
+        || (tag == "ODOMETRY")) {
+
+      // Read transform
       double x, y, yaw;
-      double v1, v2, v3, v4, v5, v6;
-
       is >> id1 >> id2 >> x >> y >> yaw;
-      is >> v1 >> v2 >> v3 >> v4 >> v5 >> v6;
+      Pose2 l1Xl2(x, y, yaw);
 
-      // Try to guess covariance matrix layout
+      // read noise model
-      Matrix m(3,3);
+      SharedNoiseModel modelInFile = readNoiseModel(is, smart, noiseFormat,
-      if(v1 != 0.0 && v2 == 0.0 && v3 != 0.0 && v4 != 0.0 && v5 == 0.0 && v6 == 0.0)
+          kernelFunctionType);
-      {
-        // Looks like [ v1 v2 v5; v2' v3 v6; v5' v6' v4 ]
-        m <<  v1, v2, v5,  v2, v3, v6,  v5, v6, v4;
-      }
-      else if(v1 != 0.0 && v2 == 0.0 && v3 == 0.0 && v4 != 0.0 && v5 == 0.0 && v6 != 0.0)
-      {
-        // Looks like [ v1 v2 v3; v2' v4 v5; v3' v5' v6 ]
-        m << v1, v2, v3,  v2, v4, v5,  v3, v5, v6;
-      }
-      else
-      {
-        throw std::invalid_argument("load2D: unrecognized covariance matrix format in dataset file");
-      }
 
       // optional filter
       if (maxID && (id1 >= maxID || id2 >= maxID))
         continue;
 
-      Pose2 l1Xl2(x, y, yaw);
+      if (!model)
-
+        model = modelInFile;
-      // SharedNoiseModel noise = noiseModel::Gaussian::Covariance(m, smart);
-      if (!model) {
-        Vector variances = (Vector(3) << m(0, 0), m(1, 1), m(2, 2));
-        model = noiseModel::Diagonal::Variances(variances, smart);
-      }
 
       if (addNoise)
-        l1Xl2 = l1Xl2.retract(sampler.sampleNewModel(*model));
+        l1Xl2 = l1Xl2.retract(sampler.sample());
 
       // Insert vertices if pure odometry file
       if (!initial->exists(id1))
@@ -165,7 +247,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
         initial->insert(id2, initial->at<Pose2>(id1) * l1Xl2);
 
       NonlinearFactor::shared_ptr factor(
-          new BetweenFactor<Pose2>(id1, id2, l1Xl2, *model));
+          new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
       graph->push_back(factor);
     }
 
@@ -185,23 +267,22 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
       is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3;
 
       // Convert x,y to bearing,range
-      bearing = std::atan2(lmy, lmx);
+      bearing = atan2(lmy, lmx);
-      range = std::sqrt(lmx*lmx + lmy*lmy);
+      range = sqrt(lmx * lmx + lmy * lmy);
 
       // In our experience, the x-y covariance on landmark sightings is not very good, so assume
       // it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty.
-      if(std::abs(v1 - v3) < 1e-4)
+      if (std::abs(v1 - v3) < 1e-4) {
-      {
         bearing_std = sqrt(v1 / 10.0);
         range_std = sqrt(v1);
-      }
+      } else {
-      else
-      {
         bearing_std = 1;
         range_std = 1;
-        if(!haveLandmark) {
+        if (!haveLandmark) {
-          cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n"
+          cout
-              "non-uniform covariance on LANDMARK measurements in this file." << endl;
+              << "Warning: load2D is a very simple dataset loader and is ignoring the\n"
+                  "non-uniform covariance on LANDMARK measurements in this file."
+              << endl;
           haveLandmark = true;
         }
       }
@@ -227,7 +308,7 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
         initial->insert(id1, Pose2());
       if (!initial->exists(L(id2))) {
         Pose2 pose = initial->at<Pose2>(id1);
-        Point2 local(cos(bearing)*range,sin(bearing)*range);
+        Point2 local(cos(bearing) * range, sin(bearing) * range);
         Point2 global = pose.transform_from(local);
         initial->insert(L(id2), global);
       }
@@ -235,12 +316,15 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
     is.ignore(LINESIZE, '\n');
   }
 
-  cout << "load2D read a graph file with " << initial->size()
-      << " vertices and " << graph->nrFactors() << " factors" << endl;
-
   return make_pair(graph, initial);
 }
 
+/* ************************************************************************* */
+GraphAndValues load2D_robust(const string& filename,
+    noiseModel::Base::shared_ptr& model, int maxID) {
+  return load2D(filename, model, maxID);
+}
+
 /* ************************************************************************* */
 void save2D(const NonlinearFactorGraph& graph, const Values& config,
     const noiseModel::Diagonal::shared_ptr model, const string& filename) {
@@ -248,18 +332,16 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
   fstream stream(filename.c_str(), fstream::out);
 
   // save poses
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config)
+  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, config) {
-  {
     const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
-    stream << "VERTEX2 " << key_value.key << " " << pose.x() << " "
+    stream << "VERTEX2 " << key_value.key << " " << pose.x() << " " << pose.y()
-        << pose.y() << " " << pose.theta() << endl;
+        << " " << pose.theta() << endl;
   }
 
   // save edges
   Matrix R = model->R();
   Matrix RR = trans(R) * R; //prod(trans(R),R);
-  BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph)
+  BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
-  {
     boost::shared_ptr<BetweenFactor<Pose2> > factor =
         boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
     if (!factor)
@@ -267,14 +349,62 @@ void save2D(const NonlinearFactorGraph& graph, const Values& config,
 
     Pose2 pose = factor->measured().inverse();
     stream << "EDGE2 " << factor->key2() << " " << factor->key1() << " "
-        << pose.x() << " " << pose.y() << " " << pose.theta() << " "
+        << pose.x() << " " << pose.y() << " " << pose.theta() << " " << RR(0, 0)
-        << RR(0, 0) << " " << RR(0, 1) << " " << RR(1, 1) << " "
+        << " " << RR(0, 1) << " " << RR(1, 1) << " " << RR(2, 2) << " "
-        << RR(2, 2) << " " << RR(0, 2) << " " << RR(1, 2) << endl;
+        << RR(0, 2) << " " << RR(1, 2) << endl;
   }
 
   stream.close();
 }
 
+/* ************************************************************************* */
+GraphAndValues readG2o(const string& g2oFile,
+    KernelFunctionType kernelFunctionType) {
+  // just call load2D
+  int maxID = 0;
+  bool addNoise = false;
+  bool smart = true;
+  return load2D(g2oFile, SharedNoiseModel(), maxID, addNoise, smart,
+      NoiseFormatG2O, kernelFunctionType);
+}
+
+/* ************************************************************************* */
+void writeG2o(const NonlinearFactorGraph& graph, const Values& estimate,
+    const string& filename) {
+
+  fstream stream(filename.c_str(), fstream::out);
+
+  // save poses
+  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate) {
+    const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
+    stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
+        << pose.y() << " " << pose.theta() << endl;
+  }
+
+  // save edges
+  BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph) {
+    boost::shared_ptr<BetweenFactor<Pose2> > factor =
+        boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
+    if (!factor)
+      continue;
+
+    SharedNoiseModel model = factor->get_noiseModel();
+    boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
+        boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
+    if (!diagonalModel)
+      throw invalid_argument(
+          "writeG2o: invalid noise model (current version assumes diagonal noise model)!");
+
+    Pose2 pose = factor->measured(); //.inverse();
+    stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
+        << pose.x() << " " << pose.y() << " " << pose.theta() << " "
+        << diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " "
+        << diagonalModel->precision(1) << " " << 0.0 << " "
+        << diagonalModel->precision(2) << endl;
+  }
+  stream.close();
+}
+
 /* ************************************************************************* */
 bool load3D(const string& filename) {
   ifstream is(filename.c_str());
@@ -318,161 +448,60 @@ bool load3D(const string& filename) {
 }
 
 /* ************************************************************************* */
-pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
+Rot3 openGLFixedRotation() { // this is due to different convention for cameras in gtsam and openGL
-    const string& filename, noiseModel::Base::shared_ptr& model, int maxID) {
-  cout << "Will try to read " << filename << endl;
-  ifstream is(filename.c_str());
-  if (!is)
-    throw std::invalid_argument("load2D: can not find the file!");
-
-  Values::shared_ptr initial(new Values);
-  NonlinearFactorGraph::shared_ptr graph(new NonlinearFactorGraph);
-
-  string tag;
-
-  // load the poses
-  while (is) {
-    is >> tag;
-
-    if ((tag == "VERTEX2") || (tag == "VERTEX")) {
-      int id;
-      double x, y, yaw;
-      is >> id >> x >> y >> yaw;
-      // optional filter
-      if (maxID && id >= maxID)
-        continue;
-      initial->insert(id, Pose2(x, y, yaw));
-    }
-    is.ignore(LINESIZE, '\n');
-  }
-  is.clear(); /* clears the end-of-file and error flags */
-  is.seekg(0, ios::beg);
-
-  // Create a sampler with random number generator
-  Sampler sampler(42u);
-
-  // load the factors
-  while (is) {
-    is >> tag;
-
-    if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) {
-      int id1, id2;
-      double x, y, yaw;
-
-      is >> id1 >> id2 >> x >> y >> yaw;
-      Matrix m = eye(3);
-      is >> m(0, 0) >> m(0, 1) >> m(1, 1) >> m(2, 2) >> m(0, 2) >> m(1, 2);
-      m(2, 0) = m(0, 2);
-      m(2, 1) = m(1, 2);
-      m(1, 0) = m(0, 1);
-
-      // optional filter
-      if (maxID && (id1 >= maxID || id2 >= maxID))
-        continue;
-
-      Pose2 l1Xl2(x, y, yaw);
-
-      // Insert vertices if pure odometry file
-      if (!initial->exists(id1))
-        initial->insert(id1, Pose2());
-      if (!initial->exists(id2))
-        initial->insert(id2, initial->at<Pose2>(id1) * l1Xl2);
-
-      NonlinearFactor::shared_ptr factor(
-          new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
-      graph->push_back(factor);
-    }
-    if (tag == "BR") {
-      int id1, id2;
-      double bearing, range, bearing_std, range_std;
-
-      is >> id1 >> id2 >> bearing >> range >> bearing_std >> range_std;
-
-      // optional filter
-      if (maxID && (id1 >= maxID || id2 >= maxID))
-        continue;
-
-      noiseModel::Diagonal::shared_ptr measurementNoise =
-          noiseModel::Diagonal::Sigmas((Vector(2) << bearing_std, range_std));
-      *graph += BearingRangeFactor<Pose2, Point2>(id1, id2, bearing, range, measurementNoise);
-
-      // Insert poses or points if they do not exist yet
-      if (!initial->exists(id1))
-        initial->insert(id1, Pose2());
-      if (!initial->exists(id2)) {
-        Pose2 pose = initial->at<Pose2>(id1);
-        Point2 local(cos(bearing)*range,sin(bearing)*range);
-        Point2 global = pose.transform_from(local);
-        initial->insert(id2, global);
-      }
-    }
-    is.ignore(LINESIZE, '\n');
-  }
-
-  cout << "load2D read a graph file with " << initial->size()
-                        << " vertices and " << graph->nrFactors() << " factors" << endl;
-
-  return make_pair(graph, initial);
-}
-
-/* ************************************************************************* */
-Rot3 openGLFixedRotation(){ // this is due to different convention for cameras in gtsam and openGL
   /* R = [ 1   0   0
    *       0  -1   0
    *       0   0  -1]
    */
-  Matrix3  R_mat = Matrix3::Zero(3,3);
+  Matrix3 R_mat = Matrix3::Zero(3, 3);
-  R_mat(0,0) = 1.0;  R_mat(1,1) = -1.0; R_mat(2,2) = -1.0;
+  R_mat(0, 0) = 1.0;
+  R_mat(1, 1) = -1.0;
+  R_mat(2, 2) = -1.0;
   return Rot3(R_mat);
 }
 
 /* ************************************************************************* */
-Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz)
+Pose3 openGL2gtsam(const Rot3& R, double tx, double ty, double tz) {
-{
   Rot3 R90 = openGLFixedRotation();
-  Rot3 wRc = (  R.inverse() ).compose(R90);
+  Rot3 wRc = (R.inverse()).compose(R90);
 
   // Our camera-to-world translation wTc = -R'*t
-  return Pose3 (wRc, R.unrotate(Point3(-tx,-ty,-tz)));
+  return Pose3(wRc, R.unrotate(Point3(-tx, -ty, -tz)));
 }
 
 /* ************************************************************************* */
-Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz)
+Pose3 gtsam2openGL(const Rot3& R, double tx, double ty, double tz) {
-{
   Rot3 R90 = openGLFixedRotation();
-  Rot3 cRw_openGL = R90.compose(  R.inverse() );
+  Rot3 cRw_openGL = R90.compose(R.inverse());
-  Point3 t_openGL = cRw_openGL.rotate(Point3(-tx,-ty,-tz));
+  Point3 t_openGL = cRw_openGL.rotate(Point3(-tx, -ty, -tz));
   return Pose3(cRw_openGL, t_openGL);
 }
 
 /* ************************************************************************* */
-Pose3 gtsam2openGL(const Pose3& PoseGTSAM)
+Pose3 gtsam2openGL(const Pose3& PoseGTSAM) {
-{
+  return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(),
-  return gtsam2openGL(PoseGTSAM.rotation(), PoseGTSAM.x(), PoseGTSAM.y(), PoseGTSAM.z());
+      PoseGTSAM.z());
 }
 
 /* ************************************************************************* */
-bool readBundler(const string& filename, SfM_data &data)
+bool readBundler(const string& filename, SfM_data &data) {
-{
   // Load the data file
-  ifstream is(filename.c_str(),ifstream::in);
+  ifstream is(filename.c_str(), ifstream::in);
-  if(!is)
+  if (!is) {
-  {
     cout << "Error in readBundler: can not find the file!!" << endl;
     return false;
   }
 
   // Ignore the first line
   char aux[500];
-  is.getline(aux,500);
+  is.getline(aux, 500);
 
   // Get the number of camera poses and 3D points
   size_t nrPoses, nrPoints;
   is >> nrPoses >> nrPoints;
 
   // Get the information for the camera poses
-  for( size_t i = 0; i < nrPoses; i++ )
+  for (size_t i = 0; i < nrPoses; i++) {
-  {
     // Get the focal length and the radial distortion parameters
     float f, k1, k2;
     is >> f >> k1 >> k2;
@@ -482,20 +511,15 @@ bool readBundler(const string& filename, SfM_data &data)
     float r11, r12, r13;
     float r21, r22, r23;
     float r31, r32, r33;
-    is >> r11 >> r12 >> r13
+    is >> r11 >> r12 >> r13 >> r21 >> r22 >> r23 >> r31 >> r32 >> r33;
-    >> r21 >> r22 >> r23
-    >> r31 >> r32 >> r33;
 
     // Bundler-OpenGL rotation matrix
-    Rot3 R(
+    Rot3 R(r11, r12, r13, r21, r22, r23, r31, r32, r33);
-        r11, r12, r13,
-        r21, r22, r23,
-        r31, r32, r33);
 
     // Check for all-zero R, in which case quit
-    if(r11==0 && r12==0 && r13==0)
+    if (r11 == 0 && r12 == 0 && r13 == 0) {
-    {
+      cout << "Error in readBundler: zero rotation matrix for pose " << i
-      cout << "Error in readBundler: zero rotation matrix for pose " << i << endl;
+          << endl;
       return false;
     }
 
@@ -503,38 +527,36 @@ bool readBundler(const string& filename, SfM_data &data)
     float tx, ty, tz;
     is >> tx >> ty >> tz;
 
-    Pose3 pose = openGL2gtsam(R,tx,ty,tz);
+    Pose3 pose = openGL2gtsam(R, tx, ty, tz);
 
-    data.cameras.push_back(SfM_Camera(pose,K));
+    data.cameras.push_back(SfM_Camera(pose, K));
   }
 
   // Get the information for the 3D points
-  for( size_t j = 0; j < nrPoints; j++ )
+  for (size_t j = 0; j < nrPoints; j++) {
-  {
     SfM_Track track;
 
     // Get the 3D position
     float x, y, z;
     is >> x >> y >> z;
-    track.p = Point3(x,y,z);
+    track.p = Point3(x, y, z);
 
     // Get the color information
     float r, g, b;
     is >> r >> g >> b;
-    track.r = r/255.f;
+    track.r = r / 255.f;
-    track.g = g/255.f;
+    track.g = g / 255.f;
-    track.b = b/255.f;
+    track.b = b / 255.f;
 
     // Now get the visibility information
     size_t nvisible = 0;
     is >> nvisible;
 
-    for( size_t k = 0; k < nvisible; k++ )
+    for (size_t k = 0; k < nvisible; k++) {
-    {
       size_t cam_idx = 0, point_idx = 0;
       float u, v;
       is >> cam_idx >> point_idx >> u >> v;
-      track.measurements.push_back(make_pair(cam_idx,Point2(u,-v)));
+      track.measurements.push_back(make_pair(cam_idx, Point2(u, -v)));
     }
 
     data.tracks.push_back(track);
@@ -545,123 +567,10 @@ bool readBundler(const string& filename, SfM_data &data)
 }
 
 /* ************************************************************************* */
-bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial,
+bool readBAL(const string& filename, SfM_data &data) {
-    const kernelFunctionType kernelFunction){
-
-  ifstream is(g2oFile.c_str());
-  if (!is){
-    throw std::invalid_argument("File not found!");
-    return false;
-  }
-
-  // READ INITIAL GUESS FROM G2O FILE
-  string tag;
-  while (is) {
-    if(! (is >> tag))
-      break;
-
-    if (tag == "VERTEX_SE2" || tag == "VERTEX2") {
-      int id;
-      double x, y, yaw;
-      is >> id >> x >> y >> yaw;
-      initial.insert(id, Pose2(x, y, yaw));
-    }
-    is.ignore(LINESIZE, '\n');
-  }
-  is.clear(); /* clears the end-of-file and error flags */
-  is.seekg(0, ios::beg);
-  // initial.print("initial guess");
-
-  // READ MEASUREMENTS FROM G2O FILE
-  while (is) {
-    if(! (is >> tag))
-      break;
-
-    if (tag == "EDGE_SE2" || tag == "EDGE2") {
-      int id1, id2;
-      double x, y, yaw;
-      double I11, I12, I13, I22, I23, I33;
-
-      is >> id1 >> id2 >> x >> y >> yaw;
-      is >> I11 >> I12 >> I13 >> I22 >> I23 >> I33;
-      Pose2 l1Xl2(x, y, yaw);
-      noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << I11, I22, I33));
-
-      switch (kernelFunction) {
-      {case QUADRATIC:
-        NonlinearFactor::shared_ptr factor(new BetweenFactor<Pose2>(id1, id2, l1Xl2, model));
-        graph.add(factor);
-        break;}
-      {case HUBER:
-        NonlinearFactor::shared_ptr huberFactor(new BetweenFactor<Pose2>(id1, id2, l1Xl2,
-            noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), model)));
-        graph.add(huberFactor);
-        break;}
-      {case TUKEY:
-        NonlinearFactor::shared_ptr tukeyFactor(new BetweenFactor<Pose2>(id1, id2, l1Xl2,
-            noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), model)));
-        graph.add(tukeyFactor);
-        break;}
-      }
-    }
-    is.ignore(LINESIZE, '\n');
-  }
-  // Output which kernel is used
-  switch (kernelFunction) {
-  case QUADRATIC:
-      break;
-  case HUBER:
-    std::cout << "Robust kernel: Huber" << std::endl; break;
-  case TUKEY:
-    std::cout << "Robust kernel: Tukey" << std::endl; break;
-  }
-  return true;
-}
-
-/* ************************************************************************* */
-bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate){
-
-  fstream stream(filename.c_str(), fstream::out);
-
-  // save poses
-  BOOST_FOREACH(const Values::ConstKeyValuePair& key_value, estimate)
-  {
-    const Pose2& pose = dynamic_cast<const Pose2&>(key_value.value);
-    stream << "VERTEX_SE2 " << key_value.key << " " << pose.x() << " "
-        << pose.y() << " " << pose.theta() << endl;
-  }
-
-  // save edges
-  BOOST_FOREACH(boost::shared_ptr<NonlinearFactor> factor_, graph)
-  {
-    boost::shared_ptr<BetweenFactor<Pose2> > factor =
-        boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor_);
-    if (!factor)
-      continue;
-
-    SharedNoiseModel model = factor->get_noiseModel();
-    boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
-        boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
-    if (!diagonalModel)
-      throw std::invalid_argument("writeG2o: invalid noise model (current version assumes diagonal noise model)!");
-
-    Pose2 pose = factor->measured(); //.inverse();
-    stream << "EDGE_SE2 " << factor->key1() << " " << factor->key2() << " "
-        << pose.x() << " " << pose.y() << " " << pose.theta() << " "
-        << diagonalModel->precision(0) << " " << 0.0 << " " << 0.0 << " "
-        << diagonalModel->precision(1) << " " << 0.0 << " " << diagonalModel->precision(2) << endl;
-  }
-  stream.close();
-  return true;
-}
-
-/* ************************************************************************* */
-bool readBAL(const string& filename, SfM_data &data)
-{
   // Load the data file
-  ifstream is(filename.c_str(),ifstream::in);
+  ifstream is(filename.c_str(), ifstream::in);
-  if(!is)
+  if (!is) {
-  {
     cout << "Error in readBAL: can not find the file!!" << endl;
     return false;
   }
@@ -673,44 +582,41 @@ bool readBAL(const string& filename, SfM_data &data)
   data.tracks.resize(nrPoints);
 
   // Get the information for the observations
-  for( size_t k = 0; k < nrObservations; k++ )
+  for (size_t k = 0; k < nrObservations; k++) {
-  {
     size_t i = 0, j = 0;
     float u, v;
     is >> i >> j >> u >> v;
-    data.tracks[j].measurements.push_back(make_pair(i,Point2(u,-v)));
+    data.tracks[j].measurements.push_back(make_pair(i, Point2(u, -v)));
   }
 
   // Get the information for the camera poses
-  for( size_t i = 0; i < nrPoses; i++ )
+  for (size_t i = 0; i < nrPoses; i++) {
-  {
     // Get the rodriguez vector
     float wx, wy, wz;
     is >> wx >> wy >> wz;
-    Rot3 R = Rot3::rodriguez(wx, wy, wz);// BAL-OpenGL rotation matrix
+    Rot3 R = Rot3::rodriguez(wx, wy, wz); // BAL-OpenGL rotation matrix
 
     // Get the translation vector
     float tx, ty, tz;
     is >> tx >> ty >> tz;
 
-    Pose3 pose = openGL2gtsam(R,tx,ty,tz);
+    Pose3 pose = openGL2gtsam(R, tx, ty, tz);
 
     // Get the focal length and the radial distortion parameters
     float f, k1, k2;
     is >> f >> k1 >> k2;
     Cal3Bundler K(f, k1, k2);
 
-    data.cameras.push_back(SfM_Camera(pose,K));
+    data.cameras.push_back(SfM_Camera(pose, K));
   }
 
   // Get the information for the 3D points
-  for( size_t j = 0; j < nrPoints; j++ )
+  for (size_t j = 0; j < nrPoints; j++) {
-  {
     // Get the 3D position
     float x, y, z;
     is >> x >> y >> z;
     SfM_Track& track = data.tracks[j];
-    track.p = Point3(x,y,z);
+    track.p = Point3(x, y, z);
     track.r = 0.4f;
     track.g = 0.4f;
     track.b = 0.4f;
@@ -721,8 +627,7 @@ bool readBAL(const string& filename, SfM_data &data)
 }
 
 /* ************************************************************************* */
-bool writeBAL(const string& filename, SfM_data &data)
+bool writeBAL(const string& filename, SfM_data &data) {
-{
   // Open the output file
   ofstream os;
   os.open(filename.c_str());
@@ -733,36 +638,42 @@ bool writeBAL(const string& filename, SfM_data &data)
   }
 
   // Write the number of camera poses and 3D points
-  size_t nrObservations=0;
+  size_t nrObservations = 0;
-  for (size_t j = 0; j < data.number_tracks(); j++){
+  for (size_t j = 0; j < data.number_tracks(); j++) {
     nrObservations += data.tracks[j].number_measurements();
   }
 
   // Write observations
-  os <<  data.number_cameras() << " " << data.number_tracks() << " " << nrObservations << endl;
+  os << data.number_cameras() << " " << data.number_tracks() << " "
+      << nrObservations << endl;
   os << endl;
 
-  for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j
+  for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
     SfM_Track track = data.tracks[j];
 
-    for(size_t k = 0; k < track.number_measurements(); k++){ // for each observation of the 3D point j
+    for (size_t k = 0; k < track.number_measurements(); k++) { // for each observation of the 3D point j
       size_t i = track.measurements[k].first; // camera id
       double u0 = data.cameras[i].calibration().u0();
       double v0 = data.cameras[i].calibration().v0();
 
-      if(u0 != 0 || v0 != 0){cout<< "writeBAL has not been tested for calibration with nonzero (u0,v0)"<< endl;}
+      if (u0 != 0 || v0 != 0) {
+        cout
+            << "writeBAL has not been tested for calibration with nonzero (u0,v0)"
+            << endl;
+      }
 
       double pixelBALx = track.measurements[k].second.x() - u0; // center of image is the origin
-      double pixelBALy = - (track.measurements[k].second.y() - v0); // center of image is the origin
+      double pixelBALy = -(track.measurements[k].second.y() - v0); // center of image is the origin
       Point2 pixelMeasurement(pixelBALx, pixelBALy);
-      os <<  i /*camera id*/ << " " << j /*point id*/ << " "
+      os << i /*camera id*/<< " " << j /*point id*/<< " "
-          << pixelMeasurement.x() /*u of the pixel*/ << " " << pixelMeasurement.y() /*v of the pixel*/ << endl;
+          << pixelMeasurement.x() /*u of the pixel*/<< " "
+          << pixelMeasurement.y() /*v of the pixel*/<< endl;
     }
   }
   os << endl;
 
   // Write cameras
-  for (size_t i = 0; i < data.number_cameras(); i++){ // for each camera
+  for (size_t i = 0; i < data.number_cameras(); i++) { // for each camera
     Pose3 poseGTSAM = data.cameras[i].pose();
     Cal3Bundler cameraCalibration = data.cameras[i].calibration();
     Pose3 poseOpenGL = gtsam2openGL(poseGTSAM);
@@ -775,7 +686,7 @@ bool writeBAL(const string& filename, SfM_data &data)
   }
 
   // Write the points
-  for (size_t j = 0; j < data.number_tracks(); j++){ // for each 3D point j
+  for (size_t j = 0; j < data.number_tracks(); j++) { // for each 3D point j
     Point3 point = data.tracks[j].p;
     os << point.x() << endl;
     os << point.y() << endl;
@@ -787,48 +698,55 @@ bool writeBAL(const string& filename, SfM_data &data)
   return true;
 }
 
-bool writeBALfromValues(const string& filename, const SfM_data &data, Values& values){
+bool writeBALfromValues(const string& filename, const SfM_data &data,
+    Values& values) {
 
   SfM_data dataValues = data;
 
   // Store poses or cameras in SfM_data
   Values valuesPoses = values.filter<Pose3>();
-  if( valuesPoses.size() == dataValues.number_cameras() ){ // we only estimated camera poses
+  if (valuesPoses.size() == dataValues.number_cameras()) { // we only estimated camera poses
-    for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera
+    for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
-      Key poseKey = symbol('x',i);
+      Key poseKey = symbol('x', i);
       Pose3 pose = values.at<Pose3>(poseKey);
       Cal3Bundler K = dataValues.cameras[i].calibration();
       PinholeCamera<Cal3Bundler> camera(pose, K);
       dataValues.cameras[i] = camera;
     }
   } else {
-    Values valuesCameras = values.filter< PinholeCamera<Cal3Bundler> >();
+    Values valuesCameras = values.filter<PinholeCamera<Cal3Bundler> >();
-    if ( valuesCameras.size() == dataValues.number_cameras() ){  // we only estimated camera poses and calibration
+    if (valuesCameras.size() == dataValues.number_cameras()) { // we only estimated camera poses and calibration
-      for (size_t i = 0; i < dataValues.number_cameras(); i++){ // for each camera
+      for (size_t i = 0; i < dataValues.number_cameras(); i++) { // for each camera
         Key cameraKey = i; // symbol('c',i);
-        PinholeCamera<Cal3Bundler> camera = values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
+        PinholeCamera<Cal3Bundler> camera =
+            values.at<PinholeCamera<Cal3Bundler> >(cameraKey);
         dataValues.cameras[i] = camera;
       }
-    }else{
+    } else {
-      cout << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= " << dataValues.number_cameras()
+      cout
-              <<") and values (#cameras " << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!" << endl;
+          << "writeBALfromValues: different number of cameras in SfM_dataValues (#cameras= "
+          << dataValues.number_cameras() << ") and values (#cameras "
+          << valuesPoses.size() << ", #poses " << valuesCameras.size() << ")!!"
+          << endl;
       return false;
     }
   }
 
   // Store 3D points in SfM_data
   Values valuesPoints = values.filter<Point3>();
-  if( valuesPoints.size() != dataValues.number_tracks()){
+  if (valuesPoints.size() != dataValues.number_tracks()) {
-    cout << "writeBALfromValues: different number of points in SfM_dataValues (#points= " << dataValues.number_tracks()
+    cout
-            <<") and values (#points " << valuesPoints.size() << ")!!" << endl;
+        << "writeBALfromValues: different number of points in SfM_dataValues (#points= "
+        << dataValues.number_tracks() << ") and values (#points "
+        << valuesPoints.size() << ")!!" << endl;
   }
 
-  for (size_t j = 0; j < dataValues.number_tracks(); j++){ // for each point
+  for (size_t j = 0; j < dataValues.number_tracks(); j++) { // for each point
     Key pointKey = P(j);
-    if(values.exists(pointKey)){
+    if (values.exists(pointKey)) {
       Point3 point = values.at<Point3>(pointKey);
       dataValues.tracks[j].p = point;
-    }else{
+    } else {
       dataValues.tracks[j].r = 1.0;
       dataValues.tracks[j].g = 0.0;
       dataValues.tracks[j].b = 0.0;

gtsam/slam/dataset.h

@@ -35,7 +35,7 @@ namespace gtsam {
 /**
  * Find the full path to an example dataset distributed with gtsam.  The name
  * may be specified with or without a file extension - if no extension is
- * give, this function first looks for the .graph extension, then .txt.  We
+ * given, this function first looks for the .graph extension, then .txt.  We
  * first check the gtsam source tree for the file, followed by the installed
  * example dataset location.  Both the source tree and installed locations
  * are obtained from CMake during compilation.
@@ -44,8 +44,30 @@ namespace gtsam {
  * search process described above.
  */
 GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
+
+/**
+ * Creates a temporary file name that needs to be ignored in .gitingnore
+ * for checking read-write oprations
+ */
+GTSAM_EXPORT std::string createRewrittenFileName(const std::string& name);
 #endif
 
+/// Indicates how noise parameters are stored in file
+enum NoiseFormat {
+  NoiseFormatG2O, ///< Information matrix I11, I12, I13, I22, I23, I33
+  NoiseFormatTORO, ///< Information matrix, but inf_ff inf_fs inf_ss inf_rr inf_fr inf_sr
+  NoiseFormatGRAPH, ///< default: toro-style order, but covariance matrix !
+  NoiseFormatCOV ///< Covariance matrix C11, C12, C13, C22, C23, C33
+};
+
+/// Robust kernel type to wrap around quadratic noise model
+enum KernelFunctionType {
+  KernelFunctionTypeNONE, KernelFunctionTypeHUBER, KernelFunctionTypeTUKEY
+};
+
+/// Return type for load functions
+typedef std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> GraphAndValues;
+
 /**
  * Load TORO 2D Graph
  * @param dataset/model pair as constructed by [dataset]
@@ -53,31 +75,57 @@ GTSAM_EXPORT std::string findExampleDataFile(const std::string& name);
  * @param addNoise add noise to the edges
  * @param smart try to reduce complexity of covariance to cheapest model
  */
-GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
+GTSAM_EXPORT GraphAndValues load2D(
-    std::pair<std::string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset,
+    std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0,
-    int maxID = 0, bool addNoise = false, bool smart = true);
+    bool addNoise = false,
+    bool smart = true, //
+    NoiseFormat noiseFormat = NoiseFormatGRAPH,
+    KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
 
 /**
- * Load TORO 2D Graph
+ * Load TORO/G2O style graph files
  * @param filename
  * @param model optional noise model to use instead of one specified by file
  * @param maxID if non-zero cut out vertices >= maxID
  * @param addNoise add noise to the edges
  * @param smart try to reduce complexity of covariance to cheapest model
+ * @param noiseFormat how noise parameters are stored
+ * @param kernelFunctionType whether to wrap the noise model in a robust kernel
+ * @return graph and initial values
  */
-GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
+GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
-    const std::string& filename,
+    SharedNoiseModel model = SharedNoiseModel(), int maxID = 0, bool addNoise =
-    boost::optional<gtsam::SharedDiagonal> model = boost::optional<
+        false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatGRAPH, //
-    noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false,
+    KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
-    bool smart = true);
 
-GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
+/// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel
-    const std::string& filename,
+GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename,
-    gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0);
+    noiseModel::Base::shared_ptr& model, int maxID = 0);
 
 /** save 2d graph */
-GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config,
+GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
-    const noiseModel::Diagonal::shared_ptr model, const std::string& filename);
+    const Values& config, const noiseModel::Diagonal::shared_ptr model,
+    const std::string& filename);
+
+/**
+ * @brief This function parses a g2o file and stores the measurements into a
+ * NonlinearFactorGraph and the initial guess in a Values structure
+ * @param filename The name of the g2o file
+ * @param kernelFunctionType whether to wrap the noise model in a robust kernel
+ * @return graph and initial values
+ */
+GTSAM_EXPORT GraphAndValues readG2o(const std::string& g2oFile,
+    KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
+
+/**
+ * @brief This function writes a g2o file from
+ * NonlinearFactorGraph and a Values structure
+ * @param filename The name of the g2o file to write
+ * @param graph NonlinearFactor graph storing the measurements
+ * @param estimate Values
+ */
+GTSAM_EXPORT void writeG2o(const NonlinearFactorGraph& graph,
+    const Values& estimate, const std::string& filename);
 
 /**
  * Load TORO 3D Graph
@@ -85,27 +133,31 @@ GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config
 GTSAM_EXPORT bool load3D(const std::string& filename);
 
 /// A measurement with its camera index
-typedef std::pair<size_t,gtsam::Point2> SfM_Measurement;
+typedef std::pair<size_t, Point2> SfM_Measurement;
 
 /// Define the structure for the 3D points
-struct SfM_Track
+struct SfM_Track {
-{
+  Point3 p; ///< 3D position of the point
-  gtsam::Point3 p; ///< 3D position of the point
+  float r, g, b; ///< RGB color of the 3D point
-  float r,g,b; ///< RGB color of the 3D point
   std::vector<SfM_Measurement> measurements; ///< The 2D image projections (id,(u,v))
-  size_t number_measurements() const { return measurements.size();}
+  size_t number_measurements() const {
+    return measurements.size();
+  }
 };
 
 /// Define the structure for the camera poses
-typedef gtsam::PinholeCamera<gtsam::Cal3Bundler> SfM_Camera;
+typedef PinholeCamera<Cal3Bundler> SfM_Camera;
 
 /// Define the structure for SfM data
-struct SfM_data
+struct SfM_data {
-{
   std::vector<SfM_Camera> cameras; ///< Set of cameras
   std::vector<SfM_Track> tracks; ///< Sparse set of points
-  size_t number_cameras() const { return cameras.size();}   ///< The number of camera poses
+  size_t number_cameras() const {
-  size_t number_tracks()  const { return tracks.size();}  ///< The number of reconstructed 3D points
+    return cameras.size();
+  } ///< The number of camera poses
+  size_t number_tracks() const {
+    return tracks.size();
+  } ///< The number of reconstructed 3D points
 };
 
 /**
@@ -117,25 +169,6 @@ struct SfM_data
  */
 GTSAM_EXPORT bool readBundler(const std::string& filename, SfM_data &data);
 
-/**
- * @brief This function parses a g2o file and stores the measurements into a
- * NonlinearFactorGraph and the initial guess in a Values structure
- * @param filename The name of the g2o file
- * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2). NOTE: information matrix is assumed diagonal.
- * @return initial Values containing the initial guess (VERTEX_SE2)
- */
-enum kernelFunctionType { QUADRATIC, HUBER, TUKEY };
-GTSAM_EXPORT bool readG2o(const std::string& g2oFile, NonlinearFactorGraph& graph, Values& initial, const kernelFunctionType kernelFunction = QUADRATIC);
-
-/**
- * @brief This function writes a g2o file from
- * NonlinearFactorGraph and a Values structure
- * @param filename The name of the g2o file to write
- * @param graph NonlinearFactor graph storing the measurements (EDGE_SE2)
- * @return estimate Values containing the values (VERTEX_SE2)
- */
-GTSAM_EXPORT bool writeG2o(const std::string& filename, const NonlinearFactorGraph& graph, const Values& estimate);
-
 /**
  * @brief This function parses a "Bundle Adjustment in the Large" (BAL) file and stores the data into a
  * SfM_data structure
@@ -165,7 +198,8 @@ GTSAM_EXPORT bool writeBAL(const std::string& filename, SfM_data &data);
  * assumes that the keys are "x1" for pose 1 (or "c1" for camera 1) and "l1" for landmark 1
  * @return true if the parsing was successful, false otherwise
  */
-GTSAM_EXPORT bool writeBALfromValues(const std::string& filename, const SfM_data &data, Values& values);
+GTSAM_EXPORT bool writeBALfromValues(const std::string& filename,
+    const SfM_data &data, Values& values);
 
 /**
  * @brief This function converts an openGL camera pose to an GTSAM camera pose
@@ -208,5 +242,4 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfM_data& db);
  */
 GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_data& db);
 
-
 } // namespace gtsam

gtsam/slam/lago.cpp

@@ -10,38 +10,60 @@
  * -------------------------------------------------------------------------- */
 
 /**
- *  @file  LagoInitializer.h
+ *  @file  lago.h
  *  @author Luca Carlone
  *  @author Frank Dellaert
  *  @date   May 14, 2014
  */
 
-#include <gtsam/nonlinear/LagoInitializer.h>
+#include <gtsam/slam/lago.h>
-#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/geometry/Pose2.h>
+#include <gtsam/base/timing.h>
+
 #include <boost/math/special_functions.hpp>
 
-namespace gtsam {
+using namespace std;
 
-static Matrix I = eye(1);
+namespace gtsam {
-static Matrix I3 = eye(3);
+namespace lago {
+
+static const Matrix I = eye(1);
+static const Matrix I3 = eye(3);
+
+static const Key keyAnchor = symbol('Z', 9999999);
+static const noiseModel::Diagonal::shared_ptr priorOrientationNoise =
+    noiseModel::Diagonal::Sigmas((Vector(1) << 0));
+static const noiseModel::Diagonal::shared_ptr priorPose2Noise =
+    noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8));
 
 /* ************************************************************************* */
-double computeThetaToRoot(const Key nodeKey, const PredecessorMap<Key>& tree,
+/**
-    const key2doubleMap& deltaThetaMap, const key2doubleMap& thetaFromRootMap) {
+ * Compute the cumulative orientation (without wrapping) wrt the root of a
+ * spanning tree (tree) for a node (nodeKey). The function starts at the nodes and
+ * moves towards the root summing up the (directed) rotation measurements.
+ * Relative measurements are encoded in "deltaThetaMap".
+ * The root is assumed to have orientation zero.
+ */
+static double computeThetaToRoot(const Key nodeKey,
+    const PredecessorMap<Key>& tree, const key2doubleMap& deltaThetaMap,
+    const key2doubleMap& thetaFromRootMap) {
 
   double nodeTheta = 0;
   Key key_child = nodeKey; // the node
   Key key_parent = 0; // the initialization does not matter
-  while(1){
+  while (1) {
     // We check if we reached the root
-    if(tree.at(key_child)==key_child) // if we reached the root
+    if (tree.at(key_child) == key_child) // if we reached the root
       break;
     // we sum the delta theta corresponding to the edge parent->child
     nodeTheta += deltaThetaMap.at(key_child);
     // we get the parent
     key_parent = tree.at(key_child); // the parent
     // we check if we connected to some part of the tree we know
-    if(thetaFromRootMap.find(key_parent)!=thetaFromRootMap.end()){
+    if (thetaFromRootMap.find(key_parent) != thetaFromRootMap.end()) {
       nodeTheta += thetaFromRootMap.at(key_parent);
       break;
     }
@@ -55,53 +77,55 @@ key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
     const PredecessorMap<Key>& tree) {
 
   key2doubleMap thetaToRootMap;
-  key2doubleMap::const_iterator it;
 
   // Orientation of the roo
-  thetaToRootMap.insert(std::pair<Key, double>(keyAnchor, 0.0));
+  thetaToRootMap.insert(pair<Key, double>(keyAnchor, 0.0));
 
   // for all nodes in the tree
-  for(it = deltaThetaMap.begin(); it != deltaThetaMap.end(); ++it ){
+  BOOST_FOREACH(const key2doubleMap::value_type& it, deltaThetaMap) {
     // compute the orientation wrt root
-    Key nodeKey = it->first;
+    Key nodeKey = it.first;
     double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap,
         thetaToRootMap);
-    thetaToRootMap.insert(std::pair<Key, double>(nodeKey, nodeTheta));
+    thetaToRootMap.insert(pair<Key, double>(nodeKey, nodeTheta));
   }
   return thetaToRootMap;
 }
 
 /* ************************************************************************* */
 void getSymbolicGraph(
-    /*OUTPUTS*/ std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds, key2doubleMap& deltaThetaMap,
+/*OUTPUTS*/vector<size_t>& spanningTreeIds, vector<size_t>& chordsIds,
-    /*INPUTS*/ const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g){
+    key2doubleMap& deltaThetaMap,
+    /*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g) {
 
   // Get keys for which you want the orientation
-  size_t id=0;
+  size_t id = 0;
   // Loop over the factors
-  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g){
+  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g) {
-    if (factor->keys().size() == 2){
+    if (factor->keys().size() == 2) {
       Key key1 = factor->keys()[0];
       Key key2 = factor->keys()[1];
       // recast to a between
-      boost::shared_ptr< BetweenFactor<Pose2> > pose2Between =
+      boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
-          boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor);
+          boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
-      if (!pose2Between) continue;
+      if (!pose2Between)
+        continue;
       // get the orientation - measured().theta();
       double deltaTheta = pose2Between->measured().theta();
       // insert (directed) orientations in the map "deltaThetaMap"
-      bool inTree=false;
+      bool inTree = false;
-      if(tree.at(key1)==key2){ // key2 -> key1
+      if (tree.at(key1) == key2) { // key2 -> key1
-        deltaThetaMap.insert(std::pair<Key, double>(key1, -deltaTheta));
+        deltaThetaMap.insert(pair<Key, double>(key1, -deltaTheta));
         inTree = true;
-      } else if(tree.at(key2)==key1){ // key1 -> key2
+      } else if (tree.at(key2) == key1) { // key1 -> key2
-        deltaThetaMap.insert(std::pair<Key, double>(key2,  deltaTheta));
+        deltaThetaMap.insert(pair<Key, double>(key2, deltaTheta));
         inTree = true;
       }
       // store factor slot, distinguishing spanning tree edges from chordsIds
-      if(inTree == true)
+      if (inTree == true)
         spanningTreeIds.push_back(id);
-      else // it's a chord!
+      else
+        // it's a chord!
         chordsIds.push_back(id);
     }
     id++;
@@ -109,14 +133,16 @@ void getSymbolicGraph(
 }
 
 /* ************************************************************************* */
-void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
+// Retrieve the deltaTheta and the corresponding noise model from a BetweenFactor<Pose2>
+static void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
     Vector& deltaTheta, noiseModel::Diagonal::shared_ptr& model_deltaTheta) {
 
   // Get the relative rotation measurement from the between factor
   boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
       boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
   if (!pose2Between)
-    throw std::invalid_argument("buildLinearOrientationGraph: invalid between factor!");
+    throw invalid_argument(
+        "buildLinearOrientationGraph: invalid between factor!");
   deltaTheta = (Vector(1) << pose2Between->measured().theta());
 
   // Retrieve the noise model for the relative rotation
@@ -124,114 +150,127 @@ void getDeltaThetaAndNoise(NonlinearFactor::shared_ptr factor,
   boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
       boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
   if (!diagonalModel)
-    throw std::invalid_argument("buildLinearOrientationGraph: invalid noise model "
+    throw invalid_argument("buildLinearOrientationGraph: invalid noise model "
         "(current version assumes diagonal noise model)!");
-  Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2) ); // std on the angular measurement
+  Vector std_deltaTheta = (Vector(1) << diagonalModel->sigma(2)); // std on the angular measurement
   model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta);
 }
 
 /* ************************************************************************* */
-GaussianFactorGraph buildLinearOrientationGraph(const std::vector<size_t>& spanningTreeIds, const std::vector<size_t>& chordsIds,
+GaussianFactorGraph buildLinearOrientationGraph(
-    const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree){
+    const vector<size_t>& spanningTreeIds, const vector<size_t>& chordsIds,
+    const NonlinearFactorGraph& g, const key2doubleMap& orientationsToRoot,
+    const PredecessorMap<Key>& tree) {
 
   GaussianFactorGraph lagoGraph;
   Vector deltaTheta;
   noiseModel::Diagonal::shared_ptr model_deltaTheta;
 
   // put original measurements in the spanning tree
-  BOOST_FOREACH(const size_t& factorId, spanningTreeIds){
+  BOOST_FOREACH(const size_t& factorId, spanningTreeIds) {
     const FastVector<Key>& keys = g[factorId]->keys();
     Key key1 = keys[0], key2 = keys[1];
     getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
-    lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaTheta, model_deltaTheta));
+    lagoGraph.add(key1, -I, key2, I, deltaTheta, model_deltaTheta);
   }
   // put regularized measurements in the chordsIds
-  BOOST_FOREACH(const size_t& factorId, chordsIds){
+  BOOST_FOREACH(const size_t& factorId, chordsIds) {
     const FastVector<Key>& keys = g[factorId]->keys();
     Key key1 = keys[0], key2 = keys[1];
     getDeltaThetaAndNoise(g[factorId], deltaTheta, model_deltaTheta);
     double key1_DeltaTheta_key2 = deltaTheta(0);
-    ///std::cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << std::endl;
+    ///cout << "REG: key1= " << DefaultKeyFormatter(key1) << " key2= " << DefaultKeyFormatter(key2) << endl;
-    double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1) - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord
+    double k2pi_noise = key1_DeltaTheta_key2 + orientationsToRoot.at(key1)
-    double k = boost::math::round(k2pi_noise/(2*M_PI));
+        - orientationsToRoot.at(key2); // this coincides to summing up measurements along the cycle induced by the chord
-    //if (k2pi_noise - 2*k*M_PI > 1e-5) std::cout << k2pi_noise - 2*k*M_PI << std::endl; // for debug
+    double k = boost::math::round(k2pi_noise / (2 * M_PI));
-    Vector deltaThetaRegularized = (Vector(1) << key1_DeltaTheta_key2 - 2*k*M_PI);
+    //if (k2pi_noise - 2*k*M_PI > 1e-5) cout << k2pi_noise - 2*k*M_PI << endl; // for debug
-    lagoGraph.add(JacobianFactor(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta));
+    Vector deltaThetaRegularized = (Vector(1)
+        << key1_DeltaTheta_key2 - 2 * k * M_PI);
+    lagoGraph.add(key1, -I, key2, I, deltaThetaRegularized, model_deltaTheta);
   }
   // prior on the anchor orientation
-  lagoGraph.add(JacobianFactor(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise));
+  lagoGraph.add(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise);
   return lagoGraph;
 }
 
 /* ************************************************************************* */
-NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph){
+// Select the subgraph of betweenFactors and transforms priors into between wrt a fictitious node
+static NonlinearFactorGraph buildPose2graph(const NonlinearFactorGraph& graph) {
+  gttic(lago_buildPose2graph);
   NonlinearFactorGraph pose2Graph;
 
-  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph){
+  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, graph) {
 
     // recast to a between on Pose2
-    boost::shared_ptr< BetweenFactor<Pose2> > pose2Between =
+    boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
-        boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor);
+        boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
     if (pose2Between)
       pose2Graph.add(pose2Between);
 
     // recast PriorFactor<Pose2> to BetweenFactor<Pose2>
-    boost::shared_ptr< PriorFactor<Pose2> > pose2Prior =
+    boost::shared_ptr<PriorFactor<Pose2> > pose2Prior =
-        boost::dynamic_pointer_cast< PriorFactor<Pose2> >(factor);
+        boost::dynamic_pointer_cast<PriorFactor<Pose2> >(factor);
     if (pose2Prior)
-      pose2Graph.add(BetweenFactor<Pose2>(keyAnchor, pose2Prior->keys()[0],
+      pose2Graph.add(
+          BetweenFactor<Pose2>(keyAnchor, pose2Prior->keys()[0],
               pose2Prior->prior(), pose2Prior->get_noiseModel()));
   }
   return pose2Graph;
 }
 
 /* ************************************************************************* */
-PredecessorMap<Key> findOdometricPath(const NonlinearFactorGraph& pose2Graph) {
+static PredecessorMap<Key> findOdometricPath(
+    const NonlinearFactorGraph& pose2Graph) {
 
   PredecessorMap<Key> tree;
   Key minKey;
   bool minUnassigned = true;
 
-  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2Graph){
+  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2Graph) {
 
     Key key1 = std::min(factor->keys()[0], factor->keys()[1]);
     Key key2 = std::max(factor->keys()[0], factor->keys()[1]);
-    if(minUnassigned){
+    if (minUnassigned) {
       minKey = key1;
       minUnassigned = false;
     }
-    if( key2 - key1 == 1){ // consecutive keys
+    if (key2 - key1 == 1) { // consecutive keys
       tree.insert(key2, key1);
-      if(key1 < minKey)
+      if (key1 < minKey)
         minKey = key1;
     }
   }
-  tree.insert(minKey,keyAnchor);
+  tree.insert(minKey, keyAnchor);
-  tree.insert(keyAnchor,keyAnchor); // root
+  tree.insert(keyAnchor, keyAnchor); // root
   return tree;
 }
 
 /* ************************************************************************* */
-VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, bool useOdometricPath){
+// Return the orientations of a graph including only BetweenFactors<Pose2>
+static VectorValues computeOrientations(const NonlinearFactorGraph& pose2Graph,
+    bool useOdometricPath) {
+  gttic(lago_computeOrientations);
 
   // Find a minimum spanning tree
   PredecessorMap<Key> tree;
   if (useOdometricPath)
     tree = findOdometricPath(pose2Graph);
   else
-    tree = findMinimumSpanningTree<NonlinearFactorGraph, Key, BetweenFactor<Pose2> >(pose2Graph);
+    tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
+        BetweenFactor<Pose2> >(pose2Graph);
 
   // Create a linear factor graph (LFG) of scalars
   key2doubleMap deltaThetaMap;
-  std::vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
+  vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
-  std::vector<size_t> chordsIds;       // ids of between factors corresponding to chordsIds wrt T
+  vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
   getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, pose2Graph);
 
   // temporary structure to correct wraparounds along loops
   key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree);
 
   // regularize measurements and plug everything in a factor graph
-  GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, pose2Graph, orientationsToRoot, tree);
+  GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds,
+      chordsIds, pose2Graph, orientationsToRoot, tree);
 
   // Solve the LFG
   VectorValues orientationsLago = lagoGraph.optimize();
@@ -240,70 +279,79 @@ VectorValues computeLagoOrientations(const NonlinearFactorGraph& pose2Graph, boo
 }
 
 /* ************************************************************************* */
-VectorValues initializeOrientationsLago(const NonlinearFactorGraph& graph, bool useOdometricPath) {
+VectorValues initializeOrientations(const NonlinearFactorGraph& graph,
+    bool useOdometricPath) {
 
   // We "extract" the Pose2 subgraph of the original graph: this
   // is done to properly model priors and avoiding operating on a larger graph
   NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
 
   // Get orientations from relative orientation measurements
-  return computeLagoOrientations(pose2Graph, useOdometricPath);
+  return computeOrientations(pose2Graph, useOdometricPath);
 }
 
 /* ************************************************************************* */
-Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& orientationsLago) {
+Values computePoses(const NonlinearFactorGraph& pose2graph,
+    VectorValues& orientationsLago) {
+  gttic(lago_computePoses);
 
   // Linearized graph on full poses
   GaussianFactorGraph linearPose2graph;
 
   // We include the linear version of each between factor
-  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2graph){
+  BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, pose2graph) {
 
-    boost::shared_ptr< BetweenFactor<Pose2> > pose2Between =
+    boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
-        boost::dynamic_pointer_cast< BetweenFactor<Pose2> >(factor);
+        boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
 
-    if(pose2Between){
+    if (pose2Between) {
       Key key1 = pose2Between->keys()[0];
       double theta1 = orientationsLago.at(key1)(0);
-      double s1 = sin(theta1); double c1 = cos(theta1);
+      double s1 = sin(theta1);
+      double c1 = cos(theta1);
 
       Key key2 = pose2Between->keys()[1];
       double theta2 = orientationsLago.at(key2)(0);
 
-      double linearDeltaRot = theta2 - theta1 - pose2Between->measured().theta();
+      double linearDeltaRot = theta2 - theta1
+          - pose2Between->measured().theta();
       linearDeltaRot = Rot2(linearDeltaRot).theta(); // to normalize
 
       double dx = pose2Between->measured().x();
       double dy = pose2Between->measured().y();
 
-      Vector globalDeltaCart = (Vector(2) << c1*dx - s1*dy, s1*dx + c1*dy);
+      Vector globalDeltaCart = //
-      Vector b = (Vector(3) <<  globalDeltaCart, linearDeltaRot );// rhs
+          (Vector(2) << c1 * dx - s1 * dy, s1 * dx + c1 * dy);
-      Matrix J1 = - I3;
+      Vector b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs
-      J1(0,2) =  s1*dx + c1*dy;
+      Matrix J1 = -I3;
-      J1(1,2) = -c1*dx + s1*dy;
+      J1(0, 2) = s1 * dx + c1 * dy;
+      J1(1, 2) = -c1 * dx + s1 * dy;
       // Retrieve the noise model for the relative rotation
       boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
-          boost::dynamic_pointer_cast<noiseModel::Diagonal>(pose2Between->get_noiseModel());
+          boost::dynamic_pointer_cast<noiseModel::Diagonal>(
+              pose2Between->get_noiseModel());
 
-      linearPose2graph.add(JacobianFactor(key1, J1, key2, I3, b, diagonalModel));
+      linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel);
-    }else{
+    } else {
-      throw std::invalid_argument("computeLagoPoses: cannot manage non between factor here!");
+      throw invalid_argument(
+          "computeLagoPoses: cannot manage non between factor here!");
     }
   }
   // add prior
-  noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
+  linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0),
-  linearPose2graph.add(JacobianFactor(keyAnchor, I3, (Vector(3) << 0.0,0.0,0.0), priorModel));
+      priorPose2Noise);
 
   // optimize
   VectorValues posesLago = linearPose2graph.optimize();
 
   // put into Values structure
   Values initialGuessLago;
-  for(VectorValues::const_iterator it = posesLago.begin(); it != posesLago.end(); ++it ){
+  BOOST_FOREACH(const VectorValues::value_type& it, posesLago) {
-    Key key = it->first;
+    Key key = it.first;
-    if (key != keyAnchor){
+    if (key != keyAnchor) {
-      Vector poseVector = posesLago.at(key);
+      const Vector& poseVector = it.second;
-      Pose2 poseLago = Pose2(poseVector(0),poseVector(1),orientationsLago.at(key)(0)+poseVector(2));
+      Pose2 poseLago = Pose2(poseVector(0), poseVector(1),
+          orientationsLago.at(key)(0) + poseVector(2));
       initialGuessLago.insert(key, poseLago);
     }
   }
@@ -311,37 +359,41 @@ Values computeLagoPoses(const NonlinearFactorGraph& pose2graph, VectorValues& or
 }
 
 /* ************************************************************************* */
-Values initializeLago(const NonlinearFactorGraph& graph, bool useOdometricPath) {
+Values initialize(const NonlinearFactorGraph& graph, bool useOdometricPath) {
+  gttic(lago_initialize);
 
   // We "extract" the Pose2 subgraph of the original graph: this
   // is done to properly model priors and avoiding operating on a larger graph
   NonlinearFactorGraph pose2Graph = buildPose2graph(graph);
 
   // Get orientations from relative orientation measurements
-  VectorValues orientationsLago = computeLagoOrientations(pose2Graph, useOdometricPath);
+  VectorValues orientationsLago = computeOrientations(pose2Graph,
+      useOdometricPath);
 
   // Compute the full poses
-  return computeLagoPoses(pose2Graph, orientationsLago);
+  return computePoses(pose2Graph, orientationsLago);
 }
 
 /* ************************************************************************* */
-Values initializeLago(const NonlinearFactorGraph& graph, const Values& initialGuess) {
+Values initialize(const NonlinearFactorGraph& graph,
+    const Values& initialGuess) {
   Values initialGuessLago;
 
   // get the orientation estimates from LAGO
-  VectorValues orientations = initializeOrientationsLago(graph);
+  VectorValues orientations = initializeOrientations(graph);
 
   // for all nodes in the tree
-  for(VectorValues::const_iterator it = orientations.begin(); it != orientations.end(); ++it ){
+  BOOST_FOREACH(const VectorValues::value_type& it, orientations) {
-    Key key = it->first;
+    Key key = it.first;
-    if (key != keyAnchor){
+    if (key != keyAnchor) {
-      Pose2 pose = initialGuess.at<Pose2>(key);
+      const Pose2& pose = initialGuess.at<Pose2>(key);
-      Vector orientation = orientations.at(key);
+      const Vector& orientation = it.second;
-      Pose2 poseLago = Pose2(pose.x(),pose.y(),orientation(0));
+      Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0));
       initialGuessLago.insert(key, poseLago);
     }
   }
   return initialGuessLago;
 }
 
+} // end of namespace lago
 } // end of namespace gtsam

gtsam/slam/lago.h

@@ -0,0 +1,86 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation,
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ *  @file  lago.h
+ *  @brief Initialize Pose2 in a factor graph using LAGO
+ *  (Linear Approximation for Graph Optimization). see papers:
+ *
+ *  L. Carlone, R. Aragues, J. Castellanos, and B. Bona, A fast and accurate
+ *  approximation for planar pose graph optimization, IJRR, 2014.
+ *
+ *  L. Carlone, R. Aragues, J.A. Castellanos, and B. Bona, A linear approximation
+ *  for graph-based simultaneous localization and mapping, RSS, 2011.
+ *
+ *  @param graph: nonlinear factor graph (can include arbitrary factors but we assume
+ *  that there is a subgraph involving Pose2 and betweenFactors). Also in the current
+ *  version we assume that there is an odometric spanning path (x0->x1, x1->x2, etc)
+ *  and a prior on x0. This assumption can be relaxed by using the extra argument
+ *  useOdometricPath = false, although this part of code is not stable yet.
+ *  @return Values: initial guess from LAGO (only pose2 are initialized)
+ *
+ *  @author Luca Carlone
+ *  @author Frank Dellaert
+ *  @date   May 14, 2014
+ */
+
+#pragma once
+
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/linear/GaussianFactorGraph.h>
+#include <gtsam/linear/VectorValues.h>
+#include <gtsam/inference/graph.h>
+
+namespace gtsam {
+namespace lago {
+
+typedef std::map<Key, double> key2doubleMap;
+
+/**
+ * Compute the cumulative orientations (without wrapping)
+ * for all nodes wrt the root (root has zero orientation).
+ */
+GTSAM_EXPORT key2doubleMap computeThetasToRoot(
+    const key2doubleMap& deltaThetaMap, const PredecessorMap<Key>& tree);
+
+/**
+ * Given a factor graph "g", and a spanning tree "tree", select the nodes belonging
+ * to the tree and to g, and stores the factor slots corresponding to edges in the
+ * tree and to chordsIds wrt this tree.
+ * Also it computes deltaThetaMap which is a fast way to encode relative
+ * orientations along the tree: for a node key2, s.t. tree[key2]=key1,
+ * the value deltaThetaMap[key2] is relative orientation theta[key2]-theta[key1]
+ */
+GTSAM_EXPORT void getSymbolicGraph(
+/*OUTPUTS*/std::vector<size_t>& spanningTreeIds, std::vector<size_t>& chordsIds,
+    key2doubleMap& deltaThetaMap,
+    /*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g);
+
+/** Linear factor graph with regularized orientation measurements */
+GTSAM_EXPORT GaussianFactorGraph buildLinearOrientationGraph(
+    const std::vector<size_t>& spanningTreeIds,
+    const std::vector<size_t>& chordsIds, const NonlinearFactorGraph& g,
+    const key2doubleMap& orientationsToRoot, const PredecessorMap<Key>& tree);
+
+/** LAGO: Return the orientations of the Pose2 in a generic factor graph */
+GTSAM_EXPORT VectorValues initializeOrientations(
+    const NonlinearFactorGraph& graph, bool useOdometricPath = true);
+
+/** Return the values for the Pose2 in a generic factor graph */
+GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
+    bool useOdometricPath = true);
+
+/** Only correct the orientation part in initialGuess */
+GTSAM_EXPORT Values initialize(const NonlinearFactorGraph& graph,
+    const Values& initialGuess);
+
+} // end of namespace lago
+} // end of namespace gtsam

gtsam/slam/tests/testDataset.cpp

@@ -40,18 +40,21 @@ TEST(dataSet, findExampleDataFile) {
 }
 
 /* ************************************************************************* */
-//TEST( dataSet, load2D)
+TEST( dataSet, load2D)
-//{
+{
-//  ///< The structure where we will save the SfM data
+  ///< The structure where we will save the SfM data
-//  const string filename = findExampleDataFile("smallGraph.g2o");
+  const string filename = findExampleDataFile("w100.graph");
-//  boost::tie(graph,initialGuess) = load2D(filename, boost::none, 10000,
+  NonlinearFactorGraph::shared_ptr graph;
-//      false, false);
+  Values::shared_ptr initial;
-////  print
+  boost::tie(graph, initial) = load2D(filename);
-////
+  EXPECT_LONGS_EQUAL(300,graph->size());
-////  print
+  EXPECT_LONGS_EQUAL(100,initial->size());
-////
+  noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3);
-////  EXPECT(assert_equal(expected,actual,12));
+  BetweenFactor<Pose2> expected(1, 0, Pose2(-0.99879,0.0417574,-0.00818381), model);
-//}
+  BetweenFactor<Pose2>::shared_ptr actual = boost::dynamic_pointer_cast<
+      BetweenFactor<Pose2> >(graph->at(0));
+  EXPECT(assert_equal(expected, *actual));
+}
 
 /* ************************************************************************* */
 TEST( dataSet, Balbianello)
@@ -78,9 +81,9 @@ TEST( dataSet, Balbianello)
 TEST( dataSet, readG2o)
 {
   const string g2oFile = findExampleDataFile("pose2example");
-  NonlinearFactorGraph actualGraph;
+  NonlinearFactorGraph::shared_ptr actualGraph;
-  Values actualValues;
+  Values::shared_ptr actualValues;
-  readG2o(g2oFile, actualGraph, actualValues);
+  boost::tie(actualGraph, actualValues) = readG2o(g2oFile);
 
   Values expectedValues;
   expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000));
@@ -94,7 +97,7 @@ TEST( dataSet, readG2o)
   expectedValues.insert(8, Pose2(4.128877, 2.321481, -1.825391));
   expectedValues.insert(9, Pose2(3.884653, 1.327509, -1.953016));
   expectedValues.insert(10, Pose2(3.531067, 0.388263, -2.148934));
-  EXPECT(assert_equal(expectedValues,actualValues,1e-5));
+  EXPECT(assert_equal(expectedValues,*actualValues,1e-5));
 
   noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
   NonlinearFactorGraph expectedGraph;
@@ -110,16 +113,16 @@ TEST( dataSet, readG2o)
   expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
   expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
   expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
-  EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
+  EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
 }
 
 /* ************************************************************************* */
 TEST( dataSet, readG2oHuber)
 {
   const string g2oFile = findExampleDataFile("pose2example");
-  NonlinearFactorGraph actualGraph;
+  NonlinearFactorGraph::shared_ptr actualGraph;
-  Values actualValues;
+  Values::shared_ptr actualValues;
-  readG2o(g2oFile, actualGraph, actualValues, HUBER);
+  boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeHUBER);
 
   noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
   SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Huber::Create(1.345), baseModel);
@@ -137,16 +140,16 @@ TEST( dataSet, readG2oHuber)
   expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
   expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
   expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
-  EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
+  EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
 }
 
 /* ************************************************************************* */
 TEST( dataSet, readG2oTukey)
 {
   const string g2oFile = findExampleDataFile("pose2example");
-  NonlinearFactorGraph actualGraph;
+  NonlinearFactorGraph::shared_ptr actualGraph;
-  Values actualValues;
+  Values::shared_ptr actualValues;
-  readG2o(g2oFile, actualGraph, actualValues, TUKEY);
+  boost::tie(actualGraph, actualValues) = readG2o(g2oFile, KernelFunctionTypeTUKEY);
 
   noiseModel::Diagonal::shared_ptr baseModel = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
   SharedNoiseModel model = noiseModel::Robust::Create(noiseModel::mEstimator::Tukey::Create(4.6851), baseModel);
@@ -164,25 +167,25 @@ TEST( dataSet, readG2oTukey)
   expectedGraph.add(BetweenFactor<Pose2>(9,10, Pose2(1.003350, 0.022250, -0.195918), model));
   expectedGraph.add(BetweenFactor<Pose2>(5, 9, Pose2(0.033943, 0.032439, 3.073637), model));
   expectedGraph.add(BetweenFactor<Pose2>(3,10, Pose2(0.044020, 0.988477, -1.553511), model));
-  EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
+  EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5));
 }
 
 /* ************************************************************************* */
 TEST( dataSet, writeG2o)
 {
   const string g2oFile = findExampleDataFile("pose2example");
-  NonlinearFactorGraph expectedGraph;
+  NonlinearFactorGraph::shared_ptr expectedGraph;
-  Values expectedValues;
+  Values::shared_ptr expectedValues;
-  readG2o(g2oFile, expectedGraph, expectedValues);
+  boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile);
 
-  const string filenameToWrite = findExampleDataFile("pose2example-rewritten");
+  const string filenameToWrite = createRewrittenFileName(g2oFile);
-  writeG2o(filenameToWrite, expectedGraph, expectedValues);
+  writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
 
-  NonlinearFactorGraph actualGraph;
+  NonlinearFactorGraph::shared_ptr actualGraph;
-  Values actualValues;
+  Values::shared_ptr actualValues;
-  readG2o(filenameToWrite, actualGraph, actualValues);
+  boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite);
-  EXPECT(assert_equal(expectedValues,actualValues,1e-5));
+  EXPECT(assert_equal(*expectedValues,*actualValues,1e-5));
-  EXPECT(assert_equal(actualGraph,expectedGraph,1e-5));
+  EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-5));
 }
 
 /* ************************************************************************* */
@@ -249,7 +252,7 @@ TEST( dataSet, writeBAL_Dubrovnik)
   readBAL(filenameToRead, readData);
 
   // Write readData to file filenameToWrite
-  const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten");
+  const string filenameToWrite = createRewrittenFileName(filenameToRead);
   CHECK(writeBAL(filenameToWrite, readData));
 
   // Read what we wrote
@@ -311,7 +314,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
   }
 
   // Write values and readData to a file
-  const string filenameToWrite = findExampleDataFile("dubrovnik-3-7-pre-rewritten");
+  const string filenameToWrite = createRewrittenFileName(filenameToRead);
   writeBALfromValues(filenameToWrite, readData, value);
 
   // Read the file we wrote

gtsam/slam/tests/testLago.cpp

@@ -19,27 +19,21 @@
  *  @date   May 14, 2014
  */
 
-#include <gtsam/geometry/Pose2.h>
+#include <gtsam/slam/lago.h>
-
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/BetweenFactor.h>
+#include <gtsam/slam/PriorFactor.h>
 #include <gtsam/inference/Symbol.h>
 
-#include <gtsam/slam/dataset.h>
-#include <gtsam/slam/PriorFactor.h>
-#include <gtsam/slam/BetweenFactor.h>
-
-#include <gtsam/nonlinear/NonlinearFactorGraph.h>
-#include <gtsam/nonlinear/LagoInitializer.h>
-
-#include <gtsam/base/TestableAssertions.h>
 #include <CppUnitLite/TestHarness.h>
-#include <boost/math/constants/constants.hpp>
+
 #include <cmath>
 
 using namespace std;
 using namespace gtsam;
 using namespace boost::assign;
 
-Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3);
+static Symbol x0('x', 0), x1('x', 1), x2('x', 2), x3('x', 3);
 static SharedNoiseModel model(noiseModel::Isotropic::Sigma(3, 0.1));
 
 namespace simple {
@@ -54,10 +48,10 @@ namespace simple {
 //               x0
 //
 
-Pose2 pose0 = Pose2(0.000000, 0.000000, 0.000000);
+static Pose2 pose0 = Pose2(0.000000, 0.000000, 0.000000);
-Pose2 pose1 = Pose2(1.000000, 1.000000, 1.570796);
+static Pose2 pose1 = Pose2(1.000000, 1.000000, 1.570796);
-Pose2 pose2 = Pose2(0.000000, 2.000000, 3.141593);
+static Pose2 pose2 = Pose2(0.000000, 2.000000, 3.141593);
-Pose2 pose3 = Pose2(-1.000000, 1.000000, 4.712389);
+static Pose2 pose3 = Pose2(-1.000000, 1.000000, 4.712389);
 
 NonlinearFactorGraph graph() {
   NonlinearFactorGraph g;
@@ -77,10 +71,10 @@ TEST( Lago, checkSTandChords ) {
   PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
       BetweenFactor<Pose2> >(g);
 
-  key2doubleMap deltaThetaMap;
+  lago::key2doubleMap deltaThetaMap;
   vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
   vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
-  getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
+  lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
 
   DOUBLES_EQUAL(spanningTreeIds[0], 0, 1e-6); // factor 0 is the first in the ST (0->1)
   DOUBLES_EQUAL(spanningTreeIds[1], 3, 1e-6); // factor 3 is the second in the ST(2->0)
@@ -100,19 +94,19 @@ TEST( Lago, orientationsOverSpanningTree ) {
   EXPECT_LONGS_EQUAL(tree[x2], x0);
   EXPECT_LONGS_EQUAL(tree[x3], x0);
 
-  key2doubleMap expected;
+  lago::key2doubleMap expected;
   expected[x0]=  0;
   expected[x1]=  M_PI/2; // edge x0->x1 (consistent with edge (x0,x1))
   expected[x2]= -M_PI; // edge x0->x2 (traversed backwards wrt edge (x2,x0))
   expected[x3]= -M_PI/2;  // edge x0->x3 (consistent with edge (x0,x3))
 
-  key2doubleMap deltaThetaMap;
+  lago::key2doubleMap deltaThetaMap;
   vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
   vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
-  getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
+  lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
 
-  key2doubleMap actual;
+  lago::key2doubleMap actual;
-  actual = computeThetasToRoot(deltaThetaMap, tree);
+  actual = lago::computeThetasToRoot(deltaThetaMap, tree);
   DOUBLES_EQUAL(expected[x0], actual[x0], 1e-6);
   DOUBLES_EQUAL(expected[x1], actual[x1], 1e-6);
   DOUBLES_EQUAL(expected[x2], actual[x2], 1e-6);
@@ -125,14 +119,14 @@ TEST( Lago, regularizedMeasurements ) {
   PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
       BetweenFactor<Pose2> >(g);
 
-  key2doubleMap deltaThetaMap;
+  lago::key2doubleMap deltaThetaMap;
   vector<size_t> spanningTreeIds; // ids of between factors forming the spanning tree T
   vector<size_t> chordsIds; // ids of between factors corresponding to chordsIds wrt T
-  getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
+  lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
 
-  key2doubleMap orientationsToRoot = computeThetasToRoot(deltaThetaMap, tree);
+  lago::key2doubleMap orientationsToRoot = lago::computeThetasToRoot(deltaThetaMap, tree);
 
-  GaussianFactorGraph lagoGraph = buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree);
+  GaussianFactorGraph lagoGraph = lago::buildLinearOrientationGraph(spanningTreeIds, chordsIds, g, orientationsToRoot, tree);
   std::pair<Matrix,Vector> actualAb = lagoGraph.jacobian();
   // jacobian corresponding to the orientation measurements (last entry is the prior on the anchor and is disregarded)
   Vector actual = (Vector(5) <<  actualAb.second(0),actualAb.second(1),actualAb.second(2),actualAb.second(3),actualAb.second(4));
@@ -147,25 +141,25 @@ TEST( Lago, regularizedMeasurements ) {
 /* *************************************************************************** */
 TEST( Lago, smallGraphVectorValues ) {
   bool useOdometricPath = false;
-  VectorValues initialGuessLago = initializeOrientationsLago(simple::graph(), useOdometricPath);
+  VectorValues initial = lago::initializeOrientations(simple::graph(), useOdometricPath);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
-  EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
-  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
-  EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6));
+  EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6));
-  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
 }
 
 /* *************************************************************************** */
 TEST( Lago, smallGraphVectorValuesSP ) {
 
-  VectorValues initialGuessLago = initializeOrientationsLago(simple::graph());
+  VectorValues initial = lago::initializeOrientations(simple::graph());
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
-  EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
-  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
-  EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6));
+  EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
-  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
 }
 
 /* *************************************************************************** */
@@ -173,26 +167,26 @@ TEST( Lago, multiplePosePriors ) {
   bool useOdometricPath = false;
   NonlinearFactorGraph g = simple::graph();
   g.add(PriorFactor<Pose2>(x1, simple::pose1, model));
-  VectorValues initialGuessLago = initializeOrientationsLago(g, useOdometricPath);
+  VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
-  EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
-  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
-  EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6));
+  EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6));
-  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
 }
 
 /* *************************************************************************** */
 TEST( Lago, multiplePosePriorsSP ) {
   NonlinearFactorGraph g = simple::graph();
   g.add(PriorFactor<Pose2>(x1, simple::pose1, model));
-  VectorValues initialGuessLago = initializeOrientationsLago(g);
+  VectorValues initial = lago::initializeOrientations(g);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
-  EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
-  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
-  EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6));
+  EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
-  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
 }
 
 /* *************************************************************************** */
@@ -200,26 +194,26 @@ TEST( Lago, multiplePoseAndRotPriors ) {
   bool useOdometricPath = false;
   NonlinearFactorGraph g = simple::graph();
   g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model));
-  VectorValues initialGuessLago = initializeOrientationsLago(g, useOdometricPath);
+  VectorValues initial = lago::initializeOrientations(g, useOdometricPath);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
-  EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
-  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
-  EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initialGuessLago.at(x2), 1e-6));
+  EXPECT(assert_equal((Vector(1) << M_PI - 2*M_PI), initial.at(x2), 1e-6));
-  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initialGuessLago.at(x3), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
 }
 
 /* *************************************************************************** */
 TEST( Lago, multiplePoseAndRotPriorsSP ) {
   NonlinearFactorGraph g = simple::graph();
   g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model));
-  VectorValues initialGuessLago = initializeOrientationsLago(g);
+  VectorValues initial = lago::initializeOrientations(g);
 
   // comparison is up to M_PI, that's why we add some multiples of 2*M_PI
-  EXPECT(assert_equal((Vector(1) << 0.0), initialGuessLago.at(x0), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 0.0), initial.at(x0), 1e-6));
-  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initialGuessLago.at(x1), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
-  EXPECT(assert_equal((Vector(1) << M_PI ), initialGuessLago.at(x2), 1e-6));
+  EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
-  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initialGuessLago.at(x3), 1e-6));
+  EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
 }
 
 /* *************************************************************************** */
@@ -233,7 +227,7 @@ TEST( Lago, smallGraphValues ) {
   initialGuess.insert(x3,Pose2(simple::pose3.x(),simple::pose3.y(),0.0));
 
   // lago does not touch the Cartesian part and only fixed the orientations
-  Values actual = initializeLago(simple::graph(), initialGuess);
+  Values actual = lago::initialize(simple::graph(), initialGuess);
 
   // we are in a noiseless case
   Values expected;
@@ -249,7 +243,7 @@ TEST( Lago, smallGraphValues ) {
 TEST( Lago, smallGraph2 ) {
 
   // lago does not touch the Cartesian part and only fixed the orientations
-  Values actual = initializeLago(simple::graph());
+  Values actual = lago::initialize(simple::graph());
 
   // we are in a noiseless case
   Values expected;
@@ -264,17 +258,17 @@ TEST( Lago, smallGraph2 ) {
 /* *************************************************************************** */
 TEST( Lago, largeGraphNoisy_orientations ) {
 
-  NonlinearFactorGraph g;
-  Values initial;
   string inputFile = findExampleDataFile("noisyToyGraph");
-  readG2o(inputFile, g, initial);
+  NonlinearFactorGraph::shared_ptr g;
+  Values::shared_ptr initial;
+  boost::tie(g, initial) = readG2o(inputFile);
 
   // Add prior on the pose having index (key) = 0
-  NonlinearFactorGraph graphWithPrior = g;
+  NonlinearFactorGraph graphWithPrior = *g;
   noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
 
-  VectorValues actualVV = initializeOrientationsLago(graphWithPrior);
+  VectorValues actualVV = lago::initializeOrientations(graphWithPrior);
   Values actual;
   Key keyAnc = symbol('Z',9999999);
   for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){
@@ -285,40 +279,40 @@ TEST( Lago, largeGraphNoisy_orientations ) {
       actual.insert(key, poseLago);
     }
   }
-  NonlinearFactorGraph gmatlab;
-  Values expected;
   string matlabFile = findExampleDataFile("orientationsNoisyToyGraph");
-  readG2o(matlabFile, gmatlab, expected);
+  NonlinearFactorGraph::shared_ptr gmatlab;
+  Values::shared_ptr expected;
+  boost::tie(gmatlab, expected) = readG2o(matlabFile);
 
-  BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){
+  BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){
     Key k = key_val.key;
-    EXPECT(assert_equal(expected.at<Pose2>(k), actual.at<Pose2>(k), 1e-5));
+    EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-5));
   }
 }
 
 /* *************************************************************************** */
 TEST( Lago, largeGraphNoisy ) {
 
-  NonlinearFactorGraph g;
-  Values initial;
   string inputFile = findExampleDataFile("noisyToyGraph");
-  readG2o(inputFile, g, initial);
+  NonlinearFactorGraph::shared_ptr g;
+  Values::shared_ptr initial;
+  boost::tie(g, initial) = readG2o(inputFile);
 
   // Add prior on the pose having index (key) = 0
-  NonlinearFactorGraph graphWithPrior = g;
+  NonlinearFactorGraph graphWithPrior = *g;
   noiseModel::Diagonal::shared_ptr priorModel = noiseModel::Diagonal::Variances((Vector(3) << 1e-2, 1e-2, 1e-4));
   graphWithPrior.add(PriorFactor<Pose2>(0, Pose2(), priorModel));
 
-  Values actual = initializeLago(graphWithPrior);
+  Values actual = lago::initialize(graphWithPrior);
 
-  NonlinearFactorGraph gmatlab;
-  Values expected;
   string matlabFile = findExampleDataFile("optimizedNoisyToyGraph");
-  readG2o(matlabFile, gmatlab, expected);
+  NonlinearFactorGraph::shared_ptr gmatlab;
+  Values::shared_ptr expected;
+  boost::tie(gmatlab, expected) = readG2o(matlabFile);
 
-  BOOST_FOREACH(const Values::KeyValuePair& key_val, expected){
+  BOOST_FOREACH(const Values::KeyValuePair& key_val, *expected){
     Key k = key_val.key;
-    EXPECT(assert_equal(expected.at<Pose2>(k), actual.at<Pose2>(k), 1e-2));
+    EXPECT(assert_equal(expected->at<Pose2>(k), actual.at<Pose2>(k), 1e-2));
   }
 }
 

gtsam/slam/tests/testSmartProjectionPoseFactor.cpp

@@ -52,9 +52,9 @@ using symbol_shorthand::X;
 using symbol_shorthand::L;
 
 // tests data
-Symbol x1('X',  1);
+static Symbol x1('X',  1);
-Symbol x2('X',  2);
+static Symbol x2('X',  2);
-Symbol x3('X',  3);
+static Symbol x3('X',  3);
 
 static Key poseKey1(x1);
 static Point2 measurement1(323.0, 240.0);

gtsam/slam/tests/timeLago.cpp

@@ -0,0 +1,82 @@
+/* ----------------------------------------------------------------------------
+
+ * 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    timeVirtual.cpp
+ * @brief   Time the overhead of using virtual destructors and methods
+ * @author  Richard Roberts
+ * @date    Dec 3, 2010
+ */
+
+#include <gtsam/slam/dataset.h>
+#include <gtsam/slam/PriorFactor.h>
+#include <gtsam/slam/lago.h>
+#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
+#include <gtsam/linear/Sampler.h>
+#include <gtsam/base/timing.h>
+
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+int main(int argc, char *argv[]) {
+
+  size_t trials = 1;
+
+  // read graph
+  Values::shared_ptr solution;
+  NonlinearFactorGraph::shared_ptr g;
+  string inputFile = findExampleDataFile("w10000");
+  SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(3) << 0.05, 0.05, 5.0 * M_PI / 180.0));
+  boost::tie(g, solution) = load2D(inputFile, model);
+
+  // add noise to create initial estimate
+  Values initial;
+  Sampler sampler(42u);
+  Values::ConstFiltered<Pose2> poses = solution->filter<Pose2>();
+  SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5, 0.5, 15.0 * M_PI / 180.0));
+  BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& it, poses)
+    initial.insert(it.key, it.value.retract(sampler.sampleNewModel(noise)));
+
+  // Add prior on the pose having index (key) = 0
+  noiseModel::Diagonal::shared_ptr priorModel = //
+      noiseModel::Diagonal::Sigmas(Vector3(1e-6, 1e-6, 1e-8));
+  g->add(PriorFactor<Pose2>(0, Pose2(), priorModel));
+
+  // LAGO
+  for (size_t i = 0; i < trials; i++) {
+    {
+      gttic_(lago);
+
+      gttic_(init);
+      Values lagoInitial = lago::initialize(*g);
+      gttoc_(init);
+
+      gttic_(refine);
+      GaussNewtonOptimizer optimizer(*g, lagoInitial);
+      Values result = optimizer.optimize();
+      gttoc_(refine);
+    }
+
+    {
+      gttic_(optimize);
+      GaussNewtonOptimizer optimizer(*g, initial);
+      Values result = optimizer.optimize();
+    }
+
+    tictoc_finishedIteration_();
+  }
+
+  tictoc_print_();
+
+  return 0;
+}

gtsam_unstable/examples/SmartProjectionFactorExample.cpp

@@ -0,0 +1,121 @@
+/* ----------------------------------------------------------------------------
+
+* GTSAM Copyright 2010, Georgia Tech Research Corporation,
+* Atlanta, Georgia 30332-0415
+* All Rights Reserved
+* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+* See LICENSE for the license information
+
+* -------------------------------------------------------------------------- */
+
+/**
+* @file SmartProjectionFactorExample.cpp
+* @brief A stereo visual odometry example
+* @date May 30, 2014
+* @author Stephen Camp
+* @author Chris Beall
+*/
+
+
+/**
+ * A smart projection factor example based on stereo data, throwing away the
+ * measurement from the right camera
+ *  -robot starts at origin
+ *  -moves forward, taking periodic stereo measurements
+ *  -makes monocular observations of many landmarks
+ */
+
+#include <gtsam/geometry/Pose3.h>
+#include <gtsam/geometry/Cal3_S2Stereo.h>
+#include <gtsam/nonlinear/Values.h>
+#include <gtsam/nonlinear/NonlinearEquality.h>
+#include <gtsam/nonlinear/NonlinearFactorGraph.h>
+#include <gtsam/nonlinear/LevenbergMarquardtOptimizer.h>
+#include <gtsam/inference/Symbol.h>
+#include <gtsam/slam/dataset.h>
+
+#include <gtsam/slam/SmartProjectionPoseFactor.h>
+
+#include <string>
+#include <fstream>
+#include <iostream>
+
+using namespace std;
+using namespace gtsam;
+
+int main(int argc, char** argv){
+
+  typedef SmartProjectionPoseFactor<Pose3, Point3, Cal3_S2> SmartFactor;
+
+  Values initial_estimate;
+  NonlinearFactorGraph graph;
+  const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,1);
+
+  string calibration_loc = findExampleDataFile("VO_calibration.txt");
+  string pose_loc = findExampleDataFile("VO_camera_poses_large.txt");
+  string factor_loc = findExampleDataFile("VO_stereo_factors_large.txt");
+  
+  //read camera calibration info from file
+  // focal lengths fx, fy, skew s, principal point u0, v0, baseline b
+  cout << "Reading calibration info" << endl;
+  ifstream calibration_file(calibration_loc.c_str());
+
+  double fx, fy, s, u0, v0, b;
+  calibration_file >> fx >> fy >> s >> u0 >> v0 >> b;
+  const Cal3_S2::shared_ptr K(new Cal3_S2(fx, fy, s, u0, v0));
+
+  cout << "Reading camera poses" << endl;
+  ifstream pose_file(pose_loc.c_str());
+
+  int pose_id;
+  MatrixRowMajor m(4,4);
+  //read camera pose parameters and use to make initial estimates of camera poses
+  while (pose_file >> pose_id) {
+    for (int i = 0; i < 16; i++) {
+      pose_file >> m.data()[i];
+    }
+    initial_estimate.insert(Symbol('x', pose_id), Pose3(m));
+  }
+  
+  // camera and landmark keys
+  size_t x, l;
+
+  // pixel coordinates uL, uR, v (same for left/right images due to rectification)
+  // landmark coordinates X, Y, Z in camera frame, resulting from triangulation
+  double uL, uR, v, X, Y, Z;
+  ifstream factor_file(factor_loc.c_str());
+  cout << "Reading stereo factors" << endl;
+
+  //read stereo measurements and construct smart factors
+
+  SmartFactor::shared_ptr factor(new SmartFactor());
+  size_t current_l = 3;   // hardcoded landmark ID from first measurement
+
+  while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
+
+    if(current_l != l) {
+      graph.push_back(factor);
+      factor = SmartFactor::shared_ptr(new SmartFactor());
+      current_l = l;
+    }
+    factor->add(Point2(uL,v), Symbol('x',x), model, K);
+  }
+
+  Pose3 first_pose = initial_estimate.at<Pose3>(Symbol('x',1));
+  //constrain the first pose such that it cannot change from its original value during optimization
+  // NOTE: NonlinearEquality forces the optimizer to use QR rather than Cholesky
+  // QR is much slower than Cholesky, but numerically more stable
+  graph.push_back(NonlinearEquality<Pose3>(Symbol('x',1),first_pose));
+
+  cout << "Optimizing" << endl;
+  //create Levenberg-Marquardt optimizer to optimize the factor graph
+  LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
+  Values result = optimizer.optimize();
+
+  cout << "Final result sample:" << endl;
+  Values pose_values = result.filter<Pose3>();
+  pose_values.print("Final camera poses:\n");
+
+  return 0;
+}

wrap/FileWriter.cpp

@@ -28,7 +28,7 @@ void FileWriter::emit(bool add_header, bool force_overwrite) const {
   bool file_exists = true;
   try {
     existing_contents = file_contents(filename_.c_str(), add_header);
-  } catch (CantOpenFile& e) {
+  } catch (CantOpenFile) {
     file_exists = false;
   }