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

Merge branch 'develop' 

Conflicts:
	.cproject
release/4.3a0
Luca 2014-05-28 18:59:04 -04:00
parent 2af4b4ec4c bc13ec9f92
commit 515b612ca0
66 changed files with 2394 additions and 1631 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

540
.cproject
View File

@ -840,6 +840,102 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraphUnordered.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianFactorGraphUnordered.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianBayesNetUnordered.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianBayesNetUnordered.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianConditional.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianConditional.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianDensity.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianDensity.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianJunctionTree.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testHessianFactor.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testHessianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testJacobianFactor.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testJacobianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testKalmanFilter.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testKalmanFilter.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNoiseModel.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testNoiseModel.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSampler.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSampler.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSerializationLinear.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSerializationLinear.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVectorValues.run" path="build/gtsam/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVectorValues.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInvDepthFactor3.run" path="build/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -904,6 +1000,14 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testTSAMFactors.run" path="build/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testTSAMFactors.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="clean" path="build/wrap/gtsam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
@ -1407,92 +1511,44 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testEliminationTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testVectorValues.run</buildTarget>
<buildTarget>testEliminationTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testNoiseModel.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testInference.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testNoiseModel.run</buildTarget>
<buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testHessianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testKey.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testHessianFactor.run</buildTarget>
<buildTarget>testKey.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianConditional.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testSymbolicBayesTree.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianConditional.run</buildTarget>
<buildArguments>-j1</buildArguments>
<buildTarget>testSymbolicBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianFactorGraphUnordered.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<target name="testSymbolicSequentialSolver.run" path="build/gtsam/inference/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianFactorGraphUnordered.run</buildTarget>
<buildArguments>-j1</buildArguments>
<buildTarget>testSymbolicSequentialSolver.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianJunctionTree.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianJunctionTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testKalmanFilter.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testKalmanFilter.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testGaussianDensity.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testGaussianDensity.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSerializationLinear.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSerializationLinear.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testJacobianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testJacobianFactor.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSampler.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testSampler.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
@ -1953,6 +2009,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>
@ -2073,6 +2257,14 @@
<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>
@ -2121,6 +2313,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>
@ -2169,38 +2369,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testInference.run" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSymbolicSequentialSolver.run" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j1</buildArguments>
<buildTarget>testSymbolicSequentialSolver.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testEliminationTree.run" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testEliminationTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testSymbolicBayesTree.run" path="build/gtsam/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j1</buildArguments>
<buildTarget>testSymbolicBayesTree.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2225,6 +2393,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>
@ -2494,158 +2678,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testDataset.run" path="build/gtsam/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testDataset.run</buildTarget>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testStereoCamera.run" path="build/gtsam/geometry" 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="testRot3M.run" path="build/gtsam/geometry" 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="testPoint3.run" path="build/gtsam/geometry" 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="testCalibratedCamera.run" path="build/gtsam/geometry" 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="timeStereoCamera.run" path="build/gtsam/geometry" 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="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">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testPoint2.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">
<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" 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>
<stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments>
@ -2798,14 +2830,6 @@
<useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders>
</target>
<target name="testLagoInitializer.run" path="build/gtsam/nonlinear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments>
<buildTarget>testLagoInitializer.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>
@ -2934,14 +2958,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
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()

8
gtsam.h
View File

@ -2363,6 +2363,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 +2381,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

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()
option(GTSAM_BUILD_METIS "Build metis library" ON)

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

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

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;

7
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -75,6 +75,13 @@ namespace gtsam {
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;

63
gtsam/linear/NoiseModel.cpp
View File

@ -47,22 +47,50 @@ 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
static 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::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::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);
}
full: return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
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 +194,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?
{
}
@ -191,12 +219,17 @@ Diagonal::shared_ptr Diagonal::Variances(const Vector& variances, bool smart) {
/* ************************************************************************* */
Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
if (smart) {
DenseIndex j, n = sigmas.size();
// look for zeros to make a constraint
for (size_t i=0; i< (size_t) sigmas.size(); ++i)
if (sigmas(i)<1e-8)
for (j=0; j< n; ++j)
if (sigmas(j)<1e-8)
return Constrained::MixedSigmas(sigmas);
// check whether all the same entry
for (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));
}
/* ************************************************************************* */

6
gtsam/linear/NoiseModel.h
View File

@ -159,10 +159,10 @@ 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 a covariance matrix.

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

@ -267,6 +267,24 @@ 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, 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.

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

77
gtsam/slam/dataset.cpp
View File

@ -112,14 +112,14 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
// Create a sampler with random number generator
Sampler sampler(42u);
// load the factors
// Parse the pose constraints
int id1, id2;
bool haveLandmark = false;
while (is) {
if(! (is >> tag))
break;
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) {
int id1, id2;
double x, y, yaw;
double v1, v2, v3, v4, v5, v6;
@ -168,68 +168,75 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
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 = std::atan2(lmy, lmx);
range = std::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;
// it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty.
if(std::abs(v1 - v3) < 1e-4)
{
double rangeVar = v1;
double bearingVar = v1 / 10.0;
measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearingVar, rangeVar));
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;
<< " vertices and " << graph->nrFactors() << " factors" << endl;
return make_pair(graph, initial);
}
@ -817,7 +824,7 @@ bool writeBALfromValues(const string& filename, const SfM_data &data, Values& va
}
for (size_t j = 0; j < dataValues.number_tracks(); j++){ // for each point
Key pointKey = symbol('l',j);
Key pointKey = P(j);
if(values.exists(pointKey)){
Point3 point = values.at<Point3>(pointKey);
dataValues.tracks[j].p = point;

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

@ -19,12 +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;
@ -303,7 +305,7 @@ 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);
}
@ -335,7 +337,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));
}

118
gtsam_unstable/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
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,

14
gtsam_unstable/slam/JacobianFactorSVD.h
View File

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

250
gtsam_unstable/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;
}
@ -321,8 +325,8 @@ public:
const Cameras& cameras, const Point3& point,
const double lambda = 0.0) const {
size_t numKeys = this->keys_.size();
std::vector < KeyMatrix2D > Fblocks;
int numKeys = this->keys_.size();
std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
lambda);
F = zeros(2 * numKeys, D * numKeys);
@ -345,7 +349,7 @@ 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();
@ -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:

58
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -54,6 +54,10 @@ public:
double f;
};
enum linearizationType {
HESSIAN, JACOBIAN_SVD, JACOBIAN_Q
};
/**
* SmartProjectionFactor: triangulates point
* TODO: why LANDMARK parameter?
@ -91,6 +95,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 +126,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 +248,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& e) {
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 +424,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 +436,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

34
gtsam_unstable/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,6 +41,8 @@ template<class POSE, class LANDMARK, class CALIBRATION>
class SmartProjectionPoseFactor: public SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> {
protected:
linearizationType linearizeTo_;
// Known calibration
std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera)
@ -56,8 +68,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,
linearizationType linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}
/** Virtual destructor */
virtual ~SmartProjectionPoseFactor() {}
@ -139,11 +154,22 @@ public:
}
/**
* linearize returns a Hessian factor contraining the poses
* linear factor on the poses
*/
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;
}
}
/**

265
gtsam_unstable/slam/tests/testSmartProjectionHessianPoseFactor.cpp → gtsam_unstable/slam/tests/testSmartProjectionPoseFactor.cpp
View File

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

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!'

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

772
wrap/Class.cpp
View File

@ -7,62 +7,62 @@
* See LICENSE for the license information
* -------------------------------------------------------------------------- */
* -------------------------------------------------------------------------- */
/**
* @file Class.cpp
* @author Frank Dellaert
* @author Andrew Melim
* @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 {
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 {
// Create namespace folders
createNamespaceStructure(namespaces, toolboxPath);
createNamespaceStructure(namespaces, toolboxPath);
// open destination classFile
string classFile = toolboxPath;
if(!namespaces.empty())
classFile += "/+" + wrap::qualifiedName("/+", namespaces);
classFile += "/" + name + ".m";
FileWriter proxyFile(classFile, verbose_, "%");
string classFile = toolboxPath;
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 matlabBaseName = wrap::qualifiedName(".", qualifiedParent);
const string cppBaseName = wrap::qualifiedName("::", qualifiedParent);
const string matlabQualName = qualifiedName("."), matlabUniqueName =
qualifiedName(), cppName = qualifiedName("::");
const string matlabBaseName = wrap::qualifiedName(".", qualifiedParent);
const string cppBaseName = wrap::qualifiedName("::", qualifiedParent);
// emit class proxy code
// we want our class to inherit the handle class for memory purposes
const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName;
const string parent = qualifiedParent.empty() ? "handle" : matlabBaseName;
comment_fragment(proxyFile);
proxyFile.oss << "classdef " << name << " < " << parent << endl;
proxyFile.oss << "classdef " << name << " < " << parent << endl;
proxyFile.oss << " properties\n";
proxyFile.oss << " ptr_" << matlabUniqueName << " = 0\n";
proxyFile.oss << " end\n";
proxyFile.oss << " methods\n";
// Constructor
proxyFile.oss << " function obj = " << name << "(varargin)\n";
// Special pointer constructors - one in MATLAB to create an object and
@ -72,284 +72,325 @@ 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);
wrapperFile.oss << "\n";
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);
const string wrapFunctionName = constructor.wrapper_fragment(wrapperFile,
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 << " end\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";
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);
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 << " end\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();
deconstructor.proxy_fragment(proxyFile, wrapperName, matlabUniqueName, id);
proxyFile.oss << "\n";
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";
{
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);
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";
// 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);
proxyFile.oss << "\n";
wrapperFile.oss << "\n";
}
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);
proxyFile.oss << "\n";
wrapperFile.oss << "\n";
}
if (hasSerialization)
serialization_fragments(proxyFile, wrapperFile, wrapperName, functionNames);
proxyFile.oss << " end\n";
proxyFile.oss << "\n";
proxyFile.oss << " methods(Static = true)\n";
proxyFile.oss << " end\n";
proxyFile.oss << "\n";
proxyFile.oss << " methods(Static = true)\n";
// 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);
proxyFile.oss << "\n";
wrapperFile.oss << "\n";
}
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);
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";
// Close file
proxyFile.emit(true);
}
/* ************************************************************************* */
string Class::qualifiedName(const string& delim) const {
return ::wrap::qualifiedName(delim, namespaces, name);
}
/* ************************************************************************* */
void Class::pointer_constructor_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, const string& wrapperName, vector<string>& functionNames) const {
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);
functionNames.push_back(collectorInsertFunctionName);
int upcastFromVoidId;
string upcastFromVoidFunctionName;
if(isVirtual) {
upcastFromVoidId = (int)functionNames.size();
upcastFromVoidFunctionName = matlabUniqueName + "_upcastFromVoid_" + boost::lexical_cast<string>(upcastFromVoidId);
functionNames.push_back(upcastFromVoidFunctionName);
}
proxyFile.emit(true);
}
/* ************************************************************************* */
string Class::qualifiedName(const string& delim) const {
return ::wrap::qualifiedName(delim, namespaces, name);
}
/* ************************************************************************* */
void Class::pointer_constructor_fragments(FileWriter& proxyFile,
FileWriter& wrapperFile, const string& wrapperName,
vector<string>& functionNames) const {
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);
functionNames.push_back(collectorInsertFunctionName);
int upcastFromVoidId;
string upcastFromVoidFunctionName;
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')))";
} else {
proxyFile.oss << " if nargin == 2";
}
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 << " 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
proxyFile.oss << " ";
else
proxyFile.oss << " base_ptr = ";
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 << " 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 << " 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
proxyFile.oss << " ";
else
proxyFile.oss << " base_ptr = ";
proxyFile.oss << wrapperName << "(" << collectorInsertId << ", my_ptr);\n"; // Call collector insert and get base class ptr
// C++ function to add pointer from MATLAB to collector. The pointer always
// 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 << "{\n";
wrapperFile.oss << " mexAtExit(&_deleteAllObjects);\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";
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";
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()) {
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 << "}\n";
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 << "}\n";
// If this is a virtual function, C++ function to dynamic upcast it from a
// 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";
}
/* ************************************************************************* */
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);
instArg.type = instName.back();
} else if(arg.type == "This") {
instArg.namespaces.assign(expandedClassNamespace.begin(), expandedClassNamespace.end());
instArg.type = expandedClassName;
}
instArgList.push_back(instArg);
}
result.push_back(instArgList);
}
return result;
}
/* ************************************************************************* */
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> 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.returnVals.clear();
BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) {
ReturnValue instRetVal = retVal;
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());
instRetVal.type1 = expandedClassName;
}
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());
instRetVal.type2 = expandedClassName;
}
instMethod.returnVals.push_back(instRetVal);
}
result.insert(make_pair(name_method.first, instMethod));
}
return result;
}
/* ************************************************************************* */
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;
inst.typedefName = cls.typedefName;
inst.isVirtual = cls.isVirtual;
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> 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);
instArg.type = instName.back();
} else if (arg.type == "This") {
instArg.namespaces.assign(expandedClassNamespace.begin(),
expandedClassNamespace.end());
instArg.type = expandedClassName;
}
instArgList.push_back(instArg);
}
result.push_back(instArgList);
}
return result;
}
/* ************************************************************************* */
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> 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.returnVals.clear();
BOOST_FOREACH(const ReturnValue& retVal, method.returnVals) {
ReturnValue instRetVal = retVal;
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());
instRetVal.type1 = expandedClassName;
}
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());
instRetVal.type2 = expandedClassName;
}
instMethod.returnVals.push_back(instRetVal);
}
result.insert(make_pair(name_method.first, instMethod));
}
return result;
}
/* ************************************************************************* */
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;
inst.typedefName = cls.typedefName;
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.namespaces = cls.namespaces;
inst.constructor = cls.constructor;
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;
inst.verbose_ = cls.verbose_;
return inst;
}
/* ************************************************************************* */
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);
inst.name = expandedName;
inst.templateArgs.clear();
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);
}
/* ************************************************************************* */
std::string Class::getTypedef() const {
string result;
BOOST_FOREACH(const string& namesp, namespaces) {
result += ("namespace " + namesp + " { ");
}
result += ("typedef " + typedefName + " " + name + ";");
for (size_t i = 0; i<namespaces.size(); ++i) {
result += " }";
}
return result;
}
/* ************************************************************************* */
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.namespaces = cls.namespaces;
inst.constructor = cls.constructor;
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;
inst.verbose_ = cls.verbose_;
return inst;
}
/* ************************************************************************* */
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);
inst.name = expandedName;
inst.templateArgs.clear();
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);
}
/* ************************************************************************* */
std::string Class::getTypedef() const {
string result;
BOOST_FOREACH(const string& namesp, namespaces) {
result += ("namespace " + namesp + " { ");
}
result += ("typedef " + typedefName + " " + name + ";");
for (size_t i = 0; i < namespaces.size(); ++i) {
result += " }";
}
return result;
}
/* ************************************************************************* */
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 {

21
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

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

171
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);
}
proxyFile.oss << " else\n";
proxyFile.oss << " error('Arguments do not match any overload of function " <<
matlabQualName << "." << upperName << "');" << endl;
file.oss << " else\n";
file.oss << " error('Arguments do not match any overload of function "
<< matlabQualName << "." << upperName << "');" << endl;
file.oss << " end\n";
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 {
string StaticMethod::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";
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

21
wrap/tests/expected_namespaces/+ns2/ClassA.m
View File

@ -40,22 +40,12 @@ classdef ClassA < handle
function varargout = memberFunction(this, varargin)
% MEMBERFUNCTION usage: memberFunction() : returns double
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% memberFunction()
if length(varargin) == 0
varargout{1} = testNamespaces_wrapper(9, this, varargin{:});
else
error('Arguments do not match any overload of function ns2.ClassA.memberFunction');
end
varargout{1} = testNamespaces_wrapper(9, this, varargin{:});
end
function varargout = nsArg(this, varargin)
% NSARG usage: nsArg(ClassB arg) : returns int
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% nsArg(ClassB arg)
if length(varargin) == 1 && isa(varargin{1},'ns1.ClassB')
varargout{1} = testNamespaces_wrapper(10, this, varargin{:});
else
@ -66,14 +56,7 @@ classdef ClassA < handle
function varargout = nsReturn(this, varargin)
% NSRETURN usage: nsReturn(double q) : returns ns2::ns3::ClassB
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html
%
% Method Overloads
% nsReturn(double q)
if length(varargin) == 1 && isa(varargin{1},'double')
varargout{1} = testNamespaces_wrapper(11, this, varargin{:});
else
error('Arguments do not match any overload of function ns2.ClassA.nsReturn');
end
varargout{1} = testNamespaces_wrapper(11, this, varargin{:});
end
end

8
wrap/tests/expected_namespaces/+ns2/overloadedGlobalFunction.m Normal file
View File

@ -0,0 +1,8 @@
function varargout = overloadedGlobalFunction(varargin)
if length(varargin) == 1 && isa(varargin{1},'ns1.ClassA')
varargout{1} = testNamespaces_wrapper(24, varargin{:});
elseif length(varargin) == 2 && isa(varargin{1},'ns1.ClassA') && isa(varargin{2},'double')
varargout{1} = testNamespaces_wrapper(25, varargin{:});
else
error('Arguments do not match any overload of function ns2.overloadedGlobalFunction');
end

21
wrap/tests/expected_namespaces/testNamespaces_wrapper.cpp
View File

@ -342,6 +342,21 @@ void ns2aGlobalFunction_23(int nargout, mxArray *out[], int nargin, const mxArra
checkArguments("ns2aGlobalFunction",nargout,nargin,0);
out[0] = wrap< Vector >(ns2::aGlobalFunction());
}
void ns2overloadedGlobalFunction_24(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
typedef boost::shared_ptr<ns1::ClassA> SharedClassA;
checkArguments("ns2overloadedGlobalFunction",nargout,nargin,1);
ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA");
out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a))),"ns1.ClassA", false);
}
void ns2overloadedGlobalFunction_25(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
typedef boost::shared_ptr<ns1::ClassA> SharedClassA;
checkArguments("ns2overloadedGlobalFunction",nargout,nargin,2);
ns1::ClassA& a = *unwrap_shared_ptr< ns1::ClassA >(in[0], "ptr_ns1ClassA");
double b = unwrap< double >(in[1]);
out[0] = wrap_shared_ptr(SharedClassA(new ns1::ClassA(ns2::overloadedGlobalFunction(a,b))),"ns1.ClassA", false);
}
void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
{
@ -426,6 +441,12 @@ void mexFunction(int nargout, mxArray *out[], int nargin, const mxArray *in[])
case 23:
ns2aGlobalFunction_23(nargout, out, nargin-1, in+1);
break;
case 24:
ns2overloadedGlobalFunction_24(nargout, out, nargin-1, in+1);
break;
case 25:
ns2overloadedGlobalFunction_25(nargout, out, nargin-1, in+1);
break;
}
} catch(const std::exception& e) {
mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str());

4
wrap/tests/geometry.h
View File

@ -87,6 +87,10 @@ class Test {
Vector aGlobalFunction();
// An overloaded global function
Vector overloadedGlobalFunction(int a);
Vector overloadedGlobalFunction(int a, double b);
// comments at the end!
// even more comments at the end!

4
wrap/tests/testNamespaces.h
View File

@ -47,6 +47,10 @@ class ClassC {
// separate namespace global function, same name
Vector aGlobalFunction();
// An overloaded global function
ns1::ClassA overloadedGlobalFunction(const ns1::ClassA& a);
ns1::ClassA overloadedGlobalFunction(const ns1::ClassA& a, double b);
} //\namespace ns2
class ClassD {

39
wrap/tests/testWrap.cpp
View File

@ -15,16 +15,18 @@
* @author Frank Dellaert
**/
#include <wrap/utilities.h>
#include <wrap/Module.h>
#include <CppUnitLite/TestHarness.h>
#include <boost/assign/std/vector.hpp>
#include <boost/filesystem.hpp>
#include <stdlib.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <boost/assign/std/vector.hpp>
#include <boost/filesystem.hpp>
#include <CppUnitLite/TestHarness.h>
#include <wrap/utilities.h>
#include <wrap/Module.h>
using namespace std;
using namespace boost::assign;
@ -305,8 +307,8 @@ TEST( wrap, parse_geometry ) {
}
// evaluate global functions
// Vector aGlobalFunction();
LONGS_EQUAL(1, module.global_functions.size());
// Vector aGlobalFunction();
LONGS_EQUAL(2, module.global_functions.size());
CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end());
{
GlobalFunction gfunc = module.global_functions.at("aGlobalFunction");
@ -380,7 +382,7 @@ TEST( wrap, parse_namespaces ) {
// evaluate global functions
// Vector ns1::aGlobalFunction();
// Vector ns2::aGlobalFunction();
LONGS_EQUAL(1, module.global_functions.size());
LONGS_EQUAL(2, module.global_functions.size());
CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end());
{
GlobalFunction gfunc = module.global_functions.at("aGlobalFunction");
@ -415,13 +417,17 @@ TEST( wrap, matlab_code_namespaces ) {
module.matlab_code("actual_namespaces", headerPath);
EXPECT(files_equal(exp_path + "ClassD.m" , act_path + "ClassD.m" ));
EXPECT(files_equal(exp_path + "+ns1/ClassA.m" , act_path + "+ns1/ClassA.m" ));
EXPECT(files_equal(exp_path + "+ns1/ClassB.m" , act_path + "+ns1/ClassB.m" ));
EXPECT(files_equal(exp_path + "+ns2/ClassA.m" , act_path + "+ns2/ClassA.m" ));
EXPECT(files_equal(exp_path + "+ns2/ClassC.m" , act_path + "+ns2/ClassC.m" ));
EXPECT(files_equal(exp_path + "+ns2/+ns3/ClassB.m" , act_path + "+ns2/+ns3/ClassB.m" ));
EXPECT(files_equal(exp_path + "testNamespaces_wrapper.cpp" , act_path + "testNamespaces_wrapper.cpp" ));
EXPECT(files_equal(exp_path + "ClassD.m", act_path + "ClassD.m" ));
EXPECT(files_equal(exp_path + "+ns1/ClassA.m", act_path + "+ns1/ClassA.m" ));
EXPECT(files_equal(exp_path + "+ns1/ClassB.m", act_path + "+ns1/ClassB.m" ));
EXPECT(files_equal(exp_path + "+ns2/ClassA.m", act_path + "+ns2/ClassA.m" ));
EXPECT(files_equal(exp_path + "+ns2/ClassC.m", act_path + "+ns2/ClassC.m" ));
EXPECT(
files_equal(exp_path + "+ns2/overloadedGlobalFunction.m", exp_path + "+ns2/overloadedGlobalFunction.m" ));
EXPECT(
files_equal(exp_path + "+ns2/+ns3/ClassB.m", act_path + "+ns2/+ns3/ClassB.m" ));
EXPECT(
files_equal(exp_path + "testNamespaces_wrapper.cpp", act_path + "testNamespaces_wrapper.cpp" ));
}
/* ************************************************************************* */
@ -445,6 +451,7 @@ TEST( wrap, matlab_code_geometry ) {
EXPECT(files_equal(epath + "Point3.m" , apath + "Point3.m" ));
EXPECT(files_equal(epath + "Test.m" , apath + "Test.m" ));
EXPECT(files_equal(epath + "aGlobalFunction.m" , apath + "aGlobalFunction.m" ));
EXPECT(files_equal(epath + "overloadedGlobalFunction.m" , apath + "overloadedGlobalFunction.m" ));
}
/* ************************************************************************* */

4
wrap/utilities.cpp
View File

@ -88,9 +88,11 @@ bool files_equal(const string& expected, const string& actual, bool skipheader)
string actual_contents = file_contents(actual, skipheader);
bool equal = actual_contents == expected_contents;
if (!equal) {
cerr << "<<< DIFF OUTPUT (if none, white-space differences only):\n";
stringstream command;
command << "diff " << actual << " " << expected << endl;
command << "diff --ignore-all-space " << expected << " " << actual << endl;
system(command.str().c_str());
cerr << ">>>\n";
}
return equal;
}