Merged gtborg/gtsam into develop

2014-06-03 10:04:19 -04:00 · 2014-06-03 10:04:19 -04:00 · 917c9c46c8
parent a5c938f8ca e60f21a22f
commit 917c9c46c8
44 changed files with 1512 additions and 1147 deletions
--- a/.cproject
+++ b/.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>
--- a/.gitignore
+++ b/.gitignore
@ -2,3 +2,5 @@
 /doc*
 *.pyc
 *.DS_Store
 /examples/Data/dubrovnik-3-7-pre-rewritten.txt
 /examples/Data/pose2example-rewritten.txt
--- a/CMakeLists.txt
+++ b/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()
--- a/examples/Data/dubrovnik-3-7-pre-rewritten.txt
+++ b/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
--- a/examples/Data/pose2example-rewritten.txt
+++ b/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
--- a/examples/Pose2SLAMExample_g2o.cpp
+++ b/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;
-  const string outputFile = argv[2];
+  if (argc < 3) {
-  std::cout << "Writing results to file: " << outputFile << std::endl;
+    result.print("result");
-  writeG2o(outputFile, graph, result);
+  } else {
-  std::cout << "done! " << std::endl;
+    const string outputFile = argv[2];
-
+    std::cout << "Writing results to file: " << outputFile << std::endl;
    writeG2o(*graph, result, outputFile);
    std::cout << "done! " << std::endl;
  }
  return 0;
 }
--- a/examples/Pose2SLAMExample_lago.cpp
+++ b/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;
-  const string outputFile = argv[2];
+  if (argc < 3) {
-  std::cout << "Writing results to file: " << outputFile << std::endl;
+    estimateLago.print("estimateLago");
-  writeG2o(outputFile, graph, estimateLago);
+  } else {
-  std::cout << "done! " << std::endl;
+    const string outputFile = argv[2];
    std::cout << "Writing results to file: " << outputFile << std::endl;
    writeG2o(*graph, estimateLago, outputFile);
    std::cout << "done! " << std::endl;
  }
  return 0;
 }
--- a/examples/SFMExample_SmartFactor.cpp
+++ b/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;
 }
 /* ************************************************************************* */
--- a/examples/StereoVOExample_large.cpp
+++ b/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(
+    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));
+            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 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));
+      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
+      //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();
--- a/gtsam.h
+++ b/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
--- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd.c
+++ b/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
--- a/gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c
+++ b/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
--- a/gtsam_unstable/geometry/TriangulationFactor.h
+++ b/gtsam_unstable/geometry/TriangulationFactor.h
--- a/gtsam_unstable/geometry/tests/testTriangulation.cpp
+++ b/gtsam_unstable/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>
--- a/gtsam/geometry/tests/testSphere2.cpp
+++ b/gtsam/geometry/tests/testSphere2.cpp
--- a/gtsam_unstable/geometry/triangulation.cpp
+++ b/gtsam_unstable/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>
--- a/gtsam_unstable/geometry/triangulation.h
+++ b/gtsam_unstable/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);
 /**
--- a/gtsam/inference/VariableIndex-inl.h
+++ b/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);
  }
 }
--- a/gtsam/linear/GaussianFactorGraph.cpp
+++ b/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;
    }
--- a/gtsam/linear/NoiseModel.cpp
+++ b/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)));
 }
 /* ************************************************************************* */
--- a/gtsam/linear/NoiseModel.h
+++ b/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.
--- a/gtsam/linear/tests/testNoiseModel.cpp
+++ b/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 )
 {
--- a/gtsam/navigation/MagFactor.h
+++ b/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;
  }
--- a/gtsam/nonlinear/LagoInitializer.h
+++ b/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
--- a/gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
+++ b/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;
    }
--- a/gtsam_unstable/slam/ImplicitSchurFactor.h
+++ b/gtsam_unstable/slam/ImplicitSchurFactor.h
--- a/gtsam_unstable/slam/JacobianFactorQ.h
+++ b/gtsam_unstable/slam/JacobianFactorQ.h
--- a/gtsam_unstable/slam/JacobianFactorQR.h
+++ b/gtsam_unstable/slam/JacobianFactorQR.h
@ -6,7 +6,7 @@
 */
 #pragma once
-#include <gtsam_unstable/slam/JacobianSchurFactor.h>
+#include <gtsam/slam/JacobianSchurFactor.h>
 namespace gtsam {
 /**
--- a/gtsam_unstable/slam/JacobianFactorSVD.h
+++ b/gtsam_unstable/slam/JacobianFactorSVD.h
@ -5,7 +5,7 @@
 */
 #pragma once
-#include "gtsam_unstable/slam/JacobianSchurFactor.h"
+#include "gtsam/slam/JacobianSchurFactor.h"
 namespace gtsam {
 /**
--- a/gtsam_unstable/slam/JacobianSchurFactor.h
+++ b/gtsam_unstable/slam/JacobianSchurFactor.h
--- a/gtsam_unstable/slam/RegularHessianFactor.h
+++ b/gtsam_unstable/slam/RegularHessianFactor.h
--- a/gtsam_unstable/slam/SmartFactorBase.h
+++ b/gtsam_unstable/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;
--- a/gtsam_unstable/slam/SmartProjectionFactor.h
+++ b/gtsam_unstable/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;
--- a/gtsam_unstable/slam/SmartProjectionPoseFactor.h
+++ b/gtsam_unstable/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
-   * 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_;
  }
--- a/gtsam/slam/dataset.cpp
+++ b/gtsam/slam/dataset.cpp
--- a/gtsam/slam/dataset.h
+++ b/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_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
--- a/gtsam/nonlinear/LagoInitializer.cpp
+++ b/gtsam/nonlinear/LagoInitializer.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(
-          pose2Prior->prior(), pose2Prior->get_noiseModel()));
+          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
--- a/gtsam/slam/lago.h
+++ b/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
--- a/gtsam/slam/tests/testDataset.cpp
+++ b/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
--- a/gtsam/nonlinear/tests/testLagoInitializer.cpp
+++ b/gtsam/nonlinear/tests/testLagoInitializer.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));
  }
 }
--- a/gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp
+++ b/gtsam_unstable/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);
--- a/gtsam/slam/tests/timeLago.cpp
+++ b/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;
 }
--- a/gtsam_unstable/examples/SmartProjectionFactorExample.cpp
+++ b/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;
 }
--- a/wrap/FileWriter.cpp
+++ b/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;
  }