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

Merge branch 'release/3.1.0'

release/4.3a0
cbeall3 2015-03-07 00:06:17 -05:00
parent ff95ea3f6a 280c7e26e9
commit 0ed343723f
107 changed files with 4712 additions and 2094 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

440
.cproject
View File

@ -732,46 +732,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>
@ -2015,6 +1975,134 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCal3Bundler.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testCal3Bundler.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCal3DS2.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testCal3DS2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCalibratedCamera.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEssentialMatrix.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testEssentialMatrix.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testHomography2.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j1 VERBOSE=1</buildArguments>
<buildTarget>testHomography2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPinholeCamera.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPinholeCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoint2.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPoint2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoint3.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPoint3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose3.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot3M.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testRot3M.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSphere2.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSphere2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testStereoCamera.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testStereoCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeCalibratedCamera.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timePinholeCamera.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timePinholeCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeStereoCamera.run" path="build/gtsam/geometry/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeStereoCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2095,70 +2183,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>
@ -2191,6 +2215,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testWrap.run" path="build/wrap/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testWrap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDiscreteFactor.run" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2263,6 +2295,22 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testMatrix.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testMatrix.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVector.run" path="build/gtsam/base/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -2535,146 +2583,66 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testAntiFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testStereoCamera.run</buildTarget>
<buildTarget>testAntiFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testRot3M.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testBetweenFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testRot3M.run</buildTarget>
<buildTarget>testBetweenFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoint3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testDataset.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPoint3.run</buildTarget>
<buildTarget>testDataset.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testEssentialMatrixFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testCalibratedCamera.run</buildTarget>
<buildTarget>testEssentialMatrixFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testGeneralSFMFactor_Cal3Bundler.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeStereoCamera.run</buildTarget>
<buildTarget>testGeneralSFMFactor_Cal3Bundler.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testHomography2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j1 VERBOSE=1</buildArguments>
<buildTarget>testHomography2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPoint2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testGeneralSFMFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPoint2.run</buildTarget>
<buildTarget>testGeneralSFMFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testProjectionFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose2.run</buildTarget>
<buildTarget>testProjectionFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testRotateFactor.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPose3.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timeCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timeCalibratedCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testPinholeCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPinholeCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="timePinholeCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>timePinholeCamera.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCal3DS2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testCal3DS2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testCal3Bundler.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testCal3Bundler.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSphere2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSphere2.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEssentialMatrix.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testEssentialMatrix.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVector.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVector.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testMatrix.run" path="build/gtsam/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testMatrix.run</buildTarget>
<buildTarget>testRotateFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
@ -2831,6 +2799,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>
@ -2958,14 +2990,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testWrap.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>

2
.gitignore vendored
View File

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

11
CMakeLists.txt
View File

@ -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}")
@ -91,10 +91,10 @@ set(CPACK_GENERATOR "TGZ" CACHE STRING "CPack Default Binary Generator")
# If using Boost shared libs, disable auto linking
if(MSVC)
# Some libraries, at least Boost Program Options, rely on this to export DLL symbols
add_definitions(-DBOOST_ALL_DYN_LINK)
# Disable autolinking
if(NOT Boost_USE_STATIC_LIBS)
add_definitions(-DBOOST_ALL_NO_LIB)
add_definitions(-DBOOST_ALL_DYN_LINK)
endif()
endif()
@ -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()

2
cmake/GtsamBuildTypes.cmake
View File

@ -56,7 +56,7 @@ endif()
# Clang on Mac uses a template depth that is less than standard and is too small
if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0")
add_definitions(-ftemplate-depth=1024)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -ftemplate-depth=1024")
endif()
endif()

95
cmake/GtsamMatlabWrap.cmake
View File

@ -93,7 +93,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
# Paths for generated files
set(generated_files_path "${PROJECT_BINARY_DIR}/wrap/${moduleName}")
set(generated_cpp_file "${PROJECT_BINARY_DIR}/wrap/${moduleName}/${moduleName}_wrapper.cpp")
set(generated_cpp_file "${generated_files_path}/${moduleName}_wrapper.cpp")
set(compiled_mex_modules_root "${PROJECT_BINARY_DIR}/wrap/${moduleName}_mex")
message(STATUS "Building wrap module ${moduleName}")
@ -109,23 +109,86 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
set(matlab_h_path "${installed_includes_path}/wrap")
endif()
# Add -shared or -static suffix to targets
# If building a static mex module, add all cmake-linked libraries to the
# explicit link libraries list so that the next block of code can unpack
# any static libraries
set(automaticDependencies "")
foreach(lib ${moduleName} ${linkLibraries})
#message("MODULE NAME: ${moduleName}")
if(TARGET "${lib}")
get_target_property(dependentLibraries ${lib} INTERFACE_LINK_LIBRARIES)
# message("DEPENDENT LIBRARIES: ${dependentLibraries}")
if(dependentLibraries)
list(APPEND automaticDependencies ${dependentLibraries})
endif()
endif()
endforeach()
## CHRIS: Temporary fix. On my system the get_target_property above returned Not-found for gtsam module
## This needs to be fixed!!
if(UNIX AND NOT APPLE)
list(APPEND automaticDependencies ${Boost_SERIALIZATION_LIBRARY_RELEASE} ${Boost_FILESYSTEM_LIBRARY_RELEASE}
${Boost_SYSTEM_LIBRARY_RELEASE} ${Boost_THREAD_LIBRARY_RELEASE} ${Boost_DATE_TIME_LIBRARY_RELEASE}
${Boost_REGEX_LIBRARY_RELEASE})
if(Boost_TIMER_LIBRARY_RELEASE AND NOT GTSAM_DISABLE_NEW_TIMERS) # Only present in Boost >= 1.48.0
list(APPEND automaticDependencies ${Boost_TIMER_LIBRARY_RELEASE} ${Boost_CHRONO_LIBRARY_RELEASE})
if(GTSAM_MEX_BUILD_STATIC_MODULE)
#list(APPEND automaticDependencies -Wl,--no-as-needed -lrt)
endif()
endif()
endif()
#message("AUTOMATIC DEPENDENCIES: ${automaticDependencies}")
## CHRIS: End temporary fix
# Separate dependencies
set(correctedOtherLibraries "")
set(otherLibraryTargets "")
set(otherLibraryNontargets "")
foreach(lib ${moduleName} ${linkLibraries})
if(TARGET ${lib})
list(APPEND correctedOtherLibraries ${lib})
list(APPEND otherLibraryTargets ${lib})
elseif(TARGET ${lib}-shared) # Prefer the shared library if we have both shared and static)
list(APPEND correctedOtherLibraries ${lib}-shared)
list(APPEND otherLibraryTargets ${lib}-shared)
elseif(TARGET ${lib}-static)
list(APPEND correctedOtherLibraries ${lib}-static)
list(APPEND otherLibraryTargets ${lib}-static)
set(otherSourcesAndObjects "")
foreach(lib ${moduleName} ${linkLibraries} ${automaticDependencies})
if(TARGET "${lib}")
if(GTSAM_MEX_BUILD_STATIC_MODULE)
get_target_property(target_sources ${lib} SOURCES)
list(APPEND otherSourcesAndObjects ${target_sources})
else()
list(APPEND correctedOtherLibraries ${lib})
list(APPEND otherLibraryTargets ${lib})
endif()
else()
list(APPEND correctedOtherLibraries ${lib})
list(APPEND otherLibraryNontargets ${lib})
get_filename_component(file_extension "${lib}" EXT)
get_filename_component(lib_name "${lib}" NAME_WE)
if(file_extension STREQUAL ".a" AND GTSAM_MEX_BUILD_STATIC_MODULE)
# For building a static MEX module, unpack the static library
# and compile its object files into our module
file(MAKE_DIRECTORY "${generated_files_path}/${lib_name}_objects")
execute_process(COMMAND ar -x "${lib}"
WORKING_DIRECTORY "${generated_files_path}/${lib_name}_objects"
RESULT_VARIABLE ar_result)
if(NOT ar_result EQUAL 0)
message(FATAL_ERROR "Failed extracting ${lib}")
endif()
# Get list of object files
execute_process(COMMAND ar -t "${lib}"
OUTPUT_VARIABLE object_files
RESULT_VARIABLE ar_result)
if(NOT ar_result EQUAL 0)
message(FATAL_ERROR "Failed listing ${lib}")
endif()
# Add directory to object files
string(REPLACE "\n" ";" object_files_list "${object_files}")
foreach(object_file ${object_files_list})
get_filename_component(file_extension "${object_file}" EXT)
if(file_extension STREQUAL ".o")
list(APPEND otherSourcesAndObjects "${generated_files_path}/${lib_name}_objects/${object_file}")
endif()
endforeach()
else()
list(APPEND correctedOtherLibraries ${lib})
list(APPEND otherLibraryNontargets ${lib})
endif()
endif()
endforeach()
@ -144,7 +207,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
file(MAKE_DIRECTORY "${generated_files_path}")
add_custom_command(
OUTPUT ${generated_cpp_file}
DEPENDS ${interfaceHeader} wrap ${module_library_target} ${otherLibraryTargets}
DEPENDS ${interfaceHeader} wrap ${module_library_target} ${otherLibraryTargets} ${otherSourcesAndObjects}
COMMAND
wrap
${modulePath}
@ -157,7 +220,7 @@ function(wrap_library_internal interfaceHeader linkLibraries extraIncludeDirs ex
# Set up building of mex module
string(REPLACE ";" " " extraMexFlagsSpaced "${extraMexFlags}")
string(REPLACE ";" " " mexFlagsSpaced "${GTSAM_BUILD_MEX_BINARY_FLAGS}")
add_library(${moduleName}_wrapper MODULE ${generated_cpp_file} ${interfaceHeader})
add_library(${moduleName}_wrapper MODULE ${generated_cpp_file} ${interfaceHeader} ${otherSourcesAndObjects})
target_link_libraries(${moduleName}_wrapper ${correctedOtherLibraries})
set_target_properties(${moduleName}_wrapper PROPERTIES
OUTPUT_NAME "${moduleName}_wrapper"

80
examples/Data/dubrovnik-3-7-pre-rewritten.txt
View File

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

9
examples/Data/noisyToyGraph.txt Normal file
View File

@ -0,0 +1,9 @@
VERTEX_SE2 0 0.000000 0.000000 0.000000
VERTEX_SE2 1 0.774115 1.183389 1.576173
VERTEX_SE2 2 -0.262420 2.047059 -3.127594
VERTEX_SE2 3 -1.605649 0.993891 -1.561134
EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000

9
examples/Data/optimizedNoisyToyGraph.txt Normal file
View File

@ -0,0 +1,9 @@
VERTEX_SE2 0 0.000000 -0.000000 0.000000
VERTEX_SE2 1 0.955797 1.137643 1.543041
VERTEX_SE2 2 0.129867 1.989651 3.201259
VERTEX_SE2 3 -1.047715 0.933789 4.743682
EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000

9
examples/Data/orientationsNoisyToyGraph.txt Normal file
View File

@ -0,0 +1,9 @@
VERTEX_SE2 0 0.000000 0.000000 0.000000
VERTEX_SE2 1 0.000000 0.000000 1.565449
VERTEX_SE2 2 0.000000 0.000000 3.134143
VERTEX_SE2 3 0.000000 0.000000 4.710123
EDGE_SE2 0 1 0.774115 1.183389 1.576173 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 1 2 0.869231 1.031877 1.579418 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 3 1.357840 1.034262 1.566460 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 2 0 0.303492 1.865011 -3.113898 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000
EDGE_SE2 0 3 -0.928526 0.993695 -1.563542 1.000000 0.000000 0.000000 1.000000 0.000000 1.000000

23
examples/Data/pose2example.txt Normal file
View File

@ -0,0 +1,23 @@
VERTEX_SE2 0 0.000000 0.000000 0.000000
VERTEX_SE2 1 1.030390 0.011350 -0.081596
VERTEX_SE2 2 2.036137 -0.129733 -0.301887
VERTEX_SE2 3 3.015097 -0.442395 -0.345514
VERTEX_SE2 4 3.343949 0.506678 1.214715
VERTEX_SE2 5 3.684491 1.464049 1.183785
VERTEX_SE2 6 4.064626 2.414783 1.176333
VERTEX_SE2 7 4.429778 3.300180 1.259169
VERTEX_SE2 8 4.128877 2.321481 -1.825391
VERTEX_SE2 9 3.884653 1.327509 -1.953016
VERTEX_SE2 10 3.531067 0.388263 -2.148934
EDGE_SE2 0 1 1.030390 0.011350 -0.081596 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 1 2 1.013900 -0.058639 -0.220291 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 2 3 1.027650 -0.007456 -0.043627 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 3 4 -0.012016 1.004360 1.560229 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 4 5 1.016030 0.014565 -0.030930 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 5 6 1.023890 0.006808 -0.007452 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 6 7 0.957734 0.003159 0.082836 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 7 8 -1.023820 -0.013668 -3.084560 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 8 9 1.023440 0.013984 -0.127624 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 9 10 1.003350 0.022250 -0.195918 44.721360 0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 5 9 0.033943 0.032439 3.073637 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699
EDGE_SE2 3 10 0.044020 0.988477 -1.553511 44.721360 -0.000000 0.000000 44.721360 0.000000 30.901699

62
examples/Pose2SLAMExample_g2o.cpp Normal file
View File

@ -0,0 +1,62 @@
/* ----------------------------------------------------------------------------
* 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 Pose2SLAMExample_g2o.cpp
* @brief A 2D Pose SLAM example that reads input from g2o, converts it to a factor graph and does the
* optimization. Output is written on a file, in g2o format
* Syntax for the script is ./Pose2SLAMExample_g2o input.g2o output.g2o
* @date May 15, 2014
* @author Luca Carlone
*/
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <gtsam/nonlinear/GaussNewtonOptimizer.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = "../../examples/Data/noisyToyGraph.txt";
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
boost::tie(graph, initial) = readG2o(g2oFile);
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph;
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);
Values result = optimizer.optimize();
std::cout << "Optimization complete" << std::endl;
if (argc < 3) {
result.print("result");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, result, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}

64
examples/Pose2SLAMExample_lago.cpp Normal file
View File

@ -0,0 +1,64 @@
/* ----------------------------------------------------------------------------
* 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 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 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/slam/lago.h>
#include <gtsam/slam/dataset.h>
#include <gtsam/slam/PriorFactor.h>
#include <fstream>
using namespace std;
using namespace gtsam;
int main(const int argc, const char *argv[]) {
// Read graph from file
string g2oFile;
if (argc < 2)
g2oFile = "../../examples/Data/noisyToyGraph.txt";
else
g2oFile = argv[1];
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
boost::tie(graph, initial) = readG2o(g2oFile);
// Add prior on the pose having index (key) = 0
NonlinearFactorGraph graphWithPrior = *graph;
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 = lago::initialize(graphWithPrior);
std::cout << "done!" << std::endl;
if (argc < 3) {
estimateLago.print("estimateLago");
} else {
const string outputFile = argv[2];
std::cout << "Writing results to file: " << outputFile << std::endl;
writeG2o(*graph, estimateLago, outputFile);
std::cout << "done! " << std::endl;
}
return 0;
}

168
examples/SFMExample_SmartFactor.cpp Normal file
View File

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

75
examples/StereoVOExample.cpp Normal file
View File

@ -0,0 +1,75 @@
/* ----------------------------------------------------------------------------
* 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 SteroVOExample.cpp
* @brief A stereo visual odometry example
* @date May 25, 2014
* @author Stephen Camp
*/
/**
* A 3D stereo visual odometry example
* - robot starts at origin
* -moves forward 1 meter
* -takes stereo readings on three landmarks
*/
#include <gtsam/geometry/Pose3.h>
#include <gtsam/inference/Key.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>
using namespace std;
using namespace gtsam;
int main(int argc, char** argv){
//create graph object, add first pose at origin with key '1'
NonlinearFactorGraph graph;
Pose3 first_pose = Pose3();
graph.push_back(NonlinearEquality<Pose3>(1, first_pose));
//create factor noise model with 3 sigmas of value 1
const noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,1);
//create stereo camera calibration object with .2m between cameras
const Cal3_S2Stereo::shared_ptr K(new Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2));
//create and add stereo factors between first pose (key value 1) and the three landmarks
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(520, 480, 440), model, 1, 3, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(120, 80, 440), model, 1, 4, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 280, 140), model, 1, 5, K));
//create and add stereo factors between second pose and the three landmarks
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(570, 520, 490), model, 2, 3, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(70, 20, 490), model, 2, 4, K));
graph.push_back(GenericStereoFactor<Pose3,Point3>(StereoPoint2(320, 270, 115), model, 2, 5, K));
//create Values object to contain initial estimates of camera poses and landmark locations
Values initial_estimate = Values();
//create and add iniital estimates
initial_estimate.insert(1, first_pose);
initial_estimate.insert(2, Pose3(Rot3(), Point3(0.1, -0.1, 1.1)));
initial_estimate.insert(3, Point3(1, 1, 5));
initial_estimate.insert(4, Point3(-1, 1, 5));
initial_estimate.insert(5, Point3(0, -0.5, 5));
//create Levenberg-Marquardt optimizer for resulting factor graph, optimize
LevenbergMarquardtOptimizer optimizer = LevenbergMarquardtOptimizer(graph, initial_estimate);
Values result = optimizer.optimize();
result.print("Final result:\n");
return 0;
}

114
examples/StereoVOExample_large.cpp Normal file
View File

@ -0,0 +1,114 @@
/* ----------------------------------------------------------------------------
* GTSAM Copyright 2010, Georgia Tech Research Corporation,
* Atlanta, Georgia 30332-0415
* All Rights Reserved
* Authors: Frank Dellaert, et al. (see THANKS for the full author list)
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
/**
* @file SteroVOExample.cpp
* @brief A stereo visual odometry example
* @date May 25, 2014
* @author Stephen Camp
*/
/**
* A 3D stereo visual odometry example
* - robot starts at origin
* -moves forward, taking periodic stereo measurements
* -takes stereo readings 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/StereoFactor.h>
#include <gtsam/slam/dataset.h>
#include <string>
#include <fstream>
#include <iostream>
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);
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
double fx, fy, s, u0, v0, b;
ifstream calibration_file(calibration_loc.c_str());
cout << "Reading calibration info" << endl;
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,u0,v0,b));
ifstream pose_file(pose_loc.c_str());
cout << "Reading camera poses" << endl;
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 measurement details from file and use to create and add GenericStereoFactor objects to the graph representation
while (factor_file >> x >> l >> uL >> uR >> v >> X >> Y >> Z) {
graph.push_back(
GenericStereoFactor<Pose3, Point3>(StereoPoint2(uL, uR, v), model,
Symbol('x', x), Symbol('l', l), K));
//if the landmark variable included in this factor has not yet been added to the initial variable value estimate, add it
if (!initial_estimate.exists(Symbol('l', l))) {
Pose3 camPose = initial_estimate.at<Pose3>(Symbol('x', x));
//transform_from() transforms the input Point3 from the camera pose space, camPose, to the global space
Point3 worldPoint = camPose.transform_from(Point3(X, Y, Z));
initial_estimate.insert(Symbol('l', l), worldPoint);
}
}
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;
}

15
gtsam.h
View File

@ -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
@ -2363,6 +2370,12 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
namespace utilities {
#include <matlab.h>
gtsam::KeyList createKeyList(Vector I);
gtsam::KeyList createKeyList(string s, Vector I);
gtsam::KeyVector createKeyVector(Vector I);
gtsam::KeyVector createKeyVector(string s, Vector I);
gtsam::KeySet createKeySet(Vector I);
gtsam::KeySet createKeySet(string s, Vector I);
Matrix extractPoint2(const gtsam::Values& values);
Matrix extractPoint3(const gtsam::Values& values);
Matrix extractPose2(const gtsam::Values& values);
@ -2375,6 +2388,8 @@ namespace utilities {
void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K);
void insertProjectionFactors(gtsam::NonlinearFactorGraph& graph, size_t i, Vector J, Matrix Z, const gtsam::noiseModel::Base* model, const gtsam::Cal3_S2* K, const gtsam::Pose3& body_P_sensor);
Matrix reprojectionErrors(const gtsam::NonlinearFactorGraph& graph, const gtsam::Values& values);
gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base);
gtsam::Values localToWorld(const gtsam::Values& local, const gtsam::Pose2& base, const gtsam::KeyVector& keys);
} //\namespace utilities

3
gtsam/3rdparty/CCOLAMD/Source/ccolamd.c vendored
View File

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

3
gtsam/3rdparty/CCOLAMD/Source/ccolamd_global.c vendored
View File

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

17
gtsam/3rdparty/CMakeLists.txt vendored
View File

@ -16,13 +16,30 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
endif()
endforeach(eigen_dir)
# do the same for the unsupported eigen folder
file(GLOB_RECURSE unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/*.h")
file(GLOB unsupported_eigen_dir_headers_all "Eigen/unsupported/Eigen/*")
foreach(unsupported_eigen_dir ${unsupported_eigen_dir_headers_all})
get_filename_component(filename ${unsupported_eigen_dir} NAME)
if (NOT ((${filename} MATCHES "CMakeLists.txt") OR (${filename} MATCHES "src") OR (${filename} MATCHES ".svn")))
list(APPEND unsupported_eigen_headers "${CMAKE_CURRENT_SOURCE_DIR}/Eigen/unsupported/Eigen/${filename}")
install(FILES Eigen/unsupported/Eigen/${filename} DESTINATION include/gtsam/3rdparty/Eigen/unsupported/Eigen)
endif()
endforeach(unsupported_eigen_dir)
# Add to project source
set(eigen_headers ${eigen_headers} PARENT_SCOPE)
# set(unsupported_eigen_headers ${unsupported_eigen_headers} PARENT_SCOPE)
# install Eigen - only the headers in our 3rdparty directory
install(DIRECTORY Eigen/Eigen
DESTINATION include/gtsam/3rdparty/Eigen
FILES_MATCHING PATTERN "*.h")
install(DIRECTORY Eigen/unsupported/Eigen
DESTINATION include/gtsam/3rdparty/Eigen/unsupported/
FILES_MATCHING PATTERN "*.h")
endif()
############ NOTE: When updating GeographicLib be sure to disable building their examples

47
gtsam/base/Matrix.cpp
View File

@ -591,41 +591,17 @@ Matrix3 skewSymmetric(double wx, double wy, double wz)
}
/* ************************************************************************* */
/** Numerical Recipes in C wrappers
* create Numerical Recipes in C structure
* pointers are subtracted by one to provide base 1 access
*/
/* ************************************************************************* */
// FIXME: assumes row major, rather than column major
//double** createNRC(Matrix& A) {
// const size_t m=A.rows();
// double** a = new double* [m];
// for(size_t i = 0; i < m; i++)
// a[i] = &A(i,0)-1;
// return a;
//}
/* ******************************************
*
* Modified from Justin's codebase
*
* Idea came from other public domain code. Takes a S.P.D. matrix
* and computes the LL^t decomposition. returns L, which is lower
* triangular. Note this is the opposite convention from Matlab,
* which calculates Q'Q where Q is upper triangular.
*
* ******************************************/
Matrix LLt(const Matrix& A)
{
Matrix L = zeros(A.rows(), A.rows());
Eigen::LLT<Matrix> llt;
llt.compute(A);
Eigen::LLT<Matrix> llt(A);
return llt.matrixL();
}
/* ************************************************************************* */
Matrix RtR(const Matrix &A)
{
return LLt(A).transpose();
Eigen::LLT<Matrix> llt(A);
return llt.matrixU();
}
/*
@ -633,13 +609,10 @@ Matrix RtR(const Matrix &A)
*/
Matrix cholesky_inverse(const Matrix &A)
{
// FIXME: replace with real algorithm
return A.inverse();
// Matrix L = LLt(A);
// Matrix inv(eye(A.rows()));
// inplace_solve (L, inv, BNU::lower_tag ());
// return BNU::prod(trans(inv), inv);
Eigen::LLT<Matrix> llt(A);
Matrix inv = eye(A.rows());
llt.matrixU().solveInPlace<Eigen::OnTheRight>(inv);
return inv*inv.transpose();
}
/* ************************************************************************* */
@ -648,9 +621,9 @@ Matrix cholesky_inverse(const Matrix &A)
// inv(B) * inv(B)' == A
// inv(B' * B) == A
Matrix inverse_square_root(const Matrix& A) {
Matrix R = RtR(A);
Eigen::LLT<Matrix> llt(A);
Matrix inv = eye(A.rows());
R.triangularView<Eigen::Upper>().solveInPlace<Eigen::OnTheRight>(inv);
llt.matrixU().solveInPlace<Eigen::OnTheRight>(inv);
return inv.transpose();
}

4
gtsam/base/SymmetricBlockMatrix.h
View File

@ -236,6 +236,10 @@ namespace gtsam {
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int version) {
// Fill in the lower triangle part of the matrix, so boost::serialization won't
// complain about uninitialized data with an input_stream_error exception
// http://www.boost.org/doc/libs/1_37_0/libs/serialization/doc/exceptions.html#stream_error
matrix_.triangularView<Eigen::Lower>() = matrix_.triangularView<Eigen::Upper>().transpose();
ar & BOOST_SERIALIZATION_NVP(matrix_);
ar & BOOST_SERIALIZATION_NVP(variableColOffsets_);
ar & BOOST_SERIALIZATION_NVP(blockStart_);

36
gtsam/base/tests/testMatrix.cpp
View File

@ -1022,22 +1022,32 @@ TEST( matrix, inverse_square_root )
/* *********************************************************************** */
// M was generated as the covariance of a set of random numbers. L that
// we are checking against was generated via chol(M)' on octave
namespace cholesky {
Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463,
0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363,
0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463,
0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363,
-0.0021113, 0.0036415, 0.0909464);
Matrix expected = (Matrix(5, 5) <<
0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000,
-0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000,
0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000,
0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000,
0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247);
}
TEST( matrix, LLt )
{
Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463,
0.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363,
0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463,
0.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363,
-0.0021113, 0.0036415, 0.0909464);
EQUALITY(cholesky::expected, LLt(cholesky::M));
}
TEST( matrix, RtR )
{
EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M));
}
Matrix expected = (Matrix(5, 5) <<
0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000,
-0.010437374483502, 0.295235094820875, 0.000000000000000, 0.000000000000000, 0.000000000000000,
0.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000,
0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000,
0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247);
EQUALITY(expected, LLt(M));
TEST( matrix, cholesky_inverse )
{
EQUALITY(cholesky::M.inverse(), cholesky_inverse(cholesky::M));
}
/* ************************************************************************* */

1
gtsam/geometry/Pose2.cpp
View File

@ -178,6 +178,7 @@ Pose2 Pose2::between(const Pose2& p2, boost::optional<Matrix&> H1,
// Calculate delta translation = unrotate(R1, dt);
Point2 dt = p2.t() - t_;
double x = dt.x(), y = dt.y();
// t = R1' * (t2-t1)
Point2 t(c1 * x + s1 * y, -s1 * x + c1 * y);
// FD: This is just -AdjointMap(between(p2,p1)) inlined and re-using above

0
gtsam_unstable/geometry/TriangulationFactor.h → gtsam/geometry/TriangulationFactor.h
View File

4
gtsam/geometry/Unit3.cpp
View File

@ -151,9 +151,9 @@ Vector Unit3::localCoordinates(const Unit3& y) const {
double dot = p.dot(q);
// Check for special cases
if (std::abs(dot - 1.0) < 1e-20)
if (std::abs(dot - 1.0) < 1e-16)
return (Vector(2) << 0, 0);
else if (std::abs(dot + 1.0) < 1e-20)
else if (std::abs(dot + 1.0) < 1e-16)
return (Vector(2) << M_PI, 0);
else {
// no special case

4
gtsam/geometry/tests/testPose2.cpp
View File

@ -398,7 +398,7 @@ TEST( Pose2, matrix )
TEST( Pose2, compose_matrix )
{
Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) loooking at negative x
Pose2 _1T2(M_PI, Point2(-1,4)); // local robot at (-1,4) looking at negative x
Matrix gM1(matrix(gT1)),_1M2(matrix(_1T2));
EXPECT(assert_equal(gM1*_1M2,matrix(gT1.compose(_1T2)))); // RIGHT DOES NOT
}
@ -412,7 +412,7 @@ TEST( Pose2, between )
//
// *--0--*--*
Pose2 gT1(M_PI/2.0, Point2(1,2)); // robot at (1,2) looking towards y
Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) loooking at negative x
Pose2 gT2(M_PI, Point2(-1,4)); // robot at (-1,4) looking at negative x
Matrix actualH1,actualH2;
Pose2 expected(M_PI/2.0, Point2(2,2));

2
gtsam_unstable/geometry/tests/testTriangulation.cpp → gtsam/geometry/tests/testTriangulation.cpp
View File

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

0
gtsam/geometry/tests/testSphere2.cpp → gtsam/geometry/tests/testUnit3.cpp
View File

2
gtsam_unstable/geometry/triangulation.cpp → gtsam/geometry/triangulation.cpp
View File

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

8
gtsam_unstable/geometry/triangulation.h → gtsam/geometry/triangulation.h
View File

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

60
gtsam/inference/VariableIndex-inl.h
View File

@ -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)
{
if(factors[i])
{
for (size_t i = 0; i < factors.size(); ++i) {
if (factors[i]) {
const size_t globalI =
newFactorIndices ?
(*newFactorIndices)[i] :
nFactors_;
BOOST_FOREACH(const Key key, *factors[i])
{
newFactorIndices ? (*newFactorIndices)[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)[i] >= nFactors_)
if (newFactorIndices) {
if ((*newFactorIndices)[i] >= nFactors_)
nFactors_ = (*newFactorIndices)[i] + 1;
}
else
{
++ nFactors_;
} else {
++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) {
if(i >= factors.size())
throw std::invalid_argument("Internal error, requested inconsistent number of factor indices and factors in VariableIndex::remove");
if(factors[i]) {
for (; factorIndex != lastFactor; ++factorIndex, ++i) {
if (i >= factors.size())
throw std::invalid_argument(
"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);
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");
Factors::iterator entry = std::find(factorEntries.begin(),
factorEntries.end(), *factorIndex);
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())
throw std::invalid_argument("Asking to remove variables from the variable index that are not unused");
if (!entry->second.empty())
throw std::invalid_argument(
"Asking to remove variables from the variable index that are not unused");
index_.erase(entry);
}
}

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

@ -22,7 +22,7 @@
#ifdef __GNUC__
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wunused-variable"
#pragma GCC diagnostic ignored "-Wunneeded-internal-declaration"
//#pragma GCC diagnostic ignored "-Wunneeded-internal-declaration"
#endif
#include <boost/graph/breadth_first_search.hpp>
#ifdef __GNUC__
@ -73,34 +73,41 @@ SDGraph<KEY> toBoostGraph(const G& graph) {
SDGraph<KEY> g;
typedef typename boost::graph_traits<SDGraph<KEY> >::vertex_descriptor BoostVertex;
std::map<KEY, BoostVertex> key2vertex;
BoostVertex v1, v2;
typename G::const_iterator itFactor;
// Loop over the factors
for(itFactor=graph.begin(); itFactor!=graph.end(); itFactor++) {
if ((*itFactor)->keys().size() > 2)
throw(std::invalid_argument("toBoostGraph: only support factors with at most two keys"));
if ((*itFactor)->keys().size() == 1)
// Ignore factors that are not binary
if ((*itFactor)->keys().size() != 2)
continue;
// Cast the factor to the user-specified factor type F
boost::shared_ptr<F> factor = boost::dynamic_pointer_cast<F>(*itFactor);
// Ignore factors that are not of type F
if (!factor) continue;
KEY key1 = factor->key1();
KEY key2 = factor->key2();
// Retrieve the 2 keys (nodes) the factor (edge) is incident on
KEY key1 = factor->keys()[0];
KEY key2 = factor->keys()[1];
BoostVertex v1, v2;
// If key1 is a new key, add it to the key2vertex map, else get the corresponding vertex id
if (key2vertex.find(key1) == key2vertex.end()) {
v1 = add_vertex(key1, g);
key2vertex.insert(make_pair(key1, v1));
key2vertex.insert(std::pair<KEY,KEY>(key1, v1));
} else
v1 = key2vertex[key1];
// If key2 is a new key, add it to the key2vertex map, else get the corresponding vertex id
if (key2vertex.find(key2) == key2vertex.end()) {
v2 = add_vertex(key2, g);
key2vertex.insert(make_pair(key2, v2));
key2vertex.insert(std::pair<KEY,KEY>(key2, v2));
} else
v2 = key2vertex[key2];
// Add an edge with weight 1.0
boost::property<boost::edge_weight_t, double> edge_property(1.0); // assume constant edge weight here
boost::add_edge(v1, v2, edge_property, g);
}
@ -222,12 +229,11 @@ boost::shared_ptr<Values> composePoses(const G& graph, const PredecessorMap<KEY>
return config;
}
/* ************************************************************************* */
/* ************************************************************************* */
template<class G, class KEY, class FACTOR2>
PredecessorMap<KEY> findMinimumSpanningTree(const G& fg) {
// Convert to a graph that boost understands
SDGraph<KEY> g = gtsam::toBoostGraph<G, FACTOR2, KEY>(fg);
// find minimum spanning tree
@ -237,13 +243,12 @@ PredecessorMap<KEY> findMinimumSpanningTree(const G& fg) {
// convert edge to string pairs
PredecessorMap<KEY> tree;
typename SDGraph<KEY>::vertex_iterator itVertex = boost::vertices(g).first;
typename std::vector<typename SDGraph<KEY>::Vertex>::iterator vi;
for (vi = p_map.begin(); vi != p_map.end(); itVertex++, vi++) {
BOOST_FOREACH(const typename SDGraph<KEY>::Vertex& vi, p_map){
KEY key = boost::get(boost::vertex_name, g, *itVertex);
KEY parent = boost::get(boost::vertex_name, g, *vi);
KEY parent = boost::get(boost::vertex_name, g, vi);
tree.insert(key, parent);
itVertex++;
}
return tree;
}

3
gtsam/linear/GaussianFactor.h
View File

@ -130,6 +130,9 @@ namespace gtsam {
/// A'*b for Jacobian, eta for Hessian
virtual VectorValues gradientAtZero() const = 0;
/// A'*b for Jacobian, eta for Hessian (raw memory version)
virtual void gradientAtZero(double* d) const = 0;
private:
/** Serialization function */
friend class boost::serialization::access;

9
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -70,11 +70,18 @@ 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;
}
/* ************************************************************************* */
GaussianFactorGraph::shared_ptr GaussianFactorGraph::cloneToPtr() const {
gtsam::GaussianFactorGraph::shared_ptr result(new GaussianFactorGraph());
*result = *this;
return result;
}
/* ************************************************************************* */
GaussianFactorGraph GaussianFactorGraph::clone() const {
GaussianFactorGraph result;

10
gtsam/linear/GaussianFactorGraph.h
View File

@ -160,7 +160,13 @@ namespace gtsam {
* Cloning preserves null factors so indices for the original graph are still
* valid for the cloned graph.
*/
GaussianFactorGraph clone() const;
virtual GaussianFactorGraph clone() const;
/**
* CloneToPtr() performs a simple assignment to a new graph and returns it.
* There is no preservation of null factors!
*/
virtual GaussianFactorGraph::shared_ptr cloneToPtr() const;
/**
* Returns the negation of all factors in this graph - corresponds to antifactors.
@ -257,7 +263,7 @@ namespace gtsam {
* @param [output] g A VectorValues to store the gradient, which must be preallocated,
* see allocateVectorValues
* @return The gradient as a VectorValues */
VectorValues gradientAtZero() const;
virtual VectorValues gradientAtZero() const;
/** Optimize along the gradient direction, with a closed-form computation to perform the line
* search. The gradient is computed about \f$ \delta x=0 \f$.

17
gtsam/linear/HessianFactor.cpp
View File

@ -599,6 +599,23 @@ VectorValues HessianFactor::gradientAtZero() const {
return g;
}
/* ************************************************************************* */
// TODO: currently assumes all variables of the same size 9 and keys arranged from 0 to n
void HessianFactor::gradientAtZero(double* d) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, 9, 1> DVector;
typedef Eigen::Map<DVector> DMap;
// Loop over all variables in the factor
for (DenseIndex pos = 0; pos < (DenseIndex)size(); ++pos) {
Key j = keys_[pos];
// Get the diagonal block, and insert its diagonal
DVector dj = -info_(pos,size()).knownOffDiagonal();
DMap(d + 9 * j) += dj;
}
}
/* ************************************************************************* */
std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)

2
gtsam/linear/HessianFactor.h
View File

@ -387,6 +387,8 @@ namespace gtsam {
/// eta for Hessian
VectorValues gradientAtZero() const;
virtual void gradientAtZero(double* d) const;
/**
* Densely partially eliminate with Cholesky factorization. JacobianFactors are
* left-multiplied with their transpose to form the Hessian using the conversion constructor

5
gtsam/linear/JacobianFactor.cpp
View File

@ -573,6 +573,11 @@ VectorValues JacobianFactor::gradientAtZero() const {
return g;
}
/* ************************************************************************* */
void JacobianFactor::gradientAtZero(double* d) const {
//throw std::runtime_error("gradientAtZero not implemented for Jacobian factor");
}
/* ************************************************************************* */
pair<Matrix, Vector> JacobianFactor::jacobian() const {
pair<Matrix, Vector> result = jacobianUnweighted();

3
gtsam/linear/JacobianFactor.h
View File

@ -286,6 +286,9 @@ namespace gtsam {
/// A'*b for Jacobian
VectorValues gradientAtZero() const;
/* ************************************************************************* */
virtual void gradientAtZero(double* d) const;
/** Return a whitened version of the factor, i.e. with unit diagonal noise model. */
JacobianFactor whiten() const;

95
gtsam/linear/NoiseModel.cpp
View File

@ -47,22 +47,73 @@ void updateAb(MATRIX& Ab, int j, const Vector& a, const Vector& rd) {
Ab.middleCols(j+1,n-j) -= a * rd.segment(j+1, n-j).transpose();
}
/* ************************************************************************* */
// check *above the diagonal* for non-zero entries
boost::optional<Vector> checkIfDiagonal(const Matrix M) {
size_t m = M.rows(), n = M.cols();
// check all non-diagonal entries
bool full = false;
size_t i, j;
for (i = 0; i < m; i++)
if (!full)
for (j = i + 1; j < n; j++)
if (fabs(M(i, j)) > 1e-9) {
full = true;
break;
}
if (full) {
return boost::none;
} else {
Vector diagonal(n);
for (j = 0; j < n; j++)
diagonal(j) = M(j, j);
return diagonal;
}
}
/* ************************************************************************* */
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 (smart) {
// check all non-diagonal entries
size_t i,j;
for (i = 0; i < m; i++)
for (j = 0; j < n; j++)
if (i != j && fabs(covariance(i, j)) > 1e-9) goto full;
Vector variances(n);
for (j = 0; j < n; j++) variances(j) = covariance(j,j);
return Diagonal::Variances(variances,true);
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");
boost::optional<Vector> diagonal = boost::none;
if (smart)
diagonal = checkIfDiagonal(R);
if (diagonal)
return Diagonal::Sigmas(reciprocal(*diagonal), true);
else
return shared_ptr(new Gaussian(R.rows(), R));
}
/* ************************************************************************* */
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));
}
full: return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
}
/* ************************************************************************* */
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");
boost::optional<Vector> variances = boost::none;
if (smart)
variances = checkIfDiagonal(covariance);
if (variances)
return Diagonal::Variances(*variances, true);
else
return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
}
/* ************************************************************************* */
@ -166,7 +217,7 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const
// Diagonal
/* ************************************************************************* */
Diagonal::Diagonal() :
Gaussian(1)//, sigmas_(ones(1)), invsigmas_(ones(1)), precisions_(ones(1))
Gaussian(1) // TODO: Frank asks: really sure about this?
{
}
@ -180,8 +231,8 @@ Diagonal::Diagonal(const Vector& sigmas) :
Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
if (smart) {
// check whether all the same entry
DenseIndex j, n = variances.size();
for (j = 1; j < n; j++)
size_t n = variances.size();
for (size_t j = 1; j < n; j++)
if (variances(j) != variances(0)) goto full;
return Isotropic::Variance(n, variances(0), true);
}
@ -191,12 +242,18 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
/* ************************************************************************* */
Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
if (smart) {
size_t n = sigmas.size();
if (n==0) goto full;
// look for zeros to make a constraint
for (size_t i=0; i< (size_t) sigmas.size(); ++i)
if (sigmas(i)<1e-8)
for (size_t j=0; j< n; ++j)
if (sigmas(j)<1e-8)
return Constrained::MixedSigmas(sigmas);
// check whether all the same entry
for (size_t j = 1; j < n; j++)
if (sigmas(j) != sigmas(0)) goto full;
return Isotropic::Sigma(n, sigmas(0), true);
}
return Diagonal::shared_ptr(new Diagonal(sigmas));
full: return Diagonal::shared_ptr(new Diagonal(sigmas));
}
/* ************************************************************************* */

16
gtsam/linear/NoiseModel.h
View File

@ -159,10 +159,17 @@ namespace gtsam {
/**
* A Gaussian noise model created by specifying a square root information matrix.
* @param R The (upper-triangular) square root information matrix
* @param smart check if can be simplified to derived class
*/
static shared_ptr SqrtInformation(const Matrix& R) {
return shared_ptr(new Gaussian(R.rows(),R));
}
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.
@ -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.

29
gtsam/linear/tests/testNoiseModel.cpp
View File

@ -267,6 +267,35 @@ TEST(NoiseModel, QRNan )
EXPECT(assert_equal(expectedAb,Ab));
}
/* ************************************************************************* */
TEST(NoiseModel, SmartSqrtInformation )
{
bool smart = true;
gtsam::SharedGaussian expected = Unit::Create(3);
gtsam::SharedGaussian actual = Gaussian::SqrtInformation(eye(3), smart);
EXPECT(assert_equal(*expected,*actual));
}
/* ************************************************************************* */
TEST(NoiseModel, SmartSqrtInformation2 )
{
bool smart = true;
gtsam::SharedGaussian expected = Unit::Isotropic::Sigma(3,2);
gtsam::SharedGaussian actual = Gaussian::SqrtInformation(0.5*eye(3), smart);
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 )
{

5
gtsam/navigation/CombinedImuFactor.h
View File

@ -34,6 +34,11 @@ namespace gtsam {
*
* @addtogroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
* independent sets in factor graphs: a unifying perspective based on smart factors,
* Int. Conf. on Robotics and Automation (ICRA), 2014.
*
* REFERENCES:
* [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built

8
gtsam/navigation/ImuFactor.h
View File

@ -33,7 +33,13 @@ namespace gtsam {
/**
*
* @addtogroup SLAM
* * REFERENCES:
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
* independent sets in factor graphs: a unifying perspective based on smart factors,
* Int. Conf. on Robotics and Automation (ICRA), 2014.
*
** REFERENCES:
* [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008.
* [2] T. Lupton and S.Sukkarieh, "Visual-Inertial-Aided Navigation for High-Dynamic Motion in Built
* Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.

7
gtsam/navigation/MagFactor.h
View File

@ -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)
*HR = HR->col(2);
if (HR) {
// assign to temporary first to avoid error in Win-Debug mode
Matrix H = HR->col(2);
*HR = H;
}
return q;
}

2
gtsam/nonlinear/DoglegOptimizerImpl.h
View File

@ -191,7 +191,7 @@ typename DoglegOptimizerImpl::IterationResult DoglegOptimizerImpl::Iterate(
if(mode == ONE_STEP_PER_ITERATION || mode == SEARCH_REDUCE_ONLY)
stay = false; // If not searching, just return with the new Delta
else if(mode == SEARCH_EACH_ITERATION) {
if(newDelta == Delta || lastAction == DECREASED_DELTA)
if(fabs(newDelta - Delta) < 1e-15 || lastAction == DECREASED_DELTA)
stay = false; // Searching, but Newton's solution is within trust region so keep the same trust region
else {
stay = true; // Searching and increased Delta, so try again to increase Delta

81
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -138,7 +138,7 @@ void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) {
}
/* ************************************************************************* */
GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
const GaussianFactorGraph& linear) {
gttic(damp);
@ -159,7 +159,8 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
// for each of the variables, add a prior
double sigma = 1.0 / std::sqrt(state_.lambda);
GaussianFactorGraph damped = linear;
GaussianFactorGraph::shared_ptr dampedPtr = linear.cloneToPtr();
GaussianFactorGraph &damped = (*dampedPtr);
damped.reserve(damped.size() + state_.values.size());
if (params_.diagonalDamping) {
BOOST_FOREACH(const VectorValues::KeyValuePair& key_vector, state_.hessianDiagonal) {
@ -188,7 +189,20 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
}
}
gttoc(damp);
return damped;
return dampedPtr;
}
/* ************************************************************************* */
// Log current error/lambda to file
inline void LevenbergMarquardtOptimizer::writeLogFile(double currentError){
if (!params_.logFile.empty()) {
ofstream os(params_.logFile.c_str(), ios::app);
boost::posix_time::ptime currentTime = boost::posix_time::microsec_clock::universal_time();
os << /*inner iterations*/ state_.totalNumberInnerIterations << ","
<< 1e-6 * (currentTime - state_.startTime).total_microseconds() << ","
<< /*current error*/ currentError << "," << state_.lambda << ","
<< /*outer iterations*/ state_.iterations << endl;
}
}
/* ************************************************************************* */
@ -205,6 +219,9 @@ void LevenbergMarquardtOptimizer::iterate() {
cout << "linearizing = " << endl;
GaussianFactorGraph::shared_ptr linear = linearize();
if(state_.totalNumberInnerIterations==0) // write initial error
writeLogFile(state_.error);
// Keep increasing lambda until we make make progress
while (true) {
@ -212,21 +229,8 @@ void LevenbergMarquardtOptimizer::iterate() {
cout << "trying lambda = " << state_.lambda << endl;
// Build damped system for this lambda (adds prior factors that make it like gradient descent)
GaussianFactorGraph dampedSystem = buildDampedSystem(*linear);
// Log current error/lambda to file
if (!params_.logFile.empty()) {
ofstream os(params_.logFile.c_str(), ios::app);
boost::posix_time::ptime currentTime =
boost::posix_time::microsec_clock::universal_time();
os << state_.totalNumberInnerIterations << ","
<< 1e-6 * (currentTime - state_.startTime).total_microseconds() << ","
<< state_.error << "," << state_.lambda << endl;
}
++state_.totalNumberInnerIterations;
GaussianFactorGraph::shared_ptr dampedSystemPtr = buildDampedSystem(*linear);
GaussianFactorGraph &dampedSystem = (*dampedSystemPtr);
// Try solving
double modelFidelity = 0.0;
@ -240,7 +244,7 @@ void LevenbergMarquardtOptimizer::iterate() {
try {
delta = solve(dampedSystem, state_.values, params_);
systemSolvedSuccessfully = true;
} catch (IndeterminantLinearSystemException& e) {
} catch (IndeterminantLinearSystemException) {
systemSolvedSuccessfully = false;
}
@ -256,6 +260,9 @@ void LevenbergMarquardtOptimizer::iterate() {
double newlinearizedError = linear->error(delta);
double linearizedCostChange = state_.error - newlinearizedError;
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "newlinearizedError = " << newlinearizedError <<
" linearizedCostChange = " << linearizedCostChange << endl;
if (linearizedCostChange >= 0) { // step is valid
// update values
@ -266,50 +273,62 @@ void LevenbergMarquardtOptimizer::iterate() {
// compute new error
gttic(compute_error);
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "calculating error" << endl;
cout << "calculating error:" << endl;
newError = graph_.error(newValues);
gttoc(compute_error);
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "old error (" << state_.error
<< ") new (tentative) error (" << newError << ")" << endl;
// cost change in the original, nonlinear system (old - new)
double costChange = state_.error - newError;
if (linearizedCostChange > 1e-15) { // the error has to decrease to satify this condition
if (linearizedCostChange > 1e-20) { // the (linear) error has to decrease to satisfy this condition
// fidelity of linearized model VS original system between
modelFidelity = costChange / linearizedCostChange;
// if we decrease the error in the nonlinear system and modelFidelity is above threshold
step_is_successful = modelFidelity > params_.minModelFidelity;
} else {
step_is_successful = true; // linearizedCostChange close to zero
}
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "modelFidelity: " << modelFidelity << endl;
} // else we consider the step non successful and we either increase lambda or stop if error change is small
double minAbsoluteTolerance = params_.relativeErrorTol * state_.error;
// if the change is small we terminate
if (fabs(costChange) < minAbsoluteTolerance)
if (fabs(costChange) < minAbsoluteTolerance){
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "fabs(costChange)="<<fabs(costChange) << " minAbsoluteTolerance="<< minAbsoluteTolerance
<< " (relativeErrorTol=" << params_.relativeErrorTol << ")" << endl;
stopSearchingLambda = true;
}
}
}
++state_.totalNumberInnerIterations;
if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity
state_.values.swap(newValues);
state_.error = newError;
decreaseLambda(modelFidelity);
writeLogFile(state_.error);
break;
} else if (!stopSearchingLambda) { // we failed to solved the system or we had no decrease in cost
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "increasing lambda: old error (" << state_.error
<< ") new error (" << newError << ")" << endl;
cout << "increasing lambda" << endl;
increaseLambda();
writeLogFile(state_.error);
// check if lambda is too big
if (state_.lambda >= params_.lambdaUpperBound) {
if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION)
cout
<< "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda"
<< endl;
cout << "Warning: Levenberg-Marquardt giving up because "
"cannot decrease error with maximum lambda" << endl;
break;
}
} else { // the change in the cost is very small and it is not worth trying bigger lambdas
writeLogFile(state_.error);
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "Levenberg-Marquardt: stopping as relative cost reduction is small" << endl;
break;
}
} // end while

5
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

@ -113,7 +113,6 @@ public:
inline void setDiagonalDamping(bool flag) {
diagonalDamping = flag;
}
inline void setUseFixedLambdaFactor(bool flag) {
useFixedLambdaFactor_ = flag;
}
@ -255,9 +254,11 @@ public:
}
/** Build a damped system for a specific lambda */
GaussianFactorGraph buildDampedSystem(const GaussianFactorGraph& linear);
GaussianFactorGraph::shared_ptr buildDampedSystem(const GaussianFactorGraph& linear);
friend class ::NonlinearOptimizerMoreOptimizationTest;
void writeLogFile(double currentError);
/// @}
protected:

118
gtsam_unstable/slam/ImplicitSchurFactor.h → gtsam/slam/ImplicitSchurFactor.h
View File

@ -24,7 +24,6 @@ class ImplicitSchurFactor: public GaussianFactor {
public:
typedef ImplicitSchurFactor This; ///< Typedef to this class
typedef JacobianFactor Base; ///< Typedef to base class
typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
protected:
@ -87,7 +86,8 @@ public:
}
/// print
void print(const std::string& s, const KeyFormatter& formatter) const {
void print(const std::string& s = "",
const KeyFormatter& keyFormatter = DefaultKeyFormatter) const {
std::cout << " ImplicitSchurFactor " << std::endl;
Factor::print(s);
std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl;
@ -188,19 +188,24 @@ public:
/// Return the block diagonal of the Hessian for this factor
virtual std::map<Key, Matrix> hessianBlockDiagonal() const {
std::map<Key, Matrix> blocks;
// F'*(I - E*P*E')*F
for (size_t pos = 0; pos < size(); ++pos) {
Key j = keys_[pos];
const Matrix2D& Fj = Fblocks_[pos].second;
// F'*F - F'*E*P*E'*F (9*2)*(2*9) - (9*2)*(2*3)*(3*3)*(3*2)*(2*9)
Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
* E_.block<2, 3>(2 * pos, 0);
blocks[j] = Fj.transpose() * Fj
- FtE * PointCovariance_ * FtE.transpose();
const Matrix2D& Fj = Fblocks_[pos].second;
// Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
// * E_.block<2, 3>(2 * pos, 0);
// blocks[j] = Fj.transpose() * Fj
// - FtE * PointCovariance_ * FtE.transpose();
const Matrix23& Ej = E_.block<2, 3>(2 * pos, 0);
blocks[j] = Fj.transpose() * (Fj - Ej * PointCovariance_ * Ej.transpose() * Fj);
// F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
// static const Eigen::Matrix<double, 2, 2> I2 = eye(2);
// Eigen::Matrix<double, 2, 2> Q = //
// I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose();
// blocks[j] = Fj.transpose() * Q * Fj;
// static const Eigen::Matrix<double, 2, 2> I2 = eye(2);
// Eigen::Matrix<double, 2, 2> Q = //
// I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose();
// blocks[j] = Fj.transpose() * Q * Fj;
}
return blocks;
}
@ -235,25 +240,33 @@ public:
typedef std::vector<Vector2> Error2s;
/**
* @brief Calculate corrected error Q*e = (I - E*P*E')*e
* @brief Calculate corrected error Q*(e-2*b) = (I - E*P*E')*(e-2*b)
*/
void projectError(const Error2s& e1, Error2s& e2) const {
void projectError2(const Error2s& e1, Error2s& e2) const {
// d1 = E.transpose() * e1 = (3*2m)*2m
// d1 = E.transpose() * (e1-2*b) = (3*2m)*2m
Vector3 d1;
d1.setZero();
for (size_t k = 0; k < size(); k++)
d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * e1[k];
d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * (e1[k] - 2 * b_.segment < 2 > (k * 2));
// d2 = E.transpose() * e1 = (3*2m)*2m
Vector3 d2 = PointCovariance_ * d1;
// e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
for (size_t k = 0; k < size(); k++)
e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2;
e2[k] = e1[k] - 2 * b_.segment < 2 > (k * 2) - E_.block < 2, 3 > (2 * k, 0) * d2;
}
/// needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
/*
* This definition matches the linearized error in the Hessian Factor:
* LinError(x) = x'*H*x - 2*x'*eta + f
* with:
* H = F' * (I-E'*P*E) * F = F' * Q * F
* eta = F' * (I-E'*P*E) * b = F' * Q * b
* f = nonlinear error
* (x'*H*x - 2*x'*eta + f) = x'*F'*Q*F*x - 2*x'*F'*Q *b + f = x'*F'*Q*(F*x - 2*b) + f
*/
virtual double error(const VectorValues& x) const {
// resize does not do malloc if correct size
@ -262,15 +275,56 @@ public:
// e1 = F * x - b = (2m*dm)*dm
for (size_t k = 0; k < size(); ++k)
e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2);
projectError(e1, e2);
e1[k] = Fblocks_[k].second * x.at(keys_[k]);
projectError2(e1, e2);
double result = 0;
for (size_t k = 0; k < size(); ++k)
result += dot(e2[k], e2[k]);
return 0.5 * result;
result += dot(e1[k], e2[k]);
double f = b_.squaredNorm();
return 0.5 * (result + f);
}
/// needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
// This is wrong and does not match the definition in Hessian
// virtual double error(const VectorValues& x) const {
//
// // resize does not do malloc if correct size
// e1.resize(size());
// e2.resize(size());
//
// // e1 = F * x - b = (2m*dm)*dm
// for (size_t k = 0; k < size(); ++k)
// e1[k] = Fblocks_[k].second * x.at(keys_[k]) - b_.segment < 2 > (k * 2);
// projectError(e1, e2);
//
// double result = 0;
// for (size_t k = 0; k < size(); ++k)
// result += dot(e2[k], e2[k]);
//
// std::cout << "implicitFactor::error result " << result << std::endl;
// return 0.5 * result;
// }
/**
* @brief Calculate corrected error Q*e = (I - E*P*E')*e
*/
void projectError(const Error2s& e1, Error2s& e2) const {
// d1 = E.transpose() * e1 = (3*2m)*2m
Vector3 d1;
d1.setZero();
for (size_t k = 0; k < size(); k++)
d1 += E_.block < 2, 3 > (2 * k, 0).transpose() * e1[k];
// d2 = E.transpose() * e1 = (3*2m)*2m
Vector3 d2 = PointCovariance_ * d1;
// e3 = alpha*(e1 - E*d2) = 1*[2m-(2m*3)*3]
for (size_t k = 0; k < size(); k++)
e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2;
}
/// Scratch space for multiplyHessianAdd
mutable Error2s e1, e2;
@ -377,6 +431,28 @@ public:
return g;
}
/**
* Calculate gradient, which is -F'Q*b, see paper - RAW MEMORY ACCESS
*/
void gradientAtZero(double* d) const {
// Use eigen magic to access raw memory
typedef Eigen::Matrix<double, D, 1> DVector;
typedef Eigen::Map<DVector> DMap;
// calculate Q*b
e1.resize(size());
e2.resize(size());
for (size_t k = 0; k < size(); k++)
e1[k] = b_.segment < 2 > (2 * k);
projectError(e1, e2);
for (size_t k = 0; k < size(); ++k) { // for each camera in the factor
Key j = keys_[k];
DMap(d + D * j) += -Fblocks_[k].second.transpose() * e2[k];
}
}
};
// ImplicitSchurFactor

13
gtsam_unstable/slam/JacobianFactorQ.h → gtsam/slam/JacobianFactorQ.h
View File

@ -23,6 +23,19 @@ public:
JacobianFactorQ() {
}
/// Empty constructor with keys
JacobianFactorQ(const FastVector<Key>& keys,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
Matrix zeroMatrix = Matrix::Zero(0,D);
Vector zeroVector = Vector::Zero(0);
typedef std::pair<Key, Matrix> KeyMatrix;
std::vector<KeyMatrix> QF;
QF.reserve(keys.size());
BOOST_FOREACH(const Key& key, keys)
QF.push_back(KeyMatrix(key, zeroMatrix));
JacobianFactor::fillTerms(QF, zeroVector, model);
}
/// Constructor
JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b,

2
gtsam_unstable/slam/JacobianFactorQR.h → gtsam/slam/JacobianFactorQR.h
View File

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

16
gtsam_unstable/slam/JacobianFactorSVD.h → gtsam/slam/JacobianFactorSVD.h
View File

@ -5,7 +5,7 @@
*/
#pragma once
#include "gtsam_unstable/slam/JacobianSchurFactor.h"
#include "gtsam/slam/JacobianSchurFactor.h"
namespace gtsam {
/**
@ -18,10 +18,23 @@ public:
typedef Eigen::Matrix<double, 2, D> Matrix2D;
typedef std::pair<Key, Matrix2D> KeyMatrix2D;
typedef std::pair<Key, Matrix> KeyMatrix;
/// Default constructor
JacobianFactorSVD() {}
/// Empty constructor with keys
JacobianFactorSVD(const FastVector<Key>& keys,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
Matrix zeroMatrix = Matrix::Zero(0,D);
Vector zeroVector = Vector::Zero(0);
std::vector<KeyMatrix> QF;
QF.reserve(keys.size());
BOOST_FOREACH(const Key& key, keys)
QF.push_back(KeyMatrix(key, zeroMatrix));
JacobianFactor::fillTerms(QF, zeroVector, model);
}
/// Constructor
JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
@ -32,7 +45,6 @@ public:
// BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
// QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
// JacobianFactor factor(QF, Q * b);
typedef std::pair<Key, Matrix> KeyMatrix;
std::vector<KeyMatrix> QF;
QF.reserve(numKeys);
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)

0
gtsam_unstable/slam/JacobianSchurFactor.h → gtsam/slam/JacobianSchurFactor.h
View File

0
gtsam_unstable/slam/RegularHessianFactor.h → gtsam/slam/RegularHessianFactor.h
View File

250
gtsam_unstable/slam/SmartFactorBase.h → gtsam/slam/SmartFactorBase.h
View File

@ -20,6 +20,7 @@
#pragma once
#include "JacobianFactorQ.h"
#include "JacobianFactorSVD.h"
#include "ImplicitSchurFactor.h"
#include "RegularHessianFactor.h"
@ -135,12 +136,12 @@ public:
}
/** return the measurements */
const Vector& measured() const {
const std::vector<Point2>& measured() const {
return measured_;
}
/** return the noise model */
const SharedNoiseModel& noise() const {
const std::vector<SharedNoiseModel>& noise() const {
return noise_;
}
@ -192,7 +193,7 @@ public:
b[2 * i + 1] = e.y();
} catch (CheiralityException& e) {
std::cout << "Cheirality exception " << std::endl;
exit (EXIT_FAILURE);
exit(EXIT_FAILURE);
}
i += 1;
}
@ -222,7 +223,7 @@ public:
* this->noise_.at(i)->distance(reprojectionError.vector());
} catch (CheiralityException&) {
std::cout << "Cheirality exception " << std::endl;
exit (EXIT_FAILURE);
exit(EXIT_FAILURE);
}
i += 1;
}
@ -244,7 +245,7 @@ public:
cameras[i].project(point, boost::none, Ei);
} catch (CheiralityException& e) {
std::cout << "Cheirality exception " << std::endl;
exit (EXIT_FAILURE);
exit(EXIT_FAILURE);
}
this->noise_.at(i)->WhitenSystem(Ei, b);
E.block<2, 3>(2 * i, 0) = Ei;
@ -274,7 +275,7 @@ public:
-(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
} catch (CheiralityException&) {
std::cout << "Cheirality exception " << std::endl;
exit (EXIT_FAILURE);
exit(EXIT_FAILURE);
}
this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi);
@ -302,15 +303,18 @@ public:
// Point covariance inv(E'*E)
Matrix3 EtE = E.transpose() * E;
Matrix3 DMatrix = eye(E.cols()); // damping matrix
if (diagonalDamping) { // diagonal of the hessian
DMatrix(0, 0) = EtE(0, 0);
DMatrix(1, 1) = EtE(1, 1);
DMatrix(2, 2) = EtE(2, 2);
EtE(0, 0) += lambda * EtE(0, 0);
EtE(1, 1) += lambda * EtE(1, 1);
EtE(2, 2) += lambda * EtE(2, 2);
}else{
EtE(0, 0) += lambda;
EtE(1, 1) += lambda;
EtE(2, 2) += lambda;
}
PointCov.noalias() = (EtE + lambda * DMatrix).inverse();
PointCov.noalias() = (EtE).inverse();
return f;
}
@ -322,7 +326,7 @@ public:
const double lambda = 0.0) const {
size_t numKeys = this->keys_.size();
std::vector < KeyMatrix2D > Fblocks;
std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
lambda);
F = zeros(2 * numKeys, D * numKeys);
@ -345,10 +349,10 @@ public:
diagonalDamping); // diagonalDamping should have no effect (only on PointCov)
// Do SVD on A
Eigen::JacobiSVD < Matrix > svd(E, Eigen::ComputeFullU);
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;
@ -361,7 +365,7 @@ public:
const Cameras& cameras, const Point3& point) const {
int numKeys = this->keys_.size();
std::vector < KeyMatrix2D > Fblocks;
std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
F.resize(2 * numKeys, D * numKeys);
F.setZero();
@ -380,14 +384,14 @@ public:
int numKeys = this->keys_.size();
std::vector < KeyMatrix2D > Fblocks;
std::vector<KeyMatrix2D> Fblocks;
Matrix E;
Matrix3 PointCov;
Vector b;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping);
//#define HESSIAN_BLOCKS
//#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix
#ifdef HESSIAN_BLOCKS
// Create structures for Hessian Factors
std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
@ -400,46 +404,23 @@ public:
//std::vector < Vector > gs2(gs.begin(), gs.end());
return boost::make_shared < RegularHessianFactor<D>
> (this->keys_, Gs, gs, f);
#else
> (this->keys_, Gs, gs, f);
#else // we create directly a SymmetricBlockMatrix
size_t n1 = D * numKeys + 1;
std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, D);
dims.back() = 1;
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1)
SymmetricBlockMatrix augmentedHessian(dims, Matrix::Zero(n1, n1)); // for 10 cameras, size should be (10*D+1 x 10*D+1)
sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
augmentedHessian(numKeys,numKeys)(0,0) = f;
return boost::make_shared<RegularHessianFactor<D> >(
this->keys_, augmentedHessian);
augmentedHessian(numKeys, numKeys)(0, 0) = f;
return boost::make_shared<RegularHessianFactor<D> >(this->keys_,
augmentedHessian);
#endif
}
// ****************************************************************************************************
boost::shared_ptr<RegularHessianFactor<D> > updateAugmentedHessian(
const Cameras& cameras, const Point3& point, const double lambda,
bool diagonalDamping, SymmetricBlockMatrix& augmentedHessian) const {
int numKeys = this->keys_.size();
std::vector < KeyMatrix2D > Fblocks;
Matrix E;
Matrix3 PointCov;
Vector b;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping);
std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, D);
dims.back() = 1;
updateSparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
std::cout << "f "<< f <<std::endl;
augmentedHessian(numKeys,numKeys)(0,0) += f;
}
// ****************************************************************************************************
// slow version - works on full (sparse) matrices
void schurComplement(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix& PointCov, const Vector& b,
/*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
@ -466,7 +447,7 @@ public:
int GsCount2 = 0;
for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera
DenseIndex i1D = i1 * D;
gs.at(i1) = gs_vector.segment < D > (i1D);
gs.at(i1) = gs_vector.segment<D>(i1D);
for (DenseIndex i2 = 0; i2 < numKeys; i2++) {
if (i2 >= i1) {
Gs.at(GsCount2) = H.block<D, D>(i1D, i2 * D);
@ -476,53 +457,6 @@ public:
}
}
// ****************************************************************************************************
void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
// Schur complement trick
// Gs = F' * F - F' * E * P * E' * F
// gs = F' * (b - E * P * E' * b)
// a single point is observed in numKeys cameras
size_t numKeys = this->keys_.size(); // cameras observing current point
size_t aug_numKeys = augmentedHessian.rows() - 1; // all cameras in the group
// Blockwise Schur complement
for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera
const Matrix2D& Fi1 = Fblocks.at(i1).second;
const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P;
// D = (Dx2) * (2)
// (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
size_t aug_i1 = this->keys_[i1];
std::cout << "i1 "<< i1 <<std::endl;
std::cout << "aug_i1 "<< aug_i1 <<std::endl;
std::cout << "aug_numKeys "<< aug_numKeys <<std::endl;
augmentedHessian(aug_i1,aug_numKeys) = //augmentedHessian(aug_i1,aug_numKeys) +
Fi1.transpose() * b.segment < 2 > (2 * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
std::cout << "filled 1 " <<std::endl;
augmentedHessian(aug_i1,aug_i1) = //augmentedHessian(aug_i1,aug_i1) +
Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
// upper triangular part of the hessian
for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera
const Matrix2D& Fi2 = Fblocks.at(i2).second;
size_t aug_i2 = this->keys_[i2];
std::cout << "i2 "<< i2 <<std::endl;
std::cout << "aug_i2 "<< aug_i2 <<std::endl;
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian(aug_i1, aug_i2) = //augmentedHessian(aug_i1, aug_i2)
- Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
}
} // end of for over cameras
}
// ****************************************************************************************************
void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
@ -542,20 +476,20 @@ public:
// D = (Dx2) * (2)
// (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
augmentedHessian(i1,numKeys) = Fi1.transpose() * b.segment < 2 > (2 * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
augmentedHessian(i1, numKeys) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
augmentedHessian(i1,i1) =
Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
augmentedHessian(i1, i1) = Fi1.transpose()
* (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
// upper triangular part of the hessian
for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera
for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
const Matrix2D& Fi2 = Fblocks.at(i2).second;
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian(i1,i2) =
-Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
augmentedHessian(i1, i2) = -Fi1.transpose()
* (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
}
} // end of for over cameras
}
@ -586,24 +520,109 @@ public:
{ // for i1 = i2
// D = (Dx2) * (2)
gs.at(i1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b
// D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
gs.at(i1) -= Fi1.transpose() * (Ei1_P * (E.transpose() * b));
gs.at(i1) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b
-Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
Gs.at(GsIndex) = Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
Gs.at(GsIndex) = Fi1.transpose()
* (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
GsIndex++;
}
// upper triangular part of the hessian
for (size_t i2 = i1+1; i2 < numKeys; i2++) { // for each camera
for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
const Matrix2D& Fi2 = Fblocks.at(i2).second;
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
Gs.at(GsIndex) = -Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
Gs.at(GsIndex) = -Fi1.transpose()
* (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
GsIndex++;
}
} // end of for over cameras
}
// ****************************************************************************************************
void updateAugmentedHessian(const Cameras& cameras, const Point3& point,
const double lambda, bool diagonalDamping,
SymmetricBlockMatrix& augmentedHessian,
const FastVector<Key> allKeys) const {
// int numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks;
Matrix E;
Matrix3 PointCov;
Vector b;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping);
updateSparseSchurComplement(Fblocks, E, PointCov, b, f, allKeys, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
}
// ****************************************************************************************************
void updateSparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
const double f, const FastVector<Key> allKeys,
/*output ->*/SymmetricBlockMatrix& augmentedHessian) const {
// Schur complement trick
// Gs = F' * F - F' * E * P * E' * F
// gs = F' * (b - E * P * E' * b)
MatrixDD matrixBlock;
typedef SymmetricBlockMatrix::Block Block; ///< A block from the Hessian matrix
FastMap<Key,size_t> KeySlotMap;
for (size_t slot=0; slot < allKeys.size(); slot++)
KeySlotMap.insert(std::make_pair<Key,size_t>(allKeys[slot],slot));
// a single point is observed in numKeys cameras
size_t numKeys = this->keys_.size(); // cameras observing current point
size_t aug_numKeys = (augmentedHessian.rows() - 1) / D; // all cameras in the group
// Blockwise Schur complement
for (size_t i1 = 0; i1 < numKeys; i1++) { // for each camera in the current factor
const Matrix2D& Fi1 = Fblocks.at(i1).second;
const Matrix23 Ei1_P = E.block<2, 3>(2 * i1, 0) * P;
// D = (Dx2) * (2)
// allKeys are the list of all camera keys in the group, e.g, (1,3,4,5,7)
// we should map those to a slot in the local (grouped) hessian (0,1,2,3,4)
// Key cameraKey_i1 = this->keys_[i1];
DenseIndex aug_i1 = KeySlotMap[this->keys_[i1]];
// information vector - store previous vector
// vectorBlock = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i1, aug_numKeys) = augmentedHessian(aug_i1, aug_numKeys).knownOffDiagonal()
+ Fi1.transpose() * b.segment<2>(2 * i1) // F' * b
- Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
// main block diagonal - store previous block
matrixBlock = augmentedHessian(aug_i1, aug_i1);
// add contribution of current factor
augmentedHessian(aug_i1, aug_i1) = matrixBlock +
( Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1) );
// upper triangular part of the hessian
for (size_t i2 = i1 + 1; i2 < numKeys; i2++) { // for each camera
const Matrix2D& Fi2 = Fblocks.at(i2).second;
//Key cameraKey_i2 = this->keys_[i2];
DenseIndex aug_i2 = KeySlotMap[this->keys_[i2]];
// (DxD) = (Dx2) * ( (2x2) * (2xD) )
// off diagonal block - store previous block
// matrixBlock = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal();
// add contribution of current factor
augmentedHessian(aug_i1, aug_i2) = augmentedHessian(aug_i1, aug_i2).knownOffDiagonal()
- Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
}
} // end of for over cameras
augmentedHessian(aug_numKeys, aug_numKeys)(0, 0) += f;
}
// ****************************************************************************************************
boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0,
@ -620,13 +639,24 @@ public:
boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const {
std::vector < KeyMatrix2D > Fblocks;
std::vector<KeyMatrix2D> Fblocks;
Matrix E;
Matrix3 PointCov;
Vector b;
computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping);
return boost::make_shared < JacobianFactorQ<D> > (Fblocks, E, PointCov, b);
return boost::make_shared<JacobianFactorQ<D> >(Fblocks, E, PointCov, b);
}
// ****************************************************************************************************
boost::shared_ptr<JacobianFactor> createJacobianSVDFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0) const {
size_t numKeys = this->keys_.size();
std::vector < KeyMatrix2D > Fblocks;
Vector b;
Matrix Enull(2*numKeys, 2*numKeys-3);
computeJacobiansSVD(Fblocks, Enull, b, cameras, point, lambda);
return boost::make_shared< JacobianFactorSVD<6> >(Fblocks, Enull, b);
}
private:

61
gtsam_unstable/slam/SmartProjectionFactor.h → gtsam/slam/SmartProjectionFactor.h
View File

@ -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,6 +53,10 @@ public:
double f;
};
enum LinearizationMode {
HESSIAN, JACOBIAN_SVD, JACOBIAN_Q
};
/**
* SmartProjectionFactor: triangulates point
* TODO: why LANDMARK parameter?
@ -91,6 +94,13 @@ protected:
/// shorthand for base class type
typedef SmartFactorBase<POSE, CALIBRATION, D> Base;
double landmarkDistanceThreshold_; // if the landmark is triangulated at a
// distance larger than that the factor is considered degenerate
double dynamicOutlierRejectionThreshold_; // if this is nonnegative the factor will check if the
// average reprojection error is smaller than this threshold after triangulation,
// and the factor is disregarded if the error is large
/// shorthand for this class
typedef SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This;
@ -115,12 +125,15 @@ public:
SmartProjectionFactor(const double rankTol, const double linThreshold,
const bool manageDegeneracy, const bool enableEPI,
boost::optional<POSE> body_P_sensor = boost::none,
SmartFactorStatePtr state = SmartFactorStatePtr(
new SmartProjectionFactorState())) :
double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1,
SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) :
Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_(
1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
false), verboseCheirality_(false), state_(state) {
false), verboseCheirality_(false), state_(state),
landmarkDistanceThreshold_(landmarkDistanceThreshold),
dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) {
}
/** Virtual destructor */
@ -234,6 +247,31 @@ public:
rankTolerance_, enableEPI_);
degenerate_ = false;
cheiralityException_ = false;
// Check landmark distance and reprojection errors to avoid outliers
double totalReprojError = 0.0;
size_t i=0;
BOOST_FOREACH(const Camera& camera, cameras) {
Point3 cameraTranslation = camera.pose().translation();
// we discard smart factors corresponding to points that are far away
if(cameraTranslation.distance(point_) > landmarkDistanceThreshold_){
degenerate_ = true;
break;
}
const Point2& zi = this->measured_.at(i);
try {
Point2 reprojectionError(camera.project(point_) - zi);
totalReprojError += reprojectionError.vector().norm();
} catch (CheiralityException) {
cheiralityException_ = true;
}
i += 1;
}
// we discard smart factors that have large reprojection error
if(dynamicOutlierRejectionThreshold_ > 0 &&
totalReprojError/m > dynamicOutlierRejectionThreshold_)
degenerate_ = true;
} catch (TriangulationUnderconstrainedException&) {
// if TriangulationUnderconstrainedException can be
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before
@ -385,7 +423,7 @@ public:
if (triangulateForLinearize(cameras))
return Base::createJacobianQFactor(cameras, point_, lambda);
else
return boost::shared_ptr<JacobianFactorQ<D> >();
return boost::make_shared< JacobianFactorQ<D> >(this->keys_);
}
/// Create a factor, takes values
@ -397,7 +435,16 @@ public:
if (nonDegenerate)
return createJacobianQFactor(myCameras, lambda);
else
return boost::shared_ptr<JacobianFactorQ<D> >();
return boost::make_shared< JacobianFactorQ<D> >(this->keys_);
}
/// different (faster) way to compute Jacobian factor
boost::shared_ptr< JacobianFactor > createJacobianSVDFactor(const Cameras& cameras,
double lambda) const {
if (triangulateForLinearize(cameras))
return Base::createJacobianSVDFactor(cameras, point_, lambda);
else
return boost::make_shared< JacobianFactorSVD<D> >(this->keys_);
}
/// Returns true if nonDegenerate

60
gtsam_unstable/slam/SmartProjectionPoseFactor.h → gtsam/slam/SmartProjectionPoseFactor.h
View File

@ -22,6 +22,16 @@
#include "SmartProjectionFactor.h"
namespace gtsam {
/**
*
* @addtogroup SLAM
*
* If you are using the factor, please cite:
* L. Carlone, Z. Kira, C. Beall, V. Indelman, F. Dellaert, Eliminating conditionally
* independent sets in factor graphs: a unifying perspective based on smart factors,
* Int. Conf. on Robotics and Automation (ICRA), 2014.
*
*/
/**
* The calibration is known here. The factor only constraints poses (variable dimension is 6)
@ -31,7 +41,8 @@ template<class POSE, class LANDMARK, class CALIBRATION>
class SmartProjectionPoseFactor: public SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> {
protected:
// Known calibration
LinearizationMode linearizeTo_; ///< How to linearize the factor (HESSIAN, JACOBIAN_SVD, JACOBIAN_Q)
std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
public:
@ -56,8 +67,11 @@ 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) :
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor) {}
const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none,
LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}
/** Virtual destructor */
virtual ~SmartProjectionPoseFactor() {}
@ -65,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
*/
@ -77,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,
@ -90,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) {
@ -126,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;
@ -139,11 +164,24 @@ public:
}
/**
* linearize returns a Hessian factor contraining 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 {
return this->createHessianFactor(cameras(values));
// depending on flag set on construction we may linearize to different linear factors
switch(linearizeTo_){
case JACOBIAN_SVD :
return this->createJacobianSVDFactor(cameras(values), 0.0);
break;
case JACOBIAN_Q :
return this->createJacobianQFactor(cameras(values), 0.0);
break;
default:
return this->createHessianFactor(cameras(values));
break;
}
}
/**
@ -158,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_;
}

652
gtsam/slam/dataset.cpp
View File

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

110
gtsam/slam/dataset.h
View File

@ -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(
std::pair<std::string, boost::optional<noiseModel::Diagonal::shared_ptr> > dataset,
int maxID = 0, bool addNoise = false, bool smart = true);
GTSAM_EXPORT GraphAndValues load2D(
std::pair<std::string, SharedNoiseModel> dataset, int maxID = 0,
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(
const std::string& filename,
boost::optional<gtsam::SharedDiagonal> model = boost::optional<
noiseModel::Diagonal::shared_ptr>(), int maxID = 0, bool addNoise = false,
bool smart = true);
GTSAM_EXPORT GraphAndValues load2D(const std::string& filename,
SharedNoiseModel model = SharedNoiseModel(), int maxID = 0, bool addNoise =
false, bool smart = true, NoiseFormat noiseFormat = NoiseFormatGRAPH, //
KernelFunctionType kernelFunctionType = KernelFunctionTypeNONE);
GTSAM_EXPORT std::pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D_robust(
const std::string& filename,
gtsam::noiseModel::Base::shared_ptr& model, int maxID = 0);
/// @deprecated load2D now allows for arbitrary models and wrapping a robust kernel
GTSAM_EXPORT GraphAndValues load2D_robust(const std::string& filename,
noiseModel::Base::shared_ptr& model, int maxID = 0);
/** save 2d graph */
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph, const Values& config,
const noiseModel::Diagonal::shared_ptr model, const std::string& filename);
GTSAM_EXPORT void save2D(const NonlinearFactorGraph& graph,
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
{
gtsam::Point3 p; ///< 3D position of the point
float r,g,b; ///< RGB color of the 3D point
struct SfM_Track {
Point3 p; ///< 3D position of the 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
{
std::vector<SfM_Camera> cameras; ///< Set of cameras
struct SfM_data {
std::vector<SfM_Camera> cameras; ///< Set of cameras
std::vector<SfM_Track> tracks; ///< Sparse set of points
size_t number_cameras() const { return cameras.size();} ///< The number of camera poses
size_t number_tracks() const { return tracks.size();} ///< The number of reconstructed 3D points
size_t number_cameras() const {
return cameras.size();
} ///< The number of camera poses
size_t number_tracks() const {
return tracks.size();
} ///< The number of reconstructed 3D points
};
/**
@ -146,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
@ -189,5 +242,4 @@ GTSAM_EXPORT Values initialCamerasEstimate(const SfM_data& db);
*/
GTSAM_EXPORT Values initialCamerasAndPointsEstimate(const SfM_data& db);
} // namespace gtsam

399
gtsam/slam/lago.cpp Normal file
View File

@ -0,0 +1,399 @@
/* ----------------------------------------------------------------------------
* 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
* @author Luca Carlone
* @author Frank Dellaert
* @date May 14, 2014
*/
#include <gtsam/slam/lago.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>
using namespace std;
namespace gtsam {
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));
/* ************************************************************************* */
/**
* 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) {
// We check 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()) {
nodeTheta += thetaFromRootMap.at(key_parent);
break;
}
key_child = key_parent; // we move upwards in the tree
}
return nodeTheta;
}
/* ************************************************************************* */
key2doubleMap computeThetasToRoot(const key2doubleMap& deltaThetaMap,
const PredecessorMap<Key>& tree) {
key2doubleMap thetaToRootMap;
// Orientation of the roo
thetaToRootMap.insert(pair<Key, double>(keyAnchor, 0.0));
// for all nodes in the tree
BOOST_FOREACH(const key2doubleMap::value_type& it, deltaThetaMap) {
// compute the orientation wrt root
Key nodeKey = it.first;
double nodeTheta = computeThetaToRoot(nodeKey, tree, deltaThetaMap,
thetaToRootMap);
thetaToRootMap.insert(pair<Key, double>(nodeKey, nodeTheta));
}
return thetaToRootMap;
}
/* ************************************************************************* */
void getSymbolicGraph(
/*OUTPUTS*/vector<size_t>& spanningTreeIds, vector<size_t>& chordsIds,
key2doubleMap& deltaThetaMap,
/*INPUTS*/const PredecessorMap<Key>& tree, const NonlinearFactorGraph& g) {
// Get keys for which you want the orientation
size_t id = 0;
// Loop over the factors
BOOST_FOREACH(const boost::shared_ptr<NonlinearFactor>& factor, g) {
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::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
if (!pose2Between)
continue;
// get the orientation - measured().theta();
double deltaTheta = pose2Between->measured().theta();
// insert (directed) orientations in the map "deltaThetaMap"
bool inTree = false;
if (tree.at(key1) == key2) { // key2 -> key1
deltaThetaMap.insert(pair<Key, double>(key1, -deltaTheta));
inTree = true;
} else if (tree.at(key2) == key1) { // key1 -> key2
deltaThetaMap.insert(pair<Key, double>(key2, deltaTheta));
inTree = true;
}
// store factor slot, distinguishing spanning tree edges from chordsIds
if (inTree == true)
spanningTreeIds.push_back(id);
else
// it's a chord!
chordsIds.push_back(id);
}
id++;
}
}
/* ************************************************************************* */
// 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 invalid_argument(
"buildLinearOrientationGraph: invalid between factor!");
deltaTheta = (Vector(1) << pose2Between->measured().theta());
// Retrieve the noise model for the relative rotation
SharedNoiseModel model = pose2Between->get_noiseModel();
boost::shared_ptr<noiseModel::Diagonal> diagonalModel =
boost::dynamic_pointer_cast<noiseModel::Diagonal>(model);
if (!diagonalModel)
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
model_deltaTheta = noiseModel::Diagonal::Sigmas(std_deltaTheta);
}
/* ************************************************************************* */
GaussianFactorGraph buildLinearOrientationGraph(
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) {
const FastVector<Key>& keys = g[factorId]->keys();
Key key1 = keys[0], key2 = keys[1];
getDeltaThetaAndNoise(g[factorId], 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) {
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);
///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 k = boost::math::round(k2pi_noise / (2 * M_PI));
//if (k2pi_noise - 2*k*M_PI > 1e-5) cout << k2pi_noise - 2*k*M_PI << endl; // for debug
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(keyAnchor, I, (Vector(1) << 0.0), priorOrientationNoise);
return lagoGraph;
}
/* ************************************************************************* */
// 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) {
// recast to a between on Pose2
boost::shared_ptr<BetweenFactor<Pose2> > pose2Between =
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::dynamic_pointer_cast<PriorFactor<Pose2> >(factor);
if (pose2Prior)
pose2Graph.add(
BetweenFactor<Pose2>(keyAnchor, pose2Prior->keys()[0],
pose2Prior->prior(), pose2Prior->get_noiseModel()));
}
return 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) {
Key key1 = std::min(factor->keys()[0], factor->keys()[1]);
Key key2 = std::max(factor->keys()[0], factor->keys()[1]);
if (minUnassigned) {
minKey = key1;
minUnassigned = false;
}
if (key2 - key1 == 1) { // consecutive keys
tree.insert(key2, key1);
if (key1 < minKey)
minKey = key1;
}
}
tree.insert(minKey, keyAnchor);
tree.insert(keyAnchor, keyAnchor); // root
return tree;
}
/* ************************************************************************* */
// 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);
// Create a linear factor graph (LFG) of scalars
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, 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);
// Solve the LFG
VectorValues orientationsLago = lagoGraph.optimize();
return orientationsLago;
}
/* ************************************************************************* */
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 computeOrientations(pose2Graph, useOdometricPath);
}
/* ************************************************************************* */
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::shared_ptr<BetweenFactor<Pose2> > pose2Between =
boost::dynamic_pointer_cast<BetweenFactor<Pose2> >(factor);
if (pose2Between) {
Key key1 = pose2Between->keys()[0];
double theta1 = orientationsLago.at(key1)(0);
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();
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 b = (Vector(3) << globalDeltaCart, linearDeltaRot); // rhs
Matrix J1 = -I3;
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());
linearPose2graph.add(key1, J1, key2, I3, b, diagonalModel);
} else {
throw invalid_argument(
"computeLagoPoses: cannot manage non between factor here!");
}
}
// add prior
linearPose2graph.add(keyAnchor, I3, (Vector(3) << 0.0, 0.0, 0.0),
priorPose2Noise);
// optimize
VectorValues posesLago = linearPose2graph.optimize();
// put into Values structure
Values initialGuessLago;
BOOST_FOREACH(const VectorValues::value_type& it, posesLago) {
Key key = it.first;
if (key != keyAnchor) {
const Vector& poseVector = it.second;
Pose2 poseLago = Pose2(poseVector(0), poseVector(1),
orientationsLago.at(key)(0) + poseVector(2));
initialGuessLago.insert(key, poseLago);
}
}
return initialGuessLago;
}
/* ************************************************************************* */
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 = computeOrientations(pose2Graph,
useOdometricPath);
// Compute the full poses
return computePoses(pose2Graph, orientationsLago);
}
/* ************************************************************************* */
Values initialize(const NonlinearFactorGraph& graph,
const Values& initialGuess) {
Values initialGuessLago;
// get the orientation estimates from LAGO
VectorValues orientations = initializeOrientations(graph);
// for all nodes in the tree
BOOST_FOREACH(const VectorValues::value_type& it, orientations) {
Key key = it.first;
if (key != keyAnchor) {
const Pose2& pose = initialGuess.at<Pose2>(key);
const Vector& orientation = it.second;
Pose2 poseLago = Pose2(pose.x(), pose.y(), orientation(0));
initialGuessLago.insert(key, poseLago);
}
}
return initialGuessLago;
}
} // end of namespace lago
} // end of namespace gtsam

86
gtsam/slam/lago.h Normal file
View File

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

140
gtsam/slam/tests/testDataset.cpp
View File

@ -19,10 +19,14 @@
#include <boost/algorithm/string.hpp>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/dataset.h>
using namespace gtsam::symbol_shorthand;
using namespace std;
using namespace gtsam;
@ -35,6 +39,23 @@ TEST(dataSet, findExampleDataFile) {
EXPECT(assert_equal(expected_end, actual_end));
}
/* ************************************************************************* */
TEST( dataSet, load2D)
{
///< The structure where we will save the SfM data
const string filename = findExampleDataFile("w100.graph");
NonlinearFactorGraph::shared_ptr graph;
Values::shared_ptr initial;
boost::tie(graph, initial) = load2D(filename);
EXPECT_LONGS_EQUAL(300,graph->size());
EXPECT_LONGS_EQUAL(100,initial->size());
noiseModel::Unit::shared_ptr model = noiseModel::Unit::Create(3);
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)
{
@ -56,6 +77,117 @@ TEST( dataSet, Balbianello)
EXPECT(assert_equal(expected,actual,1));
}
/* ************************************************************************* */
TEST( dataSet, readG2o)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(g2oFile);
Values expectedValues;
expectedValues.insert(0, Pose2(0.000000, 0.000000, 0.000000));
expectedValues.insert(1, Pose2(1.030390, 0.011350, -0.081596));
expectedValues.insert(2, Pose2(2.036137, -0.129733, -0.301887));
expectedValues.insert(3, Pose2(3.015097, -0.442395, -0.345514));
expectedValues.insert(4, Pose2(3.343949, 0.506678, 1.214715));
expectedValues.insert(5, Pose2(3.684491, 1.464049, 1.183785));
expectedValues.insert(6, Pose2(4.064626, 2.414783, 1.176333));
expectedValues.insert(7, Pose2(4.429778, 3.300180, 1.259169));
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));
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Precisions((Vector(3) << 44.721360, 44.721360, 30.901699));
NonlinearFactorGraph expectedGraph;
expectedGraph.add(BetweenFactor<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model));
expectedGraph.add(BetweenFactor<Pose2>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model));
expectedGraph.add(BetweenFactor<Pose2>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model));
expectedGraph.add(BetweenFactor<Pose2>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model));
expectedGraph.add(BetweenFactor<Pose2>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model));
expectedGraph.add(BetweenFactor<Pose2>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model));
expectedGraph.add(BetweenFactor<Pose2>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model));
expectedGraph.add(BetweenFactor<Pose2>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model));
expectedGraph.add(BetweenFactor<Pose2>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model));
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(expectedGraph,*actualGraph,1e-5));
}
/* ************************************************************************* */
TEST( dataSet, readG2oHuber)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
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);
NonlinearFactorGraph expectedGraph;
expectedGraph.add(BetweenFactor<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model));
expectedGraph.add(BetweenFactor<Pose2>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model));
expectedGraph.add(BetweenFactor<Pose2>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model));
expectedGraph.add(BetweenFactor<Pose2>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model));
expectedGraph.add(BetweenFactor<Pose2>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model));
expectedGraph.add(BetweenFactor<Pose2>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model));
expectedGraph.add(BetweenFactor<Pose2>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model));
expectedGraph.add(BetweenFactor<Pose2>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model));
expectedGraph.add(BetweenFactor<Pose2>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model));
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(expectedGraph,*actualGraph,1e-5));
}
/* ************************************************************************* */
TEST( dataSet, readG2oTukey)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
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);
NonlinearFactorGraph expectedGraph;
expectedGraph.add(BetweenFactor<Pose2>(0, 1, Pose2(1.030390, 0.011350, -0.081596), model));
expectedGraph.add(BetweenFactor<Pose2>(1, 2, Pose2(1.013900, -0.058639, -0.220291), model));
expectedGraph.add(BetweenFactor<Pose2>(2, 3, Pose2(1.027650, -0.007456, -0.043627), model));
expectedGraph.add(BetweenFactor<Pose2>(3, 4, Pose2(-0.012016, 1.004360, 1.560229), model));
expectedGraph.add(BetweenFactor<Pose2>(4, 5, Pose2(1.016030, 0.014565, -0.030930), model));
expectedGraph.add(BetweenFactor<Pose2>(5, 6, Pose2(1.023890, 0.006808, -0.007452), model));
expectedGraph.add(BetweenFactor<Pose2>(6, 7, Pose2(0.957734, 0.003159, 0.082836), model));
expectedGraph.add(BetweenFactor<Pose2>(7, 8, Pose2(-1.023820, -0.013668, -3.084560), model));
expectedGraph.add(BetweenFactor<Pose2>(8, 9, Pose2(1.023440, 0.013984, -0.127624), model));
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(expectedGraph,*actualGraph,1e-5));
}
/* ************************************************************************* */
TEST( dataSet, writeG2o)
{
const string g2oFile = findExampleDataFile("pose2example");
NonlinearFactorGraph::shared_ptr expectedGraph;
Values::shared_ptr expectedValues;
boost::tie(expectedGraph, expectedValues) = readG2o(g2oFile);
const string filenameToWrite = createRewrittenFileName(g2oFile);
writeG2o(*expectedGraph, *expectedValues, filenameToWrite);
NonlinearFactorGraph::shared_ptr actualGraph;
Values::shared_ptr actualValues;
boost::tie(actualGraph, actualValues) = readG2o(filenameToWrite);
EXPECT(assert_equal(*expectedValues,*actualValues,1e-5));
EXPECT(assert_equal(*expectedGraph,*actualGraph,1e-5));
}
/* ************************************************************************* */
TEST( dataSet, readBAL_Dubrovnik)
{
@ -120,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
@ -176,13 +308,13 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
value.insert(poseKey, pose);
}
for(size_t j=0; j < readData.number_tracks(); j++){ // for each point
Key pointKey = symbol('l',j);
Key pointKey = P(j);
Point3 point = poseChange.transform_from( readData.tracks[j].p );
value.insert(pointKey, point);
}
// 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
@ -208,7 +340,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
EXPECT(assert_equal(expectedPose,actualPose, 1e-7));
Point3 expectedPoint = track0.p;
Key pointKey = symbol('l',0);
Key pointKey = P(0);
Point3 actualPoint = value.at<Point3>(pointKey);
EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6));
}

326
gtsam/slam/tests/testLago.cpp Normal file
View File

@ -0,0 +1,326 @@
/* ----------------------------------------------------------------------------
* 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 testPlanarSLAMExample_lago.cpp
* @brief Unit tests for planar SLAM example using the initialization technique
* LAGO (Linear Approximation for Graph Optimization)
*
* @author Luca Carlone
* @author Frank Dellaert
* @date May 14, 2014
*/
#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 <CppUnitLite/TestHarness.h>
#include <cmath>
using namespace std;
using namespace gtsam;
using namespace boost::assign;
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 {
// We consider a small graph:
// symbolic FG
// x2 0 1
// / | \ 1 2
// / | \ 2 3
// x3 | x1 2 0
// \ | / 0 3
// \ | /
// x0
//
static Pose2 pose0 = Pose2(0.000000, 0.000000, 0.000000);
static Pose2 pose1 = Pose2(1.000000, 1.000000, 1.570796);
static Pose2 pose2 = Pose2(0.000000, 2.000000, 3.141593);
static Pose2 pose3 = Pose2(-1.000000, 1.000000, 4.712389);
NonlinearFactorGraph graph() {
NonlinearFactorGraph g;
g.add(BetweenFactor<Pose2>(x0, x1, pose0.between(pose1), model));
g.add(BetweenFactor<Pose2>(x1, x2, pose1.between(pose2), model));
g.add(BetweenFactor<Pose2>(x2, x3, pose2.between(pose3), model));
g.add(BetweenFactor<Pose2>(x2, x0, pose2.between(pose0), model));
g.add(BetweenFactor<Pose2>(x0, x3, pose0.between(pose3), model));
g.add(PriorFactor<Pose2>(x0, pose0, model));
return g;
}
}
/* *************************************************************************** */
TEST( Lago, checkSTandChords ) {
NonlinearFactorGraph g = simple::graph();
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g);
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
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)
DOUBLES_EQUAL(spanningTreeIds[2], 4, 1e-6); // factor 4 is the third in the ST(0->3)
}
/* *************************************************************************** */
TEST( Lago, orientationsOverSpanningTree ) {
NonlinearFactorGraph g = simple::graph();
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g);
// check the tree structure
EXPECT_LONGS_EQUAL(tree[x0], x0);
EXPECT_LONGS_EQUAL(tree[x1], x0);
EXPECT_LONGS_EQUAL(tree[x2], x0);
EXPECT_LONGS_EQUAL(tree[x3], x0);
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))
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
lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
lago::key2doubleMap actual;
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);
DOUBLES_EQUAL(expected[x3], actual[x3], 1e-6);
}
/* *************************************************************************** */
TEST( Lago, regularizedMeasurements ) {
NonlinearFactorGraph g = simple::graph();
PredecessorMap<Key> tree = findMinimumSpanningTree<NonlinearFactorGraph, Key,
BetweenFactor<Pose2> >(g);
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
lago::getSymbolicGraph(spanningTreeIds, chordsIds, deltaThetaMap, tree, g);
lago::key2doubleMap orientationsToRoot = lago::computeThetasToRoot(deltaThetaMap, 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));
// this is the whitened error, so we multiply by the std to unwhiten
actual = 0.1 * actual;
// Expected regularized measurements (same for the spanning tree, corrected for the chordsIds)
Vector expected = (Vector(5) << M_PI/2, M_PI, -M_PI/2, M_PI/2 - 2*M_PI , M_PI/2);
EXPECT(assert_equal(expected, actual, 1e-6));
}
/* *************************************************************************** */
TEST( Lago, smallGraphVectorValues ) {
bool useOdometricPath = false;
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), initial.at(x0), 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), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI - 2*M_PI), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
TEST( Lago, smallGraphVectorValuesSP ) {
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), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
TEST( Lago, multiplePosePriors ) {
bool useOdometricPath = false;
NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Pose2>(x1, simple::pose1, model));
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), initial.at(x0), 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), initial.at(x2), 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 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), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
TEST( Lago, multiplePoseAndRotPriors ) {
bool useOdometricPath = false;
NonlinearFactorGraph g = simple::graph();
g.add(PriorFactor<Rot2>(x1, simple::pose1.theta(), model));
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), initial.at(x0), 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), initial.at(x2), 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 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), initial.at(x0), 1e-6));
EXPECT(assert_equal((Vector(1) << 0.5 * M_PI), initial.at(x1), 1e-6));
EXPECT(assert_equal((Vector(1) << M_PI ), initial.at(x2), 1e-6));
EXPECT(assert_equal((Vector(1) << 1.5 * M_PI ), initial.at(x3), 1e-6));
}
/* *************************************************************************** */
TEST( Lago, smallGraphValues ) {
// we set the orientations in the initial guess to zero
Values initialGuess;
initialGuess.insert(x0,Pose2(simple::pose0.x(),simple::pose0.y(),0.0));
initialGuess.insert(x1,Pose2(simple::pose1.x(),simple::pose1.y(),0.0));
initialGuess.insert(x2,Pose2(simple::pose2.x(),simple::pose2.y(),0.0));
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 = lago::initialize(simple::graph(), initialGuess);
// we are in a noiseless case
Values expected;
expected.insert(x0,simple::pose0);
expected.insert(x1,simple::pose1);
expected.insert(x2,simple::pose2);
expected.insert(x3,simple::pose3);
EXPECT(assert_equal(expected, actual, 1e-6));
}
/* *************************************************************************** */
TEST( Lago, smallGraph2 ) {
// lago does not touch the Cartesian part and only fixed the orientations
Values actual = lago::initialize(simple::graph());
// we are in a noiseless case
Values expected;
expected.insert(x0,simple::pose0);
expected.insert(x1,simple::pose1);
expected.insert(x2,simple::pose2);
expected.insert(x3,simple::pose3);
EXPECT(assert_equal(expected, actual, 1e-6));
}
/* *************************************************************************** */
TEST( Lago, largeGraphNoisy_orientations ) {
string inputFile = findExampleDataFile("noisyToyGraph");
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;
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 = lago::initializeOrientations(graphWithPrior);
Values actual;
Key keyAnc = symbol('Z',9999999);
for(VectorValues::const_iterator it = actualVV.begin(); it != actualVV.end(); ++it ){
Key key = it->first;
if (key != keyAnc){
Vector orientation = actualVV.at(key);
Pose2 poseLago = Pose2(0.0,0.0,orientation(0));
actual.insert(key, poseLago);
}
}
string matlabFile = findExampleDataFile("orientationsNoisyToyGraph");
NonlinearFactorGraph::shared_ptr gmatlab;
Values::shared_ptr expected;
boost::tie(gmatlab, expected) = readG2o(matlabFile);
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));
}
}
/* *************************************************************************** */
TEST( Lago, largeGraphNoisy ) {
string inputFile = findExampleDataFile("noisyToyGraph");
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;
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 = lago::initialize(graphWithPrior);
string matlabFile = findExampleDataFile("optimizedNoisyToyGraph");
NonlinearFactorGraph::shared_ptr gmatlab;
Values::shared_ptr expected;
boost::tie(gmatlab, expected) = readG2o(matlabFile);
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));
}
}
/* ************************************************************************* */
int main() {
TestResult tr;
return TestRegistry::runAllTests(tr);
}
/* ************************************************************************* */

271
gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp → gtsam/slam/tests/testSmartProjectionPoseFactor.cpp
View File

@ -52,9 +52,9 @@ using symbol_shorthand::X;
using symbol_shorthand::L;
// tests data
Symbol x1('X', 1);
Symbol x2('X', 2);
Symbol x3('X', 3);
static Symbol x1('X', 1);
static Symbol x2('X', 2);
static Symbol x3('X', 3);
static Key poseKey1(x1);
static Point2 measurement1(323.0, 240.0);
@ -369,6 +369,271 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ){
if(isDebugTest) tictoc_print_();
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, jacobianSVD ){
std::vector<Key> views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera cam1(pose1, *K);
// create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
SimpleCamera cam2(pose2, *K);
// create third camera 1 meter above the first camera
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
SimpleCamera cam3(pose3, *K);
// three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2);
Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(3, 0, 3.0);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
// 1. Project three landmarks into three cameras and triangulate
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD));
smartFactor3->add(measurements_cam3, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
Values values;
values.insert(x1, pose1);
values.insert(x2, pose2);
values.insert(x3, pose3*noise_pose);
LevenbergMarquardtParams params;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, landmarkDistance ){
double excludeLandmarksFutherThanDist = 2;
std::vector<Key> views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera cam1(pose1, *K);
// create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
SimpleCamera cam2(pose2, *K);
// create third camera 1 meter above the first camera
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
SimpleCamera cam3(pose3, *K);
// three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2);
Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(3, 0, 3.0);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
// 1. Project three landmarks into three cameras and triangulate
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD, excludeLandmarksFutherThanDist));
smartFactor3->add(measurements_cam3, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
Values values;
values.insert(x1, pose1);
values.insert(x2, pose2);
values.insert(x3, pose3*noise_pose);
// All factors are disabled and pose should remain where it is
LevenbergMarquardtParams params;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
EXPECT(assert_equal(values.at<Pose3>(x3),result.at<Pose3>(x3)));
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ){
double excludeLandmarksFutherThanDist = 1e10;
double dynamicOutlierRejectionThreshold = 1; // max 1 pixel of average reprojection error
std::vector<Key> views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera cam1(pose1, *K);
// create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
SimpleCamera cam2(pose2, *K);
// create third camera 1 meter above the first camera
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
SimpleCamera cam3(pose3, *K);
// three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2);
Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(3, 0, 3.0);
Point3 landmark4(5, -0.5, 1);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3, measurements_cam4;
// 1. Project three landmarks into three cameras and triangulate
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4);
measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10,10); // add outlier
SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none,
JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor3->add(measurements_cam3, views, model, K);
SmartFactor::shared_ptr smartFactor4(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_SVD,
excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold));
smartFactor4->add(measurements_cam4, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(smartFactor4);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
Values values;
values.insert(x1, pose1);
values.insert(x2, pose2);
values.insert(x3, pose3);
// All factors are disabled and pose should remain where it is
LevenbergMarquardtParams params;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, jacobianQ ){
std::vector<Key> views;
views.push_back(x1);
views.push_back(x2);
views.push_back(x3);
// create first camera. Looking along X-axis, 1 meter above ground plane (x-y)
Pose3 pose1 = Pose3(Rot3::ypr(-M_PI/2, 0., -M_PI/2), gtsam::Point3(0,0,1));
SimpleCamera cam1(pose1, *K);
// create second camera 1 meter to the right of first camera
Pose3 pose2 = pose1 * Pose3(Rot3(), Point3(1,0,0));
SimpleCamera cam2(pose2, *K);
// create third camera 1 meter above the first camera
Pose3 pose3 = pose1 * Pose3(Rot3(), Point3(0,-1,0));
SimpleCamera cam3(pose3, *K);
// three landmarks ~5 meters infront of camera
Point3 landmark1(5, 0.5, 1.2);
Point3 landmark2(5, -0.5, 1.2);
Point3 landmark3(3, 0, 3.0);
vector<Point2> measurements_cam1, measurements_cam2, measurements_cam3;
// 1. Project three landmarks into three cameras and triangulate
projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1);
projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2);
projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3);
SmartFactor::shared_ptr smartFactor1(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
smartFactor1->add(measurements_cam1, views, model, K);
SmartFactor::shared_ptr smartFactor2(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
smartFactor2->add(measurements_cam2, views, model, K);
SmartFactor::shared_ptr smartFactor3(new SmartFactor(1, -1, false, false, boost::none, JACOBIAN_Q));
smartFactor3->add(measurements_cam3, views, model, K);
const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10);
NonlinearFactorGraph graph;
graph.push_back(smartFactor1);
graph.push_back(smartFactor2);
graph.push_back(smartFactor3);
graph.push_back(PriorFactor<Pose3>(x1, pose1, noisePrior));
graph.push_back(PriorFactor<Pose3>(x2, pose2, noisePrior));
// Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/10, 0., -M_PI/10), gtsam::Point3(0.5,0.1,0.3)); // noise from regular projection factor test below
Pose3 noise_pose = Pose3(Rot3::ypr(-M_PI/100, 0., -M_PI/100), gtsam::Point3(0.1,0.1,0.1)); // smaller noise
Values values;
values.insert(x1, pose1);
values.insert(x2, pose2);
values.insert(x3, pose3*noise_pose);
LevenbergMarquardtParams params;
Values result;
LevenbergMarquardtOptimizer optimizer(graph, values, params);
result = optimizer.optimize();
EXPECT(assert_equal(pose3,result.at<Pose3>(x3)));
}
/* *************************************************************************/
TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){
// cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl;

82
gtsam/slam/tests/timeLago.cpp Normal file
View File

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

121
gtsam_unstable/examples/SmartProjectionFactorExample.cpp Normal file
View File

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

338
matlab.h
View File

@ -28,130 +28,228 @@
#include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h>
#include <boost/foreach.hpp>
#include <exception>
namespace gtsam {
namespace utilities {
/// Extract all Point2 values into a single matrix [x y]
Matrix extractPoint2(const Values& values) {
size_t j=0;
Values::ConstFiltered<Point2> points = values.filter<Point2>();
Matrix result(points.size(),2);
BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& key_value, points)
result.row(j++) = key_value.value.vector();
return result;
}
/// Extract all Point3 values into a single matrix [x y z]
Matrix extractPoint3(const Values& values) {
Values::ConstFiltered<Point3> points = values.filter<Point3>();
Matrix result(points.size(),3);
size_t j=0;
BOOST_FOREACH(const Values::ConstFiltered<Point3>::KeyValuePair& key_value, points)
result.row(j++) = key_value.value.vector();
return result;
}
/// Extract all Pose2 values into a single matrix [x y theta]
Matrix extractPose2(const Values& values) {
Values::ConstFiltered<Pose2> poses = values.filter<Pose2>();
Matrix result(poses.size(),3);
size_t j=0;
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, poses)
result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta();
return result;
}
/// Extract all Pose3 values
Values allPose3s(const Values& values) {
return values.filter<Pose3>();
}
/// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z]
Matrix extractPose3(const Values& values) {
Values::ConstFiltered<Pose3> poses = values.filter<Pose3>();
Matrix result(poses.size(),12);
size_t j=0;
BOOST_FOREACH(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value, poses) {
result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0);
result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1);
result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2);
result.row(j).tail(3) = key_value.value.translation().vector();
j++;
}
return result;
}
/// Perturb all Point2 values using normally distributed noise
void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) {
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,sigma);
Sampler sampler(model, seed);
BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& key_value, values.filter<Point2>()) {
values.update(key_value.key, key_value.value.retract(sampler.sample()));
}
}
/// Perturb all Pose2 values using normally distributed noise
void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = 42u) {
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(
Vector3(sigmaT, sigmaT, sigmaR));
Sampler sampler(model, seed);
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, values.filter<Pose2>()) {
values.update(key_value.key, key_value.value.retract(sampler.sample()));
}
}
/// Perturb all Point3 values using normally distributed noise
void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) {
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma);
Sampler sampler(model, seed);
BOOST_FOREACH(const Values::ConstFiltered<Point3>::KeyValuePair& key_value, values.filter<Point3>()) {
values.update(key_value.key, key_value.value.retract(sampler.sample()));
}
}
/// Insert a number of initial point values by backprojecting
void insertBackprojections(Values& values, const SimpleCamera& camera, const Vector& J, const Matrix& Z, double depth) {
if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K");
if (Z.cols() != J.size()) throw std::invalid_argument("insertBackProjections: J and Z must have same number of entries");
for(int k=0;k<Z.cols();k++) {
Point2 p(Z(0,k),Z(1,k));
Point3 P = camera.backproject(p, depth);
values.insert(J(k), P);
}
}
/// Insert multiple projection factors for a single pose key
void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, const Vector& J, const Matrix& Z,
const SharedNoiseModel& model, const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) {
if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K");
if (Z.cols() != J.size()) throw std::invalid_argument(
"addMeasurements: J and Z must have same number of entries");
for (int k = 0; k < Z.cols(); k++) {
graph.push_back(
boost::make_shared<GenericProjectionFactor<Pose3, Point3> >
(Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor));
}
}
/// Calculate the errors of all projection factors in a graph
Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) {
// first count
size_t K = 0, k=0;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph)
if (boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(f)) ++K;
// now fill
Matrix errors(2,K);
BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) {
boost::shared_ptr<const GenericProjectionFactor<Pose3, Point3> > p = boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(f);
if (p) errors.col(k++) = p->unwhitenedError(values);
}
return errors;
}
}
namespace utilities {
// Create a KeyList from indices
FastList<Key> createKeyList(const Vector& I) {
FastList<Key> set;
for (int i = 0; i < I.size(); i++)
set.push_back(I[i]);
return set;
}
// Create a KeyList from indices using symbol
FastList<Key> createKeyList(string s, const Vector& I) {
FastList<Key> set;
char c = s[0];
for (int i = 0; i < I.size(); i++)
set.push_back(Symbol(c, I[i]));
return set;
}
// Create a KeyVector from indices
FastVector<Key> createKeyVector(const Vector& I) {
FastVector<Key> set;
for (int i = 0; i < I.size(); i++)
set.push_back(I[i]);
return set;
}
// Create a KeyVector from indices using symbol
FastVector<Key> createKeyVector(string s, const Vector& I) {
FastVector<Key> set;
char c = s[0];
for (int i = 0; i < I.size(); i++)
set.push_back(Symbol(c, I[i]));
return set;
}
// Create a KeySet from indices
FastSet<Key> createKeySet(const Vector& I) {
FastSet<Key> set;
for (int i = 0; i < I.size(); i++)
set.insert(I[i]);
return set;
}
// Create a KeySet from indices using symbol
FastSet<Key> createKeySet(string s, const Vector& I) {
FastSet<Key> set;
char c = s[0];
for (int i = 0; i < I.size(); i++)
set.insert(symbol(c, I[i]));
return set;
}
/// Extract all Point2 values into a single matrix [x y]
Matrix extractPoint2(const Values& values) {
size_t j = 0;
Values::ConstFiltered<Point2> points = values.filter<Point2>();
Matrix result(points.size(), 2);
BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& key_value, points)
result.row(j++) = key_value.value.vector();
return result;
}
/// Extract all Point3 values into a single matrix [x y z]
Matrix extractPoint3(const Values& values) {
Values::ConstFiltered<Point3> points = values.filter<Point3>();
Matrix result(points.size(), 3);
size_t j = 0;
BOOST_FOREACH(const Values::ConstFiltered<Point3>::KeyValuePair& key_value, points)
result.row(j++) = key_value.value.vector();
return result;
}
/// Extract all Pose2 values into a single matrix [x y theta]
Matrix extractPose2(const Values& values) {
Values::ConstFiltered<Pose2> poses = values.filter<Pose2>();
Matrix result(poses.size(), 3);
size_t j = 0;
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, poses)
result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta();
return result;
}
/// Extract all Pose3 values
Values allPose3s(const Values& values) {
return values.filter<Pose3>();
}
/// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z]
Matrix extractPose3(const Values& values) {
Values::ConstFiltered<Pose3> poses = values.filter<Pose3>();
Matrix result(poses.size(), 12);
size_t j = 0;
BOOST_FOREACH(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value, poses) {
result.row(j).segment(0, 3) << key_value.value.rotation().matrix().row(0);
result.row(j).segment(3, 3) << key_value.value.rotation().matrix().row(1);
result.row(j).segment(6, 3) << key_value.value.rotation().matrix().row(2);
result.row(j).tail(3) = key_value.value.translation().vector();
j++;
}
return result;
}
/// Perturb all Point2 values using normally distributed noise
void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) {
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,
sigma);
Sampler sampler(model, seed);
BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& key_value, values.filter<Point2>()) {
values.update(key_value.key, key_value.value.retract(sampler.sample()));
}
}
/// Perturb all Pose2 values using normally distributed noise
void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed =
42u) {
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(
Vector3(sigmaT, sigmaT, sigmaR));
Sampler sampler(model, seed);
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, values.filter<Pose2>()) {
values.update(key_value.key, key_value.value.retract(sampler.sample()));
}
}
/// Perturb all Point3 values using normally distributed noise
void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) {
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,
sigma);
Sampler sampler(model, seed);
BOOST_FOREACH(const Values::ConstFiltered<Point3>::KeyValuePair& key_value, values.filter<Point3>()) {
values.update(key_value.key, key_value.value.retract(sampler.sample()));
}
}
/// Insert a number of initial point values by backprojecting
void insertBackprojections(Values& values, const SimpleCamera& camera,
const Vector& J, const Matrix& Z, double depth) {
if (Z.rows() != 2)
throw std::invalid_argument("insertBackProjections: Z must be 2*K");
if (Z.cols() != J.size())
throw std::invalid_argument(
"insertBackProjections: J and Z must have same number of entries");
for (int k = 0; k < Z.cols(); k++) {
Point2 p(Z(0, k), Z(1, k));
Point3 P = camera.backproject(p, depth);
values.insert(J(k), P);
}
}
/// Insert multiple projection factors for a single pose key
void insertProjectionFactors(NonlinearFactorGraph& graph, Key i,
const Vector& J, const Matrix& Z, const SharedNoiseModel& model,
const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) {
if (Z.rows() != 2)
throw std::invalid_argument("addMeasurements: Z must be 2*K");
if (Z.cols() != J.size())
throw std::invalid_argument(
"addMeasurements: J and Z must have same number of entries");
for (int k = 0; k < Z.cols(); k++) {
graph.push_back(
boost::make_shared<GenericProjectionFactor<Pose3, Point3> >(
Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor));
}
}
/// Calculate the errors of all projection factors in a graph
Matrix reprojectionErrors(const NonlinearFactorGraph& graph,
const Values& values) {
// first count
size_t K = 0, k = 0;
BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph)
if (boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
f))
++K;
// now fill
Matrix errors(2, K);
BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) {
boost::shared_ptr<const GenericProjectionFactor<Pose3, Point3> > p =
boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
f);
if (p)
errors.col(k++) = p->unwhitenedError(values);
}
return errors;
}
/// Convert from local to world coordinates
Values localToWorld(const Values& local, const Pose2& base,
const FastVector<Key> user_keys = FastVector<Key>()) {
Values world;
// if no keys given, get all keys from local values
FastVector<Key> keys(user_keys);
if (keys.size()==0)
keys = FastVector<Key>(local.keys());
// Loop over all keys
BOOST_FOREACH(Key key, keys) {
try {
// if value is a Pose2, compose it with base pose
Pose2 pose = local.at<Pose2>(key);
world.insert(key, base.compose(pose));
} catch (std::exception e1) {
try {
// if value is a Point2, transform it from base pose
Point2 point = local.at<Point2>(key);
world.insert(key, base.transform_from(point));
} catch (std::exception e2) {
// if not Pose2 or Point2, do nothing
}
}
}
return world;
}
}
}

4
matlab/+gtsam/Contents.m
View File

@ -175,6 +175,9 @@
% symbolIndex - get index from a symbol key
%
%% Wrapped C++ Convenience Functions for use within MATLAB
% utilities.createKeyList - Create KeyList from indices
% utilities.createKeyVector - Create KeyVector from indices
% utilities.createKeySet - Create KeySet from indices
% utilities.extractPoint2 - Extract all Point2 values into a single matrix [x y]
% utilities.extractPoint3 - Extract all Point3 values into a single matrix [x y z]
% utilities.extractPose2 - Extract all Pose2 values into a single matrix [x y theta]
@ -186,3 +189,4 @@
% utilities.insertBackprojections - Insert a number of initial point values by backprojecting
% utilities.insertProjectionFactors - Insert multiple projection factors for a single pose key
% utilities.reprojectionErrors - Calculate the errors of all projection factors in a graph
% utilities.localToWorld - Convert from local to world coordinates

54
matlab/+gtsam/plot2DTrajectory.m
View File

@ -8,53 +8,41 @@ function plot2DTrajectory(values, linespec, marginals)
import gtsam.*
if ~exist('linespec', 'var') || isempty(linespec)
linespec = 'k*-';
linespec = 'k*-';
end
haveMarginals = exist('marginals', 'var');
keys = KeyVector(values.keys);
holdstate = ishold;
hold on
% Plot poses and covariance matrices
lastIndex = [];
for i = 0:keys.size-1
% Do something very efficient to draw trajectory
poses = utilities.extractPose2(values);
X = poses(:,1);
Y = poses(:,2);
theta = poses(:,3);
plot(X,Y,linespec);
% Quiver can also be vectorized if no marginals asked
if ~haveMarginals
C = cos(theta);
S = sin(theta);
quiver(X,Y,C,S,0.1,linespec);
else
% plotPose2 does both quiver and covariance matrix
keys = KeyVector(values.keys);
for i = 0:keys.size-1
key = keys.at(i);
x = values.at(key);
if isa(x, 'gtsam.Pose2')
if ~isempty(lastIndex)
% Draw line from last pose then covariance ellipse on top of
% last pose.
lastKey = keys.at(lastIndex);
lastPose = values.at(lastKey);
plot([ x.x; lastPose.x ], [ x.y; lastPose.y ], linespec);
if haveMarginals
P = marginals.marginalCovariance(lastKey);
gtsam.plotPose2(lastPose, 'g', P);
else
gtsam.plotPose2(lastPose, 'g', []);
end
end
lastIndex = i;
end
end
% Draw final covariance ellipse
if ~isempty(lastIndex)
lastKey = keys.at(lastIndex);
lastPose = values.at(lastKey);
if haveMarginals
P = marginals.marginalCovariance(lastKey);
gtsam.plotPose2(lastPose, 'g', P);
else
gtsam.plotPose2(lastPose, 'g', []);
P = marginals.marginalCovariance(key);
gtsam.plotPose2(x,linespec(1), P);
end
end
end
if ~holdstate
hold off
hold off
end
end

8
matlab/gtsam_examples/Pose2SLAMExample_advanced.m
View File

@ -59,16 +59,16 @@ params.setAbsoluteErrorTol(1e-15);
params.setRelativeErrorTol(1e-15);
params.setVerbosity('ERROR');
params.setVerbosityDL('VERBOSE');
params.setOrdering(graph.orderingCOLAMD(initialEstimate));
params.setOrdering(graph.orderingCOLAMD());
optimizer = DoglegOptimizer(graph, initialEstimate, params);
result = optimizer.optimizeSafely();
result.print('final result');
%% Get the corresponding dense matrix
ord = graph.orderingCOLAMD(result);
gfg = graph.linearize(result,ord);
denseAb = gfg.denseJacobian;
ord = graph.orderingCOLAMD();
gfg = graph.linearize(result);
denseAb = gfg.augmentedJacobian;
%% Get sparse matrix A and RHS b
IJS = gfg.sparseJacobian_();

2
matlab/gtsam_examples/Pose2SLAMExample_graph.m
View File

@ -36,7 +36,9 @@ toc
hold on; plot2DTrajectory(result, 'b-*');
%% Plot Covariance Ellipses
tic
marginals = Marginals(graph, result);
toc
P={};
for i=1:result.size()-1
pose_i = result.at(i);

2
matlab/gtsam_examples/Pose3SLAMExample_graph.m
View File

@ -12,6 +12,8 @@
import gtsam.*
%% PLEASE NOTE THAT PLOTTING TAKES A VERY LONG TIME HERE
%% Find data file
N = 2500;
% dataset = 'sphere_smallnoise.graph';

47
matlab/gtsam_tests/testUtilities.m Normal file
View File

@ -0,0 +1,47 @@
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% 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
%
% @brief Checks for results of functions in utilities namespace
% @author Frank Dellaert
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
import gtsam.*
%% Create keys for variables
x1 = symbol('x',1); x2 = symbol('x',2); x3 = symbol('x',3);
actual = utilities.createKeyList([1;2;3]);
CHECK('KeyList', isa(actual,'gtsam.KeyList'));
CHECK('size==3', actual.size==3);
CHECK('actual.front==1', actual.front==1);
actual = utilities.createKeyList('x',[1;2;3]);
CHECK('KeyList', isa(actual,'gtsam.KeyList'));
CHECK('size==3', actual.size==3);
CHECK('actual.front==x1', actual.front==x1);
actual = utilities.createKeyVector([1;2;3]);
CHECK('KeyVector', isa(actual,'gtsam.KeyVector'));
CHECK('size==3', actual.size==3);
CHECK('actual.at(0)==1', actual.at(0)==1);
actual = utilities.createKeyVector('x',[1;2;3]);
CHECK('KeyVector', isa(actual,'gtsam.KeyVector'));
CHECK('size==3', actual.size==3);
CHECK('actual.at(0)==x1', actual.at(0)==x1);
actual = utilities.createKeySet([1;2;3]);
CHECK('KeySet', isa(actual,'gtsam.KeySet'));
CHECK('size==3', actual.size==3);
CHECK('actual.count(1)', actual.count(1));
actual = utilities.createKeySet('x',[1;2;3]);
CHECK('KeySet', isa(actual,'gtsam.KeySet'));
CHECK('size==3', actual.size==3);
CHECK('actual.count(x1)', actual.count(x1));

3
matlab/gtsam_tests/test_gtsam.m
View File

@ -33,5 +33,8 @@ testVisualISAMExample
display 'Starting: testSerialization'
testSerialization
display 'Starting: testUtilities'
testUtilities
% end of tests
display 'Tests complete!'

0
package_scripts/compile_static_boost.sh Normal file → Executable file
View File

30
package_scripts/toolbox_package_unix.sh
View File

@ -33,22 +33,21 @@ fi
# Run cmake
cmake -DCMAKE_BUILD_TYPE=Release \
-DGTSAM_INSTALL_MATLAB_TOOLBOX:bool=true \
-DGTSAM_INSTALL_MATLAB_TOOLBOX:BOOL=ON \
-DCMAKE_INSTALL_PREFIX="$PWD/stage" \
-DBoost_NO_SYSTEM_PATHS:bool=true \
-DBoost_USE_STATIC_LIBS:bool=true \
-DBoost_NO_SYSTEM_PATHS:BOOL=ON \
-DBoost_USE_STATIC_LIBS:BOOL=ON \
-DBOOST_ROOT="$1" \
-DGTSAM_BUILD_SHARED_LIBRARY:bool=false \
-DGTSAM_BUILD_STATIC_LIBRARY:bool=false \
-DGTSAM_BUILD_TESTS:bool=false \
-DGTSAM_BUILD_EXAMPLES:bool=false \
-DGTSAM_BUILD_UNSTABLE:bool=false \
-DGTSAM_DISABLE_EXAMPLES_ON_INSTALL:bool=true \
-DGTSAM_DISABLE_TESTS_ON_INSTALL:bool=true \
-DGTSAM_BUILD_CONVENIENCE_LIBRARIES:bool=false \
-DGTSAM_MEX_BUILD_STATIC_MODULE:bool=true ..
-DGTSAM_BUILD_TESTS:BOOL=OFF \
-DGTSAM_BUILD_TIMING:BOOL=OFF \
-DGTSAM_BUILD_EXAMPLES_ALWAYS:BOOL=OFF \
-DGTSAM_WITH_TBB:BOOL=OFF \
-DGTSAM_BUILD_METIS:BOOL=OFF \
-DGTSAM_INSTALL_GEOGRAPHICLIB:BOOL=OFF \
-DGTSAM_BUILD_UNSTABLE:BOOL=OFF \
-DGTSAM_MEX_BUILD_STATIC_MODULE:BOOL=ON ..
if [ ! $? ]; then
if [ $? -ne 0 ]; then
echo "CMake failed"
exit 1
fi
@ -56,5 +55,10 @@ fi
# Compile
make -j8 install
if [ $? -ne 0 ]; then
echo "Compile failed"
exit 1
fi
# Create package
tar czf gtsam-toolbox-3.0.0-$platform.tgz -C stage/gtsam_toolbox toolbox

53
tests/testGraph.cpp
View File

@ -18,7 +18,10 @@
#include <gtsam/slam/BetweenFactor.h>
#include <gtsam/nonlinear/NonlinearFactorGraph.h>
#include <gtsam/linear/GaussianFactorGraph.h>
#include <gtsam/linear/JacobianFactor.h>
#include <gtsam/inference/graph.h>
#include <gtsam/inference/Symbol.h>
#include <gtsam/geometry/Pose2.h>
#include <CppUnitLite/TestHarness.h>
@ -105,24 +108,38 @@ TEST( Graph, composePoses )
CHECK(assert_equal(expected, *actual));
}
// SL-FIX TEST( GaussianFactorGraph, findMinimumSpanningTree )
//{
// GaussianFactorGraph g;
// Matrix I = eye(2);
// Vector b = Vector_(0, 0, 0);
// g += X(1), I, X(2), I, b, model;
// g += X(1), I, X(3), I, b, model;
// g += X(1), I, X(4), I, b, model;
// g += X(2), I, X(3), I, b, model;
// g += X(2), I, X(4), I, b, model;
// g += X(3), I, X(4), I, b, model;
//
// map<string, string> tree = g.findMinimumSpanningTree<string, GaussianFactor>();
// EXPECT(tree[X(1)].compare(X(1))==0);
// EXPECT(tree[X(2)].compare(X(1))==0);
// EXPECT(tree[X(3)].compare(X(1))==0);
// EXPECT(tree[X(4)].compare(X(1))==0);
//}
/* ************************************************************************* */
TEST( GaussianFactorGraph, findMinimumSpanningTree )
{
GaussianFactorGraph g;
Matrix I = eye(2);
Vector2 b(0, 0);
const SharedDiagonal model = noiseModel::Diagonal::Sigmas((Vector(2) << 0.5, 0.5));
using namespace symbol_shorthand;
g += JacobianFactor(X(1), I, X(2), I, b, model);
g += JacobianFactor(X(1), I, X(3), I, b, model);
g += JacobianFactor(X(1), I, X(4), I, b, model);
g += JacobianFactor(X(2), I, X(3), I, b, model);
g += JacobianFactor(X(2), I, X(4), I, b, model);
g += JacobianFactor(X(3), I, X(4), I, b, model);
PredecessorMap<Key> tree = findMinimumSpanningTree<GaussianFactorGraph, Key, JacobianFactor>(g);
EXPECT_LONGS_EQUAL(X(1),tree[X(1)]);
EXPECT_LONGS_EQUAL(X(1),tree[X(2)]);
EXPECT_LONGS_EQUAL(X(1),tree[X(3)]);
EXPECT_LONGS_EQUAL(X(1),tree[X(4)]);
// we add a disconnected component - does not work yet
// g += JacobianFactor(X(5), I, X(6), I, b, model);
//
// PredecessorMap<Key> forest = findMinimumSpanningTree<GaussianFactorGraph, Key, JacobianFactor>(g);
// EXPECT_LONGS_EQUAL(X(1),forest[X(1)]);
// EXPECT_LONGS_EQUAL(X(1),forest[X(2)]);
// EXPECT_LONGS_EQUAL(X(1),forest[X(3)]);
// EXPECT_LONGS_EQUAL(X(1),forest[X(4)]);
// EXPECT_LONGS_EQUAL(X(5),forest[X(5)]);
// EXPECT_LONGS_EQUAL(X(5),forest[X(6)]);
}
///* ************************************************************************* */
// SL-FIX TEST( GaussianFactorGraph, split )

4
tests/testNonlinearOptimizer.cpp
View File

@ -295,7 +295,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {
// test the diagonal
GaussianFactorGraph::shared_ptr linear = optimizer.linearize();
GaussianFactorGraph damped = optimizer.buildDampedSystem(*linear);
GaussianFactorGraph damped = *optimizer.buildDampedSystem(*linear);
VectorValues d = linear->hessianDiagonal(), //
expectedDiagonal = d + params.lambdaInitial * d;
EXPECT(assert_equal(expectedDiagonal, damped.hessianDiagonal()));
@ -309,7 +309,7 @@ TEST_UNSAFE(NonlinearOptimizer, MoreOptimization) {
EXPECT(assert_equal(expectedGradient,linear->gradientAtZero()));
// Check that the gradient is zero for damped system (it is not!)
damped = optimizer.buildDampedSystem(*linear);
damped = *optimizer.buildDampedSystem(*linear);
VectorValues actualGradient = damped.gradientAtZero();
EXPECT(assert_equal(expectedGradient,actualGradient));

116
wrap/Argument.cpp
View File

@ -16,13 +16,14 @@
* @author Richard Roberts
**/
#include <iostream>
#include <fstream>
#include <sstream>
#include "Argument.h"
#include <boost/foreach.hpp>
#include <boost/regex.hpp>
#include "Argument.h"
#include <iostream>
#include <fstream>
#include <sstream>
using namespace std;
using namespace wrap;
@ -31,18 +32,24 @@ using namespace wrap;
string Argument::matlabClass(const string& delim) const {
string result;
BOOST_FOREACH(const string& ns, namespaces)
result += ns + delim;
if (type=="string" || type=="unsigned char" || type=="char")
result += ns + delim;
if (type == "string" || type == "unsigned char" || type == "char")
return result + "char";
if (type=="Vector" || type=="Matrix")
if (type == "Vector" || type == "Matrix")
return result + "double";
if (type=="int" || type=="size_t")
if (type == "int" || type == "size_t")
return result + "numeric";
if (type=="bool")
if (type == "bool")
return result + "logical";
return result + type;
}
/* ************************************************************************* */
bool Argument::isScalar() const {
return (type == "bool" || type == "char" || type == "unsigned char"
|| type == "int" || type == "size_t" || type == "double");
}
/* ************************************************************************* */
void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
file.oss << " ";
@ -52,7 +59,8 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
if (is_ptr)
// A pointer: emit an "unwrap_shared_ptr" call which returns a pointer
file.oss << "boost::shared_ptr<" << cppType << "> " << name << " = unwrap_shared_ptr< ";
file.oss << "boost::shared_ptr<" << cppType << "> " << name
<< " = unwrap_shared_ptr< ";
else if (is_ref)
// A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer
file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< ";
@ -65,23 +73,28 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
file.oss << cppType << " " << name << " = unwrap< ";
file.oss << cppType << " >(" << matlabName;
if (is_ptr || is_ref) file.oss << ", \"ptr_" << matlabUniqueType << "\"";
if (is_ptr || is_ref)
file.oss << ", \"ptr_" << matlabUniqueType << "\"";
file.oss << ");" << endl;
}
/* ************************************************************************* */
string Argument::qualifiedType(const string& delim) const {
string result;
BOOST_FOREACH(const string& ns, namespaces) result += ns + delim;
BOOST_FOREACH(const string& ns, namespaces)
result += ns + delim;
return result + type;
}
/* ************************************************************************* */
string ArgumentList::types() const {
string str;
bool first=true;
bool first = true;
BOOST_FOREACH(Argument arg, *this) {
if (!first) str += ","; str += arg.type; first=false;
if (!first)
str += ",";
str += arg.type;
first = false;
}
return str;
}
@ -89,16 +102,17 @@ string ArgumentList::types() const {
/* ************************************************************************* */
string ArgumentList::signature() const {
string sig;
bool cap=false;
bool cap = false;
BOOST_FOREACH(Argument arg, *this) {
BOOST_FOREACH(char ch, arg.type)
if(isupper(ch)) {
sig += ch;
//If there is a capital letter, we don't want to read it below
cap=true;
}
if(!cap) sig += arg.type[0];
if (isupper(ch)) {
sig += ch;
//If there is a capital letter, we don't want to read it below
cap = true;
}
if (!cap)
sig += arg.type[0];
//Reset to default
cap = false;
}
@ -109,23 +123,77 @@ string ArgumentList::signature() const {
/* ************************************************************************* */
string ArgumentList::names() const {
string str;
bool first=true;
bool first = true;
BOOST_FOREACH(Argument arg, *this) {
if (!first) str += ","; str += arg.name; first=false;
if (!first)
str += ",";
str += arg.name;
first = false;
}
return str;
}
/* ************************************************************************* */
bool ArgumentList::allScalar() const {
BOOST_FOREACH(Argument arg, *this)
if (!arg.isScalar()) return false;
return true;
}
/* ************************************************************************* */
void ArgumentList::matlab_unwrap(FileWriter& file, int start) const {
int index = start;
BOOST_FOREACH(Argument arg, *this) {
stringstream buf;
buf << "in[" << index << "]";
arg.matlab_unwrap(file,buf.str());
arg.matlab_unwrap(file, buf.str());
index++;
}
}
/* ************************************************************************* */
void ArgumentList::emit_prototype(FileWriter& file, const string& name) const {
file.oss << name << "(";
bool first = true;
BOOST_FOREACH(Argument arg, *this) {
if (!first)
file.oss << ", ";
file.oss << arg.type << " " << arg.name;
first = false;
}
file.oss << ")";
}
/* ************************************************************************* */
void ArgumentList::emit_call(FileWriter& file, const ReturnValue& returnVal,
const string& wrapperName, int id, bool staticMethod) const {
returnVal.emit_matlab(file);
file.oss << wrapperName << "(" << id;
if (!staticMethod)
file.oss << ", this";
file.oss << ", varargin{:});\n";
}
/* ************************************************************************* */
void ArgumentList::emit_conditional_call(FileWriter& file,
const ReturnValue& returnVal, const string& wrapperName, int id,
bool staticMethod) const {
// Check nr of arguments
file.oss << "if length(varargin) == " << size();
if (size() > 0)
file.oss << " && ";
// ...and their types
bool first = true;
for (size_t i = 0; i < size(); i++) {
if (!first)
file.oss << " && ";
file.oss << "isa(varargin{" << i + 1 << "},'" << (*this)[i].matlabClass(".")
<< "')";
first = false;
}
file.oss << "\n";
// output call to C++ wrapper
file.oss << " ";
emit_call(file, returnVal, wrapperName, id, staticMethod);
}
/* ************************************************************************* */

37
wrap/Argument.h
View File

@ -19,11 +19,12 @@
#pragma once
#include "FileWriter.h"
#include "ReturnValue.h"
#include <string>
#include <vector>
#include "FileWriter.h"
namespace wrap {
/// Argument class
@ -40,6 +41,9 @@ struct Argument {
/// return MATLAB class for use in isa(x,class)
std::string matlabClass(const std::string& delim = "") const;
/// Check if will be unwrapped using scalar login in wrap/matlab.h
bool isScalar() const;
/// adds namespaces to type
std::string qualifiedType(const std::string& delim = "") const;
@ -59,6 +63,9 @@ struct ArgumentList: public std::vector<Argument> {
/// create a comma-separated string listing all argument names, used in m-files
std::string names() const;
/// Check if all arguments scalar
bool allScalar() const;
// MATLAB code generation:
/**
@ -68,6 +75,32 @@ struct ArgumentList: public std::vector<Argument> {
*/
void matlab_unwrap(FileWriter& file, int start = 0) const; // MATLAB to C++
/**
* emit MATLAB prototype
* @param file output stream
* @param name of method or function
*/
void emit_prototype(FileWriter& file, const std::string& name) const;
/**
* emit emit MATLAB call to wrapper
* @param file output stream
* @param returnVal the return value
* @param wrapperName of method or function
* @param staticMethod flag to emit "this" in call
*/
void emit_call(FileWriter& file, const ReturnValue& returnVal,
const std::string& wrapperName, int id, bool staticMethod = false) const;
/**
* emit conditional MATLAB call to wrapper (checking arguments first)
* @param file output stream
* @param returnVal the return value
* @param wrapperName of method or function
* @param staticMethod flag to emit "this" in call
*/
void emit_conditional_call(FileWriter& file, const ReturnValue& returnVal,
const std::string& wrapperName, int id, bool staticMethod = false) const;
};
} // \namespace wrap

406
wrap/Class.cpp
View File

@ -16,40 +16,40 @@
* @author Richard Roberts
**/
#include "Class.h"
#include "utilities.h"
#include "Argument.h"
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <vector>
#include <iostream>
#include <fstream>
//#include <cstdint> // on Linux GCC: fails with error regarding needing C++0x std flags
//#include <cinttypes> // same failure as above
#include <stdint.h> // works on Linux GCC
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include "Class.h"
#include "utilities.h"
#include "Argument.h"
using namespace std;
using namespace wrap;
/* ************************************************************************* */
void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
const TypeAttributesTable& typeAttributes,
FileWriter& wrapperFile, vector<string>& functionNames) const {
const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile,
vector<string>& functionNames) const {
// Create namespace folders
createNamespaceStructure(namespaces, toolboxPath);
// open destination classFile
string classFile = toolboxPath;
if(!namespaces.empty())
if (!namespaces.empty())
classFile += "/+" + wrap::qualifiedName("/+", namespaces);
classFile += "/" + name + ".m";
FileWriter proxyFile(classFile, verbose_, "%");
// get the name of actual matlab object
const string matlabQualName = qualifiedName("."), matlabUniqueName = qualifiedName(), cppName = qualifiedName("::");
const string matlabQualName = qualifiedName("."), matlabUniqueName =
qualifiedName(), cppName = qualifiedName("::");
const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent);
const string cppBaseName = wrap::qualifiedName("::", qualifiedParent);
@ -72,42 +72,49 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
// other wrap modules - to add these to their collectors the pointer is
// passed from one C++ module into matlab then back into the other C++
// module.
pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName, functionNames);
pointer_constructor_fragments(proxyFile, wrapperFile, wrapperName,
functionNames);
wrapperFile.oss << "\n";
// Regular constructors
BOOST_FOREACH(ArgumentList a, constructor.args_list)
{
const int id = (int)functionNames.size();
constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(), id, a);
BOOST_FOREACH(ArgumentList a, constructor.args_list) {
const int id = (int) functionNames.size();
constructor.proxy_fragment(proxyFile, wrapperName, !qualifiedParent.empty(),
id, a);
const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile,
cppName, matlabUniqueName, cppBaseName, id, a);
cppName, matlabUniqueName, cppBaseName, id, a);
wrapperFile.oss << "\n";
functionNames.push_back(wrapFunctionName);
}
proxyFile.oss << " else\n";
proxyFile.oss << " error('Arguments do not match any overload of " << matlabQualName << " constructor');\n";
proxyFile.oss << " error('Arguments do not match any overload of "
<< matlabQualName << " constructor');\n";
proxyFile.oss << " end\n";
if(!qualifiedParent.empty())
proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64(" << ptr_constructor_key << "), base_ptr);\n";
if (!qualifiedParent.empty())
proxyFile.oss << " obj = obj@" << matlabBaseName << "(uint64("
<< ptr_constructor_key << "), base_ptr);\n";
proxyFile.oss << " obj.ptr_" << matlabUniqueName << " = my_ptr;\n";
proxyFile.oss << " end\n\n";
// Deconstructor
{
const int id = (int)functionNames.size();
const int id = (int) functionNames.size();
deconstructor.proxy_fragment(proxyFile, wrapperName, matlabUniqueName, id);
proxyFile.oss << "\n";
const string functionName = deconstructor.wrapper_fragment(wrapperFile, cppName, matlabUniqueName, id);
const string functionName = deconstructor.wrapper_fragment(wrapperFile,
cppName, matlabUniqueName, id);
wrapperFile.oss << "\n";
functionNames.push_back(functionName);
}
proxyFile.oss << " function display(obj), obj.print(''); end\n %DISPLAY Calls print on the object\n";
proxyFile.oss << " function disp(obj), obj.display; end\n %DISP Calls print on the object\n";
proxyFile.oss
<< " function display(obj), obj.print(''); end\n %DISPLAY Calls print on the object\n";
proxyFile.oss
<< " function disp(obj), obj.display; end\n %DISP Calls print on the object\n";
// Methods
BOOST_FOREACH(const Methods::value_type& name_m, methods) {
const Method& m = name_m.second;
m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames);
m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName,
matlabUniqueName, wrapperName, typeAttributes, functionNames);
proxyFile.oss << "\n";
wrapperFile.oss << "\n";
}
@ -121,12 +128,14 @@ void Class::matlab_proxy(const string& toolboxPath, const string& wrapperName,
// Static methods
BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) {
const StaticMethod& m = name_m.second;
m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName, matlabUniqueName, wrapperName, typeAttributes, functionNames);
m.proxy_wrapper_fragments(proxyFile, wrapperFile, cppName, matlabQualName,
matlabUniqueName, wrapperName, typeAttributes, functionNames);
proxyFile.oss << "\n";
wrapperFile.oss << "\n";
}
if (hasSerialization)
deserialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames);
deserialization_fragments(proxyFile, wrapperFile, wrapperName,
functionNames);
proxyFile.oss << " end\n";
proxyFile.oss << "end\n";
@ -141,41 +150,50 @@ string Class::qualifiedName(const string& delim) const {
}
/* ************************************************************************* */
void Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const string& wrapperName, vector<string>& functionNames) const {
void Class::pointer_constructor_fragments(FileWriter& proxyFile,
FileWriter& wrapperFile, const string& wrapperName,
vector<string>& functionNames) const {
const string matlabUniqueName = qualifiedName(), cppName = qualifiedName("::");
const string matlabUniqueName = qualifiedName(), cppName = qualifiedName(
"::");
const string baseCppName = wrap::qualifiedName("::", qualifiedParent);
const int collectorInsertId = (int)functionNames.size();
const string collectorInsertFunctionName = matlabUniqueName + "_collectorInsertAndMakeBase_" + boost::lexical_cast<string>(collectorInsertId);
const int collectorInsertId = (int) functionNames.size();
const string collectorInsertFunctionName = matlabUniqueName
+ "_collectorInsertAndMakeBase_"
+ boost::lexical_cast<string>(collectorInsertId);
functionNames.push_back(collectorInsertFunctionName);
int upcastFromVoidId;
string upcastFromVoidFunctionName;
if(isVirtual) {
upcastFromVoidId = (int)functionNames.size();
upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_" + boost::lexical_cast<string>(upcastFromVoidId);
if (isVirtual) {
upcastFromVoidId = (int) functionNames.size();
upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_"
+ boost::lexical_cast<string>(upcastFromVoidId);
functionNames.push_back(upcastFromVoidFunctionName);
}
// MATLAB constructor that assigns pointer to matlab object then calls c++
// function to add the object to the collector.
if(isVirtual) {
proxyFile.oss << " if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))";
if (isVirtual) {
proxyFile.oss
<< " if (nargin == 2 || (nargin == 3 && strcmp(varargin{3}, 'void')))";
} else {
proxyFile.oss << " if nargin == 2";
}
proxyFile.oss << " && isa(varargin{1}, 'uint64') && varargin{1} == uint64(" << ptr_constructor_key << ")\n";
if(isVirtual) {
proxyFile.oss << " && isa(varargin{1}, 'uint64') && varargin{1} == uint64("
<< ptr_constructor_key << ")\n";
if (isVirtual) {
proxyFile.oss << " if nargin == 2\n";
proxyFile.oss << " my_ptr = varargin{2};\n";
proxyFile.oss << " else\n";
proxyFile.oss << " my_ptr = " << wrapperName << "(" << upcastFromVoidId << ", varargin{2});\n";
proxyFile.oss << " my_ptr = " << wrapperName << "("
<< upcastFromVoidId << ", varargin{2});\n";
proxyFile.oss << " end\n";
} else {
proxyFile.oss << " my_ptr = varargin{2};\n";
}
if(qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back
if (qualifiedParent.empty()) // If this class has a base class, we'll get a base class pointer back
proxyFile.oss << " ";
else
proxyFile.oss << " base_ptr = ";
@ -185,22 +203,27 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wra
// comes from a C++ return value; this mechanism allows the object to be added
// to a collector in a different wrap module. If this class has a base class,
// a new pointer to the base class is allocated and returned.
wrapperFile.oss << "void " << collectorInsertFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
wrapperFile.oss << "void " << collectorInsertFunctionName
<< "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
wrapperFile.oss << "{\n";
wrapperFile.oss << " mexAtExit(&_deleteAllObjects);\n";
// Typedef boost::shared_ptr
wrapperFile.oss << " typedef boost::shared_ptr<" << cppName << "> Shared;\n";
wrapperFile.oss << "\n";
// Get self pointer passed in
wrapperFile.oss << " Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));\n";
wrapperFile.oss
<< " Shared *self = *reinterpret_cast<Shared**> (mxGetData(in[0]));\n";
// Add to collector
wrapperFile.oss << " collector_" << matlabUniqueName << ".insert(self);\n";
// If we have a base class, return the base class pointer (MATLAB will call the base class collectorInsertAndMakeBase to add this to the collector and recurse the heirarchy)
if(!qualifiedParent.empty()) {
if (!qualifiedParent.empty()) {
wrapperFile.oss << "\n";
wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName << "> SharedBase;\n";
wrapperFile.oss << " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n";
wrapperFile.oss << " *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);\n";
wrapperFile.oss << " typedef boost::shared_ptr<" << baseCppName
<< "> SharedBase;\n";
wrapperFile.oss
<< " out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n";
wrapperFile.oss
<< " *reinterpret_cast<SharedBase**>(mxGetData(out[0])) = new SharedBase(*self);\n";
}
wrapperFile.oss << "}\n";
@ -208,31 +231,38 @@ void Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wra
// shared_ptr<void>. This mechanism allows automatic dynamic creation of the
// real underlying derived-most class when a C++ method returns a virtual
// base class.
if(isVirtual)
wrapperFile.oss <<
"\n"
"void " << upcastFromVoidFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {\n"
" mexAtExit(&_deleteAllObjects);\n"
" typedef boost::shared_ptr<" << cppName << "> Shared;\n"
" boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));\n"
" out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"
" Shared *self = new Shared(boost::static_pointer_cast<" << cppName << ">(*asVoid));\n"
" *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;\n"
"}\n";
if (isVirtual)
wrapperFile.oss << "\n"
"void " << upcastFromVoidFunctionName
<< "(int nargout, mxArray *out[], int nargin, const mxArray *in[]) {\n"
" mexAtExit(&_deleteAllObjects);\n"
" typedef boost::shared_ptr<" << cppName
<< "> Shared;\n"
" boost::shared_ptr<void> *asVoid = *reinterpret_cast<boost::shared_ptr<void>**> (mxGetData(in[0]));\n"
" out[0] = mxCreateNumericMatrix(1, 1, mxUINT32OR64_CLASS, mxREAL);\n"
" Shared *self = new Shared(boost::static_pointer_cast<" << cppName
<< ">(*asVoid));\n"
" *reinterpret_cast<Shared**>(mxGetData(out[0])) = self;\n"
"}\n";
}
/* ************************************************************************* */
vector<ArgumentList> expandArgumentListsTemplate(const vector<ArgumentList>& argLists, const string& templateArg, const vector<string>& instName, const std::vector<string>& expandedClassNamespace, const string& expandedClassName) {
vector<ArgumentList> expandArgumentListsTemplate(
const vector<ArgumentList>& argLists, const string& templateArg,
const vector<string>& instName,
const std::vector<string>& expandedClassNamespace,
const string& expandedClassName) {
vector<ArgumentList> result;
BOOST_FOREACH(const ArgumentList& argList, argLists) {
ArgumentList instArgList;
BOOST_FOREACH(const Argument& arg, argList) {
Argument instArg = arg;
if(arg.type == templateArg) {
instArg.namespaces.assign(instName.begin(), instName.end()-1);
if (arg.type == templateArg) {
instArg.namespaces.assign(instName.begin(), instName.end() - 1);
instArg.type = instName.back();
} else if(arg.type == "This") {
instArg.namespaces.assign(expandedClassNamespace.begin(), expandedClassNamespace.end());
} else if (arg.type == "This") {
instArg.namespaces.assign(expandedClassNamespace.begin(),
expandedClassNamespace.end());
instArg.type = expandedClassName;
}
instArgList.push_back(instArg);
@ -244,28 +274,34 @@ vector<ArgumentList> expandArgumentListsTemplate(const vector<ArgumentList>& arg
/* ************************************************************************* */
template<class METHOD>
map<string, METHOD> expandMethodTemplate(const map<string, METHOD>& methods, const string& templateArg, const vector<string>& instName, const std::vector<string>& expandedClassNamespace, const string& expandedClassName) {
map<string, METHOD> expandMethodTemplate(const map<string, METHOD>& methods,
const string& templateArg, const vector<string>& instName,
const std::vector<string>& expandedClassNamespace,
const string& expandedClassName) {
map<string, METHOD> result;
typedef pair<const string, METHOD> Name_Method;
BOOST_FOREACH(const Name_Method& name_method, methods) {
const METHOD& method = name_method.second;
METHOD instMethod = method;
instMethod.argLists = expandArgumentListsTemplate(method.argLists, templateArg, instName, expandedClassNamespace, expandedClassName);
instMethod.argLists = expandArgumentListsTemplate(method.argLists,
templateArg, instName, expandedClassNamespace, expandedClassName);
instMethod.returnVals.clear();
BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) {
ReturnValue instRetVal = retVal;
if(retVal.type1 == templateArg) {
instRetVal.namespaces1.assign(instName.begin(), instName.end()-1);
if (retVal.type1 == templateArg) {
instRetVal.namespaces1.assign(instName.begin(), instName.end() - 1);
instRetVal.type1 = instName.back();
} else if(retVal.type1 == "This") {
instRetVal.namespaces1.assign(expandedClassNamespace.begin(), expandedClassNamespace.end());
} else if (retVal.type1 == "This") {
instRetVal.namespaces1.assign(expandedClassNamespace.begin(),
expandedClassNamespace.end());
instRetVal.type1 = expandedClassName;
}
if(retVal.type2 == templateArg) {
instRetVal.namespaces2.assign(instName.begin(), instName.end()-1);
if (retVal.type2 == templateArg) {
instRetVal.namespaces2.assign(instName.begin(), instName.end() - 1);
instRetVal.type2 = instName.back();
} else if(retVal.type1 == "This") {
instRetVal.namespaces2.assign(expandedClassNamespace.begin(), expandedClassNamespace.end());
} else if (retVal.type1 == "This") {
instRetVal.namespaces2.assign(expandedClassNamespace.begin(),
expandedClassNamespace.end());
instRetVal.type2 = expandedClassName;
}
instMethod.returnVals.push_back(instRetVal);
@ -276,7 +312,10 @@ map<string, METHOD> expandMethodTemplate(const map<string, METHOD>& methods, con
}
/* ************************************************************************* */
Class expandClassTemplate(const Class& cls, const string& templateArg, const vector<string>& instName, const std::vector<string>& expandedClassNamespace, const string& expandedClassName) {
Class expandClassTemplate(const Class& cls, const string& templateArg,
const vector<string>& instName,
const std::vector<string>& expandedClassNamespace,
const string& expandedClassName) {
Class inst;
inst.name = cls.name;
inst.templateArgs = cls.templateArgs;
@ -284,11 +323,15 @@ Class expandClassTemplate(const Class& cls, const string& templateArg, const vec
inst.isVirtual = cls.isVirtual;
inst.isSerializable = cls.isSerializable;
inst.qualifiedParent = cls.qualifiedParent;
inst.methods = expandMethodTemplate(cls.methods, templateArg, instName, expandedClassNamespace, expandedClassName);
inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg, instName, expandedClassNamespace, expandedClassName);
inst.methods = expandMethodTemplate(cls.methods, templateArg, instName,
expandedClassNamespace, expandedClassName);
inst.static_methods = expandMethodTemplate(cls.static_methods, templateArg,
instName, expandedClassNamespace, expandedClassName);
inst.namespaces = cls.namespaces;
inst.constructor = cls.constructor;
inst.constructor.args_list = expandArgumentListsTemplate(cls.constructor.args_list, templateArg, instName, expandedClassNamespace, expandedClassName);
inst.constructor.args_list = expandArgumentListsTemplate(
cls.constructor.args_list, templateArg, instName, expandedClassNamespace,
expandedClassName);
inst.constructor.name = inst.name;
inst.deconstructor = cls.deconstructor;
inst.deconstructor.name = inst.name;
@ -297,22 +340,29 @@ Class expandClassTemplate(const Class& cls, const string& templateArg, const vec
}
/* ************************************************************************* */
vector<Class> Class::expandTemplate(const string& templateArg, const vector<vector<string> >& instantiations) const {
vector<Class> Class::expandTemplate(const string& templateArg,
const vector<vector<string> >& instantiations) const {
vector<Class> result;
BOOST_FOREACH(const vector<string>& instName, instantiations) {
const string expandedName = name + instName.back();
Class inst = expandClassTemplate(*this, templateArg, instName, this->namespaces, expandedName);
Class inst = expandClassTemplate(*this, templateArg, instName,
this->namespaces, expandedName);
inst.name = expandedName;
inst.templateArgs.clear();
inst.typedefName = qualifiedName("::") + "<" + wrap::qualifiedName("::", instName) + ">";
inst.typedefName = qualifiedName("::") + "<"
+ wrap::qualifiedName("::", instName) + ">";
result.push_back(inst);
}
return result;
}
/* ************************************************************************* */
Class Class::expandTemplate(const string& templateArg, const vector<string>& instantiation, const std::vector<string>& expandedClassNamespace, const string& expandedClassName) const {
return expandClassTemplate(*this, templateArg, instantiation, expandedClassNamespace, expandedClassName);
Class Class::expandTemplate(const string& templateArg,
const vector<string>& instantiation,
const std::vector<string>& expandedClassNamespace,
const string& expandedClassName) const {
return expandClassTemplate(*this, templateArg, instantiation,
expandedClassNamespace, expandedClassName);
}
/* ************************************************************************* */
@ -322,7 +372,7 @@ std::string Class::getTypedef() const {
result += ("namespace " + namesp + " { ");
}
result += ("typedef " + typedefName + " " + name + ";");
for (size_t i = 0; i<namespaces.size(); ++i) {
for (size_t i = 0; i < namespaces.size(); ++i) {
result += " }";
}
return result;
@ -331,25 +381,16 @@ std::string Class::getTypedef() const {
/* ************************************************************************* */
void Class::comment_fragment(FileWriter& proxyFile) const {
proxyFile.oss << "%class " << name
<< ", see Doxygen page for details\n";
proxyFile.oss << "%class " << name << ", see Doxygen page for details\n";
proxyFile.oss
<< "%at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
if (!constructor.args_list.empty())
proxyFile.oss << "%\n%-------Constructors-------\n";
BOOST_FOREACH(ArgumentList argList, constructor.args_list) {
string up_name = boost::to_upper_copy(name);
proxyFile.oss << "%" << name << "(";
unsigned int i = 0;
BOOST_FOREACH(const Argument& arg, argList) {
if (i != argList.size() - 1)
proxyFile.oss << arg.type << " " << arg.name << ", ";
else
proxyFile.oss << arg.type << " " << arg.name;
i++;
}
proxyFile.oss << ")\n";
proxyFile.oss << "%";
argList.emit_prototype(proxyFile, name);
proxyFile.oss << "\n";
}
if (!methods.empty())
@ -357,17 +398,9 @@ void Class::comment_fragment(FileWriter& proxyFile) const {
BOOST_FOREACH(const Methods::value_type& name_m, methods) {
const Method& m = name_m.second;
BOOST_FOREACH(ArgumentList argList, m.argLists) {
string up_name = boost::to_upper_copy(m.name);
proxyFile.oss << "%" << m.name << "(";
unsigned int i = 0;
BOOST_FOREACH(const Argument& arg, argList) {
if (i != argList.size() - 1)
proxyFile.oss << arg.type << " " << arg.name << ", ";
else
proxyFile.oss << arg.type << " " << arg.name;
i++;
}
proxyFile.oss << ") : returns "
proxyFile.oss << "%";
argList.emit_prototype(proxyFile, m.name);
proxyFile.oss << " : returns "
<< m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl;
}
}
@ -377,18 +410,9 @@ void Class::comment_fragment(FileWriter& proxyFile) const {
BOOST_FOREACH(const StaticMethods::value_type& name_m, static_methods) {
const StaticMethod& m = name_m.second;
BOOST_FOREACH(ArgumentList argList, m.argLists) {
string up_name = boost::to_upper_copy(m.name);
proxyFile.oss << "%" << m.name << "(";
unsigned int i = 0;
BOOST_FOREACH(const Argument& arg, argList) {
if (i != argList.size() - 1)
proxyFile.oss << arg.type << " " << arg.name << ", ";
else
proxyFile.oss << arg.type << " " << arg.name;
i++;
}
proxyFile.oss << ") : returns "
proxyFile.oss << "%";
argList.emit_prototype(proxyFile, m.name);
proxyFile.oss << " : returns "
<< m.returnVals[0].return_type(false, m.returnVals[0].pair) << endl;
}
}
@ -396,15 +420,17 @@ void Class::comment_fragment(FileWriter& proxyFile) const {
if (hasSerialization) {
proxyFile.oss << "%\n%-------Serialization Interface-------\n";
proxyFile.oss << "%string_serialize() : returns string\n";
proxyFile.oss << "%string_deserialize(string serialized) : returns " << this->name << "\n";
proxyFile.oss << "%string_deserialize(string serialized) : returns "
<< this->name << "\n";
}
proxyFile.oss << "%\n";
}
/* ************************************************************************* */
void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
const std::string& wrapperName, std::vector<std::string>& functionNames) const {
void Class::serialization_fragments(FileWriter& proxyFile,
FileWriter& wrapperFile, const std::string& wrapperName,
std::vector<std::string>& functionNames) const {
//void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
//{
@ -418,30 +444,34 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi
//}
int serialize_id = functionNames.size();
const string
matlabQualName = qualifiedName("."),
matlabUniqueName = qualifiedName(),
cppClassName = qualifiedName("::");
const string wrapFunctionNameSerialize = matlabUniqueName + "_string_serialize_" + boost::lexical_cast<string>(serialize_id);
const string matlabQualName = qualifiedName("."), matlabUniqueName =
qualifiedName(), cppClassName = qualifiedName("::");
const string wrapFunctionNameSerialize = matlabUniqueName
+ "_string_serialize_" + boost::lexical_cast<string>(serialize_id);
functionNames.push_back(wrapFunctionNameSerialize);
// call
//void Point3_string_serialize_17(int nargout, mxArray *out[], int nargin, const mxArray *in[])
wrapperFile.oss << "void " << wrapFunctionNameSerialize << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
wrapperFile.oss << "void " << wrapFunctionNameSerialize
<< "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
wrapperFile.oss << "{\n";
wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl;
wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName
<< "> Shared;" << endl;
// check arguments - for serialize, no arguments
// example: checkArguments("string_serialize",nargout,nargin-1,0);
wrapperFile.oss << " checkArguments(\"string_serialize\",nargout,nargin-1,0);\n";
wrapperFile.oss
<< " checkArguments(\"string_serialize\",nargout,nargin-1,0);\n";
// get class pointer
// example: Shared obj = unwrap_shared_ptr<Point3>(in[0], "ptr_Point3");
wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
wrapperFile.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName
<< ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
// Serialization boilerplate
wrapperFile.oss << " std::ostringstream out_archive_stream;\n";
wrapperFile.oss << " boost::archive::text_oarchive out_archive(out_archive_stream);\n";
wrapperFile.oss
<< " boost::archive::text_oarchive out_archive(out_archive_stream);\n";
wrapperFile.oss << " out_archive << *obj;\n";
wrapperFile.oss << " out[0] = wrap< string >(out_archive_stream.str());\n";
@ -459,13 +489,19 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi
// end
// end
proxyFile.oss << " function varargout = string_serialize(this, varargin)\n";
proxyFile.oss << " % STRING_SERIALIZE usage: string_serialize() : returns string\n";
proxyFile.oss << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
proxyFile.oss
<< " function varargout = string_serialize(this, varargin)\n";
proxyFile.oss
<< " % STRING_SERIALIZE usage: string_serialize() : returns string\n";
proxyFile.oss
<< " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
proxyFile.oss << " if length(varargin) == 0\n";
proxyFile.oss << " varargout{1} = " << wrapperName << "(" << boost::lexical_cast<string>(serialize_id) << ", this, varargin{:});\n";
proxyFile.oss << " varargout{1} = " << wrapperName << "("
<< boost::lexical_cast<string>(serialize_id) << ", this, varargin{:});\n";
proxyFile.oss << " else\n";
proxyFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << ".string_serialize');\n";
proxyFile.oss
<< " error('Arguments do not match any overload of function "
<< matlabQualName << ".string_serialize');\n";
proxyFile.oss << " end\n";
proxyFile.oss << " end\n\n";
@ -476,14 +512,16 @@ void Class::serialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFi
// end
proxyFile.oss << " function sobj = saveobj(obj)\n";
proxyFile.oss << " % SAVEOBJ Saves the object to a matlab-readable format\n";
proxyFile.oss
<< " % SAVEOBJ Saves the object to a matlab-readable format\n";
proxyFile.oss << " sobj = obj.string_serialize();\n";
proxyFile.oss << " end\n";
}
/* ************************************************************************* */
void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
const std::string& wrapperName, std::vector<std::string>& functionNames) const {
void Class::deserialization_fragments(FileWriter& proxyFile,
FileWriter& wrapperFile, const std::string& wrapperName,
std::vector<std::string>& functionNames) const {
//void Point3_string_deserialize_18(int nargout, mxArray *out[], int nargin, const mxArray *in[])
//{
// typedef boost::shared_ptr<Point3> Shared;
@ -495,32 +533,36 @@ void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapper
// in_archive >> *output;
// out[0] = wrap_shared_ptr(output,"Point3", false);
//}
int deserialize_id = functionNames.size();
const string
matlabQualName = qualifiedName("."),
matlabUniqueName = qualifiedName(),
cppClassName = qualifiedName("::");
const string wrapFunctionNameDeserialize = matlabUniqueName + "_string_deserialize_" + boost::lexical_cast<string>(deserialize_id);
functionNames.push_back(wrapFunctionNameDeserialize);
int deserialize_id = functionNames.size();
const string matlabQualName = qualifiedName("."), matlabUniqueName =
qualifiedName(), cppClassName = qualifiedName("::");
const string wrapFunctionNameDeserialize = matlabUniqueName
+ "_string_deserialize_" + boost::lexical_cast<string>(deserialize_id);
functionNames.push_back(wrapFunctionNameDeserialize);
// call
wrapperFile.oss << "void " << wrapFunctionNameDeserialize << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
wrapperFile.oss << "{\n";
wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl;
// call
wrapperFile.oss << "void " << wrapFunctionNameDeserialize
<< "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
wrapperFile.oss << "{\n";
wrapperFile.oss << " typedef boost::shared_ptr<" << cppClassName
<< "> Shared;" << endl;
// check arguments - for deserialize, 1 string argument
wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << ".string_deserialize\",nargout,nargin,1);\n";
// check arguments - for deserialize, 1 string argument
wrapperFile.oss << " checkArguments(\"" << matlabUniqueName
<< ".string_deserialize\",nargout,nargin,1);\n";
// string argument with deserialization boilerplate
wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n";
wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n";
wrapperFile.oss << " boost::archive::text_iarchive in_archive(in_archive_stream);\n";
wrapperFile.oss << " Shared output(new " << cppClassName << "());\n";
wrapperFile.oss << " in_archive >> *output;\n";
wrapperFile.oss << " out[0] = wrap_shared_ptr(output,\"" << matlabQualName << "\", false);\n";
wrapperFile.oss << "}\n";
// string argument with deserialization boilerplate
wrapperFile.oss << " string serialized = unwrap< string >(in[0]);\n";
wrapperFile.oss << " std::istringstream in_archive_stream(serialized);\n";
wrapperFile.oss
<< " boost::archive::text_iarchive in_archive(in_archive_stream);\n";
wrapperFile.oss << " Shared output(new " << cppClassName << "());\n";
wrapperFile.oss << " in_archive >> *output;\n";
wrapperFile.oss << " out[0] = wrap_shared_ptr(output,\"" << matlabQualName
<< "\", false);\n";
wrapperFile.oss << "}\n";
// Generate matlab function
// Generate matlab function
// function varargout = string_deserialize(varargin)
// % STRING_DESERIALIZE usage: string_deserialize() : returns Point3
// % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
@ -531,32 +573,40 @@ void Class::deserialization_fragments(FileWriter& proxyFile, FileWriter& wrapper
// end
// end
proxyFile.oss << " function varargout = string_deserialize(varargin)\n";
proxyFile.oss << " % STRING_DESERIALIZE usage: string_deserialize() : returns " << matlabQualName << "\n";
proxyFile.oss << " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
proxyFile.oss << " if length(varargin) == 1\n";
proxyFile.oss << " varargout{1} = " << wrapperName << "(" << boost::lexical_cast<string>(deserialize_id) << ", varargin{:});\n";
proxyFile.oss << " else\n";
proxyFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << ".string_deserialize');\n";
proxyFile.oss << " end\n";
proxyFile.oss << " end\n\n";
proxyFile.oss << " function varargout = string_deserialize(varargin)\n";
proxyFile.oss
<< " % STRING_DESERIALIZE usage: string_deserialize() : returns "
<< matlabQualName << "\n";
proxyFile.oss
<< " % Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html\n";
proxyFile.oss << " if length(varargin) == 1\n";
proxyFile.oss << " varargout{1} = " << wrapperName << "("
<< boost::lexical_cast<string>(deserialize_id) << ", varargin{:});\n";
proxyFile.oss << " else\n";
proxyFile.oss
<< " error('Arguments do not match any overload of function "
<< matlabQualName << ".string_deserialize');\n";
proxyFile.oss << " end\n";
proxyFile.oss << " end\n\n";
// Generate matlab load function
// Generate matlab load function
// function obj = loadobj(sobj)
// % LOADOBJ Saves the object to a matlab-readable format
// obj = Point3.string_deserialize(sobj);
// end
proxyFile.oss << " function obj = loadobj(sobj)\n";
proxyFile.oss << " % LOADOBJ Saves the object to a matlab-readable format\n";
proxyFile.oss << " obj = " << matlabQualName << ".string_deserialize(sobj);\n";
proxyFile.oss << " end" << endl;
proxyFile.oss << " function obj = loadobj(sobj)\n";
proxyFile.oss
<< " % LOADOBJ Saves the object to a matlab-readable format\n";
proxyFile.oss << " obj = " << matlabQualName
<< ".string_deserialize(sobj);\n";
proxyFile.oss << " end" << endl;
}
/* ************************************************************************* */
std::string Class::getSerializationExport() const {
//BOOST_CLASS_EXPORT_GUID(gtsam::SharedDiagonal, "gtsamSharedDiagonal");
return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \"" + qualifiedName() + "\");";
return "BOOST_CLASS_EXPORT_GUID(" + qualifiedName("::") + ", \""
+ qualifiedName() + "\");";
}
/* ************************************************************************* */

1
wrap/Class.h
View File

@ -27,7 +27,6 @@
#include "Method.h"
#include "StaticMethod.h"
#include "TypeAttributesTable.h"
#include <boost/algorithm/string.hpp>
namespace wrap {

19
wrap/Constructor.h
View File

@ -18,11 +18,11 @@
#pragma once
#include "Argument.h"
#include <string>
#include <vector>
#include "Argument.h"
namespace wrap {
// Constructor class
@ -34,7 +34,7 @@ struct Constructor {
}
// Then the instance variables are set directly by the Module constructor
std::vector<ArgumentList> args_list;
std::vector<ArgumentList> args_list;
std::string name;
bool verbose_;
@ -50,21 +50,18 @@ struct Constructor {
* if nargin == 2, obj.self = new_Pose3_RP(varargin{1},varargin{2}); end
*/
void proxy_fragment(FileWriter& file, const std::string& wrapperName,
bool hasParent, const int id, const ArgumentList args) const;
bool hasParent, const int id, const ArgumentList args) const;
/// cpp wrapper
std::string wrapper_fragment(FileWriter& file,
const std::string& cppClassName,
const std::string& matlabUniqueName,
const std::string& cppBaseClassName,
int id,
const ArgumentList& al) const;
const std::string& cppClassName, const std::string& matlabUniqueName,
const std::string& cppBaseClassName, int id,
const ArgumentList& al) const;
/// constructor function
void generate_construct(FileWriter& file, const std::string& cppClassName,
std::vector<ArgumentList>& args_list) const;
std::vector<ArgumentList>& args_list) const;
};
} // \namespace wrap

2
wrap/FileWriter.cpp
View File

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

94
wrap/GlobalFunction.cpp
View File

@ -17,7 +17,8 @@ using namespace std;
/* ************************************************************************* */
void GlobalFunction::addOverload(bool verbose, const std::string& name,
const ArgumentList& args, const ReturnValue& retVal, const StrVec& ns_stack) {
const ArgumentList& args, const ReturnValue& retVal,
const StrVec& ns_stack) {
this->verbose_ = verbose;
this->name = name;
this->argLists.push_back(args);
@ -26,16 +27,16 @@ void GlobalFunction::addOverload(bool verbose, const std::string& name,
}
/* ************************************************************************* */
void GlobalFunction::matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName,
const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile,
std::vector<std::string>& functionNames) const {
void GlobalFunction::matlab_proxy(const std::string& toolboxPath,
const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
FileWriter& file, std::vector<std::string>& functionNames) const {
// cluster overloads with same namespace
// create new GlobalFunction structures around namespaces - same namespaces and names are overloads
// map of namespace to global function
typedef map<string, GlobalFunction> GlobalFunctionMap;
GlobalFunctionMap grouped_functions;
for (size_t i=0; i<namespaces.size(); ++i) {
for (size_t i = 0; i < namespaces.size(); ++i) {
StrVec ns = namespaces.at(i);
string str_ns = qualifiedName("", ns, "");
ReturnValue ret = returnVals.at(i);
@ -51,16 +52,17 @@ void GlobalFunction::matlab_proxy(const std::string& toolboxPath, const std::str
size_t lastcheck = grouped_functions.size();
BOOST_FOREACH(const GlobalFunctionMap::value_type& p, grouped_functions) {
p.second.generateSingleFunction(toolboxPath, wrapperName, typeAttributes, wrapperFile, functionNames);
p.second.generateSingleFunction(toolboxPath, wrapperName, typeAttributes,
file, functionNames);
if (--lastcheck != 0)
wrapperFile.oss << endl;
file.oss << endl;
}
}
/* ************************************************************************* */
void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, const std::string& wrapperName,
const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile,
std::vector<std::string>& functionNames) const {
void GlobalFunction::generateSingleFunction(const std::string& toolboxPath,
const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
FileWriter& file, std::vector<std::string>& functionNames) const {
// create the folder for the namespace
const StrVec& ns = namespaces.front();
@ -68,84 +70,68 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, cons
// open destination mfunctionFileName
string mfunctionFileName = toolboxPath;
if(!ns.empty())
if (!ns.empty())
mfunctionFileName += "/+" + wrap::qualifiedName("/+", ns);
mfunctionFileName += "/" + name + ".m";
FileWriter mfunctionFile(mfunctionFileName, verbose_, "%");
// get the name of actual matlab object
const string
matlabQualName = qualifiedName(".", ns, name),
matlabUniqueName = qualifiedName("", ns, name),
cppName = qualifiedName("::", ns, name);
const string matlabQualName = qualifiedName(".", ns, name), matlabUniqueName =
qualifiedName("", ns, name), cppName = qualifiedName("::", ns, name);
mfunctionFile.oss << "function varargout = " << name << "(varargin)\n";
for(size_t overload = 0; overload < argLists.size(); ++overload) {
for (size_t overload = 0; overload < argLists.size(); ++overload) {
const ArgumentList& args = argLists[overload];
const ReturnValue& returnVal = returnVals[overload];
size_t nrArgs = args.size();
const int id = functionNames.size();
// Output proxy matlab code
// check for number of arguments...
mfunctionFile.oss << (overload==0?"":"else") << "if length(varargin) == " << nrArgs;
if (nrArgs>0) mfunctionFile.oss << " && ";
// ...and their types
bool first = true;
for(size_t i=0;i<nrArgs;i++) {
if (!first) mfunctionFile.oss << " && ";
mfunctionFile.oss << "isa(varargin{" << i+1 << "},'" << args[i].matlabClass(".") << "')";
first=false;
}
mfunctionFile.oss << "\n";
// output call to C++ wrapper
string output;
if(returnVal.isPair)
output = "[ varargout{1} varargout{2} ] = ";
else if(returnVal.category1 == ReturnValue::VOID)
output = "";
else
output = "varargout{1} = ";
mfunctionFile.oss << " " << output << wrapperName << "(" << id << ", varargin{:});\n";
mfunctionFile.oss << " " << (overload == 0 ? "" : "else");
argLists[overload].emit_conditional_call(mfunctionFile,
returnVals[overload], wrapperName, id, true); // true omits "this"
// Output C++ wrapper code
const string wrapFunctionName = matlabUniqueName + "_" + boost::lexical_cast<string>(id);
const string wrapFunctionName = matlabUniqueName + "_"
+ boost::lexical_cast<string>(id);
// call
wrapperFile.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
file.oss << "void " << wrapFunctionName
<< "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
// start
wrapperFile.oss << "{\n";
file.oss << "{\n";
returnVal.wrapTypeUnwrap(wrapperFile);
returnVal.wrapTypeUnwrap(file);
// check arguments
// NOTE: for static functions, there is no object passed
wrapperFile.oss << " checkArguments(\"" << matlabUniqueName << "\",nargout,nargin," << args.size() << ");\n";
file.oss << " checkArguments(\"" << matlabUniqueName
<< "\",nargout,nargin," << args.size() << ");\n";
// unwrap arguments, see Argument.cpp
args.matlab_unwrap(wrapperFile,0); // We start at 0 because there is no self object
args.matlab_unwrap(file, 0); // We start at 0 because there is no self object
// call method with default type and wrap result
if (returnVal.type1!="void")
returnVal.wrap_result(cppName+"("+args.names()+")", wrapperFile, typeAttributes);
if (returnVal.type1 != "void")
returnVal.wrap_result(cppName + "(" + args.names() + ")", file,
typeAttributes);
else
wrapperFile.oss << cppName+"("+args.names()+");\n";
file.oss << cppName + "(" + args.names() + ");\n";
// finish
wrapperFile.oss << "}\n";
file.oss << "}\n";
// Add to function list
functionNames.push_back(wrapFunctionName);
}
mfunctionFile.oss << "else\n";
mfunctionFile.oss << " error('Arguments do not match any overload of function " << matlabQualName << "');" << endl;
mfunctionFile.oss << "end" << endl;
mfunctionFile.oss << " else\n";
mfunctionFile.oss
<< " error('Arguments do not match any overload of function "
<< matlabQualName << "');" << endl;
mfunctionFile.oss << " end" << endl;
// Close file
mfunctionFile.emit(true);
@ -153,9 +139,5 @@ void GlobalFunction::generateSingleFunction(const std::string& toolboxPath, cons
/* ************************************************************************* */
} // \namespace wrap

33
wrap/GlobalFunction.h
View File

@ -22,37 +22,38 @@ struct GlobalFunction {
std::string name;
// each overload, regardless of namespace
std::vector<ArgumentList> argLists; ///< arugments for each overload
std::vector<ReturnValue> returnVals; ///< returnVals for each overload
std::vector<StrVec> namespaces; ///< Stack of namespaces
std::vector<ArgumentList> argLists; ///< arugments for each overload
std::vector<ReturnValue> returnVals; ///< returnVals for each overload
std::vector<StrVec> namespaces; ///< Stack of namespaces
// Constructor only used in Module
GlobalFunction(bool verbose = true) : verbose_(verbose) {}
GlobalFunction(bool verbose = true) :
verbose_(verbose) {
}
// Used to reconstruct
GlobalFunction(const std::string& name_, bool verbose = true)
: verbose_(verbose), name(name_) {}
GlobalFunction(const std::string& name_, bool verbose = true) :
verbose_(verbose), name(name_) {
}
// adds an overloaded version of this function
void addOverload(bool verbose, const std::string& name,
const ArgumentList& args, const ReturnValue& retVal, const StrVec& ns_stack);
const ArgumentList& args, const ReturnValue& retVal,
const StrVec& ns_stack);
// codegen function called from Module to build the cpp and matlab versions of the function
void matlab_proxy(const std::string& toolboxPath, const std::string& wrapperName,
const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile,
std::vector<std::string>& functionNames) const;
void matlab_proxy(const std::string& toolboxPath,
const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
FileWriter& file, std::vector<std::string>& functionNames) const;
private:
// Creates a single global function - all in same namespace
void generateSingleFunction(const std::string& toolboxPath, const std::string& wrapperName,
const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile,
std::vector<std::string>& functionNames) const;
void generateSingleFunction(const std::string& toolboxPath,
const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
FileWriter& file, std::vector<std::string>& functionNames) const;
};
} // \namespace wrap

216
wrap/Method.cpp
View File

@ -15,14 +15,15 @@
* @author Richard Roberts
**/
#include <iostream>
#include <fstream>
#include "Method.h"
#include "utilities.h"
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>
#include "Method.h"
#include "utilities.h"
#include <iostream>
#include <fstream>
using namespace std;
using namespace wrap;
@ -38,155 +39,140 @@ void Method::addOverload(bool verbose, bool is_const, const std::string& name,
}
/* ************************************************************************* */
void Method::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
const string& cppClassName,
const std::string& matlabQualName,
const std::string& matlabUniqueName,
const string& wrapperName,
const TypeAttributesTable& typeAttributes,
vector<string>& functionNames) const {
void Method::proxy_wrapper_fragments(FileWriter& file, FileWriter& wrapperFile,
const string& cppClassName, const std::string& matlabQualName,
const std::string& matlabUniqueName, const string& wrapperName,
const TypeAttributesTable& typeAttributes,
vector<string>& functionNames) const {
proxyFile.oss << " function varargout = " << name << "(this, varargin)\n";
//Comments for documentation
string up_name = boost::to_upper_copy(name);
proxyFile.oss << " % " << up_name << " usage:";
// Create function header
file.oss << " function varargout = " << name << "(this, varargin)\n";
// Emit comments for documentation
string up_name = boost::to_upper_copy(name);
file.oss << " % " << up_name << " usage: ";
unsigned int argLCount = 0;
BOOST_FOREACH(ArgumentList argList, argLists)
{
proxyFile.oss << " " << name << "(";
unsigned int i = 0;
BOOST_FOREACH(const Argument& arg, argList)
{
if(i != argList.size()-1)
proxyFile.oss << arg.type << " " << arg.name << ", ";
else
proxyFile.oss << arg.type << " " << arg.name;
i++;
}
if(argLCount != argLists.size()-1)
proxyFile.oss << "), ";
else
proxyFile.oss << ") : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl;
argLCount++;
}
proxyFile.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl;
proxyFile.oss << " % " << "" << endl;
proxyFile.oss << " % " << "Method Overloads" << endl;
BOOST_FOREACH(ArgumentList argList, argLists)
{
proxyFile.oss << " % " << name << "(";
unsigned int i = 0;
BOOST_FOREACH(const Argument& arg, argList)
{
if(i != argList.size()-1)
proxyFile.oss << arg.type << " " << arg.name << ", ";
else
proxyFile.oss << arg.type << " " << arg.name;
i++;
}
proxyFile.oss << ")" << endl;
}
for(size_t overload = 0; overload < argLists.size(); ++overload) {
const ArgumentList& args = argLists[overload];
const ReturnValue& returnVal = returnVals[overload];
size_t nrArgs = args.size();
const int id = functionNames.size();
// Output proxy matlab code
// check for number of arguments...
proxyFile.oss << " " << (overload==0?"":"else") << "if length(varargin) == " << nrArgs;
if (nrArgs>0) proxyFile.oss << " && ";
// ...and their types
bool first = true;
for(size_t i=0;i<nrArgs;i++) {
if (!first) proxyFile.oss << " && ";
proxyFile.oss << "isa(varargin{" << i+1 << "},'" << args[i].matlabClass(".") << "')";
first=false;
}
proxyFile.oss << "\n";
// output call to C++ wrapper
string output;
if(returnVal.isPair)
output = "[ varargout{1} varargout{2} ] = ";
else if(returnVal.category1 == ReturnValue::VOID)
output = "";
BOOST_FOREACH(ArgumentList argList, argLists) {
argList.emit_prototype(file, name);
if (argLCount != argLists.size() - 1)
file.oss << ", ";
else
output = "varargout{1} = ";
proxyFile.oss << " " << output << wrapperName << "(" << id << ", this, varargin{:});\n";
file.oss << " : returns "
<< returnVals[0].return_type(false, returnVals[0].pair) << endl;
argLCount++;
}
// Emit URL to Doxygen page
file.oss << " % "
<< "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html"
<< endl;
// Document all overloads, if any
if (argLists.size() > 1) {
file.oss << " % " << "" << endl;
file.oss << " % " << "Method Overloads" << endl;
BOOST_FOREACH(ArgumentList argList, argLists) {
file.oss << " % ";
argList.emit_prototype(file, name);
file.oss << endl;
}
}
// Handle special case of single overload with all numeric arguments
if (argLists.size() == 1 && argLists[0].allScalar()) {
// Output proxy matlab code
file.oss << " ";
const int id = (int) functionNames.size();
argLists[0].emit_call(file, returnVals[0], wrapperName, id);
// Output C++ wrapper code
const string wrapFunctionName = wrapper_fragment(
wrapperFile, cppClassName, matlabUniqueName, overload, id, typeAttributes);
const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName,
matlabUniqueName, 0, id, typeAttributes);
// Add to function list
functionNames.push_back(wrapFunctionName);
} else {
// Check arguments for all overloads
for (size_t overload = 0; overload < argLists.size(); ++overload) {
// Output proxy matlab code
file.oss << " " << (overload == 0 ? "" : "else");
const int id = (int) functionNames.size();
argLists[overload].emit_conditional_call(file, returnVals[overload],
wrapperName, id);
// Output C++ wrapper code
const string wrapFunctionName = wrapper_fragment(wrapperFile,
cppClassName, matlabUniqueName, overload, id, typeAttributes);
// Add to function list
functionNames.push_back(wrapFunctionName);
}
file.oss << " else\n";
file.oss
<< " error('Arguments do not match any overload of function "
<< matlabQualName << "." << name << "');" << endl;
file.oss << " end\n";
}
proxyFile.oss << " else\n";
proxyFile.oss << " error('Arguments do not match any overload of function " <<
matlabQualName << "." << name << "');" << endl;
proxyFile.oss << " end\n";
proxyFile.oss << " end\n";
file.oss << " end\n";
}
/* ************************************************************************* */
string Method::wrapper_fragment(FileWriter& file,
const string& cppClassName,
const string& matlabUniqueName,
int overload,
int id,
const TypeAttributesTable& typeAttributes) const {
string Method::wrapper_fragment(FileWriter& file, const string& cppClassName,
const string& matlabUniqueName, int overload, int id,
const TypeAttributesTable& typeAttributes) const {
// generate code
const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + boost::lexical_cast<string>(id);
const string wrapFunctionName = matlabUniqueName + "_" + name + "_"
+ boost::lexical_cast<string>(id);
const ArgumentList& args = argLists[overload];
const ReturnValue& returnVal = returnVals[overload];
// call
file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
file.oss << "void " << wrapFunctionName
<< "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
// start
file.oss << "{\n";
if(returnVal.isPair)
{
if(returnVal.category1 == ReturnValue::CLASS)
file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl;
if(returnVal.category2 == ReturnValue::CLASS)
file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2 << ";"<< endl;
}
else
if(returnVal.category1 == ReturnValue::CLASS)
file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1 << ";"<< endl;
if (returnVal.isPair) {
if (returnVal.category1 == ReturnValue::CLASS)
file.oss << " typedef boost::shared_ptr<"
<< returnVal.qualifiedType1("::") << "> Shared" << returnVal.type1
<< ";" << endl;
if (returnVal.category2 == ReturnValue::CLASS)
file.oss << " typedef boost::shared_ptr<"
<< returnVal.qualifiedType2("::") << "> Shared" << returnVal.type2
<< ";" << endl;
} else if (returnVal.category1 == ReturnValue::CLASS)
file.oss << " typedef boost::shared_ptr<" << returnVal.qualifiedType1("::")
<< "> Shared" << returnVal.type1 << ";" << endl;
file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl;
file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;"
<< endl;
// check arguments
// extra argument obj -> nargin-1 is passed !
// example: checkArguments("equals",nargout,nargin-1,2);
file.oss << " checkArguments(\"" << name << "\",nargout,nargin-1," << args.size() << ");\n";
file.oss << " checkArguments(\"" << name << "\",nargout,nargin-1,"
<< args.size() << ");\n";
// get class pointer
// example: shared_ptr<Test> = unwrap_shared_ptr< Test >(in[0], "Test");
file.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName << ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
file.oss << " Shared obj = unwrap_shared_ptr<" << cppClassName
<< ">(in[0], \"ptr_" << matlabUniqueName << "\");" << endl;
// unwrap arguments, see Argument.cpp
args.matlab_unwrap(file,1);
args.matlab_unwrap(file, 1);
// call method and wrap result
// example: out[0]=wrap<bool>(self->return_field(t));
if (returnVal.type1!="void")
returnVal.wrap_result("obj->"+name+"("+args.names()+")", file, typeAttributes);
if (returnVal.type1 != "void")
returnVal.wrap_result("obj->" + name + "(" + args.names() + ")", file,
typeAttributes);
else
file.oss << " obj->"+name+"("+args.names()+");\n";
file.oss << " obj->" + name + "(" + args.names() + ");\n";
// finish
file.oss << "}\n";

7
wrap/Method.h
View File

@ -18,13 +18,12 @@
#pragma once
#include <string>
#include <list>
#include "Argument.h"
#include "ReturnValue.h"
#include "TypeAttributesTable.h"
#include <boost/algorithm/string.hpp>
#include <string>
#include <list>
namespace wrap {

2
wrap/Module.cpp
View File

@ -98,7 +98,7 @@ void Module::parseMarkup(const std::string& data) {
// The one with postfix 0 are used to reset the variables after parse.
string methodName, methodName0;
bool isConst, isConst0 = false;
ReturnValue retVal0(verbose), retVal(verbose);
ReturnValue retVal0, retVal;
Argument arg0, arg;
ArgumentList args0, args;
vector<string> arg_dup; ///keep track of duplicates

8
wrap/ReturnValue.cpp
View File

@ -141,5 +141,13 @@ void ReturnValue::wrapTypeUnwrap(FileWriter& wrapperFile) const {
}
}
/* ************************************************************************* */
void ReturnValue::emit_matlab(FileWriter& file) const {
string output;
if (isPair)
file.oss << "[ varargout{1} varargout{2} ] = ";
else if (category1 != ReturnValue::VOID)
file.oss << "varargout{1} = ";
}
/* ************************************************************************* */

30
wrap/ReturnValue.h
View File

@ -8,36 +8,36 @@
* @author Richard Roberts
*/
#include <vector>
#include <map>
#include "FileWriter.h"
#include "TypeAttributesTable.h"
#include <vector>
#pragma once
namespace wrap {
/**
* Encapsulates return value of a method or function
*/
struct ReturnValue {
/// the different supported return value categories
typedef enum {
CLASS = 1,
EIGEN = 2,
BASIS = 3,
VOID = 4
CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4
} return_category;
ReturnValue(bool enable_verbosity = true)
: verbose(enable_verbosity), isPtr1(false), isPtr2(false),
isPair(false), category1(CLASS), category2(CLASS)
{}
bool verbose;
bool isPtr1, isPtr2, isPair;
return_category category1, category2;
std::string type1, type2;
std::vector<std::string> namespaces1, namespaces2;
/// Constructor
ReturnValue() :
isPtr1(false), isPtr2(false), isPair(false), category1(CLASS), category2(
CLASS) {
}
typedef enum {
arg1, arg2, pair
} pairing;
@ -49,10 +49,12 @@ struct ReturnValue {
std::string matlab_returnType() const;
void wrap_result(const std::string& result, FileWriter& file, const TypeAttributesTable& typeAttributes) const;
void wrap_result(const std::string& result, FileWriter& file,
const TypeAttributesTable& typeAttributes) const;
void wrapTypeUnwrap(FileWriter& wrapperFile) const;
void emit_matlab(FileWriter& file) const;
};
} // \namespace wrap

169
wrap/StaticMethod.cpp
View File

@ -16,19 +16,19 @@
* @author Richard Roberts
**/
#include <iostream>
#include <fstream>
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include "StaticMethod.h"
#include "utilities.h"
#include <boost/foreach.hpp>
#include <boost/lexical_cast.hpp>
#include <boost/algorithm/string.hpp>
#include <iostream>
#include <fstream>
using namespace std;
using namespace wrap;
/* ************************************************************************* */
void StaticMethod::addOverload(bool verbose, const std::string& name,
const ArgumentList& args, const ReturnValue& retVal) {
@ -39,144 +39,103 @@ void StaticMethod::addOverload(bool verbose, const std::string& name,
}
/* ************************************************************************* */
void StaticMethod::proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
const string& cppClassName,
const std::string& matlabQualName,
const std::string& matlabUniqueName,
const string& wrapperName,
const TypeAttributesTable& typeAttributes,
vector<string>& functionNames) const {
void StaticMethod::proxy_wrapper_fragments(FileWriter& file,
FileWriter& wrapperFile, const string& cppClassName,
const std::string& matlabQualName, const std::string& matlabUniqueName,
const string& wrapperName, const TypeAttributesTable& typeAttributes,
vector<string>& functionNames) const {
string upperName = name; upperName[0] = std::toupper(upperName[0], std::locale());
string upperName = name;
upperName[0] = std::toupper(upperName[0], std::locale());
proxyFile.oss << " function varargout = " << upperName << "(varargin)\n";
file.oss << " function varargout = " << upperName << "(varargin)\n";
//Comments for documentation
string up_name = boost::to_upper_copy(name);
proxyFile.oss << " % " << up_name << " usage:";
string up_name = boost::to_upper_copy(name);
file.oss << " % " << up_name << " usage: ";
unsigned int argLCount = 0;
BOOST_FOREACH(ArgumentList argList, argLists)
{
proxyFile.oss << " " << name << "(";
unsigned int i = 0;
BOOST_FOREACH(const Argument& arg, argList)
{
if(i != argList.size()-1)
proxyFile.oss << arg.type << " " << arg.name << ", ";
else
proxyFile.oss << arg.type << " " << arg.name;
i++;
}
if(argLCount != argLists.size()-1)
proxyFile.oss << "), ";
else
proxyFile.oss << ") : returns " << returnVals[0].return_type(false, returnVals[0].pair) << endl;
argLCount++;
}
BOOST_FOREACH(ArgumentList argList, argLists) {
argList.emit_prototype(file, name);
if (argLCount != argLists.size() - 1)
file.oss << ", ";
else
file.oss << " : returns "
<< returnVals[0].return_type(false, returnVals[0].pair) << endl;
argLCount++;
}
proxyFile.oss << " % " << "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html" << endl;
proxyFile.oss << " % " << "" << endl;
proxyFile.oss << " % " << "Usage" << endl;
BOOST_FOREACH(ArgumentList argList, argLists)
{
proxyFile.oss << " % " << up_name << "(";
unsigned int i = 0;
BOOST_FOREACH(const Argument& arg, argList)
{
if(i != argList.size()-1)
proxyFile.oss << arg.type << " " << arg.name << ", ";
else
proxyFile.oss << arg.type << " " << arg.name;
i++;
}
proxyFile.oss << ")" << endl;
}
file.oss << " % "
<< "Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html"
<< endl;
file.oss << " % " << "" << endl;
file.oss << " % " << "Usage" << endl;
BOOST_FOREACH(ArgumentList argList, argLists) {
file.oss << " % ";
argList.emit_prototype(file, up_name);
file.oss << endl;
}
for(size_t overload = 0; overload < argLists.size(); ++overload) {
const ArgumentList& args = argLists[overload];
const ReturnValue& returnVal = returnVals[overload];
size_t nrArgs = args.size();
const int id = (int)functionNames.size();
// Check arguments for all overloads
for (size_t overload = 0; overload < argLists.size(); ++overload) {
// Output proxy matlab code
// check for number of arguments...
proxyFile.oss << " " << (overload==0?"":"else") << "if length(varargin) == " << nrArgs;
if (nrArgs>0) proxyFile.oss << " && ";
// ...and their types
bool first = true;
for(size_t i=0;i<nrArgs;i++) {
if (!first) proxyFile.oss << " && ";
proxyFile.oss << "isa(varargin{" << i+1 << "},'" << args[i].matlabClass(".") << "')";
first=false;
}
proxyFile.oss << "\n";
// output call to C++ wrapper
string output;
if(returnVal.isPair)
output = "[ varargout{1} varargout{2} ] = ";
else if(returnVal.category1 == ReturnValue::VOID)
output = "";
else
output = "varargout{1} = ";
proxyFile.oss << " " << output << wrapperName << "(" << id << ", varargin{:});\n";
file.oss << " " << (overload == 0 ? "" : "else");
const int id = (int) functionNames.size();
argLists[overload].emit_conditional_call(file, returnVals[overload],
wrapperName, id, true); // last bool is to indicate static !
// Output C++ wrapper code
const string wrapFunctionName = wrapper_fragment(
wrapperFile, cppClassName, matlabUniqueName, (int)overload, id, typeAttributes);
const string wrapFunctionName = wrapper_fragment(wrapperFile, cppClassName,
matlabUniqueName, (int) overload, id, typeAttributes);
// Add to function list
functionNames.push_back(wrapFunctionName);
}
file.oss << " else\n";
file.oss << " error('Arguments do not match any overload of function "
<< matlabQualName << "." << upperName << "');" << endl;
file.oss << " end\n";
proxyFile.oss << " else\n";
proxyFile.oss << " error('Arguments do not match any overload of function " <<
matlabQualName << "." << upperName << "');" << endl;
proxyFile.oss << " end\n";
proxyFile.oss << " end\n";
file.oss << " end\n";
}
/* ************************************************************************* */
string StaticMethod::wrapper_fragment(FileWriter& file,
const string& cppClassName,
const string& matlabUniqueName,
int overload,
int id,
const TypeAttributesTable& typeAttributes) const {
const string& cppClassName, const string& matlabUniqueName, int overload,
int id, const TypeAttributesTable& typeAttributes) const {
// generate code
const string wrapFunctionName = matlabUniqueName + "_" + name + "_" + boost::lexical_cast<string>(id);
const string wrapFunctionName = matlabUniqueName + "_" + name + "_"
+ boost::lexical_cast<string>(id);
const ArgumentList& args = argLists[overload];
const ReturnValue& returnVal = returnVals[overload];
// call
file.oss << "void " << wrapFunctionName << "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
file.oss << "void " << wrapFunctionName
<< "(int nargout, mxArray *out[], int nargin, const mxArray *in[])\n";
// start
file.oss << "{\n";
returnVal.wrapTypeUnwrap(file);
file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;" << endl;
file.oss << " typedef boost::shared_ptr<" << cppClassName << "> Shared;"
<< endl;
// check arguments
// NOTE: for static functions, there is no object passed
file.oss << " checkArguments(\"" << matlabUniqueName << "." << name << "\",nargout,nargin," << args.size() << ");\n";
file.oss << " checkArguments(\"" << matlabUniqueName << "." << name
<< "\",nargout,nargin," << args.size() << ");\n";
// unwrap arguments, see Argument.cpp
args.matlab_unwrap(file,0); // We start at 0 because there is no self object
args.matlab_unwrap(file, 0); // We start at 0 because there is no self object
// call method with default type and wrap result
if (returnVal.type1!="void")
returnVal.wrap_result(cppClassName+"::"+name+"("+args.names()+")", file, typeAttributes);
if (returnVal.type1 != "void")
returnVal.wrap_result(cppClassName + "::" + name + "(" + args.names() + ")",
file, typeAttributes);
else
file.oss << cppClassName+"::"+name+"("+args.names()+");\n";
file.oss << cppClassName + "::" + name + "(" + args.names() + ");\n";
// finish
file.oss << "}\n";

23
wrap/StaticMethod.h
View File

@ -19,13 +19,9 @@
#pragma once
#include <string>
#include <list>
#include "Argument.h"
#include "ReturnValue.h"
#include "TypeAttributesTable.h"
#include <boost/algorithm/string.hpp>
namespace wrap {
@ -34,7 +30,8 @@ struct StaticMethod {
/// Constructor creates empty object
StaticMethod(bool verbosity = true) :
verbose(verbosity) {}
verbose(verbosity) {
}
// Then the instance variables are set directly by the Module constructor
bool verbose;
@ -46,22 +43,20 @@ struct StaticMethod {
// with those in rhs, but in subsequent calls it adds additional argument
// lists as function overloads.
void addOverload(bool verbose, const std::string& name,
const ArgumentList& args, const ReturnValue& retVal);
const ArgumentList& args, const ReturnValue& retVal);
// MATLAB code generation
// classPath is class directory, e.g., ../matlab/@Point2
void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
const std::string& cppClassName, const std::string& matlabQualName, const std::string& matlabUniqueName,
const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
std::vector<std::string>& functionNames) const;
const std::string& cppClassName, const std::string& matlabQualName,
const std::string& matlabUniqueName, const std::string& wrapperName,
const TypeAttributesTable& typeAttributes,
std::vector<std::string>& functionNames) const;
private:
std::string wrapper_fragment(FileWriter& file,
const std::string& cppClassName,
const std::string& matlabUniqueName,
int overload,
int id,
const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper
const std::string& cppClassName, const std::string& matlabUniqueName,
int overload, int id, const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper
};
} // \namespace wrap

1
wrap/tests/expected/.gitignore vendored Normal file
View File

@ -0,0 +1 @@
*.m~

63
wrap/tests/expected/Point2.m
View File

@ -44,92 +44,43 @@ classdef Point2 < handle
function varargout = argChar(this, varargin)
% ARGCHAR usage: argChar(char a) : returns void
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% argChar(char a)
if length(varargin) == 1 && isa(varargin{1},'char')
geometry_wrapper(4, this, varargin{:});
else
error('Arguments do not match any overload of function Point2.argChar');
end
geometry_wrapper(4, this, varargin{:});
end
function varargout = argUChar(this, varargin)
% ARGUCHAR usage: argUChar(unsigned char a) : returns void
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% argUChar(unsigned char a)
if length(varargin) == 1 && isa(varargin{1},'char')
geometry_wrapper(5, this, varargin{:});
else
error('Arguments do not match any overload of function Point2.argUChar');
end
geometry_wrapper(5, this, varargin{:});
end
function varargout = dim(this, varargin)
% DIM usage: dim() : returns int
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% dim()
if length(varargin) == 0
varargout{1} = geometry_wrapper(6, this, varargin{:});
else
error('Arguments do not match any overload of function Point2.dim');
end
varargout{1} = geometry_wrapper(6, this, varargin{:});
end
function varargout = returnChar(this, varargin)
% RETURNCHAR usage: returnChar() : returns char
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% returnChar()
if length(varargin) == 0
varargout{1} = geometry_wrapper(7, this, varargin{:});
else
error('Arguments do not match any overload of function Point2.returnChar');
end
varargout{1} = geometry_wrapper(7, this, varargin{:});
end
function varargout = vectorConfusion(this, varargin)
% VECTORCONFUSION usage: vectorConfusion() : returns VectorNotEigen
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% vectorConfusion()
if length(varargin) == 0
varargout{1} = geometry_wrapper(8, this, varargin{:});
else
error('Arguments do not match any overload of function Point2.vectorConfusion');
end
varargout{1} = geometry_wrapper(8, this, varargin{:});
end
function varargout = x(this, varargin)
% X usage: x() : returns double
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% x()
if length(varargin) == 0
varargout{1} = geometry_wrapper(9, this, varargin{:});
else
error('Arguments do not match any overload of function Point2.x');
end
varargout{1} = geometry_wrapper(9, this, varargin{:});
end
function varargout = y(this, varargin)
% Y usage: y() : returns double
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% y()
if length(varargin) == 0
varargout{1} = geometry_wrapper(10, this, varargin{:});
else
error('Arguments do not match any overload of function Point2.y');
end
varargout{1} = geometry_wrapper(10, this, varargin{:});
end
end

9
wrap/tests/expected/Point3.m
View File

@ -43,14 +43,7 @@ classdef Point3 < handle
function varargout = norm(this, varargin)
% NORM usage: norm() : returns double
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% norm()
if length(varargin) == 0
varargout{1} = geometry_wrapper(14, this, varargin{:});
else
error('Arguments do not match any overload of function Point3.norm');
end
varargout{1} = geometry_wrapper(14, this, varargin{:});
end
function varargout = string_serialize(this, varargin)

105
wrap/tests/expected/Test.m
View File

@ -56,9 +56,6 @@ classdef Test < handle
function varargout = arg_EigenConstRef(this, varargin)
% ARG_EIGENCONSTREF usage: arg_EigenConstRef(Matrix value) : returns void
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% arg_EigenConstRef(Matrix value)
if length(varargin) == 1 && isa(varargin{1},'double')
geometry_wrapper(23, this, varargin{:});
else
@ -69,61 +66,30 @@ classdef Test < handle
function varargout = create_MixedPtrs(this, varargin)
% CREATE_MIXEDPTRS usage: create_MixedPtrs() : returns pair< Test, SharedTest >
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% create_MixedPtrs()
if length(varargin) == 0
[ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:});
else
error('Arguments do not match any overload of function Test.create_MixedPtrs');
end
[ varargout{1} varargout{2} ] = geometry_wrapper(24, this, varargin{:});
end
function varargout = create_ptrs(this, varargin)
% CREATE_PTRS usage: create_ptrs() : returns pair< SharedTest, SharedTest >
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% create_ptrs()
if length(varargin) == 0
[ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:});
else
error('Arguments do not match any overload of function Test.create_ptrs');
end
[ varargout{1} varargout{2} ] = geometry_wrapper(25, this, varargin{:});
end
function varargout = print(this, varargin)
% PRINT usage: print() : returns void
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% print()
if length(varargin) == 0
geometry_wrapper(26, this, varargin{:});
else
error('Arguments do not match any overload of function Test.print');
end
geometry_wrapper(26, this, varargin{:});
end
function varargout = return_Point2Ptr(this, varargin)
% RETURN_POINT2PTR usage: return_Point2Ptr(bool value) : returns Point2
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% return_Point2Ptr(bool value)
if length(varargin) == 1 && isa(varargin{1},'logical')
varargout{1} = geometry_wrapper(27, this, varargin{:});
else
error('Arguments do not match any overload of function Test.return_Point2Ptr');
end
varargout{1} = geometry_wrapper(27, this, varargin{:});
end
function varargout = return_Test(this, varargin)
% RETURN_TEST usage: return_Test(Test value) : returns Test
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% return_Test(Test value)
if length(varargin) == 1 && isa(varargin{1},'Test')
varargout{1} = geometry_wrapper(28, this, varargin{:});
else
@ -134,9 +100,6 @@ classdef Test < handle
function varargout = return_TestPtr(this, varargin)
% RETURN_TESTPTR usage: return_TestPtr(Test value) : returns Test
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% return_TestPtr(Test value)
if length(varargin) == 1 && isa(varargin{1},'Test')
varargout{1} = geometry_wrapper(29, this, varargin{:});
else
@ -147,35 +110,18 @@ classdef Test < handle
function varargout = return_bool(this, varargin)
% RETURN_BOOL usage: return_bool(bool value) : returns bool
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% return_bool(bool value)
if length(varargin) == 1 && isa(varargin{1},'logical')
varargout{1} = geometry_wrapper(30, this, varargin{:});
else
error('Arguments do not match any overload of function Test.return_bool');
end
varargout{1} = geometry_wrapper(30, this, varargin{:});
end
function varargout = return_double(this, varargin)
% RETURN_DOUBLE usage: return_double(double value) : returns double
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% return_double(double value)
if length(varargin) == 1 && isa(varargin{1},'double')
varargout{1} = geometry_wrapper(31, this, varargin{:});
else
error('Arguments do not match any overload of function Test.return_double');
end
varargout{1} = geometry_wrapper(31, this, varargin{:});
end
function varargout = return_field(this, varargin)
% RETURN_FIELD usage: return_field(Test t) : returns bool
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% return_field(Test t)
if length(varargin) == 1 && isa(varargin{1},'Test')
varargout{1} = geometry_wrapper(32, this, varargin{:});
else
@ -186,22 +132,12 @@ classdef Test < handle
function varargout = return_int(this, varargin)
% RETURN_INT usage: return_int(int value) : returns int
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% return_int(int value)
if length(varargin) == 1 && isa(varargin{1},'numeric')
varargout{1} = geometry_wrapper(33, this, varargin{:});
else
error('Arguments do not match any overload of function Test.return_int');
end
varargout{1} = geometry_wrapper(33, this, varargin{:});
end
function varargout = return_matrix1(this, varargin)
% RETURN_MATRIX1 usage: return_matrix1(Matrix value) : returns Matrix
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% return_matrix1(Matrix value)
if length(varargin) == 1 && isa(varargin{1},'double')
varargout{1} = geometry_wrapper(34, this, varargin{:});
else
@ -212,9 +148,6 @@ classdef Test < handle
function varargout = return_matrix2(this, varargin)
% RETURN_MATRIX2 usage: return_matrix2(Matrix value) : returns Matrix
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% return_matrix2(Matrix value)
if length(varargin) == 1 && isa(varargin{1},'double')
varargout{1} = geometry_wrapper(35, this, varargin{:});
else
@ -225,9 +158,6 @@ classdef Test < handle
function varargout = return_pair(this, varargin)
% RETURN_PAIR usage: return_pair(Vector v, Matrix A) : returns pair< Vector, Matrix >
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% return_pair(Vector v, Matrix A)
if length(varargin) == 2 && isa(varargin{1},'double') && isa(varargin{2},'double')
[ varargout{1} varargout{2} ] = geometry_wrapper(36, this, varargin{:});
else
@ -238,9 +168,6 @@ classdef Test < handle
function varargout = return_ptrs(this, varargin)
% RETURN_PTRS usage: return_ptrs(Test p1, Test p2) : returns pair< SharedTest, SharedTest >
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% return_ptrs(Test p1, Test p2)
if length(varargin) == 2 && isa(varargin{1},'Test') && isa(varargin{2},'Test')
[ varargout{1} varargout{2} ] = geometry_wrapper(37, this, varargin{:});
else
@ -251,22 +178,12 @@ classdef Test < handle
function varargout = return_size_t(this, varargin)
% RETURN_SIZE_T usage: return_size_t(size_t value) : returns size_t
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% return_size_t(size_t value)
if length(varargin) == 1 && isa(varargin{1},'numeric')
varargout{1} = geometry_wrapper(38, this, varargin{:});
else
error('Arguments do not match any overload of function Test.return_size_t');
end
varargout{1} = geometry_wrapper(38, this, varargin{:});
end
function varargout = return_string(this, varargin)
% RETURN_STRING usage: return_string(string value) : returns string
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% return_string(string value)
if length(varargin) == 1 && isa(varargin{1},'char')
varargout{1} = geometry_wrapper(39, this, varargin{:});
else
@ -277,9 +194,6 @@ classdef Test < handle
function varargout = return_vector1(this, varargin)
% RETURN_VECTOR1 usage: return_vector1(Vector value) : returns Vector
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% return_vector1(Vector value)
if length(varargin) == 1 && isa(varargin{1},'double')
varargout{1} = geometry_wrapper(40, this, varargin{:});
else
@ -290,9 +204,6 @@ classdef Test < handle
function varargout = return_vector2(this, varargin)
% RETURN_VECTOR2 usage: return_vector2(Vector value) : returns Vector
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% return_vector2(Vector value)
if length(varargin) == 1 && isa(varargin{1},'double')
varargout{1} = geometry_wrapper(41, this, varargin{:});
else

10
wrap/tests/expected/aGlobalFunction.m
View File

@ -1,6 +1,6 @@
function varargout = aGlobalFunction(varargin)
if length(varargin) == 0
varargout{1} = geometry_wrapper(42, varargin{:});
else
error('Arguments do not match any overload of function aGlobalFunction');
end
if length(varargin) == 0
varargout{1} = geometry_wrapper(42, varargin{:});
else
error('Arguments do not match any overload of function aGlobalFunction');
end

19
wrap/tests/expected/geometry_wrapper.cpp
View File

@ -502,6 +502,19 @@ void aGlobalFunction_42(int nargout, mxArray *out[], int nargin, const mxArray *
checkArguments("aGlobalFunction",nargout,nargin,0);
out[0] = wrap< Vector >(aGlobalFunction());
}
void overloadedGlobalFunction_43(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("overloadedGlobalFunction",nargout,nargin,1);
int a = unwrap< int >(in[0]);
out[0] = wrap< Vector >(overloadedGlobalFunction(a));
}
void overloadedGlobalFunction_44(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
checkArguments("overloadedGlobalFunction",nargout,nargin,2);
int a = unwrap< int >(in[0]);
double b = unwrap< double >(in[1]);
out[0] = wrap< Vector >(overloadedGlobalFunction(a,b));
}
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
@ -643,6 +656,12 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
case 42:
aGlobalFunction_42(nargout, out, nargin-1, in+1);
break;
case 43:
overloadedGlobalFunction_43(nargout, out, nargin-1, in+1);
break;
case 44:
overloadedGlobalFunction_44(nargout, out, nargin-1, in+1);
break;
}
} catch(const std::exception& e) {
mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str());

8
wrap/tests/expected/overloadedGlobalFunction.m Normal file
View File

@ -0,0 +1,8 @@
function varargout = overloadedGlobalFunction(varargin)
if length(varargin) == 1 && isa(varargin{1},'numeric')
varargout{1} = geometry_wrapper(43, varargin{:});
elseif length(varargin) == 2 && isa(varargin{1},'numeric') && isa(varargin{2},'double')
varargout{1} = geometry_wrapper(44, varargin{:});
else
error('Arguments do not match any overload of function overloadedGlobalFunction');
end

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