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> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="testInvDepthFactor3.run" path="build/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -904,6 +1000,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="clean" path="build/wrap/gtsam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -1407,92 +1511,44 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testVectorValues.run</buildTarget> <buildTarget>testEliminationTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testNoiseModel.run</buildTarget> <buildTarget>testInference.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
<buildTarget>testHessianFactor.run</buildTarget> <buildTarget>testKey.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j1</buildArguments>
<buildTarget>testGaussianConditional.run</buildTarget> <buildTarget>testSymbolicBayesTree.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>false</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j1</buildArguments>
<buildTarget>testGaussianFactorGraphUnordered.run</buildTarget> <buildTarget>testSymbolicSequentialSolver.run</buildTarget>
<stopOnError>true</stopOnError> <stopOnError>true</stopOnError>
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>false</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>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </target>
<target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> <target name="check" path="build/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
@ -1953,6 +2009,134 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -2073,6 +2257,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="testEssentialMatrixFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -2121,6 +2313,14 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="testDiscreteFactor.run" path="build/gtsam/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -2169,38 +2369,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="testVSLAMGraph" path="build/slam/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -2225,6 +2393,22 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="check.tests" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <buildArguments>-j5</buildArguments>
@ -2494,158 +2678,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="SimpleRotation.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j2</buildArguments> <buildArguments>-j2</buildArguments>
@ -2798,14 +2830,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="testImuFactor.run" path="build-debug/gtsam_unstable/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j4</buildArguments> <buildArguments>-j4</buildArguments>
@ -2934,14 +2958,6 @@
<useDefaultCommand>true</useDefaultCommand> <useDefaultCommand>true</useDefaultCommand>
<runAllBuilders>true</runAllBuilders> <runAllBuilders>true</runAllBuilders>
</target> </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"> <target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder">
<buildCommand>make</buildCommand> <buildCommand>make</buildCommand>
<buildArguments>-j5</buildArguments> <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 # Clang on Mac uses a template depth that is less than standard and is too small
if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang") if("${CMAKE_CXX_COMPILER_ID}" STREQUAL "Clang")
if(NOT "${CMAKE_CXX_COMPILER_VERSION}" VERSION_LESS "5.0") 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()
endif() endif()

8
gtsam.h
View File

@ -2363,6 +2363,12 @@ virtual class CombinedImuFactor : gtsam::NonlinearFactor {
namespace utilities { namespace utilities {
#include <matlab.h> #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 extractPoint2(const gtsam::Values& values);
Matrix extractPoint3(const gtsam::Values& values); Matrix extractPoint3(const gtsam::Values& values);
Matrix extractPose2(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);
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); 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); 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 } //\namespace utilities

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

@ -16,13 +16,30 @@ if(NOT GTSAM_USE_SYSTEM_EIGEN)
endif() endif()
endforeach(eigen_dir) 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 # Add to project source
set(eigen_headers ${eigen_headers} PARENT_SCOPE) 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 Eigen - only the headers in our 3rdparty directory
install(DIRECTORY Eigen/Eigen install(DIRECTORY Eigen/Eigen
DESTINATION include/gtsam/3rdparty/Eigen DESTINATION include/gtsam/3rdparty/Eigen
FILES_MATCHING PATTERN "*.h") FILES_MATCHING PATTERN "*.h")
install(DIRECTORY Eigen/unsupported/Eigen
DESTINATION include/gtsam/3rdparty/Eigen/unsupported/
FILES_MATCHING PATTERN "*.h")
endif() endif()
option(GTSAM_BUILD_METIS "Build metis library" ON) 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 LLt(const Matrix& A)
{ {
Matrix L = zeros(A.rows(), A.rows()); Eigen::LLT<Matrix> llt(A);
Eigen::LLT<Matrix> llt;
llt.compute(A);
return llt.matrixL(); return llt.matrixL();
} }
/* ************************************************************************* */
Matrix RtR(const Matrix &A) 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) Matrix cholesky_inverse(const Matrix &A)
{ {
// FIXME: replace with real algorithm Eigen::LLT<Matrix> llt(A);
return A.inverse(); Matrix inv = eye(A.rows());
llt.matrixU().solveInPlace<Eigen::OnTheRight>(inv);
// Matrix L = LLt(A); return inv*inv.transpose();
// Matrix inv(eye(A.rows()));
// inplace_solve (L, inv, BNU::lower_tag ());
// return BNU::prod(trans(inv), inv);
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -648,9 +621,9 @@ Matrix cholesky_inverse(const Matrix &A)
// inv(B) * inv(B)' == A // inv(B) * inv(B)' == A
// inv(B' * B) == A // inv(B' * B) == A
Matrix inverse_square_root(const Matrix& A) { Matrix inverse_square_root(const Matrix& A) {
Matrix R = RtR(A); Eigen::LLT<Matrix> llt(A);
Matrix inv = eye(A.rows()); Matrix inv = eye(A.rows());
R.triangularView<Eigen::Upper>().solveInPlace<Eigen::OnTheRight>(inv); llt.matrixU().solveInPlace<Eigen::OnTheRight>(inv);
return inv.transpose(); return inv.transpose();
} }

20
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 // 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 // we are checking against was generated via chol(M)' on octave
TEST( matrix, LLt ) namespace cholesky {
{ Matrix M = (Matrix(5, 5) << 0.0874197, -0.0030860, 0.0116969, 0.0081463,
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.0048741, -0.0030860, 0.0872727, 0.0183073, 0.0125325, -0.0037363,
0.0116969, 0.0183073, 0.0966217, 0.0103894, -0.0021113, 0.0081463, 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.0125325, 0.0103894, 0.0747324, 0.0036415, 0.0048741, -0.0037363,
-0.0021113, 0.0036415, 0.0909464); -0.0021113, 0.0036415, 0.0909464);
Matrix expected = (Matrix(5, 5) << Matrix expected = (Matrix(5, 5) <<
0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.295668226226627, 0.000000000000000, 0.000000000000000, 0.000000000000000, 0.000000000000000,
-0.010437374483502, 0.295235094820875, 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.039560896175007, 0.063407813693827, 0.301721866387571, 0.000000000000000, 0.000000000000000,
0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000, 0.027552165831157, 0.043423266737274, 0.021695600982708, 0.267613525371710, 0.000000000000000,
0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247); 0.016485031422565, -0.012072546984405, -0.006621889326331, 0.014405837566082, 0.300462176944247);
}
TEST( matrix, LLt )
{
EQUALITY(cholesky::expected, LLt(cholesky::M));
}
TEST( matrix, RtR )
{
EQUALITY(cholesky::expected.transpose(), RtR(cholesky::M));
}
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 /// A'*b for Jacobian, eta for Hessian
virtual VectorValues gradientAtZero() const = 0; virtual VectorValues gradientAtZero() const = 0;
/// A'*b for Jacobian, eta for Hessian (raw memory version)
virtual void gradientAtZero(double* d) const = 0;
private: private:
/** Serialization function */ /** Serialization function */
friend class boost::serialization::access; friend class boost::serialization::access;

7
gtsam/linear/GaussianFactorGraph.cpp
View File

@ -75,6 +75,13 @@ namespace gtsam {
return dims_accumulated; 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 GaussianFactorGraph::clone() const {
GaussianFactorGraph result; 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 * Cloning preserves null factors so indices for the original graph are still
* valid for the cloned graph. * 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. * 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, * @param [output] g A VectorValues to store the gradient, which must be preallocated,
* see allocateVectorValues * see allocateVectorValues
* @return The gradient as a VectorValues */ * @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 /** 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$. * 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; 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> > std::pair<boost::shared_ptr<GaussianConditional>, boost::shared_ptr<HessianFactor> >
EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys)

2
gtsam/linear/HessianFactor.h
View File

@ -387,6 +387,8 @@ namespace gtsam {
/// eta for Hessian /// eta for Hessian
VectorValues gradientAtZero() const; VectorValues gradientAtZero() const;
virtual void gradientAtZero(double* d) const;
/** /**
* Densely partially eliminate with Cholesky factorization. JacobianFactors are * Densely partially eliminate with Cholesky factorization. JacobianFactors are
* left-multiplied with their transpose to form the Hessian using the conversion constructor * 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; 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> JacobianFactor::jacobian() const {
pair<Matrix, Vector> result = jacobianUnweighted(); pair<Matrix, Vector> result = jacobianUnweighted();

3
gtsam/linear/JacobianFactor.h
View File

@ -286,6 +286,9 @@ namespace gtsam {
/// A'*b for Jacobian /// A'*b for Jacobian
VectorValues gradientAtZero() const; VectorValues gradientAtZero() const;
/* ************************************************************************* */
virtual void gradientAtZero(double* d) const;
/** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */
JacobianFactor whiten() const; 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(); 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) { Gaussian::shared_ptr Gaussian::Covariance(const Matrix& covariance, bool smart) {
size_t m = covariance.rows(), n = covariance.cols(); size_t m = covariance.rows(), n = covariance.cols();
if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square"); if (m != n) throw invalid_argument("Gaussian::Covariance: covariance not square");
if (smart) { boost::optional<Vector> variances = boost::none;
// check all non-diagonal entries if (smart)
size_t i,j; variances = checkIfDiagonal(covariance);
for (i = 0; i < m; i++) if (variances) return Diagonal::Variances(*variances,true);
for (j = 0; j < n; j++) else return shared_ptr(new Gaussian(n, inverse_square_root(covariance)));
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)));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -166,7 +194,7 @@ void Gaussian::WhitenSystem(Matrix& A1, Matrix& A2, Matrix& A3, Vector& b) const
// Diagonal // Diagonal
/* ************************************************************************* */ /* ************************************************************************* */
Diagonal::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) { Diagonal::shared_ptr Diagonal::Sigmas(const Vector& sigmas, bool smart) {
if (smart) { if (smart) {
DenseIndex j, n = sigmas.size();
// look for zeros to make a constraint // look for zeros to make a constraint
for (size_t i=0; i< (size_t) sigmas.size(); ++i) for (j=0; j< n; ++j)
if (sigmas(i)<1e-8) if (sigmas(j)<1e-8)
return Constrained::MixedSigmas(sigmas); 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. * 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) { static shared_ptr SqrtInformation(const Matrix& R, bool smart = true);
return shared_ptr(new Gaussian(R.rows(),R));
}
/** /**
* A Gaussian noise model created by specifying a covariance matrix. * 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)); 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 ) TEST(NoiseModel, SmartCovariance )
{ {

5
gtsam/navigation/CombinedImuFactor.h
View File

@ -34,6 +34,11 @@ namespace gtsam {
* *
* @addtogroup SLAM * @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: * REFERENCES:
* [1] G.S. Chirikjian, "Stochastic Models, Information Theory, and Lie Groups", Volume 2, 2008. * [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 * [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 * @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. * [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 * [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. * Environments Without Initial Conditions", TRO, 28(1):61-76, 2012.

81
gtsam/nonlinear/LevenbergMarquardtOptimizer.cpp
View File

@ -138,7 +138,7 @@ void LevenbergMarquardtOptimizer::decreaseLambda(double stepQuality) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem( GaussianFactorGraph::shared_ptr LevenbergMarquardtOptimizer::buildDampedSystem(
const GaussianFactorGraph& linear) { const GaussianFactorGraph& linear) {
gttic(damp); gttic(damp);
@ -159,7 +159,8 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
// for each of the variables, add a prior // for each of the variables, add a prior
double sigma = 1.0 / std::sqrt(state_.lambda); 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()); damped.reserve(damped.size() + state_.values.size());
if (params_.diagonalDamping) { if (params_.diagonalDamping) {
BOOST_FOREACH(const VectorValues::KeyValuePair& key_vector, state_.hessianDiagonal) { BOOST_FOREACH(const VectorValues::KeyValuePair& key_vector, state_.hessianDiagonal) {
@ -188,7 +189,20 @@ GaussianFactorGraph LevenbergMarquardtOptimizer::buildDampedSystem(
} }
} }
gttoc(damp); 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; cout << "linearizing = " << endl;
GaussianFactorGraph::shared_ptr linear = linearize(); GaussianFactorGraph::shared_ptr linear = linearize();
if(state_.totalNumberInnerIterations==0) // write initial error
writeLogFile(state_.error);
// Keep increasing lambda until we make make progress // Keep increasing lambda until we make make progress
while (true) { while (true) {
@ -212,21 +229,8 @@ void LevenbergMarquardtOptimizer::iterate() {
cout << "trying lambda = " << state_.lambda << endl; cout << "trying lambda = " << state_.lambda << endl;
// Build damped system for this lambda (adds prior factors that make it like gradient descent) // Build damped system for this lambda (adds prior factors that make it like gradient descent)
GaussianFactorGraph dampedSystem = buildDampedSystem(*linear); GaussianFactorGraph::shared_ptr dampedSystemPtr = buildDampedSystem(*linear);
GaussianFactorGraph &dampedSystem = (*dampedSystemPtr);
// 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;
// Try solving // Try solving
double modelFidelity = 0.0; double modelFidelity = 0.0;
@ -256,6 +260,9 @@ void LevenbergMarquardtOptimizer::iterate() {
double newlinearizedError = linear->error(delta); double newlinearizedError = linear->error(delta);
double linearizedCostChange = state_.error - newlinearizedError; double linearizedCostChange = state_.error - newlinearizedError;
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "newlinearizedError = " << newlinearizedError <<
" linearizedCostChange = " << linearizedCostChange << endl;
if (linearizedCostChange >= 0) { // step is valid if (linearizedCostChange >= 0) { // step is valid
// update values // update values
@ -266,50 +273,62 @@ void LevenbergMarquardtOptimizer::iterate() {
// compute new error // compute new error
gttic(compute_error); gttic(compute_error);
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "calculating error" << endl; cout << "calculating error:" << endl;
newError = graph_.error(newValues); newError = graph_.error(newValues);
gttoc(compute_error); 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) // cost change in the original, nonlinear system (old - new)
double costChange = state_.error - newError; 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 // fidelity of linearized model VS original system between
modelFidelity = costChange / linearizedCostChange; modelFidelity = costChange / linearizedCostChange;
// if we decrease the error in the nonlinear system and modelFidelity is above threshold // if we decrease the error in the nonlinear system and modelFidelity is above threshold
step_is_successful = modelFidelity > params_.minModelFidelity; step_is_successful = modelFidelity > params_.minModelFidelity;
} else { if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
step_is_successful = true; // linearizedCostChange close to zero 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; double minAbsoluteTolerance = params_.relativeErrorTol * state_.error;
// if the change is small we terminate // 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; stopSearchingLambda = true;
}
}
}
} ++state_.totalNumberInnerIterations;
}
if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity if (step_is_successful) { // we have successfully decreased the cost and we have good modelFidelity
state_.values.swap(newValues); state_.values.swap(newValues);
state_.error = newError; state_.error = newError;
decreaseLambda(modelFidelity); decreaseLambda(modelFidelity);
writeLogFile(state_.error);
break; break;
} else if (!stopSearchingLambda) { // we failed to solved the system or we had no decrease in cost } else if (!stopSearchingLambda) { // we failed to solved the system or we had no decrease in cost
if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA)
cout << "increasing lambda: old error (" << state_.error cout << "increasing lambda" << endl;
<< ") new error (" << newError << ")" << endl;
increaseLambda(); increaseLambda();
writeLogFile(state_.error);
// check if lambda is too big // check if lambda is too big
if (state_.lambda >= params_.lambdaUpperBound) { if (state_.lambda >= params_.lambdaUpperBound) {
if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION) if (nloVerbosity >= NonlinearOptimizerParams::TERMINATION)
cout cout << "Warning: Levenberg-Marquardt giving up because "
<< "Warning: Levenberg-Marquardt giving up because cannot decrease error with maximum lambda" "cannot decrease error with maximum lambda" << endl;
<< endl;
break; break;
} }
} else { // the change in the cost is very small and it is not worth trying bigger lambdas } 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; break;
} }
} // end while } // end while

5
gtsam/nonlinear/LevenbergMarquardtOptimizer.h
View File

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

73
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 // Create a sampler with random number generator
Sampler sampler(42u); Sampler sampler(42u);
// load the factors // Parse the pose constraints
int id1, id2;
bool haveLandmark = false; bool haveLandmark = false;
while (is) { while (is) {
if(! (is >> tag)) if(! (is >> tag))
break; break;
if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) { if ((tag == "EDGE2") || (tag == "EDGE") || (tag == "ODOMETRY")) {
int id1, id2;
double x, y, yaw; double x, y, yaw;
double v1, v2, v3, v4, v5, v6; double v1, v2, v3, v4, v5, v6;
@ -168,62 +168,69 @@ pair<NonlinearFactorGraph::shared_ptr, Values::shared_ptr> load2D(
new BetweenFactor<Pose2>(id1, id2, l1Xl2, *model)); new BetweenFactor<Pose2>(id1, id2, l1Xl2, *model));
graph->push_back(factor); graph->push_back(factor);
} }
if (tag == "BR") {
int id1, id2; // Parse measurements
double bearing, range, bearing_std, range_std; double bearing, range, bearing_std, range_std;
// A bearing-range measurement
if (tag == "BR") {
is >> id1 >> id2 >> 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") { if (tag == "LANDMARK") {
int id1, id2;
double lmx, lmy; double lmx, lmy;
double v1, v2, v3; double v1, v2, v3;
is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3; is >> id1 >> id2 >> lmx >> lmy >> v1 >> v2 >> v3;
// Convert x,y to bearing,range // Convert x,y to bearing,range
double bearing = std::atan2(lmy, lmx); bearing = std::atan2(lmy, lmx);
double range = std::sqrt(lmx*lmx + lmy*lmy); range = std::sqrt(lmx*lmx + lmy*lmy);
// In our experience, the x-y covariance on landmark sightings is not very good, so assume // 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 // it describes the uncertainty at a range of 10m, and convert that to bearing/range uncertainty.
// uncertainty.
SharedDiagonal measurementNoise;
if(std::abs(v1 - v3) < 1e-4) if(std::abs(v1 - v3) < 1e-4)
{ {
double rangeVar = v1; bearing_std = sqrt(v1 / 10.0);
double bearingVar = v1 / 10.0; range_std = sqrt(v1);
measurementNoise = noiseModel::Diagonal::Sigmas((Vector(2) << bearingVar, rangeVar));
} }
else else
{ {
bearing_std = 1;
range_std = 1;
if(!haveLandmark) { if(!haveLandmark) {
cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n" cout << "Warning: load2D is a very simple dataset loader and is ignoring the\n"
"non-uniform covariance on LANDMARK measurements in this file." << endl; "non-uniform covariance on LANDMARK measurements in this file." << endl;
haveLandmark = true; 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 // 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'); is.ignore(LINESIZE, '\n');
} }
@ -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 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)){ if(values.exists(pointKey)){
Point3 point = values.at<Point3>(pointKey); Point3 point = values.at<Point3>(pointKey);
dataValues.tracks[j].p = point; dataValues.tracks[j].p = point;

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

@ -19,12 +19,14 @@
#include <boost/algorithm/string.hpp> #include <boost/algorithm/string.hpp>
#include <gtsam/inference/Symbol.h>
#include <gtsam/base/TestableAssertions.h> #include <gtsam/base/TestableAssertions.h>
#include <gtsam/inference/Symbol.h> #include <gtsam/inference/Symbol.h>
#include <gtsam/slam/BetweenFactor.h> #include <gtsam/slam/BetweenFactor.h>
#include <gtsam/slam/dataset.h> #include <gtsam/slam/dataset.h>
using namespace gtsam::symbol_shorthand;
using namespace std; using namespace std;
using namespace gtsam; using namespace gtsam;
@ -303,7 +305,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
value.insert(poseKey, pose); value.insert(poseKey, pose);
} }
for(size_t j=0; j < readData.number_tracks(); j++){ // for each point 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 ); Point3 point = poseChange.transform_from( readData.tracks[j].p );
value.insert(pointKey, point); value.insert(pointKey, point);
} }
@ -335,7 +337,7 @@ TEST( dataSet, writeBALfromValues_Dubrovnik){
EXPECT(assert_equal(expectedPose,actualPose, 1e-7)); EXPECT(assert_equal(expectedPose,actualPose, 1e-7));
Point3 expectedPoint = track0.p; Point3 expectedPoint = track0.p;
Key pointKey = symbol('l',0); Key pointKey = P(0);
Point3 actualPoint = value.at<Point3>(pointKey); Point3 actualPoint = value.at<Point3>(pointKey);
EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6)); EXPECT(assert_equal(expectedPoint,actualPoint, 1e-6));
} }

134
gtsam_unstable/slam/ImplicitSchurFactor.h
View File

@ -24,7 +24,6 @@ class ImplicitSchurFactor: public GaussianFactor {
public: public:
typedef ImplicitSchurFactor This; ///< Typedef to this class 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 typedef boost::shared_ptr<This> shared_ptr; ///< shared_ptr to this class
protected: protected:
@ -87,7 +86,8 @@ public:
} }
/// print /// 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; std::cout << " ImplicitSchurFactor " << std::endl;
Factor::print(s); Factor::print(s);
std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl; std::cout << " PointCovariance_ \n" << PointCovariance_ << std::endl;
@ -188,19 +188,24 @@ public:
/// Return the block diagonal of the Hessian for this factor /// Return the block diagonal of the Hessian for this factor
virtual std::map<Key, Matrix> hessianBlockDiagonal() const { virtual std::map<Key, Matrix> hessianBlockDiagonal() const {
std::map<Key, Matrix> blocks; std::map<Key, Matrix> blocks;
// F'*(I - E*P*E')*F
for (size_t pos = 0; pos < size(); ++pos) { for (size_t pos = 0; pos < size(); ++pos) {
Key j = keys_[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) // 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() const Matrix2D& Fj = Fblocks_[pos].second;
* E_.block<2, 3>(2 * pos, 0); // Eigen::Matrix<double, D, 3> FtE = Fj.transpose()
blocks[j] = Fj.transpose() * Fj // * E_.block<2, 3>(2 * pos, 0);
- FtE * PointCovariance_ * FtE.transpose(); // 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 :-( // F'*(I - E*P*E')*F, TODO: this should work, but it does not :-(
// static const Eigen::Matrix<double, 2, 2> I2 = eye(2); // static const Eigen::Matrix<double, 2, 2> I2 = eye(2);
// Eigen::Matrix<double, 2, 2> Q = // // Eigen::Matrix<double, 2, 2> Q = //
// I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose(); // I2 - E_.block<2, 3>(2 * pos, 0) * PointCovariance_ * E_.block<2, 3>(2 * pos, 0).transpose();
// blocks[j] = Fj.transpose() * Q * Fj; // blocks[j] = Fj.transpose() * Q * Fj;
} }
return blocks; return blocks;
} }
@ -234,6 +239,73 @@ public:
typedef std::vector<Vector2> Error2s; typedef std::vector<Vector2> Error2s;
/**
* @brief Calculate corrected error Q*(e-2*b) = (I - E*P*E')*(e-2*b)
*/
void projectError2(const Error2s& e1, Error2s& e2) const {
// 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] - 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] - 2 * b_.segment < 2 > (k * 2) - E_.block < 2, 3 > (2 * k, 0) * d2;
}
/*
* 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
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]);
projectError2(e1, e2);
double result = 0;
for (size_t k = 0; k < size(); ++k)
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 * @brief Calculate corrected error Q*e = (I - E*P*E')*e
*/ */
@ -253,24 +325,6 @@ public:
e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2; e2[k] = e1[k] - E_.block < 2, 3 > (2 * k, 0) * d2;
} }
/// needed to be GaussianFactor - (I - E*P*E')*(F*x - b)
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]);
return 0.5 * result;
}
/// Scratch space for multiplyHessianAdd /// Scratch space for multiplyHessianAdd
mutable Error2s e1, e2; mutable Error2s e1, e2;
@ -377,6 +431,28 @@ public:
return g; 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 // ImplicitSchurFactor

13
gtsam_unstable/slam/JacobianFactorQ.h
View File

@ -23,6 +23,19 @@ public:
JacobianFactorQ() { 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 /// Constructor
JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D>& Fblocks, JacobianFactorQ(const std::vector<typename Base::KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix3& P, const Vector& b, 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 Eigen::Matrix<double, 2, D> Matrix2D;
typedef std::pair<Key, Matrix2D> KeyMatrix2D; typedef std::pair<Key, Matrix2D> KeyMatrix2D;
typedef std::pair<Key, Matrix> KeyMatrix;
/// Default constructor /// Default constructor
JacobianFactorSVD() {} 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 /// Constructor
JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b, JacobianFactorSVD(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& Enull, const Vector& b,
const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() { const SharedDiagonal& model = SharedDiagonal()) : JacobianSchurFactor<D>() {
@ -32,7 +45,6 @@ public:
// BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) // BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)
// QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second)); // QF.push_back(KeyMatrix(it.first, Q.block(0, 2 * j++, m2, 2) * it.second));
// JacobianFactor factor(QF, Q * b); // JacobianFactor factor(QF, Q * b);
typedef std::pair<Key, Matrix> KeyMatrix;
std::vector<KeyMatrix> QF; std::vector<KeyMatrix> QF;
QF.reserve(numKeys); QF.reserve(numKeys);
BOOST_FOREACH(const KeyMatrix2D& it, Fblocks) BOOST_FOREACH(const KeyMatrix2D& it, Fblocks)

244
gtsam_unstable/slam/SmartFactorBase.h
View File

@ -20,6 +20,7 @@
#pragma once #pragma once
#include "JacobianFactorQ.h" #include "JacobianFactorQ.h"
#include "JacobianFactorSVD.h"
#include "ImplicitSchurFactor.h" #include "ImplicitSchurFactor.h"
#include "RegularHessianFactor.h" #include "RegularHessianFactor.h"
@ -135,12 +136,12 @@ public:
} }
/** return the measurements */ /** return the measurements */
const Vector& measured() const { const std::vector<Point2>& measured() const {
return measured_; return measured_;
} }
/** return the noise model */ /** return the noise model */
const SharedNoiseModel& noise() const { const std::vector<SharedNoiseModel>& noise() const {
return noise_; return noise_;
} }
@ -192,7 +193,7 @@ public:
b[2 * i + 1] = e.y(); b[2 * i + 1] = e.y();
} catch (CheiralityException& e) { } catch (CheiralityException& e) {
std::cout << "Cheirality exception " << std::endl; std::cout << "Cheirality exception " << std::endl;
exit (EXIT_FAILURE); exit(EXIT_FAILURE);
} }
i += 1; i += 1;
} }
@ -222,7 +223,7 @@ public:
* this->noise_.at(i)->distance(reprojectionError.vector()); * this->noise_.at(i)->distance(reprojectionError.vector());
} catch (CheiralityException&) { } catch (CheiralityException&) {
std::cout << "Cheirality exception " << std::endl; std::cout << "Cheirality exception " << std::endl;
exit (EXIT_FAILURE); exit(EXIT_FAILURE);
} }
i += 1; i += 1;
} }
@ -244,7 +245,7 @@ public:
cameras[i].project(point, boost::none, Ei); cameras[i].project(point, boost::none, Ei);
} catch (CheiralityException& e) { } catch (CheiralityException& e) {
std::cout << "Cheirality exception " << std::endl; std::cout << "Cheirality exception " << std::endl;
exit (EXIT_FAILURE); exit(EXIT_FAILURE);
} }
this->noise_.at(i)->WhitenSystem(Ei, b); this->noise_.at(i)->WhitenSystem(Ei, b);
E.block<2, 3>(2 * i, 0) = Ei; 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(); -(cameras[i].project(point, Fi, Ei, Hcali) - this->measured_.at(i)).vector();
} catch (CheiralityException&) { } catch (CheiralityException&) {
std::cout << "Cheirality exception " << std::endl; std::cout << "Cheirality exception " << std::endl;
exit (EXIT_FAILURE); exit(EXIT_FAILURE);
} }
this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi); this->noise_.at(i)->WhitenSystem(Fi, Ei, Hcali, bi);
@ -302,15 +303,18 @@ public:
// Point covariance inv(E'*E) // Point covariance inv(E'*E)
Matrix3 EtE = E.transpose() * E; Matrix3 EtE = E.transpose() * E;
Matrix3 DMatrix = eye(E.cols()); // damping matrix
if (diagonalDamping) { // diagonal of the hessian if (diagonalDamping) { // diagonal of the hessian
DMatrix(0, 0) = EtE(0, 0); EtE(0, 0) += lambda * EtE(0, 0);
DMatrix(1, 1) = EtE(1, 1); EtE(1, 1) += lambda * EtE(1, 1);
DMatrix(2, 2) = EtE(2, 2); 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; return f;
} }
@ -321,8 +325,8 @@ public:
const Cameras& cameras, const Point3& point, const Cameras& cameras, const Point3& point,
const double lambda = 0.0) const { const double lambda = 0.0) const {
size_t numKeys = this->keys_.size(); int numKeys = this->keys_.size();
std::vector < KeyMatrix2D > Fblocks; std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point,
lambda); lambda);
F = zeros(2 * numKeys, D * numKeys); F = zeros(2 * numKeys, D * numKeys);
@ -345,7 +349,7 @@ public:
diagonalDamping); // diagonalDamping should have no effect (only on PointCov) diagonalDamping); // diagonalDamping should have no effect (only on PointCov)
// Do SVD on A // Do SVD on A
Eigen::JacobiSVD < Matrix > svd(E, Eigen::ComputeFullU); Eigen::JacobiSVD<Matrix> svd(E, Eigen::ComputeFullU);
Vector s = svd.singularValues(); Vector s = svd.singularValues();
// Enull = zeros(2 * numKeys, 2 * numKeys - 3); // Enull = zeros(2 * numKeys, 2 * numKeys - 3);
int numKeys = this->keys_.size(); int numKeys = this->keys_.size();
@ -361,7 +365,7 @@ public:
const Cameras& cameras, const Point3& point) const { const Cameras& cameras, const Point3& point) const {
int numKeys = this->keys_.size(); int numKeys = this->keys_.size();
std::vector < KeyMatrix2D > Fblocks; std::vector<KeyMatrix2D> Fblocks;
double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point); double f = computeJacobiansSVD(Fblocks, Enull, b, cameras, point);
F.resize(2 * numKeys, D * numKeys); F.resize(2 * numKeys, D * numKeys);
F.setZero(); F.setZero();
@ -380,14 +384,14 @@ public:
int numKeys = this->keys_.size(); int numKeys = this->keys_.size();
std::vector < KeyMatrix2D > Fblocks; std::vector<KeyMatrix2D> Fblocks;
Matrix E; Matrix E;
Matrix3 PointCov; Matrix3 PointCov;
Vector b; Vector b;
double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, double f = computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping); diagonalDamping);
//#define HESSIAN_BLOCKS //#define HESSIAN_BLOCKS // slower, as internally the Hessian factor will transform the blocks into SymmetricBlockMatrix
#ifdef HESSIAN_BLOCKS #ifdef HESSIAN_BLOCKS
// Create structures for Hessian Factors // Create structures for Hessian Factors
std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2); std::vector < Matrix > Gs(numKeys * (numKeys + 1) / 2);
@ -401,7 +405,7 @@ public:
return boost::make_shared < RegularHessianFactor<D> return boost::make_shared < RegularHessianFactor<D>
> (this->keys_, Gs, gs, f); > (this->keys_, Gs, gs, f);
#else #else // we create directly a SymmetricBlockMatrix
size_t n1 = D * numKeys + 1; size_t n1 = D * numKeys + 1;
std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term std::vector<DenseIndex> dims(numKeys + 1); // this also includes the b term
std::fill(dims.begin(), dims.end() - 1, D); std::fill(dims.begin(), dims.end() - 1, D);
@ -409,37 +413,14 @@ public:
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) = ... sparseSchurComplement(Fblocks, E, PointCov, b, augmentedHessian); // augmentedHessian.matrix().block<D,D> (i1,i2) = ...
augmentedHessian(numKeys,numKeys)(0,0) = f; augmentedHessian(numKeys, numKeys)(0, 0) = f;
return boost::make_shared<RegularHessianFactor<D> >( return boost::make_shared<RegularHessianFactor<D> >(this->keys_,
this->keys_, augmentedHessian); augmentedHessian);
#endif #endif
} }
// **************************************************************************************************** // ****************************************************************************************************
boost::shared_ptr<RegularHessianFactor<D> > updateAugmentedHessian( // slow version - works on full (sparse) matrices
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;
}
// ****************************************************************************************************
void schurComplement(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E, void schurComplement(const std::vector<KeyMatrix2D>& Fblocks, const Matrix& E,
const Matrix& PointCov, const Vector& b, const Matrix& PointCov, const Vector& b,
/*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const { /*output ->*/std::vector<Matrix>& Gs, std::vector<Vector>& gs) const {
@ -466,7 +447,7 @@ public:
int GsCount2 = 0; int GsCount2 = 0;
for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera for (DenseIndex i1 = 0; i1 < numKeys; i1++) { // for each camera
DenseIndex i1D = i1 * D; 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++) { for (DenseIndex i2 = 0; i2 < numKeys; i2++) {
if (i2 >= i1) { if (i2 >= i1) {
Gs.at(GsCount2) = H.block<D, D>(i1D, i2 * D); 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, void sparseSchurComplement(const std::vector<KeyMatrix2D>& Fblocks,
const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b, const Matrix& E, const Matrix& P /*Point Covariance*/, const Vector& b,
@ -542,20 +476,20 @@ public:
// D = (Dx2) * (2) // D = (Dx2) * (2)
// (augmentedHessian.matrix()).block<D,1> (i1,numKeys+1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b // (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 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) - Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) // (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) )
augmentedHessian(i1,i1) = augmentedHessian(i1, i1) = Fi1.transpose()
Fi1.transpose() * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1); * (Fi1 - Ei1_P * E.block<2, 3>(2 * i1, 0).transpose() * Fi1);
// upper triangular part of the hessian // 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; const Matrix2D& Fi2 = Fblocks.at(i2).second;
// (DxD) = (Dx2) * ( (2x2) * (2xD) ) // (DxD) = (Dx2) * ( (2x2) * (2xD) )
augmentedHessian(i1,i2) = augmentedHessian(i1, i2) = -Fi1.transpose()
-Fi1.transpose() * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2); * (Ei1_P * E.block<2, 3>(2 * i2, 0).transpose() * Fi2);
} }
} // end of for over cameras } // end of for over cameras
} }
@ -586,24 +520,109 @@ public:
{ // for i1 = i2 { // for i1 = i2
// D = (Dx2) * (2) // D = (Dx2) * (2)
gs.at(i1) = Fi1.transpose() * b.segment < 2 > (2 * i1); // F' * b gs.at(i1) = Fi1.transpose() * b.segment<2>(2 * i1) // F' * b
// D = (Dx2) * (2x3) * (3*2m) * (2m x 1) -Fi1.transpose() * (Ei1_P * (E.transpose() * b)); // D = (Dx2) * (2x3) * (3*2m) * (2m x 1)
gs.at(i1) -= Fi1.transpose() * (Ei1_P * (E.transpose() * b));
// (DxD) = (Dx2) * ( (2xD) - (2x3) * (3x2) * (2xD) ) // (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++; GsIndex++;
} }
// upper triangular part of the hessian // 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; const Matrix2D& Fi2 = Fblocks.at(i2).second;
// (DxD) = (Dx2) * ( (2x2) * (2xD) ) // (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++; GsIndex++;
} }
} // end of for over cameras } // 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( boost::shared_ptr<ImplicitSchurFactor<D> > createImplicitSchurFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0, const Cameras& cameras, const Point3& point, double lambda = 0.0,
@ -620,13 +639,24 @@ public:
boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor( boost::shared_ptr<JacobianFactorQ<D> > createJacobianQFactor(
const Cameras& cameras, const Point3& point, double lambda = 0.0, const Cameras& cameras, const Point3& point, double lambda = 0.0,
bool diagonalDamping = false) const { bool diagonalDamping = false) const {
std::vector < KeyMatrix2D > Fblocks; std::vector<KeyMatrix2D> Fblocks;
Matrix E; Matrix E;
Matrix3 PointCov; Matrix3 PointCov;
Vector b; Vector b;
computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda, computeJacobians(Fblocks, E, PointCov, b, cameras, point, lambda,
diagonalDamping); 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: private:

58
gtsam_unstable/slam/SmartProjectionFactor.h
View File

@ -54,6 +54,10 @@ public:
double f; double f;
}; };
enum linearizationType {
HESSIAN, JACOBIAN_SVD, JACOBIAN_Q
};
/** /**
* SmartProjectionFactor: triangulates point * SmartProjectionFactor: triangulates point
* TODO: why LANDMARK parameter? * TODO: why LANDMARK parameter?
@ -91,6 +95,13 @@ protected:
/// shorthand for base class type /// shorthand for base class type
typedef SmartFactorBase<POSE, CALIBRATION, D> Base; 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 /// shorthand for this class
typedef SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This; typedef SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, D> This;
@ -115,12 +126,15 @@ public:
SmartProjectionFactor(const double rankTol, const double linThreshold, SmartProjectionFactor(const double rankTol, const double linThreshold,
const bool manageDegeneracy, const bool enableEPI, const bool manageDegeneracy, const bool enableEPI,
boost::optional<POSE> body_P_sensor = boost::none, boost::optional<POSE> body_P_sensor = boost::none,
SmartFactorStatePtr state = SmartFactorStatePtr( double landmarkDistanceThreshold = 1e10,
new SmartProjectionFactorState())) : double dynamicOutlierRejectionThreshold = -1,
SmartFactorStatePtr state = SmartFactorStatePtr(new SmartProjectionFactorState())) :
Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_( Base(body_P_sensor), rankTolerance_(rankTol), retriangulationThreshold_(
1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_( 1e-5), manageDegeneracy_(manageDegeneracy), enableEPI_(enableEPI), linearizationThreshold_(
linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_( linThreshold), degenerate_(false), cheiralityException_(false), throwCheirality_(
false), verboseCheirality_(false), state_(state) { false), verboseCheirality_(false), state_(state),
landmarkDistanceThreshold_(landmarkDistanceThreshold),
dynamicOutlierRejectionThreshold_(dynamicOutlierRejectionThreshold) {
} }
/** Virtual destructor */ /** Virtual destructor */
@ -234,6 +248,31 @@ public:
rankTolerance_, enableEPI_); rankTolerance_, enableEPI_);
degenerate_ = false; degenerate_ = false;
cheiralityException_ = 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&) { } catch (TriangulationUnderconstrainedException&) {
// if TriangulationUnderconstrainedException can be // if TriangulationUnderconstrainedException can be
// 1) There is a single pose for triangulation - this should not happen because we checked the number of poses before // 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)) if (triangulateForLinearize(cameras))
return Base::createJacobianQFactor(cameras, point_, lambda); return Base::createJacobianQFactor(cameras, point_, lambda);
else else
return boost::shared_ptr<JacobianFactorQ<D> >(); return boost::make_shared< JacobianFactorQ<D> >(this->keys_);
} }
/// Create a factor, takes values /// Create a factor, takes values
@ -397,7 +436,16 @@ public:
if (nonDegenerate) if (nonDegenerate)
return createJacobianQFactor(myCameras, lambda); return createJacobianQFactor(myCameras, lambda);
else 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 /// Returns true if nonDegenerate

32
gtsam_unstable/slam/SmartProjectionPoseFactor.h
View File

@ -22,6 +22,16 @@
#include "SmartProjectionFactor.h" #include "SmartProjectionFactor.h"
namespace gtsam { 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) * 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> { class SmartProjectionPoseFactor: public SmartProjectionFactor<POSE, LANDMARK, CALIBRATION, 6> {
protected: protected:
linearizationType linearizeTo_;
// Known calibration // Known calibration
std::vector<boost::shared_ptr<CALIBRATION> > K_all_; ///< shared pointer to calibration object (one for each camera) 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, SmartProjectionPoseFactor(const double rankTol = 1,
const double linThreshold = -1, const bool manageDegeneracy = false, const double linThreshold = -1, const bool manageDegeneracy = false,
const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none) : const bool enableEPI = false, boost::optional<POSE> body_P_sensor = boost::none,
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor) {} linearizationType linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10,
double dynamicOutlierRejectionThreshold = -1) :
Base(rankTol, linThreshold, manageDegeneracy, enableEPI, body_P_sensor,
landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), linearizeTo_(linearizeTo) {}
/** Virtual destructor */ /** Virtual destructor */
virtual ~SmartProjectionPoseFactor() {} 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( virtual boost::shared_ptr<GaussianFactor> linearize(
const Values& values) const { const Values& values) const {
// 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)); 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_(); 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 ){ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ){
// cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl; // cout << " ************************ Normal ProjectionFactor: 3 cams + 3 landmarks **********************" << endl;

234
matlab.h
View File

@ -28,52 +28,105 @@
#include <gtsam/geometry/Cal3_S2.h> #include <gtsam/geometry/Cal3_S2.h>
#include <gtsam/geometry/SimpleCamera.h> #include <gtsam/geometry/SimpleCamera.h>
#include <boost/foreach.hpp>
#include <exception> #include <exception>
namespace gtsam { namespace gtsam {
namespace utilities { namespace utilities {
/// Extract all Point2 values into a single matrix [x y] // Create a KeyList from indices
Matrix extractPoint2(const Values& values) { FastList<Key> createKeyList(const Vector& I) {
size_t j=0; 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>(); Values::ConstFiltered<Point2> points = values.filter<Point2>();
Matrix result(points.size(),2); Matrix result(points.size(), 2);
BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& key_value, points) BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& key_value, points)
result.row(j++) = key_value.value.vector(); result.row(j++) = key_value.value.vector();
return result; return result;
} }
/// Extract all Point3 values into a single matrix [x y z] /// Extract all Point3 values into a single matrix [x y z]
Matrix extractPoint3(const Values& values) { Matrix extractPoint3(const Values& values) {
Values::ConstFiltered<Point3> points = values.filter<Point3>(); Values::ConstFiltered<Point3> points = values.filter<Point3>();
Matrix result(points.size(),3); Matrix result(points.size(), 3);
size_t j=0; size_t j = 0;
BOOST_FOREACH(const Values::ConstFiltered<Point3>::KeyValuePair& key_value, points) BOOST_FOREACH(const Values::ConstFiltered<Point3>::KeyValuePair& key_value, points)
result.row(j++) = key_value.value.vector(); result.row(j++) = key_value.value.vector();
return result; return result;
} }
/// Extract all Pose2 values into a single matrix [x y theta] /// Extract all Pose2 values into a single matrix [x y theta]
Matrix extractPose2(const Values& values) { Matrix extractPose2(const Values& values) {
Values::ConstFiltered<Pose2> poses = values.filter<Pose2>(); Values::ConstFiltered<Pose2> poses = values.filter<Pose2>();
Matrix result(poses.size(),3); Matrix result(poses.size(), 3);
size_t j=0; size_t j = 0;
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, poses) 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(); result.row(j++) << key_value.value.x(), key_value.value.y(), key_value.value.theta();
return result; return result;
} }
/// Extract all Pose3 values /// Extract all Pose3 values
Values allPose3s(const Values& values) { Values allPose3s(const Values& values) {
return values.filter<Pose3>(); return values.filter<Pose3>();
} }
/// Extract all Pose3 values into a single matrix [r11 r12 r13 r21 r22 r23 r31 r32 r33 x y z] /// 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) { Matrix extractPose3(const Values& values) {
Values::ConstFiltered<Pose3> poses = values.filter<Pose3>(); Values::ConstFiltered<Pose3> poses = values.filter<Pose3>();
Matrix result(poses.size(),12); Matrix result(poses.size(), 12);
size_t j=0; size_t j = 0;
BOOST_FOREACH(const Values::ConstFiltered<Pose3>::KeyValuePair& key_value, poses) { 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(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(3, 3) << key_value.value.rotation().matrix().row(1);
@ -82,76 +135,121 @@ namespace gtsam {
j++; j++;
} }
return result; return result;
} }
/// Perturb all Point2 values using normally distributed noise /// Perturb all Point2 values using normally distributed noise
void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) { void perturbPoint2(Values& values, double sigma, int32_t seed = 42u) {
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,sigma); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(2,
sigma);
Sampler sampler(model, seed); Sampler sampler(model, seed);
BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& key_value, values.filter<Point2>()) { BOOST_FOREACH(const Values::ConstFiltered<Point2>::KeyValuePair& key_value, values.filter<Point2>()) {
values.update(key_value.key, key_value.value.retract(sampler.sample())); values.update(key_value.key, key_value.value.retract(sampler.sample()));
} }
} }
/// Perturb all Pose2 values using normally distributed noise /// Perturb all Pose2 values using normally distributed noise
void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed = 42u) { void perturbPose2(Values& values, double sigmaT, double sigmaR, int32_t seed =
42u) {
noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas( noiseModel::Diagonal::shared_ptr model = noiseModel::Diagonal::Sigmas(
Vector3(sigmaT, sigmaT, sigmaR)); Vector3(sigmaT, sigmaT, sigmaR));
Sampler sampler(model, seed); Sampler sampler(model, seed);
BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, values.filter<Pose2>()) { BOOST_FOREACH(const Values::ConstFiltered<Pose2>::KeyValuePair& key_value, values.filter<Pose2>()) {
values.update(key_value.key, key_value.value.retract(sampler.sample())); values.update(key_value.key, key_value.value.retract(sampler.sample()));
} }
} }
/// Perturb all Point3 values using normally distributed noise /// Perturb all Point3 values using normally distributed noise
void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) { void perturbPoint3(Values& values, double sigma, int32_t seed = 42u) {
noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,sigma); noiseModel::Isotropic::shared_ptr model = noiseModel::Isotropic::Sigma(3,
sigma);
Sampler sampler(model, seed); Sampler sampler(model, seed);
BOOST_FOREACH(const Values::ConstFiltered<Point3>::KeyValuePair& key_value, values.filter<Point3>()) { BOOST_FOREACH(const Values::ConstFiltered<Point3>::KeyValuePair& key_value, values.filter<Point3>()) {
values.update(key_value.key, key_value.value.retract(sampler.sample())); values.update(key_value.key, key_value.value.retract(sampler.sample()));
} }
} }
/// Insert a number of initial point values by backprojecting /// Insert a number of initial point values by backprojecting
void insertBackprojections(Values& values, const SimpleCamera& camera, const Vector& J, const Matrix& Z, double depth) { void insertBackprojections(Values& values, const SimpleCamera& camera,
if (Z.rows() != 2) throw std::invalid_argument("insertBackProjections: Z must be 2*K"); const Vector& J, const Matrix& Z, double depth) {
if (Z.cols() != J.size()) throw std::invalid_argument("insertBackProjections: J and Z must have same number of entries"); if (Z.rows() != 2)
for(int k=0;k<Z.cols();k++) { throw std::invalid_argument("insertBackProjections: Z must be 2*K");
Point2 p(Z(0,k),Z(1,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); Point3 P = camera.backproject(p, depth);
values.insert(J(k), P); values.insert(J(k), P);
} }
} }
/// Insert multiple projection factors for a single pose key /// Insert multiple projection factors for a single pose key
void insertProjectionFactors(NonlinearFactorGraph& graph, Key i, const Vector& J, const Matrix& Z, void insertProjectionFactors(NonlinearFactorGraph& graph, Key i,
const SharedNoiseModel& model, const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) { const Vector& J, const Matrix& Z, const SharedNoiseModel& model,
if (Z.rows() != 2) throw std::invalid_argument("addMeasurements: Z must be 2*K"); const Cal3_S2::shared_ptr K, const Pose3& body_P_sensor = Pose3()) {
if (Z.cols() != J.size()) throw std::invalid_argument( 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"); "addMeasurements: J and Z must have same number of entries");
for (int k = 0; k < Z.cols(); k++) { for (int k = 0; k < Z.cols(); k++) {
graph.push_back( graph.push_back(
boost::make_shared<GenericProjectionFactor<Pose3, Point3> > boost::make_shared<GenericProjectionFactor<Pose3, Point3> >(
(Point2(Z(0, k), Z(1, k)), model, i, Key(J(k)), K, body_P_sensor)); 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 /// Calculate the errors of all projection factors in a graph
Matrix reprojectionErrors(const NonlinearFactorGraph& graph, const Values& values) { Matrix reprojectionErrors(const NonlinearFactorGraph& graph,
// first count const Values& values) {
size_t K = 0, k=0; // first count
BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) size_t K = 0, k = 0;
if (boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(f)) ++K; BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph)
// now fill if (boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(
Matrix errors(2,K); f))
BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) { ++K;
boost::shared_ptr<const GenericProjectionFactor<Pose3, Point3> > p = boost::dynamic_pointer_cast<const GenericProjectionFactor<Pose3, Point3> >(f); // now fill
if (p) errors.col(k++) = p->unwhitenedError(values); Matrix errors(2, K);
} BOOST_FOREACH(const NonlinearFactor::shared_ptr& f, graph) {
return errors; 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 % symbolIndex - get index from a symbol key
% %
%% Wrapped C++ Convenience Functions for use within MATLAB %% 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.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.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] % 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.insertBackprojections - Insert a number of initial point values by backprojecting
% utilities.insertProjectionFactors - Insert multiple projection factors for a single pose key % utilities.insertProjectionFactors - Insert multiple projection factors for a single pose key
% utilities.reprojectionErrors - Calculate the errors of all projection factors in a graph % utilities.reprojectionErrors - Calculate the errors of all projection factors in a graph
% utilities.localToWorld - Convert from local to world coordinates

48
matlab/+gtsam/plot2DTrajectory.m
View File

@ -12,44 +12,32 @@ if ~exist('linespec', 'var') || isempty(linespec)
end end
haveMarginals = exist('marginals', 'var'); haveMarginals = exist('marginals', 'var');
keys = KeyVector(values.keys);
holdstate = ishold; holdstate = ishold;
hold on hold on
% Plot poses and covariance matrices % Do something very efficient to draw trajectory
lastIndex = []; poses = utilities.extractPose2(values);
for i = 0:keys.size-1 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); key = keys.at(i);
x = values.at(key); x = values.at(key);
if isa(x, 'gtsam.Pose2') if isa(x, 'gtsam.Pose2')
if ~isempty(lastIndex) P = marginals.marginalCovariance(key);
% Draw line from last pose then covariance ellipse on top of gtsam.plotPose2(x,linespec(1), P);
% 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
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', []);
end end
end end

8
matlab/gtsam_examples/Pose2SLAMExample_advanced.m
View File

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

2
matlab/gtsam_examples/Pose2SLAMExample_graph.m
View File

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

2
matlab/gtsam_examples/Pose3SLAMExample_graph.m
View File

@ -12,6 +12,8 @@
import gtsam.* import gtsam.*
%% PLEASE NOTE THAT PLOTTING TAKES A VERY LONG TIME HERE
%% Find data file %% Find data file
N = 2500; N = 2500;
% dataset = 'sphere_smallnoise.graph'; % 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' display 'Starting: testSerialization'
testSerialization testSerialization
display 'Starting: testUtilities'
testUtilities
% end of tests % end of tests
display 'Tests complete!' display 'Tests complete!'

4
tests/testNonlinearOptimizer.cpp
View File

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

108
wrap/Argument.cpp
View File

@ -16,13 +16,14 @@
* @author Richard Roberts * @author Richard Roberts
**/ **/
#include <iostream> #include "Argument.h"
#include <fstream>
#include <sstream>
#include <boost/foreach.hpp> #include <boost/foreach.hpp>
#include <boost/regex.hpp> #include <boost/regex.hpp>
#include "Argument.h" #include <iostream>
#include <fstream>
#include <sstream>
using namespace std; using namespace std;
using namespace wrap; using namespace wrap;
@ -32,17 +33,23 @@ string Argument::matlabClass(const string& delim) const {
string result; string result;
BOOST_FOREACH(const string& ns, namespaces) BOOST_FOREACH(const string& ns, namespaces)
result += ns + delim; result += ns + delim;
if (type=="string" || type=="unsigned char" || type=="char") if (type == "string" || type == "unsigned char" || type == "char")
return result + "char"; return result + "char";
if (type=="Vector" || type=="Matrix") if (type == "Vector" || type == "Matrix")
return result + "double"; return result + "double";
if (type=="int" || type=="size_t") if (type == "int" || type == "size_t")
return result + "numeric"; return result + "numeric";
if (type=="bool") if (type == "bool")
return result + "logical"; return result + "logical";
return result + type; 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 { void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
file.oss << " "; file.oss << " ";
@ -52,7 +59,8 @@ void Argument::matlab_unwrap(FileWriter& file, const string& matlabName) const {
if (is_ptr) if (is_ptr)
// A pointer: emit an "unwrap_shared_ptr" call which returns a pointer // 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) else if (is_ref)
// A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer // A reference: emit an "unwrap_shared_ptr" call and de-reference the pointer
file.oss << cppType << "& " << name << " = *unwrap_shared_ptr< "; 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 << " " << name << " = unwrap< ";
file.oss << cppType << " >(" << matlabName; 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; file.oss << ");" << endl;
} }
/* ************************************************************************* */ /* ************************************************************************* */
string Argument::qualifiedType(const string& delim) const { string Argument::qualifiedType(const string& delim) const {
string result; string result;
BOOST_FOREACH(const string& ns, namespaces) result += ns + delim; BOOST_FOREACH(const string& ns, namespaces)
result += ns + delim;
return result + type; return result + type;
} }
/* ************************************************************************* */ /* ************************************************************************* */
string ArgumentList::types() const { string ArgumentList::types() const {
string str; string str;
bool first=true; bool first = true;
BOOST_FOREACH(Argument arg, *this) { BOOST_FOREACH(Argument arg, *this) {
if (!first) str += ","; str += arg.type; first=false; if (!first)
str += ",";
str += arg.type;
first = false;
} }
return str; return str;
} }
@ -89,16 +102,17 @@ string ArgumentList::types() const {
/* ************************************************************************* */ /* ************************************************************************* */
string ArgumentList::signature() const { string ArgumentList::signature() const {
string sig; string sig;
bool cap=false; bool cap = false;
BOOST_FOREACH(Argument arg, *this) { BOOST_FOREACH(Argument arg, *this) {
BOOST_FOREACH(char ch, arg.type) BOOST_FOREACH(char ch, arg.type)
if(isupper(ch)) { if (isupper(ch)) {
sig += ch; sig += ch;
//If there is a capital letter, we don't want to read it below //If there is a capital letter, we don't want to read it below
cap=true; cap = true;
} }
if(!cap) sig += arg.type[0]; if (!cap)
sig += arg.type[0];
//Reset to default //Reset to default
cap = false; cap = false;
} }
@ -109,23 +123,77 @@ string ArgumentList::signature() const {
/* ************************************************************************* */ /* ************************************************************************* */
string ArgumentList::names() const { string ArgumentList::names() const {
string str; string str;
bool first=true; bool first = true;
BOOST_FOREACH(Argument arg, *this) { BOOST_FOREACH(Argument arg, *this) {
if (!first) str += ","; str += arg.name; first=false; if (!first)
str += ",";
str += arg.name;
first = false;
} }
return str; 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 { void ArgumentList::matlab_unwrap(FileWriter& file, int start) const {
int index = start; int index = start;
BOOST_FOREACH(Argument arg, *this) { BOOST_FOREACH(Argument arg, *this) {
stringstream buf; stringstream buf;
buf << "in[" << index << "]"; buf << "in[" << index << "]";
arg.matlab_unwrap(file,buf.str()); arg.matlab_unwrap(file, buf.str());
index++; 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 #pragma once
#include "FileWriter.h"
#include "ReturnValue.h"
#include <string> #include <string>
#include <vector> #include <vector>
#include "FileWriter.h"
namespace wrap { namespace wrap {
/// Argument class /// Argument class
@ -40,6 +41,9 @@ struct Argument {
/// return MATLAB class for use in isa(x,class) /// return MATLAB class for use in isa(x,class)
std::string matlabClass(const std::string& delim = "") const; 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 /// adds namespaces to type
std::string qualifiedType(const std::string& delim = "") const; 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 /// create a comma-separated string listing all argument names, used in m-files
std::string names() const; std::string names() const;
/// Check if all arguments scalar
bool allScalar() const;
// MATLAB code generation: // 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++ 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 } // \namespace wrap

354
wrap/Class.cpp
View File

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

11
wrap/Constructor.h
View File

@ -18,11 +18,11 @@
#pragma once #pragma once
#include "Argument.h"
#include <string> #include <string>
#include <vector> #include <vector>
#include "Argument.h"
namespace wrap { namespace wrap {
// Constructor class // Constructor class
@ -54,10 +54,8 @@ struct Constructor {
/// cpp wrapper /// cpp wrapper
std::string wrapper_fragment(FileWriter& file, std::string wrapper_fragment(FileWriter& file,
const std::string& cppClassName, const std::string& cppClassName, const std::string& matlabUniqueName,
const std::string& matlabUniqueName, const std::string& cppBaseClassName, int id,
const std::string& cppBaseClassName,
int id,
const ArgumentList& al) const; const ArgumentList& al) const;
/// constructor function /// constructor function
@ -66,5 +64,4 @@ struct Constructor {
}; };
} // \namespace wrap } // \namespace wrap

94
wrap/GlobalFunction.cpp
View File

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

27
wrap/GlobalFunction.h
View File

@ -27,32 +27,33 @@ struct GlobalFunction {
std::vector<StrVec> namespaces; ///< Stack of namespaces std::vector<StrVec> namespaces; ///< Stack of namespaces
// Constructor only used in Module // Constructor only used in Module
GlobalFunction(bool verbose = true) : verbose_(verbose) {} GlobalFunction(bool verbose = true) :
verbose_(verbose) {
}
// Used to reconstruct // Used to reconstruct
GlobalFunction(const std::string& name_, bool verbose = true) GlobalFunction(const std::string& name_, bool verbose = true) :
: verbose_(verbose), name(name_) {} verbose_(verbose), name(name_) {
}
// adds an overloaded version of this function // adds an overloaded version of this function
void addOverload(bool verbose, const std::string& name, 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 // 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, void matlab_proxy(const std::string& toolboxPath,
const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
std::vector<std::string>& functionNames) const; FileWriter& file, std::vector<std::string>& functionNames) const;
private: private:
// Creates a single global function - all in same namespace // Creates a single global function - all in same namespace
void generateSingleFunction(const std::string& toolboxPath, const std::string& wrapperName, void generateSingleFunction(const std::string& toolboxPath,
const TypeAttributesTable& typeAttributes, FileWriter& wrapperFile, const std::string& wrapperName, const TypeAttributesTable& typeAttributes,
std::vector<std::string>& functionNames) const; FileWriter& file, std::vector<std::string>& functionNames) const;
}; };
} // \namespace wrap } // \namespace wrap

194
wrap/Method.cpp
View File

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

7
wrap/Method.h
View File

@ -18,13 +18,12 @@
#pragma once #pragma once
#include <string>
#include <list>
#include "Argument.h" #include "Argument.h"
#include "ReturnValue.h" #include "ReturnValue.h"
#include "TypeAttributesTable.h" #include "TypeAttributesTable.h"
#include <boost/algorithm/string.hpp>
#include <string>
#include <list>
namespace wrap { 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. // The one with postfix 0 are used to reset the variables after parse.
string methodName, methodName0; string methodName, methodName0;
bool isConst, isConst0 = false; bool isConst, isConst0 = false;
ReturnValue retVal0(verbose), retVal(verbose); ReturnValue retVal0, retVal;
Argument arg0, arg; Argument arg0, arg;
ArgumentList args0, args; ArgumentList args0, args;
vector<string> arg_dup; ///keep track of duplicates 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 * @author Richard Roberts
*/ */
#include <vector>
#include <map>
#include "FileWriter.h" #include "FileWriter.h"
#include "TypeAttributesTable.h" #include "TypeAttributesTable.h"
#include <vector>
#pragma once #pragma once
namespace wrap { namespace wrap {
/**
* Encapsulates return value of a method or function
*/
struct ReturnValue { struct ReturnValue {
/// the different supported return value categories
typedef enum { typedef enum {
CLASS = 1, CLASS = 1, EIGEN = 2, BASIS = 3, VOID = 4
EIGEN = 2,
BASIS = 3,
VOID = 4
} return_category; } 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; bool isPtr1, isPtr2, isPair;
return_category category1, category2; return_category category1, category2;
std::string type1, type2; std::string type1, type2;
std::vector<std::string> namespaces1, namespaces2; std::vector<std::string> namespaces1, namespaces2;
/// Constructor
ReturnValue() :
isPtr1(false), isPtr2(false), isPair(false), category1(CLASS), category2(
CLASS) {
}
typedef enum { typedef enum {
arg1, arg2, pair arg1, arg2, pair
} pairing; } pairing;
@ -49,10 +49,12 @@ struct ReturnValue {
std::string matlab_returnType() const; 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 wrapTypeUnwrap(FileWriter& wrapperFile) const;
void emit_matlab(FileWriter& file) const;
}; };
} // \namespace wrap } // \namespace wrap

157
wrap/StaticMethod.cpp
View File

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

19
wrap/StaticMethod.h
View File

@ -19,13 +19,9 @@
#pragma once #pragma once
#include <string>
#include <list>
#include "Argument.h" #include "Argument.h"
#include "ReturnValue.h" #include "ReturnValue.h"
#include "TypeAttributesTable.h" #include "TypeAttributesTable.h"
#include <boost/algorithm/string.hpp>
namespace wrap { namespace wrap {
@ -34,7 +30,8 @@ struct StaticMethod {
/// Constructor creates empty object /// Constructor creates empty object
StaticMethod(bool verbosity = true) : StaticMethod(bool verbosity = true) :
verbose(verbosity) {} verbose(verbosity) {
}
// Then the instance variables are set directly by the Module constructor // Then the instance variables are set directly by the Module constructor
bool verbose; bool verbose;
@ -51,17 +48,15 @@ struct StaticMethod {
// MATLAB code generation // MATLAB code generation
// classPath is class directory, e.g., ../matlab/@Point2 // classPath is class directory, e.g., ../matlab/@Point2
void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile, void proxy_wrapper_fragments(FileWriter& proxyFile, FileWriter& wrapperFile,
const std::string& cppClassName, const std::string& matlabQualName, const std::string& matlabUniqueName, const std::string& cppClassName, const std::string& matlabQualName,
const std::string& wrapperName, const TypeAttributesTable& typeAttributes, const std::string& matlabUniqueName, const std::string& wrapperName,
const TypeAttributesTable& typeAttributes,
std::vector<std::string>& functionNames) const; std::vector<std::string>& functionNames) const;
private: private:
std::string wrapper_fragment(FileWriter& file, std::string wrapper_fragment(FileWriter& file,
const std::string& cppClassName, const std::string& cppClassName, const std::string& matlabUniqueName,
const std::string& matlabUniqueName, int overload, int id, const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper
int overload,
int id,
const TypeAttributesTable& typeAttributes) const; ///< cpp wrapper
}; };
} // \namespace wrap } // \namespace wrap

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

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

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

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

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

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

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

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

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

@ -1,6 +1,6 @@
function varargout = aGlobalFunction(varargin) function varargout = aGlobalFunction(varargin)
if length(varargin) == 0 if length(varargin) == 0
varargout{1} = geometry_wrapper(42, varargin{:}); varargout{1} = geometry_wrapper(42, varargin{:});
else else
error('Arguments do not match any overload of function aGlobalFunction'); error('Arguments do not match any overload of function aGlobalFunction');
end 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); checkArguments("aGlobalFunction",nargout,nargin,0);
out[0] = wrap< Vector >(aGlobalFunction()); 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[]) 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: case 42:
aGlobalFunction_42(nargout, out, nargin-1, in+1); aGlobalFunction_42(nargout, out, nargin-1, in+1);
break; 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) { } catch(const std::exception& e) {
mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); 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

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

@ -40,22 +40,12 @@ classdef ClassA < handle
function varargout = memberFunction(this, varargin) function varargout = memberFunction(this, varargin)
% MEMBERFUNCTION usage: memberFunction() : returns double % MEMBERFUNCTION usage: memberFunction() : returns double
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html % 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{:}); varargout{1} = testNamespaces_wrapper(9, this, varargin{:});
else
error('Arguments do not match any overload of function ns2.ClassA.memberFunction');
end
end end
function varargout = nsArg(this, varargin) function varargout = nsArg(this, varargin)
% NSARG usage: nsArg(ClassB arg) : returns int % NSARG usage: nsArg(ClassB arg) : returns int
% Doxygen can be found at http://research.cc.gatech.edu/borg/sites/edu.borg/html/index.html % 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') if length(varargin) == 1 && isa(varargin{1},'ns1.ClassB')
varargout{1} = testNamespaces_wrapper(10, this, varargin{:}); varargout{1} = testNamespaces_wrapper(10, this, varargin{:});
else else
@ -66,14 +56,7 @@ classdef ClassA < handle
function varargout = nsReturn(this, varargin) function varargout = nsReturn(this, varargin)
% NSRETURN usage: nsReturn(double q) : returns ns2::ns3::ClassB % 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 % 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{:}); varargout{1} = testNamespaces_wrapper(11, this, varargin{:});
else
error('Arguments do not match any overload of function ns2.ClassA.nsReturn');
end
end end
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); checkArguments("ns2aGlobalFunction",nargout,nargin,0);
out[0] = wrap< Vector >(ns2::aGlobalFunction()); 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[]) 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: case 23:
ns2aGlobalFunction_23(nargout, out, nargin-1, in+1); ns2aGlobalFunction_23(nargout, out, nargin-1, in+1);
break; 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) { } catch(const std::exception& e) {
mexErrMsgTxt(("Exception from gtsam:\n" + std::string(e.what()) + "\n").c_str()); 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(); Vector aGlobalFunction();
// An overloaded global function
Vector overloadedGlobalFunction(int a);
Vector overloadedGlobalFunction(int a, double b);
// comments at the end! // comments at the end!
// even more 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 // separate namespace global function, same name
Vector aGlobalFunction(); Vector aGlobalFunction();
// An overloaded global function
ns1::ClassA overloadedGlobalFunction(const ns1::ClassA& a);
ns1::ClassA overloadedGlobalFunction(const ns1::ClassA& a, double b);
} //\namespace ns2 } //\namespace ns2
class ClassD { class ClassD {

39
wrap/tests/testWrap.cpp
View File

@ -15,16 +15,18 @@
* @author Frank Dellaert * @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 <stdlib.h>
#include <iostream> #include <iostream>
#include <fstream> #include <fstream>
#include <sstream> #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 std;
using namespace boost::assign; using namespace boost::assign;
@ -305,8 +307,8 @@ TEST( wrap, parse_geometry ) {
} }
// evaluate global functions // evaluate global functions
// Vector aGlobalFunction(); // Vector 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()); CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end());
{ {
GlobalFunction gfunc = module.global_functions.at("aGlobalFunction"); GlobalFunction gfunc = module.global_functions.at("aGlobalFunction");
@ -380,7 +382,7 @@ TEST( wrap, parse_namespaces ) {
// evaluate global functions // evaluate global functions
// Vector ns1::aGlobalFunction(); // Vector ns1::aGlobalFunction();
// Vector ns2::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()); CHECK(module.global_functions.find("aGlobalFunction") != module.global_functions.end());
{ {
GlobalFunction gfunc = module.global_functions.at("aGlobalFunction"); GlobalFunction gfunc = module.global_functions.at("aGlobalFunction");
@ -415,13 +417,17 @@ TEST( wrap, matlab_code_namespaces ) {
module.matlab_code("actual_namespaces", headerPath); module.matlab_code("actual_namespaces", headerPath);
EXPECT(files_equal(exp_path + "ClassD.m" , act_path + "ClassD.m" )); 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/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 + "+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/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/ClassC.m", act_path + "+ns2/ClassC.m" ));
EXPECT(files_equal(exp_path + "+ns2/+ns3/ClassB.m" , act_path + "+ns2/+ns3/ClassB.m" )); EXPECT(
EXPECT(files_equal(exp_path + "testNamespaces_wrapper.cpp" , act_path + "testNamespaces_wrapper.cpp" )); 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 + "Point3.m" , apath + "Point3.m" ));
EXPECT(files_equal(epath + "Test.m" , apath + "Test.m" )); EXPECT(files_equal(epath + "Test.m" , apath + "Test.m" ));
EXPECT(files_equal(epath + "aGlobalFunction.m" , apath + "aGlobalFunction.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); string actual_contents = file_contents(actual, skipheader);
bool equal = actual_contents == expected_contents; bool equal = actual_contents == expected_contents;
if (!equal) { if (!equal) {
cerr << "<<< DIFF OUTPUT (if none, white-space differences only):\n";
stringstream command; stringstream command;
command << "diff " << actual << " " << expected << endl; command << "diff --ignore-all-space " << expected << " " << actual << endl;
system(command.str().c_str()); system(command.str().c_str());
cerr << ">>>\n";
} }
return equal; return equal;
} }