Merge remote-tracking branch 'svn/trunk' into windows
Conflicts: gtsam/linear/HessianFactor.h gtsam/nonlinear/Marginals.cpprelease/4.3a0
						commit
						1ca9e7049e
					
				
							
								
								
									
										280
									
								
								.cproject
								
								
								
								
							
							
						
						
									
										280
									
								
								.cproject
								
								
								
								
							|  | @ -593,6 +593,38 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testBTree.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testBTree.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testDSF.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testDSF.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testDSFVector.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testDSFVector.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testFixedVector.run" path="build/gtsam_unstable/base" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testFixedVector.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="all" path="release" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -705,42 +737,34 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="tests/testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testKey.run</buildTarget> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/testGeneralSFMFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testGeneralSFMFactor.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="tests/testPlanarSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testGeneralSFMFactor.run</buildTarget> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/testPlanarSLAM.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testPlanarSLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="tests/testPose2SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testPlanarSLAM.run</buildTarget> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/testPose2SLAM.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testPose2SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="tests/testPose3SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testPose2SLAM.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testPose3SLAM.run" path="build/gtsam/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testPose3SLAM.run</buildTarget> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/testPose3SLAM.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
|  | @ -769,6 +793,46 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="schedulingExample.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>schedulingExample.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testCSP.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testCSP.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testScheduler.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testScheduler.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="schedulingQuals12.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>schedulingQuals12.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testSudoku.run" path="build/gtsam_unstable/discrete" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testSudoku.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="check" path="build/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -801,10 +865,10 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="check" 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> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>check</buildTarget> | ||||
| 				<buildTarget>testDiscreteFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
|  | @ -985,6 +1049,14 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testNonlinearFactor.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testNonlinearFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="all" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -1600,82 +1672,74 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="tests/testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/testVectorValues.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="linear.testNoiseModel.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="testVectorValues.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>linear.testNoiseModel.run</buildTarget> | ||||
| 				<buildTarget>testVectorValues.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="tests/testGaussianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/testGaussianFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="tests/testHessianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/testHessianFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="tests/testGaussianConditional.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/testGaussianConditional.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="tests/testGaussianFactorGraph.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/testGaussianFactorGraph.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="tests/testGaussianJunctionTree.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/testGaussianJunctionTree.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="tests/testKalmanFilter.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/testKalmanFilter.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="tests/testGaussianDensity.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/testGaussianDensity.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="linear.testSerializationLinear.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="testNoiseModel.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>linear.testSerializationLinear.run</buildTarget> | ||||
| 				<buildTarget>testNoiseModel.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testHessianFactor.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testHessianFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testGaussianConditional.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testGaussianConditional.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testGaussianFactorGraph.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testGaussianFactorGraph.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testGaussianJunctionTree.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testGaussianJunctionTree.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testKalmanFilter.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testKalmanFilter.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testGaussianDensity.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testGaussianDensity.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testSerializationLinear.run" path="build/gtsam/linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testSerializationLinear.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
|  | @ -1768,22 +1832,6 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="LocalizationExample.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>LocalizationExample.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="LocalizationExample2.run" path="build/examples" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>LocalizationExample2.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="check" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -2147,18 +2195,18 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="wrap_gtsam_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="check.discrete_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>wrap_gtsam_unstable</buildTarget> | ||||
| 				<buildTarget>check.discrete_unstable</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="check.wrap" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="check.base_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>check.wrap</buildTarget> | ||||
| 				<buildTarget>check.base_unstable</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
|  | @ -2179,26 +2227,26 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="check.base_unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="check.unstable" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>check.base_unstable</buildTarget> | ||||
| 				<buildTarget>check.unstable</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="wrap.testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testSpirit.run</buildTarget> | ||||
| 				<buildTarget>wrap.testSpirit.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="wrap.testWrap.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testWrap.run</buildTarget> | ||||
| 				<buildTarget>wrap.testWrap.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
|  |  | |||
|  | @ -69,6 +69,9 @@ option(GTSAM_INSTALL_MATLAB_EXAMPLES     "Enable/Disable installation of matlab | |||
| option(GTSAM_INSTALL_MATLAB_TESTS        "Enable/Disable installation of matlab tests"    ON) | ||||
| option(GTSAM_INSTALL_WRAP                "Enable/Disable installation of wrap utility" ON) | ||||
| 
 | ||||
| # Experimental - features disabled by default | ||||
| option(GTSAM_ENABLE_BUILD_MEX_BINARIES   "Enable/Disable building of matlab mex files" OFF) | ||||
| 
 | ||||
| # Flags for choosing default packaging tools | ||||
| set(CPACK_SOURCE_GENERATOR "TGZ" CACHE STRING "CPack Default Source Generator") | ||||
| set(CPACK_GENERATOR        "TGZ" CACHE STRING "CPack Default Binary Generator") | ||||
|  |  | |||
|  | @ -41,7 +41,7 @@ SimpleString::SimpleString (const SimpleString& other) | |||
| 
 | ||||
| SimpleString SimpleString::operator= (const SimpleString& other) | ||||
| { | ||||
| 	delete buffer_; | ||||
| 	delete [] buffer_; | ||||
| 	buffer_ = new char [other.size() + 1]; | ||||
| 	strcpy(buffer_, other.buffer_); | ||||
| 	return *this; | ||||
|  | @ -50,7 +50,7 @@ SimpleString SimpleString::operator= (const SimpleString& other) | |||
| SimpleString SimpleString::operator+ (const SimpleString& other) | ||||
| { | ||||
| 	SimpleString ret; | ||||
| 	delete ret.buffer_; | ||||
| 	delete [] ret.buffer_; | ||||
| 	ret.buffer_ = new char [size() + other.size() - 1]; | ||||
| 	strcpy(ret.buffer_, buffer_); | ||||
| 	strcat(ret.buffer_, other.buffer_); | ||||
|  |  | |||
|  | @ -113,7 +113,7 @@ int main(int argc, char** argv) { | |||
| 
 | ||||
| 	// first using sequential elimination
 | ||||
| 	LevenbergMarquardtParams lmParams; | ||||
| 	lmParams.elimination = LevenbergMarquardtParams::SEQUENTIAL; | ||||
| 	lmParams.linearSolverType = LevenbergMarquardtParams::SEQUENTIAL_CHOLESKY; | ||||
| 	Values resultSequential = LevenbergMarquardtOptimizer(graph, initial, lmParams).optimize(); | ||||
| 	resultSequential.print("final result (solved with a sequential solver)"); | ||||
| 
 | ||||
|  |  | |||
							
								
								
									
										1
									
								
								gtsam.h
								
								
								
								
							
							
						
						
									
										1
									
								
								gtsam.h
								
								
								
								
							|  | @ -489,7 +489,6 @@ class Values { | |||
|   Values(); | ||||
|   void print(string s) const; | ||||
|   void insertPose(int key, const gtsam::Pose2& pose); | ||||
|   gtsam::Symbol poseKey(int i); | ||||
|   gtsam::Pose2 pose(int i); | ||||
| }; | ||||
| 
 | ||||
|  |  | |||
|  | @ -49,7 +49,7 @@ namespace gtsam { | |||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	void DecisionTreeFactor::print(const string& s) const { | ||||
| 		cout << s << ":\n"; | ||||
| 		cout << s; | ||||
| 		IndexFactor::print("IndexFactor:"); | ||||
| 		Potentials::print("Potentials:"); | ||||
| 	} | ||||
|  |  | |||
|  | @ -72,7 +72,7 @@ namespace gtsam { | |||
| 		bool equals(const DecisionTreeFactor& other, double tol = 1e-9) const; | ||||
| 
 | ||||
| 		// print
 | ||||
| 		void print(const std::string& s = "DecisionTreeFactor: ") const; | ||||
| 		void print(const std::string& s = "DecisionTreeFactor:\n") const; | ||||
| 
 | ||||
| 		/// @}
 | ||||
| 		/// @name Standard Interface
 | ||||
|  |  | |||
|  | @ -82,7 +82,7 @@ namespace gtsam { | |||
| 		/// @{
 | ||||
| 
 | ||||
| 		// print
 | ||||
| 		virtual void print(const std::string& s = "DiscreteFactor") const { | ||||
| 		virtual void print(const std::string& s = "DiscreteFactor\n") const { | ||||
| 			IndexFactor::print(s); | ||||
| 		} | ||||
| 
 | ||||
|  |  | |||
|  | @ -1,326 +1,326 @@ | |||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @file    HessianFactor.h | ||||
|  * @brief   Contains the HessianFactor class, a general quadratic factor | ||||
|  * @author  Richard Roberts | ||||
|  * @date    Dec 8, 2010 | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/base/blockMatrices.h> | ||||
| #include <gtsam/inference/FactorGraph.h> | ||||
| #include <gtsam/linear/GaussianFactor.h> | ||||
| 
 | ||||
| // Forward declarations for friend unit tests
 | ||||
| class ConversionConstructorHessianFactorTest; | ||||
| class Constructor1HessianFactorTest; | ||||
| class Constructor1bHessianFactorTest; | ||||
| class combineHessianFactorTest; | ||||
| 
 | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|   // Forward declarations
 | ||||
|   class JacobianFactor; | ||||
|   class SharedDiagonal; | ||||
|   class GaussianConditional; | ||||
|   template<class C> class BayesNet; | ||||
| 
 | ||||
|   // Definition of Scatter, which is an intermediate data structure used when
 | ||||
|   // building a HessianFactor incrementally, to get the keys in the right
 | ||||
|   // order.
 | ||||
|   struct SlotEntry { | ||||
|     size_t slot; | ||||
|     size_t dimension; | ||||
|     SlotEntry(size_t _slot, size_t _dimension) | ||||
|     : slot(_slot), dimension(_dimension) {} | ||||
|     std::string toString() const; | ||||
|   }; | ||||
|   typedef FastMap<Index, SlotEntry> Scatter; | ||||
| 
 | ||||
|   /**
 | ||||
|    * @brief A Gaussian factor using the canonical parameters (information form) | ||||
|    * | ||||
|    * HessianFactor implements a general quadratic factor of the form | ||||
|    * \f[ E(x) = 0.5 x^T G x - x^T g + 0.5 f \f] | ||||
|    * that stores the matrix \f$ G \f$, the vector \f$ g \f$, and the constant term \f$ f \f$. | ||||
|    * | ||||
|    * When \f$ G \f$ is positive semidefinite, this factor represents a Gaussian, | ||||
|    * in which case \f$ G \f$ is the information matrix \f$ \Lambda \f$, | ||||
|    * \f$ g \f$ is the information vector \f$ \eta \f$, and \f$ f \f$ is the residual | ||||
|    * sum-square-error at the mean, when \f$ x = \mu \f$. | ||||
|    * | ||||
|    * Indeed, the negative log-likelihood of a Gaussian is (up to a constant) | ||||
|    * @f$ E(x) = 0.5(x-\mu)^T P^{-1} (x-\mu) @f$ | ||||
|    * with @f$ \mu @f$ the mean and  @f$ P @f$ the covariance matrix. Expanding the product we get | ||||
|    * @f[ | ||||
|    * E(x) = 0.5 x^T P^{-1} x - x^T P^{-1} \mu + 0.5 \mu^T P^{-1} \mu | ||||
|    * @f] | ||||
|    * We define the Information matrix (or Hessian) @f$ \Lambda = P^{-1} @f$ | ||||
|    * and the information vector @f$ \eta = P^{-1} \mu = \Lambda \mu @f$ | ||||
|    * to arrive at the canonical form of the Gaussian: | ||||
|    * @f[ | ||||
|    * E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu | ||||
|    * @f] | ||||
|    * | ||||
|    * This factor is one of the factors that can be in a GaussianFactorGraph. | ||||
|    * It may be returned from NonlinearFactor::linearize(), but is also | ||||
|    * used internally to store the Hessian during Cholesky elimination. | ||||
|    * | ||||
|    * This can represent a quadratic factor with characteristics that cannot be | ||||
|    * represented using a JacobianFactor (which has the form | ||||
|    * \f$ E(x) = \Vert Ax - b \Vert^2 \f$ and stores the Jacobian \f$ A \f$ | ||||
|    * and error vector \f$ b \f$, i.e. is a sum-of-squares factor).  For example, | ||||
|    * a HessianFactor need not be positive semidefinite, it can be indefinite or | ||||
|    * even negative semidefinite. | ||||
|    * | ||||
|    * If a HessianFactor is indefinite or negative semi-definite, then in order | ||||
|    * for solving the linear system to be possible, | ||||
|    * the Hessian of the full system must be positive definite (i.e. when all | ||||
|    * small Hessians are combined, the result must be positive definite).  If | ||||
|    * this is not the case, an error will occur during elimination. | ||||
|    * | ||||
|    * This class stores G, g, and f as an augmented matrix HessianFactor::matrix_. | ||||
|    * The upper-left n x n blocks of HessianFactor::matrix_ store the upper-right | ||||
|    * triangle of G, the upper-right-most column of length n of HessianFactor::matrix_ | ||||
|    * stores g, and the lower-right entry of HessianFactor::matrix_ stores f, i.e. | ||||
|    * \code | ||||
|      HessianFactor::matrix_ = [ G11 G12 G13 ... g1 | ||||
|                                   0 G22 G23 ... g2 | ||||
|                                   0   0 G33 ... g3 | ||||
|                                   :   :   :      : | ||||
|                                   0   0   0 ...  f ] | ||||
|      \endcode | ||||
|      Blocks can be accessed as follows: | ||||
|      \code | ||||
|      G11 = info(begin(), begin()); | ||||
|      G12 = info(begin(), begin()+1); | ||||
|      G23 = info(begin()+1, begin()+2); | ||||
|      g2 = linearTerm(begin()+1); | ||||
|      f = constantTerm(); | ||||
|      ....... | ||||
|      \endcode | ||||
|    */ | ||||
|   class HessianFactor : public GaussianFactor { | ||||
|   protected: | ||||
|     typedef Matrix InfoMatrix; ///< The full augmented Hessian
 | ||||
|     typedef SymmetricBlockView<InfoMatrix> BlockInfo; ///< A blockwise view of the Hessian
 | ||||
|     typedef GaussianFactor Base; ///< Typedef to base class
 | ||||
| 
 | ||||
|     InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1]
 | ||||
|     BlockInfo info_;    ///< The block view of the full information matrix.
 | ||||
| 
 | ||||
|     /** Update the factor by adding the information from the JacobianFactor
 | ||||
|      * (used internally during elimination). | ||||
|      * @param update The JacobianFactor containing the new information to add | ||||
|      * @param scatter A mapping from variable index to slot index in this HessianFactor | ||||
|      */ | ||||
|     void updateATA(const JacobianFactor& update, const Scatter& scatter); | ||||
| 
 | ||||
|     /** Update the factor by adding the information from the HessianFactor
 | ||||
|      * (used internally during elimination). | ||||
|      * @param update The HessianFactor containing the new information to add | ||||
|      * @param scatter A mapping from variable index to slot index in this HessianFactor | ||||
|      */ | ||||
|     void updateATA(const HessianFactor& update, const Scatter& scatter); | ||||
| 
 | ||||
|   public: | ||||
| 
 | ||||
|     typedef boost::shared_ptr<HessianFactor> shared_ptr; ///< A shared_ptr to this
 | ||||
|     typedef BlockInfo::Block Block; ///< A block from the Hessian matrix
 | ||||
|     typedef BlockInfo::constBlock constBlock; ///< A block from the Hessian matrix (const version)
 | ||||
|     typedef BlockInfo::Column Column; ///< A column containing the linear term h
 | ||||
|     typedef BlockInfo::constColumn constColumn; ///< A column containing the linear term h (const version)
 | ||||
| 
 | ||||
|     /** Copy constructor */ | ||||
|     HessianFactor(const HessianFactor& gf); | ||||
| 
 | ||||
|     /** default constructor for I/O */ | ||||
|     HessianFactor(); | ||||
| 
 | ||||
|     /** Construct a unary factor.  G is the quadratic term (Hessian matrix), g
 | ||||
|      * the linear term (a vector), and f the constant term.  The quadratic | ||||
|      * error is: | ||||
|      * 0.5*(f - 2*x'*g + x'*G*x) | ||||
|      */ | ||||
|     HessianFactor(Index j, const Matrix& G, const Vector& g, double f); | ||||
| 
 | ||||
|     /** Construct a unary factor, given a mean and covariance matrix.
 | ||||
|      * error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) | ||||
|     */ | ||||
|     HessianFactor(Index j, const Vector& mu, const Matrix& Sigma); | ||||
| 
 | ||||
|     /** Construct a binary factor.  Gxx are the upper-triangle blocks of the
 | ||||
|      * quadratic term (the Hessian matrix), gx the pieces of the linear vector | ||||
|      * term, and f the constant term. | ||||
| 		 * JacobianFactor error is \f[ 0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb \f] | ||||
| 		 * HessianFactor  error is \f[ 0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx    - x'*g   + 0.5*f    \f] | ||||
| 		 * So, with \f$ A = [A1 A2] \f$ and \f$ G=A*'M*A = [A1';A2']*M*[A1 A2] \f$ we have | ||||
| 		 \code | ||||
| 		  n1*n1 G11 = A1'*M*A1 | ||||
| 		  n1*n2 G12 = A1'*M*A2 | ||||
| 		  n2*n2 G22 = A2'*M*A2 | ||||
| 		  n1*1   g1 = A1'*M*b | ||||
| 		  n2*1   g2 = A2'*M*b | ||||
| 		   1*1    f =  b'*M*b | ||||
| 		 \endcode | ||||
|      */ | ||||
|     HessianFactor(Index j1, Index j2, | ||||
|         const Matrix& G11, const Matrix& G12, const Vector& g1, | ||||
|         const Matrix& G22, const Vector& g2, double f); | ||||
| 
 | ||||
|     /** Construct a ternary factor.  Gxx are the upper-triangle blocks of the
 | ||||
|      * quadratic term (the Hessian matrix), gx the pieces of the linear vector | ||||
|      * term, and f the constant term. | ||||
|      */ | ||||
|     HessianFactor(Index j1, Index j2, Index j3, | ||||
|         const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, | ||||
|         const Matrix& G22, const Matrix& G23, const Vector& g2, | ||||
|         const Matrix& G33, const Vector& g3, double f); | ||||
| 
 | ||||
|     /** Construct an n-way factor.  Gs contains the upper-triangle blocks of the
 | ||||
|      * quadratic term (the Hessian matrix) provided in row-order, gs the pieces | ||||
|      * of the linear vector term, and f the constant term. | ||||
|      */ | ||||
|     HessianFactor(const std::vector<Index>& js, const std::vector<Matrix>& Gs, | ||||
|         const std::vector<Vector>& gs, double f); | ||||
| 
 | ||||
|     /** Construct from Conditional Gaussian */ | ||||
|     HessianFactor(const GaussianConditional& cg); | ||||
| 
 | ||||
|     /** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */ | ||||
|     HessianFactor(const GaussianFactor& factor); | ||||
| 
 | ||||
|     /** Special constructor used in EliminateCholesky which combines the given factors */ | ||||
|     HessianFactor(const FactorGraph<GaussianFactor>& factors, | ||||
| 				const std::vector<size_t>& dimensions, const Scatter& scatter); | ||||
| 
 | ||||
|     /** Destructor */ | ||||
| 		virtual ~HessianFactor() {} | ||||
| 
 | ||||
|     /** Aassignment operator */ | ||||
| 		HessianFactor& operator=(const HessianFactor& rhs); | ||||
| 
 | ||||
| 		/** Clone this JacobianFactor */ | ||||
| 		virtual GaussianFactor::shared_ptr clone() const { | ||||
| 		  return boost::static_pointer_cast<GaussianFactor>( | ||||
| 		      shared_ptr(new HessianFactor(*this))); | ||||
| 		} | ||||
| 
 | ||||
|     /** Print the factor for debugging and testing (implementing Testable) */ | ||||
|     virtual void print(const std::string& s = "") const; | ||||
| 
 | ||||
|     /** Compare to another factor for testing (implementing Testable) */ | ||||
|     virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; | ||||
| 
 | ||||
|     /** Evaluate the factor error f(x), see above. */ | ||||
|     virtual double error(const VectorValues& c) const; /** 0.5*[x -1]'*H*[x -1] (also see constructor documentation) */ | ||||
| 
 | ||||
|     /** Return the dimension of the variable pointed to by the given key iterator
 | ||||
|      * todo: Remove this in favor of keeping track of dimensions with variables? | ||||
|      * @param variable An iterator pointing to the slot in this factor.  You can | ||||
|      * use, for example, begin() + 2 to get the 3rd variable in this factor. | ||||
|      */ | ||||
|     virtual size_t getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); } | ||||
| 
 | ||||
|     /** Return the number of columns and rows of the Hessian matrix */ | ||||
|     size_t rows() const { return info_.rows(); } | ||||
| 
 | ||||
|     /** Return a view of the block at (j1,j2) of the <emph>upper-triangular part</emph> of the
 | ||||
|      * information matrix \f$ H \f$, no data is copied.  See HessianFactor class documentation | ||||
|      * above to explain that only the upper-triangular part of the information matrix is stored | ||||
|      * and returned by this function. | ||||
|      * @param j1 Which block row to get, as an iterator pointing to the slot in this factor.  You can | ||||
|      * use, for example, begin() + 2 to get the 3rd variable in this factor. | ||||
|      * @param j2 Which block column to get, as an iterator pointing to the slot in this factor.  You can | ||||
|      * use, for example, begin() + 2 to get the 3rd variable in this factor. | ||||
|      * @return A view of the requested block, not a copy. | ||||
|      */ | ||||
|     constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); } | ||||
| 
 | ||||
|     /** Return the <emph>upper-triangular part</emph> of the full *augmented* information matrix,
 | ||||
|      * as described above.  See HessianFactor class documentation above to explain that only the | ||||
|      * upper-triangular part of the information matrix is stored and returned by this function. | ||||
|      */ | ||||
|     constBlock info() const { return info_.full(); } | ||||
| 
 | ||||
|     /** Return the constant term \f$ f \f$ as described above
 | ||||
|      * @return The constant term \f$ f \f$ | ||||
|      */ | ||||
|     double constantTerm() const; | ||||
| 
 | ||||
|     /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable.
 | ||||
|      * @param j Which block row to get, as an iterator pointing to the slot in this factor.  You can | ||||
|      * use, for example, begin() + 2 to get the 3rd variable in this factor. | ||||
|      * @return The linear term \f$ g \f$ */ | ||||
|     constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); } | ||||
| 
 | ||||
|     /** Return the complete linear term \f$ g \f$ as described above.
 | ||||
|      * @return The linear term \f$ g \f$ */ | ||||
|     constColumn linearTerm() const; | ||||
|      | ||||
|     /** Return the augmented information matrix represented by this GaussianFactor.
 | ||||
|      * The augmented information matrix contains the information matrix with an | ||||
|      * additional column holding the information vector, and an additional row | ||||
|      * holding the transpose of the information vector.  The lower-right entry | ||||
|      * contains the constant error term (when \f$ \delta x = 0 \f$).  The | ||||
|      * augmented information matrix is described in more detail in HessianFactor, | ||||
|      * which in fact stores an augmented information matrix. | ||||
|      * | ||||
|      * For HessianFactor, this is the same as info() except that this function | ||||
|      * returns a complete symmetric matrix whereas info() returns a matrix where | ||||
|      * only the upper triangle is valid, but should be interpreted as symmetric. | ||||
|      * This is because info() returns only a reference to the internal | ||||
|      * representation of the augmented information matrix, which stores only the | ||||
|      * upper triangle. | ||||
|      */ | ||||
|     virtual Matrix computeInformation() const; | ||||
| 
 | ||||
|     // Friend unit test classes
 | ||||
|     friend class ::ConversionConstructorHessianFactorTest; | ||||
|     friend class ::Constructor1HessianFactorTest; | ||||
|     friend class ::Constructor1bHessianFactorTest; | ||||
|     friend class ::combineHessianFactorTest; | ||||
| 
 | ||||
|     // Friend JacobianFactor for conversion
 | ||||
|     friend class JacobianFactor; | ||||
| 
 | ||||
|     // used in eliminateCholesky:
 | ||||
| 
 | ||||
|     /**
 | ||||
| 		 * Do Cholesky. Note that after this, the lower triangle still contains | ||||
| 		 * some untouched non-zeros that should be zero.  We zero them while | ||||
| 		 * extracting submatrices in splitEliminatedFactor. Frank says :-( | ||||
| 		 */ | ||||
|     void partialCholesky(size_t nrFrontals); | ||||
| 
 | ||||
|     /** split partially eliminated factor */ | ||||
|     boost::shared_ptr<GaussianConditional> splitEliminatedFactor(size_t nrFrontals); | ||||
| 
 | ||||
|     /** assert invariants */ | ||||
|     void assertInvariants() const; | ||||
| 
 | ||||
|   private: | ||||
|     /** Serialization function */ | ||||
|     friend class boost::serialization::access; | ||||
|     template<class ARCHIVE> | ||||
|     void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
|     	ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor); | ||||
|     	ar & BOOST_SERIALIZATION_NVP(info_); | ||||
|     	ar & BOOST_SERIALIZATION_NVP(matrix_); | ||||
|     } | ||||
|   }; | ||||
| 
 | ||||
| } | ||||
| 
 | ||||
| /* ----------------------------------------------------------------------------
 | ||||
| 
 | ||||
|  * GTSAM Copyright 2010, Georgia Tech Research Corporation, | ||||
|  * Atlanta, Georgia 30332-0415 | ||||
|  * All Rights Reserved | ||||
|  * Authors: Frank Dellaert, et al. (see THANKS for the full author list) | ||||
| 
 | ||||
|  * See LICENSE for the license information | ||||
| 
 | ||||
|  * -------------------------------------------------------------------------- */ | ||||
| 
 | ||||
| /**
 | ||||
|  * @file    HessianFactor.h | ||||
|  * @brief   Contains the HessianFactor class, a general quadratic factor | ||||
|  * @author  Richard Roberts | ||||
|  * @date    Dec 8, 2010 | ||||
|  */ | ||||
| 
 | ||||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/base/blockMatrices.h> | ||||
| #include <gtsam/inference/FactorGraph.h> | ||||
| #include <gtsam/linear/GaussianFactor.h> | ||||
| 
 | ||||
| // Forward declarations for friend unit tests
 | ||||
| class ConversionConstructorHessianFactorTest; | ||||
| class Constructor1HessianFactorTest; | ||||
| class Constructor1bHessianFactorTest; | ||||
| class combineHessianFactorTest; | ||||
| 
 | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
|   // Forward declarations
 | ||||
|   class JacobianFactor; | ||||
|   class SharedDiagonal; | ||||
|   class GaussianConditional; | ||||
|   template<class C> class BayesNet; | ||||
| 
 | ||||
|   // Definition of Scatter, which is an intermediate data structure used when
 | ||||
|   // building a HessianFactor incrementally, to get the keys in the right
 | ||||
|   // order.
 | ||||
|   struct SlotEntry { | ||||
|     size_t slot; | ||||
|     size_t dimension; | ||||
|     SlotEntry(size_t _slot, size_t _dimension) | ||||
|     : slot(_slot), dimension(_dimension) {} | ||||
|     std::string toString() const; | ||||
|   }; | ||||
|   typedef FastMap<Index, SlotEntry> Scatter; | ||||
| 
 | ||||
|   /**
 | ||||
|    * @brief A Gaussian factor using the canonical parameters (information form) | ||||
|    * | ||||
|    * HessianFactor implements a general quadratic factor of the form | ||||
|    * \f[ E(x) = 0.5 x^T G x - x^T g + 0.5 f \f] | ||||
|    * that stores the matrix \f$ G \f$, the vector \f$ g \f$, and the constant term \f$ f \f$. | ||||
|    * | ||||
|    * When \f$ G \f$ is positive semidefinite, this factor represents a Gaussian, | ||||
|    * in which case \f$ G \f$ is the information matrix \f$ \Lambda \f$, | ||||
|    * \f$ g \f$ is the information vector \f$ \eta \f$, and \f$ f \f$ is the residual | ||||
|    * sum-square-error at the mean, when \f$ x = \mu \f$. | ||||
|    * | ||||
|    * Indeed, the negative log-likelihood of a Gaussian is (up to a constant) | ||||
|    * @f$ E(x) = 0.5(x-\mu)^T P^{-1} (x-\mu) @f$ | ||||
|    * with @f$ \mu @f$ the mean and  @f$ P @f$ the covariance matrix. Expanding the product we get | ||||
|    * @f[ | ||||
|    * E(x) = 0.5 x^T P^{-1} x - x^T P^{-1} \mu + 0.5 \mu^T P^{-1} \mu | ||||
|    * @f] | ||||
|    * We define the Information matrix (or Hessian) @f$ \Lambda = P^{-1} @f$ | ||||
|    * and the information vector @f$ \eta = P^{-1} \mu = \Lambda \mu @f$ | ||||
|    * to arrive at the canonical form of the Gaussian: | ||||
|    * @f[ | ||||
|    * E(x) = 0.5 x^T \Lambda x - x^T \eta + 0.5 \mu^T \Lambda \mu | ||||
|    * @f] | ||||
|    * | ||||
|    * This factor is one of the factors that can be in a GaussianFactorGraph. | ||||
|    * It may be returned from NonlinearFactor::linearize(), but is also | ||||
|    * used internally to store the Hessian during Cholesky elimination. | ||||
|    * | ||||
|    * This can represent a quadratic factor with characteristics that cannot be | ||||
|    * represented using a JacobianFactor (which has the form | ||||
|    * \f$ E(x) = \Vert Ax - b \Vert^2 \f$ and stores the Jacobian \f$ A \f$ | ||||
|    * and error vector \f$ b \f$, i.e. is a sum-of-squares factor).  For example, | ||||
|    * a HessianFactor need not be positive semidefinite, it can be indefinite or | ||||
|    * even negative semidefinite. | ||||
|    * | ||||
|    * If a HessianFactor is indefinite or negative semi-definite, then in order | ||||
|    * for solving the linear system to be possible, | ||||
|    * the Hessian of the full system must be positive definite (i.e. when all | ||||
|    * small Hessians are combined, the result must be positive definite).  If | ||||
|    * this is not the case, an error will occur during elimination. | ||||
|    * | ||||
|    * This class stores G, g, and f as an augmented matrix HessianFactor::matrix_. | ||||
|    * The upper-left n x n blocks of HessianFactor::matrix_ store the upper-right | ||||
|    * triangle of G, the upper-right-most column of length n of HessianFactor::matrix_ | ||||
|    * stores g, and the lower-right entry of HessianFactor::matrix_ stores f, i.e. | ||||
|    * \code | ||||
|      HessianFactor::matrix_ = [ G11 G12 G13 ... g1 | ||||
|                                   0 G22 G23 ... g2 | ||||
|                                   0   0 G33 ... g3 | ||||
|                                   :   :   :      : | ||||
|                                   0   0   0 ...  f ] | ||||
|      \endcode | ||||
|      Blocks can be accessed as follows: | ||||
|      \code | ||||
|      G11 = info(begin(), begin()); | ||||
|      G12 = info(begin(), begin()+1); | ||||
|      G23 = info(begin()+1, begin()+2); | ||||
|      g2 = linearTerm(begin()+1); | ||||
|      f = constantTerm(); | ||||
|      ....... | ||||
|      \endcode | ||||
|    */ | ||||
|   class HessianFactor : public GaussianFactor { | ||||
|   protected: | ||||
|     typedef Matrix InfoMatrix; ///< The full augmented Hessian
 | ||||
|     typedef SymmetricBlockView<InfoMatrix> BlockInfo; ///< A blockwise view of the Hessian
 | ||||
|     typedef GaussianFactor Base; ///< Typedef to base class
 | ||||
| 
 | ||||
|     InfoMatrix matrix_; ///< The full augmented information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1]
 | ||||
|     BlockInfo info_;    ///< The block view of the full information matrix.
 | ||||
| 
 | ||||
|     /** Update the factor by adding the information from the JacobianFactor
 | ||||
|      * (used internally during elimination). | ||||
|      * @param update The JacobianFactor containing the new information to add | ||||
|      * @param scatter A mapping from variable index to slot index in this HessianFactor | ||||
|      */ | ||||
|     void updateATA(const JacobianFactor& update, const Scatter& scatter); | ||||
| 
 | ||||
|     /** Update the factor by adding the information from the HessianFactor
 | ||||
|      * (used internally during elimination). | ||||
|      * @param update The HessianFactor containing the new information to add | ||||
|      * @param scatter A mapping from variable index to slot index in this HessianFactor | ||||
|      */ | ||||
|     void updateATA(const HessianFactor& update, const Scatter& scatter); | ||||
| 
 | ||||
|   public: | ||||
| 
 | ||||
|     typedef boost::shared_ptr<HessianFactor> shared_ptr; ///< A shared_ptr to this
 | ||||
|     typedef BlockInfo::Block Block; ///< A block from the Hessian matrix
 | ||||
|     typedef BlockInfo::constBlock constBlock; ///< A block from the Hessian matrix (const version)
 | ||||
|     typedef BlockInfo::Column Column; ///< A column containing the linear term h
 | ||||
|     typedef BlockInfo::constColumn constColumn; ///< A column containing the linear term h (const version)
 | ||||
| 
 | ||||
|     /** Copy constructor */ | ||||
|     HessianFactor(const HessianFactor& gf); | ||||
| 
 | ||||
|     /** default constructor for I/O */ | ||||
|     HessianFactor(); | ||||
| 
 | ||||
|     /** Construct a unary factor.  G is the quadratic term (Hessian matrix), g
 | ||||
|      * the linear term (a vector), and f the constant term.  The quadratic | ||||
|      * error is: | ||||
|      * 0.5*(f - 2*x'*g + x'*G*x) | ||||
|      */ | ||||
|     HessianFactor(Index j, const Matrix& G, const Vector& g, double f); | ||||
| 
 | ||||
|     /** Construct a unary factor, given a mean and covariance matrix.
 | ||||
|      * error is 0.5*(x-mu)'*inv(Sigma)*(x-mu) | ||||
|     */ | ||||
|     HessianFactor(Index j, const Vector& mu, const Matrix& Sigma); | ||||
| 
 | ||||
|     /** Construct a binary factor.  Gxx are the upper-triangle blocks of the
 | ||||
|      * quadratic term (the Hessian matrix), gx the pieces of the linear vector | ||||
|      * term, and f the constant term. | ||||
| 		 * JacobianFactor error is \f[ 0.5* (Ax-b)' M (Ax-b) = 0.5*x'A'MAx - x'A'Mb + 0.5*b'Mb \f] | ||||
| 		 * HessianFactor  error is \f[ 0.5*(x'Gx - 2x'g + f) = 0.5*x'Gx    - x'*g   + 0.5*f    \f] | ||||
| 		 * So, with \f$ A = [A1 A2] \f$ and \f$ G=A*'M*A = [A1';A2']*M*[A1 A2] \f$ we have | ||||
| 		 \code | ||||
| 		  n1*n1 G11 = A1'*M*A1 | ||||
| 		  n1*n2 G12 = A1'*M*A2 | ||||
| 		  n2*n2 G22 = A2'*M*A2 | ||||
| 		  n1*1   g1 = A1'*M*b | ||||
| 		  n2*1   g2 = A2'*M*b | ||||
| 		   1*1    f =  b'*M*b | ||||
| 		 \endcode | ||||
|      */ | ||||
|     HessianFactor(Index j1, Index j2, | ||||
|         const Matrix& G11, const Matrix& G12, const Vector& g1, | ||||
|         const Matrix& G22, const Vector& g2, double f); | ||||
| 
 | ||||
|     /** Construct a ternary factor.  Gxx are the upper-triangle blocks of the
 | ||||
|      * quadratic term (the Hessian matrix), gx the pieces of the linear vector | ||||
|      * term, and f the constant term. | ||||
|      */ | ||||
|     HessianFactor(Index j1, Index j2, Index j3, | ||||
|         const Matrix& G11, const Matrix& G12, const Matrix& G13, const Vector& g1, | ||||
|         const Matrix& G22, const Matrix& G23, const Vector& g2, | ||||
|         const Matrix& G33, const Vector& g3, double f); | ||||
| 
 | ||||
|     /** Construct an n-way factor.  Gs contains the upper-triangle blocks of the
 | ||||
|      * quadratic term (the Hessian matrix) provided in row-order, gs the pieces | ||||
|      * of the linear vector term, and f the constant term. | ||||
|      */ | ||||
|     HessianFactor(const std::vector<Index>& js, const std::vector<Matrix>& Gs, | ||||
|         const std::vector<Vector>& gs, double f); | ||||
| 
 | ||||
|     /** Construct from Conditional Gaussian */ | ||||
|     HessianFactor(const GaussianConditional& cg); | ||||
| 
 | ||||
|     /** Convert from a JacobianFactor (computes A^T * A) or HessianFactor */ | ||||
|     HessianFactor(const GaussianFactor& factor); | ||||
| 
 | ||||
|     /** Special constructor used in EliminateCholesky which combines the given factors */ | ||||
|     HessianFactor(const FactorGraph<GaussianFactor>& factors, | ||||
| 				const std::vector<size_t>& dimensions, const Scatter& scatter); | ||||
| 
 | ||||
|     /** Destructor */ | ||||
| 		virtual ~HessianFactor() {} | ||||
| 
 | ||||
|     /** Aassignment operator */ | ||||
| 		HessianFactor& operator=(const HessianFactor& rhs); | ||||
| 
 | ||||
| 		/** Clone this JacobianFactor */ | ||||
| 		virtual GaussianFactor::shared_ptr clone() const { | ||||
| 		  return boost::static_pointer_cast<GaussianFactor>( | ||||
| 		      shared_ptr(new HessianFactor(*this))); | ||||
| 		} | ||||
| 
 | ||||
|     /** Print the factor for debugging and testing (implementing Testable) */ | ||||
|     virtual void print(const std::string& s = "") const; | ||||
| 
 | ||||
|     /** Compare to another factor for testing (implementing Testable) */ | ||||
|     virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; | ||||
| 
 | ||||
|     /** Evaluate the factor error f(x), see above. */ | ||||
|     virtual double error(const VectorValues& c) const; /** 0.5*[x -1]'*H*[x -1] (also see constructor documentation) */ | ||||
| 
 | ||||
|     /** Return the dimension of the variable pointed to by the given key iterator
 | ||||
|      * todo: Remove this in favor of keeping track of dimensions with variables? | ||||
|      * @param variable An iterator pointing to the slot in this factor.  You can | ||||
|      * use, for example, begin() + 2 to get the 3rd variable in this factor. | ||||
|      */ | ||||
|     virtual size_t getDim(const_iterator variable) const { return info_(variable-this->begin(), 0).rows(); } | ||||
| 
 | ||||
|     /** Return the number of columns and rows of the Hessian matrix */ | ||||
|     size_t rows() const { return info_.rows(); } | ||||
| 
 | ||||
|     /** Return a view of the block at (j1,j2) of the <emph>upper-triangular part</emph> of the
 | ||||
|      * information matrix \f$ H \f$, no data is copied.  See HessianFactor class documentation | ||||
|      * above to explain that only the upper-triangular part of the information matrix is stored | ||||
|      * and returned by this function. | ||||
|      * @param j1 Which block row to get, as an iterator pointing to the slot in this factor.  You can | ||||
|      * use, for example, begin() + 2 to get the 3rd variable in this factor. | ||||
|      * @param j2 Which block column to get, as an iterator pointing to the slot in this factor.  You can | ||||
|      * use, for example, begin() + 2 to get the 3rd variable in this factor. | ||||
|      * @return A view of the requested block, not a copy. | ||||
|      */ | ||||
|     constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); } | ||||
| 
 | ||||
|     /** Return the <emph>upper-triangular part</emph> of the full *augmented* information matrix,
 | ||||
|      * as described above.  See HessianFactor class documentation above to explain that only the | ||||
|      * upper-triangular part of the information matrix is stored and returned by this function. | ||||
|      */ | ||||
|     constBlock info() const { return info_.full(); } | ||||
| 
 | ||||
|     /** Return the constant term \f$ f \f$ as described above
 | ||||
|      * @return The constant term \f$ f \f$ | ||||
|      */ | ||||
|     double constantTerm() const; | ||||
| 
 | ||||
|     /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable.
 | ||||
|      * @param j Which block row to get, as an iterator pointing to the slot in this factor.  You can | ||||
|      * use, for example, begin() + 2 to get the 3rd variable in this factor. | ||||
|      * @return The linear term \f$ g \f$ */ | ||||
|     constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); } | ||||
| 
 | ||||
|     /** Return the complete linear term \f$ g \f$ as described above.
 | ||||
|      * @return The linear term \f$ g \f$ */ | ||||
|     constColumn linearTerm() const; | ||||
|      | ||||
|     /** Return the augmented information matrix represented by this GaussianFactor.
 | ||||
|      * The augmented information matrix contains the information matrix with an | ||||
|      * additional column holding the information vector, and an additional row | ||||
|      * holding the transpose of the information vector.  The lower-right entry | ||||
|      * contains the constant error term (when \f$ \delta x = 0 \f$).  The | ||||
|      * augmented information matrix is described in more detail in HessianFactor, | ||||
|      * which in fact stores an augmented information matrix. | ||||
|      * | ||||
|      * For HessianFactor, this is the same as info() except that this function | ||||
|      * returns a complete symmetric matrix whereas info() returns a matrix where | ||||
|      * only the upper triangle is valid, but should be interpreted as symmetric. | ||||
|      * This is because info() returns only a reference to the internal | ||||
|      * representation of the augmented information matrix, which stores only the | ||||
|      * upper triangle. | ||||
|      */ | ||||
|     virtual Matrix computeInformation() const; | ||||
| 
 | ||||
|     // Friend unit test classes
 | ||||
|     friend class ::ConversionConstructorHessianFactorTest; | ||||
|     friend class ::Constructor1HessianFactorTest; | ||||
|     friend class ::Constructor1bHessianFactorTest; | ||||
|     friend class ::combineHessianFactorTest; | ||||
| 
 | ||||
|     // Friend JacobianFactor for conversion
 | ||||
|     friend class JacobianFactor; | ||||
| 
 | ||||
|     // used in eliminateCholesky:
 | ||||
| 
 | ||||
|     /**
 | ||||
| 		 * Do Cholesky. Note that after this, the lower triangle still contains | ||||
| 		 * some untouched non-zeros that should be zero.  We zero them while | ||||
| 		 * extracting submatrices in splitEliminatedFactor. Frank says :-( | ||||
| 		 */ | ||||
|     void partialCholesky(size_t nrFrontals); | ||||
| 
 | ||||
|     /** split partially eliminated factor */ | ||||
|     boost::shared_ptr<GaussianConditional> splitEliminatedFactor(size_t nrFrontals); | ||||
| 
 | ||||
|     /** assert invariants */ | ||||
|     void assertInvariants() const; | ||||
| 
 | ||||
|   private: | ||||
|     /** Serialization function */ | ||||
|     friend class boost::serialization::access; | ||||
|     template<class ARCHIVE> | ||||
|     void serialize(ARCHIVE & ar, const unsigned int version) { | ||||
|     	ar & BOOST_SERIALIZATION_BASE_OBJECT_NVP(GaussianFactor); | ||||
|     	ar & BOOST_SERIALIZATION_NVP(info_); | ||||
|     	ar & BOOST_SERIALIZATION_NVP(matrix_); | ||||
|     } | ||||
|   }; | ||||
| 
 | ||||
| } | ||||
| 
 | ||||
|  |  | |||
|  | @ -154,6 +154,16 @@ namespace gtsam { | |||
|     Vector unweighted_error(const VectorValues& c) const; /** (A*x-b) */ | ||||
|     Vector error_vector(const VectorValues& c) const; /** (A*x-b)/sigma */ | ||||
|     virtual double error(const VectorValues& c) const; /**  0.5*(A*x-b)'*D*(A*x-b) */ | ||||
| 
 | ||||
|     /** Return the augmented information matrix represented by this GaussianFactor.
 | ||||
|      * The augmented information matrix contains the information matrix with an | ||||
|      * additional column holding the information vector, and an additional row | ||||
|      * holding the transpose of the information vector.  The lower-right entry | ||||
|      * contains the constant error term (when \f$ \delta x = 0 \f$).  The | ||||
|      * augmented information matrix is described in more detail in HessianFactor, | ||||
|      * which in fact stores an augmented information matrix. | ||||
|      */ | ||||
|     virtual Matrix computeInformation() const; | ||||
| 
 | ||||
|     /** Return the augmented information matrix represented by this GaussianFactor.
 | ||||
|      * The augmented information matrix contains the information matrix with an | ||||
|  |  | |||
|  | @ -33,25 +33,25 @@ void DoglegOptimizer::iterate(void) { | |||
|   const Ordering& ordering = *params_.ordering; | ||||
|   GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values, ordering); | ||||
| 
 | ||||
|   // Get elimination method
 | ||||
|   GaussianFactorGraph::Eliminate eliminationMethod = params_.getEliminationFunction(); | ||||
| 
 | ||||
|   // Pull out parameters we'll use
 | ||||
|   const bool dlVerbose = (params_.verbosityDL > DoglegParams::SILENT); | ||||
| 
 | ||||
|   // Do Dogleg iteration with either Multifrontal or Sequential elimination
 | ||||
|   DoglegOptimizerImpl::IterationResult result; | ||||
| 
 | ||||
|   if(params_.elimination == DoglegParams::MULTIFRONTAL) { | ||||
|   if ( params_.isMultifrontal() ) { | ||||
|     GaussianBayesTree bt; | ||||
|     bt.insert(GaussianJunctionTree(*linear).eliminate(eliminationMethod)); | ||||
|     bt.insert(GaussianJunctionTree(*linear).eliminate(params_.getEliminationFunction())); | ||||
|     result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, bt, graph_, state_.values, ordering, state_.error, dlVerbose); | ||||
| 
 | ||||
|   } else if(params_.elimination == DoglegParams::SEQUENTIAL) { | ||||
|     GaussianBayesNet::shared_ptr bn = EliminationTree<GaussianFactor>::Create(*linear)->eliminate(eliminationMethod); | ||||
|   } | ||||
|   else if ( params_.isSequential() ) { | ||||
|     GaussianBayesNet::shared_ptr bn = EliminationTree<GaussianFactor>::Create(*linear)->eliminate(params_.getEliminationFunction()); | ||||
|     result = DoglegOptimizerImpl::Iterate(state_.Delta, DoglegOptimizerImpl::ONE_STEP_PER_ITERATION, *bn, graph_, state_.values, ordering, state_.error, dlVerbose); | ||||
| 
 | ||||
|   } else { | ||||
|   } | ||||
|   else if ( params_.isCG() ) { | ||||
|     throw runtime_error("todo: "); | ||||
|   } | ||||
|   else { | ||||
|     throw runtime_error("Optimization parameter is invalid: DoglegParams::elimination"); | ||||
|   } | ||||
| 
 | ||||
|  |  | |||
|  | @ -35,13 +35,18 @@ void GaussNewtonOptimizer::iterate() { | |||
|   // Optimize
 | ||||
|   VectorValues delta; | ||||
|   { | ||||
|     GaussianFactorGraph::Eliminate eliminationMethod = params_.getEliminationFunction(); | ||||
|     if(params_.elimination == GaussNewtonParams::MULTIFRONTAL) | ||||
|       delta = GaussianJunctionTree(*linear).optimize(eliminationMethod); | ||||
|     else if(params_.elimination == GaussNewtonParams::SEQUENTIAL) | ||||
|       delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(*linear)->eliminate(eliminationMethod)); | ||||
|     else | ||||
|     if ( params_.isMultifrontal() ) { | ||||
|       delta = GaussianJunctionTree(*linear).optimize(params_.getEliminationFunction()); | ||||
|     } | ||||
|     else if ( params_.isSequential() ) { | ||||
|       delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(*linear)->eliminate(params_.getEliminationFunction())); | ||||
|     } | ||||
|     else if ( params_.isCG() ) { | ||||
|       throw runtime_error("todo: "); | ||||
|     } | ||||
|     else { | ||||
|       throw runtime_error("Optimization parameter is invalid: GaussNewtonParams::elimination"); | ||||
|     } | ||||
|   } | ||||
| 
 | ||||
|   // Maybe show output
 | ||||
|  |  | |||
|  | @ -490,6 +490,9 @@ public: | |||
|   /** Access the current ordering */ | ||||
|   const Ordering& getOrdering() const { return ordering_; } | ||||
| 
 | ||||
|   /** Access the nonlinear variable index */ | ||||
|   const VariableIndex& getVariableIndex() const { return variableIndex_; } | ||||
| 
 | ||||
|   size_t lastAffectedVariableCount; | ||||
|   size_t lastAffectedFactorCount; | ||||
|   size_t lastAffectedCliqueCount; | ||||
|  |  | |||
|  | @ -34,9 +34,6 @@ void LevenbergMarquardtOptimizer::iterate() { | |||
|   // Linearize graph
 | ||||
|   GaussianFactorGraph::shared_ptr linear = graph_.linearize(state_.values, *params_.ordering); | ||||
| 
 | ||||
|   // Get elimination method
 | ||||
|   GaussianFactorGraph::Eliminate eliminationMethod = params_.getEliminationFunction(); | ||||
| 
 | ||||
|   // Pull out parameters we'll use
 | ||||
|   const NonlinearOptimizerParams::Verbosity nloVerbosity = params_.verbosity; | ||||
|   const LevenbergMarquardtParams::VerbosityLM lmVerbosity = params_.verbosityLM; | ||||
|  | @ -69,12 +66,18 @@ void LevenbergMarquardtOptimizer::iterate() { | |||
| 
 | ||||
|       // Optimize
 | ||||
|       VectorValues delta; | ||||
|       if(params_.elimination == SuccessiveLinearizationParams::MULTIFRONTAL) | ||||
|         delta = GaussianJunctionTree(dampedSystem).optimize(eliminationMethod); | ||||
|       else if(params_.elimination == SuccessiveLinearizationParams::SEQUENTIAL) | ||||
|         delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(dampedSystem)->eliminate(eliminationMethod)); | ||||
|       else | ||||
|       if ( params_.isMultifrontal() ) { | ||||
|         delta = GaussianJunctionTree(dampedSystem).optimize(params_.getEliminationFunction()); | ||||
|       } | ||||
|       else if ( params_.isSequential() ) { | ||||
|         delta = gtsam::optimize(*EliminationTree<GaussianFactor>::Create(dampedSystem)->eliminate(params_.getEliminationFunction())); | ||||
|       } | ||||
|       else if ( params_.isCG() ) { | ||||
|         throw runtime_error("todo: "); | ||||
|       } | ||||
|       else { | ||||
|         throw runtime_error("Optimization parameter is invalid: LevenbergMarquardtParams::elimination"); | ||||
|       } | ||||
| 
 | ||||
|       if (lmVerbosity >= LevenbergMarquardtParams::TRYLAMBDA) cout << "linear delta norm = " << delta.vector().norm() << endl; | ||||
|       if (lmVerbosity >= LevenbergMarquardtParams::TRYDELTA) delta.print("delta"); | ||||
|  |  | |||
|  | @ -157,11 +157,33 @@ public: | |||
|   /**
 | ||||
|    * Creates a shared_ptr clone of the factor - needs to be specialized to allow | ||||
|    * for subclasses | ||||
|    * | ||||
|    * By default, throws exception if subclass does not implement the function. | ||||
|    */ | ||||
|   virtual shared_ptr clone() const =0; | ||||
|   virtual shared_ptr clone() const { | ||||
|   	// TODO: choose better exception to throw here
 | ||||
|   	throw std::runtime_error("NonlinearFactor::clone(): Attempting to clone factor with no clone() implemented!"); | ||||
|   	return shared_ptr(); | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|    * Clones a factor and replaces its keys | ||||
|    * Creates a shared_ptr clone of the factor with different keys using | ||||
|    * a map from old->new keys | ||||
|    */ | ||||
|   shared_ptr rekey(const std::map<Key,Key>& rekey_mapping) const { | ||||
|   	shared_ptr new_factor = clone(); | ||||
|   	for (size_t i=0; i<new_factor->size(); ++i) { | ||||
|   		Key& cur_key = new_factor->keys()[i]; | ||||
|   		std::map<Key,Key>::const_iterator mapping = rekey_mapping.find(cur_key); | ||||
|   		if (mapping != rekey_mapping.end()) | ||||
|   			cur_key = mapping->second; | ||||
|   	} | ||||
|   	return new_factor; | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|    * Clones a factor and fully replaces its keys | ||||
|    * @param new_keys is the full replacement set of keys | ||||
|    */ | ||||
|   shared_ptr rekey(const std::vector<Key>& new_keys) const { | ||||
|   	assert(new_keys.size() == this->keys().size()); | ||||
|  |  | |||
|  | @ -27,112 +27,136 @@ using namespace std; | |||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	double NonlinearFactorGraph::probPrime(const Values& c) const { | ||||
| 		return exp(-0.5 * error(c)); | ||||
| /* ************************************************************************* */ | ||||
| double NonlinearFactorGraph::probPrime(const Values& c) const { | ||||
| 	return exp(-0.5 * error(c)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const { | ||||
| 	cout << str << "size: " << size() << endl; | ||||
| 	for (size_t i = 0; i < factors_.size(); i++) { | ||||
| 		stringstream ss; | ||||
| 		ss << "factor " << i << ": "; | ||||
| 		if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter); | ||||
| 	} | ||||
| } | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	void NonlinearFactorGraph::print(const std::string& str, const KeyFormatter& keyFormatter) const { | ||||
|     cout << str << "size: " << size() << endl; | ||||
|     for (size_t i = 0; i < factors_.size(); i++) { | ||||
|       stringstream ss; | ||||
|       ss << "factor " << i << ": "; | ||||
|       if (factors_[i] != NULL) factors_[i]->print(ss.str(), keyFormatter); | ||||
|     } | ||||
| /* ************************************************************************* */ | ||||
| double NonlinearFactorGraph::error(const Values& c) const { | ||||
| 	double total_error = 0.; | ||||
| 	// iterate over all the factors_ to accumulate the log probabilities
 | ||||
| 	BOOST_FOREACH(const sharedFactor& factor, this->factors_) { | ||||
| 		if(factor) | ||||
| 			total_error += factor->error(c); | ||||
| 	} | ||||
| 	return total_error; | ||||
| } | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	double NonlinearFactorGraph::error(const Values& c) const { | ||||
| 		double total_error = 0.; | ||||
| 		// iterate over all the factors_ to accumulate the log probabilities
 | ||||
| 		BOOST_FOREACH(const sharedFactor& factor, this->factors_) { | ||||
| 		  if(factor) | ||||
| 		    total_error += factor->error(c); | ||||
| 		} | ||||
| 		return total_error; | ||||
| /* ************************************************************************* */ | ||||
| std::set<Key> NonlinearFactorGraph::keys() const { | ||||
| 	std::set<Key> keys; | ||||
| 	BOOST_FOREACH(const sharedFactor& factor, this->factors_) { | ||||
| 		if(factor) | ||||
| 			keys.insert(factor->begin(), factor->end()); | ||||
| 	} | ||||
| 	return keys; | ||||
| } | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	std::set<Key> NonlinearFactorGraph::keys() const { | ||||
| 		std::set<Key> keys; | ||||
| 		BOOST_FOREACH(const sharedFactor& factor, this->factors_) { | ||||
| 		  if(factor) | ||||
| 		    keys.insert(factor->begin(), factor->end()); | ||||
| 		} | ||||
| 		return keys; | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD( | ||||
| 			const Values& config) const { | ||||
| 
 | ||||
| 		// Create symbolic graph and initial (iterator) ordering
 | ||||
| 		SymbolicFactorGraph::shared_ptr symbolic; | ||||
| 		Ordering::shared_ptr ordering; | ||||
| 		boost::tie(symbolic, ordering) = this->symbolic(config); | ||||
| 
 | ||||
| 		// Compute the VariableIndex (column-wise index)
 | ||||
| 		VariableIndex variableIndex(*symbolic, ordering->size()); | ||||
| 		if (config.size() != variableIndex.size()) throw std::runtime_error( | ||||
| 				"orderingCOLAMD: some variables in the graph are not constrained!"); | ||||
| 
 | ||||
| 		// Compute a fill-reducing ordering with COLAMD
 | ||||
| 		Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMD( | ||||
| 				variableIndex)); | ||||
| 
 | ||||
| 		// Permute the Ordering with the COLAMD ordering
 | ||||
| 		ordering->permuteWithInverse(*colamdPerm->inverse()); | ||||
| 
 | ||||
| 		// Return the Ordering and VariableIndex to be re-used during linearization
 | ||||
| 		// and elimination
 | ||||
| 		return ordering; | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const { | ||||
| 		// Generate the symbolic factor graph
 | ||||
| 		SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph); | ||||
| 		symbolicfg->reserve(this->size()); | ||||
| 
 | ||||
| 		BOOST_FOREACH(const sharedFactor& factor, this->factors_) { | ||||
| 		  if(factor) | ||||
| 		    symbolicfg->push_back(factor->symbolic(ordering)); | ||||
| 		  else | ||||
| 		    symbolicfg->push_back(SymbolicFactorGraph::sharedFactor()); | ||||
| 		} | ||||
| 
 | ||||
| 		return symbolicfg; | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic( | ||||
| /* ************************************************************************* */ | ||||
| Ordering::shared_ptr NonlinearFactorGraph::orderingCOLAMD( | ||||
| 		const Values& config) const { | ||||
| 		// Generate an initial key ordering in iterator order
 | ||||
| 		Ordering::shared_ptr ordering(config.orderingArbitrary()); | ||||
| 		return make_pair(symbolic(*ordering), ordering); | ||||
| 
 | ||||
| 	// Create symbolic graph and initial (iterator) ordering
 | ||||
| 	SymbolicFactorGraph::shared_ptr symbolic; | ||||
| 	Ordering::shared_ptr ordering; | ||||
| 	boost::tie(symbolic, ordering) = this->symbolic(config); | ||||
| 
 | ||||
| 	// Compute the VariableIndex (column-wise index)
 | ||||
| 	VariableIndex variableIndex(*symbolic, ordering->size()); | ||||
| 	if (config.size() != variableIndex.size()) throw std::runtime_error( | ||||
| 			"orderingCOLAMD: some variables in the graph are not constrained!"); | ||||
| 
 | ||||
| 	// Compute a fill-reducing ordering with COLAMD
 | ||||
| 	Permutation::shared_ptr colamdPerm(inference::PermutationCOLAMD( | ||||
| 			variableIndex)); | ||||
| 
 | ||||
| 	// Permute the Ordering with the COLAMD ordering
 | ||||
| 	ordering->permuteWithInverse(*colamdPerm->inverse()); | ||||
| 
 | ||||
| 	// Return the Ordering and VariableIndex to be re-used during linearization
 | ||||
| 	// and elimination
 | ||||
| 	return ordering; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| SymbolicFactorGraph::shared_ptr NonlinearFactorGraph::symbolic(const Ordering& ordering) const { | ||||
| 	// Generate the symbolic factor graph
 | ||||
| 	SymbolicFactorGraph::shared_ptr symbolicfg(new SymbolicFactorGraph); | ||||
| 	symbolicfg->reserve(this->size()); | ||||
| 
 | ||||
| 	BOOST_FOREACH(const sharedFactor& factor, this->factors_) { | ||||
| 		if(factor) | ||||
| 			symbolicfg->push_back(factor->symbolic(ordering)); | ||||
| 		else | ||||
| 			symbolicfg->push_back(SymbolicFactorGraph::sharedFactor()); | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize( | ||||
| 			const Values& config, const Ordering& ordering) const { | ||||
| 	return symbolicfg; | ||||
| } | ||||
| 
 | ||||
| 		// create an empty linear FG
 | ||||
| 		GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph); | ||||
| 		linearFG->reserve(this->size()); | ||||
| /* ************************************************************************* */ | ||||
| pair<SymbolicFactorGraph::shared_ptr, Ordering::shared_ptr> NonlinearFactorGraph::symbolic( | ||||
| 		const Values& config) const { | ||||
| 	// Generate an initial key ordering in iterator order
 | ||||
| 	Ordering::shared_ptr ordering(config.orderingArbitrary()); | ||||
| 	return make_pair(symbolic(*ordering), ordering); | ||||
| } | ||||
| 
 | ||||
| 		// linearize all factors
 | ||||
| 		BOOST_FOREACH(const sharedFactor& factor, this->factors_) { | ||||
| 		  if(factor) { | ||||
| 		    GaussianFactor::shared_ptr lf = factor->linearize(config, ordering); | ||||
| 		    if (lf) linearFG->push_back(lf); | ||||
| 		  } else | ||||
| 		    linearFG->push_back(GaussianFactor::shared_ptr()); | ||||
| 		} | ||||
| /* ************************************************************************* */ | ||||
| GaussianFactorGraph::shared_ptr NonlinearFactorGraph::linearize( | ||||
| 		const Values& config, const Ordering& ordering) const { | ||||
| 
 | ||||
| 		return linearFG; | ||||
| 	// create an empty linear FG
 | ||||
| 	GaussianFactorGraph::shared_ptr linearFG(new GaussianFactorGraph); | ||||
| 	linearFG->reserve(this->size()); | ||||
| 
 | ||||
| 	// linearize all factors
 | ||||
| 	BOOST_FOREACH(const sharedFactor& factor, this->factors_) { | ||||
| 		if(factor) { | ||||
| 			GaussianFactor::shared_ptr lf = factor->linearize(config, ordering); | ||||
| 			if (lf) linearFG->push_back(lf); | ||||
| 		} else | ||||
| 			linearFG->push_back(GaussianFactor::shared_ptr()); | ||||
| 	} | ||||
| 
 | ||||
| 	return linearFG; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| NonlinearFactorGraph NonlinearFactorGraph::clone() const { | ||||
| 	NonlinearFactorGraph result; | ||||
| 	BOOST_FOREACH(const sharedFactor& f, *this) { | ||||
| 		if (f) | ||||
| 			result.push_back(f->clone()); | ||||
| 		else | ||||
| 			result.push_back(sharedFactor()); // Passes on null factors so indices remain valid
 | ||||
| 	} | ||||
| 	return result; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| NonlinearFactorGraph NonlinearFactorGraph::rekey(const std::map<Key,Key>& rekey_mapping) const { | ||||
| 	NonlinearFactorGraph result; | ||||
| 	BOOST_FOREACH(const sharedFactor& f, *this) { | ||||
| 		if (f) | ||||
| 			result.push_back(f->rekey(rekey_mapping)); | ||||
| 		else | ||||
| 			result.push_back(sharedFactor()); | ||||
| 	} | ||||
| 	return result; | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| 
 | ||||
| } // namespace gtsam
 | ||||
|  |  | |||
|  | @ -87,6 +87,22 @@ namespace gtsam { | |||
| 		boost::shared_ptr<GaussianFactorGraph > | ||||
| 				linearize(const Values& config, const Ordering& ordering) const; | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * Clone() performs a deep-copy of the graph, including all of the factors | ||||
| 		 */ | ||||
| 		NonlinearFactorGraph clone() const; | ||||
| 
 | ||||
| 		/**
 | ||||
| 		 * Rekey() performs a deep-copy of all of the factors, and changes | ||||
| 		 * keys according to a mapping. | ||||
| 		 * | ||||
| 		 * Keys not specified in the mapping will remain unchanged. | ||||
| 		 * | ||||
| 		 * @param rekey_mapping is a map of old->new keys | ||||
| 		 * @result a cloned graph with updated keys | ||||
| 		 */ | ||||
| 		NonlinearFactorGraph rekey(const std::map<Key,Key>& rekey_mapping) const; | ||||
| 
 | ||||
| 	private: | ||||
| 
 | ||||
| 		/** Serialization function */ | ||||
|  |  | |||
|  | @ -93,7 +93,8 @@ public: | |||
|   /// behavior of std::map)
 | ||||
|   Index& operator[](Key key) { | ||||
|     iterator i=order_.find(key); | ||||
|     if(i == order_.end())  throw std::out_of_range(std::string("Attempting to access a key from an ordering that does not contain that key")); | ||||
|     if(i == order_.end())  throw std::out_of_range( | ||||
|     		std::string("Attempting to access a key from an ordering that does not contain that key:") + DefaultKeyFormatter(key)); | ||||
|     else                   return i->second; } | ||||
| 
 | ||||
|   /// Access the index for the requested key, throws std::out_of_range if the
 | ||||
|  | @ -101,7 +102,8 @@ public: | |||
|   /// behavior of std::map)
 | ||||
|   Index operator[](Key key) const { | ||||
|     const_iterator i=order_.find(key); | ||||
|     if(i == order_.end())  throw std::out_of_range(std::string("Attempting to access a key from an ordering that does not contain that key")); | ||||
|     if(i == order_.end())  throw std::out_of_range( | ||||
|     		std::string("Attempting to access a key from an ordering that does not contain that key:") + DefaultKeyFormatter(key)); | ||||
|     else                   return i->second; } | ||||
| 
 | ||||
|   /** Returns an iterator pointing to the symbol/index pair with the requested,
 | ||||
|  |  | |||
|  | @ -19,47 +19,59 @@ | |||
| #pragma once | ||||
| 
 | ||||
| #include <gtsam/nonlinear/NonlinearOptimizer.h> | ||||
| #include <gtsam/linear/IterativeOptimizationParameters.h> | ||||
| 
 | ||||
| namespace gtsam { | ||||
| 
 | ||||
| class SuccessiveLinearizationParams : public NonlinearOptimizerParams { | ||||
| public: | ||||
|   /** See SuccessiveLinearizationParams::elimination */ | ||||
|   enum Elimination { | ||||
|     MULTIFRONTAL, | ||||
|     SEQUENTIAL | ||||
|   /** See SuccessiveLinearizationParams::linearSolverType */ | ||||
|   enum LinearSolverType { | ||||
|     MULTIFRONTAL_CHOLESKY, | ||||
|     MULTIFRONTAL_QR, | ||||
|     SEQUENTIAL_CHOLESKY, | ||||
|     SEQUENTIAL_QR, | ||||
|     CHOLMOD,    /* Experimental Flag */ | ||||
|     PCG,        /* Experimental Flag */ | ||||
|     LSPCG       /* Experimental Flag */ | ||||
|   }; | ||||
| 
 | ||||
|   /** See SuccessiveLinearizationParams::factorization */ | ||||
|   enum Factorization { | ||||
|     CHOLESKY, | ||||
|     QR, | ||||
|   }; | ||||
| 
 | ||||
|   Elimination elimination; ///< The elimination algorithm to use (default: MULTIFRONTAL)
 | ||||
|   Factorization factorization; ///< The numerical factorization (default: Cholesky)
 | ||||
| 	LinearSolverType linearSolverType; ///< The type of linear solver to use in the nonlinear optimizer
 | ||||
|   boost::optional<Ordering> ordering; ///< The variable elimination ordering, or empty to use COLAMD (default: empty)
 | ||||
|   boost::optional<IterativeOptimizationParameters::shared_ptr> iterativeParams; ///< The container for iterativeOptimization parameters.
 | ||||
| 
 | ||||
|   SuccessiveLinearizationParams() : | ||||
|     elimination(MULTIFRONTAL), factorization(CHOLESKY) {} | ||||
|   SuccessiveLinearizationParams() : linearSolverType(MULTIFRONTAL_CHOLESKY) {} | ||||
| 
 | ||||
|   virtual ~SuccessiveLinearizationParams() {} | ||||
| 
 | ||||
|   virtual void print(const std::string& str = "") const { | ||||
|     NonlinearOptimizerParams::print(str); | ||||
|     if(elimination == MULTIFRONTAL) | ||||
|       std::cout << "         elimination method: MULTIFRONTAL\n"; | ||||
|     else if(elimination == SEQUENTIAL) | ||||
|       std::cout << "         elimination method: SEQUENTIAL\n"; | ||||
|     else | ||||
|       std::cout << "         elimination method: (invalid)\n"; | ||||
| 
 | ||||
|     if(factorization == CHOLESKY) | ||||
|       std::cout << "       factorization method: CHOLESKY\n"; | ||||
|     else if(factorization == QR) | ||||
|       std::cout << "       factorization method: QR\n"; | ||||
|     else | ||||
|       std::cout << "       factorization method: (invalid)\n"; | ||||
|     switch ( linearSolverType ) { | ||||
|     case MULTIFRONTAL_CHOLESKY: | ||||
|       std::cout << "         linear solver type: MULTIFRONTAL CHOLESKY\n"; | ||||
|       break; | ||||
|     case MULTIFRONTAL_QR: | ||||
|       std::cout << "         linear solver type: MULTIFRONTAL QR\n"; | ||||
|       break; | ||||
|     case SEQUENTIAL_CHOLESKY: | ||||
|       std::cout << "         linear solver type: SEQUENTIAL CHOLESKY\n"; | ||||
|       break; | ||||
|     case SEQUENTIAL_QR: | ||||
|       std::cout << "         linear solver type: SEQUENTIAL QR\n"; | ||||
|       break; | ||||
|     case CHOLMOD: | ||||
|       std::cout << "         linear solver type: CHOLMOD\n"; | ||||
|       break; | ||||
|     case PCG: | ||||
|       std::cout << "         linear solver type: PCG\n"; | ||||
|       break; | ||||
|     case LSPCG: | ||||
|       std::cout << "         linear solver type: LSPCG\n"; | ||||
|       break; | ||||
|     default: | ||||
|       std::cout << "         linear solver type: (invalid)\n"; | ||||
|       break; | ||||
|     } | ||||
| 
 | ||||
|     if(ordering) | ||||
|       std::cout << "                   ordering: custom\n"; | ||||
|  | @ -69,13 +81,36 @@ public: | |||
|     std::cout.flush(); | ||||
|   } | ||||
| 
 | ||||
|   GaussianFactorGraph::Eliminate getEliminationFunction() const { | ||||
|     if(factorization == SuccessiveLinearizationParams::CHOLESKY) | ||||
|   inline bool isMultifrontal() const { | ||||
|     return (linearSolverType == MULTIFRONTAL_CHOLESKY) || (linearSolverType == MULTIFRONTAL_QR); | ||||
|   } | ||||
| 
 | ||||
|   inline bool isSequential() const { | ||||
|     return (linearSolverType == SEQUENTIAL_CHOLESKY) || (linearSolverType == SEQUENTIAL_QR); | ||||
|   } | ||||
| 
 | ||||
|   inline bool isCholmod() const { | ||||
|     return (linearSolverType == CHOLMOD); | ||||
|   } | ||||
| 
 | ||||
|   inline bool isCG() const { | ||||
|     return (linearSolverType == PCG || linearSolverType == LSPCG); | ||||
|   } | ||||
| 
 | ||||
|   GaussianFactorGraph::Eliminate getEliminationFunction() { | ||||
|     switch (linearSolverType) { | ||||
|     case MULTIFRONTAL_CHOLESKY: | ||||
|     case MULTIFRONTAL_QR: | ||||
|       return EliminatePreferCholesky; | ||||
|     else if(factorization == SuccessiveLinearizationParams::QR) | ||||
| 
 | ||||
|     case SEQUENTIAL_CHOLESKY: | ||||
|     case SEQUENTIAL_QR: | ||||
|       return EliminateQR; | ||||
|     else | ||||
| 
 | ||||
|     default: | ||||
|       throw runtime_error("Nonlinear optimization parameter \"factorization\" is invalid"); | ||||
|       break; | ||||
|     } | ||||
|   } | ||||
| }; | ||||
| 
 | ||||
|  |  | |||
|  | @ -7,6 +7,8 @@ set (gtsam_unstable_subdirs | |||
|     slam  | ||||
| ) | ||||
| 
 | ||||
| add_custom_target(check.unstable COMMAND ${CMAKE_CTEST_COMMAND} --output-on-failure) | ||||
| 
 | ||||
| # assemble core libaries | ||||
| foreach(subdir ${gtsam_unstable_subdirs}) | ||||
|     # Build convenience libraries | ||||
|  | @ -86,6 +88,17 @@ if (GTSAM_BUILD_WRAP) | |||
|         ${CMAKE_BINARY_DIR}/wrap/wrap ${GTSAM_MEX_BIN_EXTENSION} ${CMAKE_CURRENT_SOURCE_DIR} ${moduleName} ${toolbox_path} "${mexFlags}" | ||||
|         DEPENDS wrap) | ||||
|      | ||||
|     # Build command | ||||
|     # Experimental: requires matlab to be on your path | ||||
|     if (GTSAM_ENABLE_BUILD_MEX_BINARIES) | ||||
|         # Actually compile the mex files when building the library | ||||
|         set(TOOLBOX_MAKE_FLAGS "-j2")     | ||||
|         add_custom_target(wrap_gtsam_unstable_build  | ||||
|             COMMAND make ${TOOLBOX_MAKE_FLAGS}  | ||||
|             WORKING_DIRECTORY ${toolbox_path}  | ||||
|             DEPENDS wrap_gtsam_unstable) | ||||
|     endif (GTSAM_ENABLE_BUILD_MEX_BINARIES)     | ||||
|      | ||||
|     if (GTSAM_INSTALL_MATLAB_TOOLBOX) | ||||
|         # Primary toolbox files | ||||
|         message(STATUS "Installing Matlab Toolbox to ${GTSAM_TOOLBOX_INSTALL_PATH}") | ||||
|  |  | |||
|  | @ -16,4 +16,4 @@ set (base_excluded_tests "") | |||
| 
 | ||||
| # Add all tests | ||||
| gtsam_add_subdir_tests(base_unstable "${base_local_libs}" "${base_full_libs}" "${base_excluded_tests}")  | ||||
| 
 | ||||
| add_dependencies(check.unstable check.base_unstable) | ||||
|  |  | |||
|  | @ -21,7 +21,7 @@ namespace gtsam { | |||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	void AllDiff::print(const std::string& s) const { | ||||
| 		std::cout << s << ": AllDiff on "; | ||||
| 		std::cout << s << "AllDiff on "; | ||||
| 		BOOST_FOREACH (Index dkey, keys_) | ||||
| 			std::cout << dkey << " "; | ||||
| 		std::cout << std::endl; | ||||
|  |  | |||
|  | @ -34,7 +34,7 @@ namespace gtsam { | |||
| 
 | ||||
| 		// print
 | ||||
| 		virtual void print(const std::string& s = "") const { | ||||
| 			std::cout << s << ": BinaryAllDiff on " << keys_[0] << " and " << keys_[1] | ||||
| 			std::cout << s << "BinaryAllDiff on " << keys_[0] << " and " << keys_[1] | ||||
| 					<< std::endl; | ||||
| 		} | ||||
| 
 | ||||
|  |  | |||
|  | @ -16,18 +16,19 @@ set (discrete_full_libs | |||
|     gtsam_unstable-static) | ||||
| 
 | ||||
| # Exclude tests that don't work | ||||
| set (discrete_excluded_tests  | ||||
| "${CMAKE_CURRENT_SOURCE_DIR}/tests/testScheduler.cpp" | ||||
| "${CMAKE_CURRENT_SOURCE_DIR}/tests/testCSP.cpp") | ||||
| #set (discrete_excluded_tests  | ||||
| #"${CMAKE_CURRENT_SOURCE_DIR}/tests/testScheduler.cpp" | ||||
| #) | ||||
| 
 | ||||
| 
 | ||||
| # Add all tests | ||||
| gtsam_add_subdir_tests(discrete_unstable "${discrete_local_libs}" "${discrete_full_libs}" "${discrete_excluded_tests}")  | ||||
| add_dependencies(check.unstable check.discrete_unstable) | ||||
| 
 | ||||
| # List examples to build - comment out here to exclude from compilation | ||||
| set(discrete_unstable_examples | ||||
| #schedulingExample | ||||
| #schedulingQuals12 | ||||
| schedulingExample | ||||
| schedulingQuals12 | ||||
| ) | ||||
| 
 | ||||
| if (GTSAM_BUILD_EXAMPLES) | ||||
|  |  | |||
|  | @ -49,8 +49,9 @@ namespace gtsam { | |||
| 					// if not already a singleton
 | ||||
| 					if (!domains[v].isSingleton()) { | ||||
| 						// get the constraint and call its ensureArcConsistency method
 | ||||
| 						Constraint::shared_ptr factor = (*this)[f]; | ||||
| 						changed[v] = factor->ensureArcConsistency(v,domains) || changed[v]; | ||||
| 						Constraint::shared_ptr constraint = boost::dynamic_pointer_cast<Constraint>((*this)[f]); | ||||
| 						if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor"); | ||||
| 						changed[v] = constraint->ensureArcConsistency(v,domains) || changed[v]; | ||||
| 					} | ||||
| 				} // f
 | ||||
| 				if (changed[v]) anyChange = true; | ||||
|  | @ -84,8 +85,10 @@ namespace gtsam { | |||
| 		// TODO: create a new ordering as we go, to ensure a connected graph
 | ||||
| 		// KeyOrdering ordering;
 | ||||
| 		// vector<Index> dkeys;
 | ||||
| 		BOOST_FOREACH(const Constraint::shared_ptr& factor, factors_) { | ||||
| 			Constraint::shared_ptr reduced = factor->partiallyApply(domains); | ||||
| 		BOOST_FOREACH(const DiscreteFactor::shared_ptr& f, factors_) { | ||||
| 			Constraint::shared_ptr constraint = boost::dynamic_pointer_cast<Constraint>(f); | ||||
| 			if (!constraint) throw runtime_error("CSP:runArcConsistency: non-constraint factor"); | ||||
| 			Constraint::shared_ptr reduced = constraint->partiallyApply(domains); | ||||
| 			if (print) reduced->print(); | ||||
| 		} | ||||
| #endif | ||||
|  |  | |||
|  | @ -18,7 +18,7 @@ namespace gtsam { | |||
| 	 * A specialization of a DiscreteFactorGraph. | ||||
| 	 * It knows about CSP-specific constraints and algorithms | ||||
| 	 */ | ||||
| 	class CSP: public FactorGraph<Constraint> { | ||||
| 	class CSP: public DiscreteFactorGraph { | ||||
| 	public: | ||||
| 
 | ||||
| 		/** A map from keys to values */ | ||||
|  | @ -27,30 +27,10 @@ namespace gtsam { | |||
| 		typedef boost::shared_ptr<Values> sharedValues; | ||||
| 
 | ||||
| 	public: | ||||
| 		/// Constructor
 | ||||
| 		CSP() { | ||||
| 		} | ||||
| 
 | ||||
| 		template<class SOURCE> | ||||
| 		void add(const DiscreteKey& j, SOURCE table) { | ||||
| 			DiscreteKeys keys; | ||||
| 			keys.push_back(j); | ||||
| 			push_back(boost::make_shared<DecisionTreeFactor>(keys, table)); | ||||
| 		} | ||||
| 
 | ||||
| 		template<class SOURCE> | ||||
| 		void add(const DiscreteKey& j1, const DiscreteKey& j2, SOURCE table) { | ||||
| 			DiscreteKeys keys; | ||||
| 			keys.push_back(j1); | ||||
| 			keys.push_back(j2); | ||||
| 			push_back(boost::make_shared<DecisionTreeFactor>(keys, table)); | ||||
| 		} | ||||
| 
 | ||||
| 		/** add shared discreteFactor immediately from arguments */ | ||||
| 		template<class SOURCE> | ||||
| 		void add(const DiscreteKeys& keys, SOURCE table) { | ||||
| 			push_back(boost::make_shared<DecisionTreeFactor>(keys, table)); | ||||
| 		} | ||||
| //		/// Constructor
 | ||||
| //		CSP() {
 | ||||
| //		}
 | ||||
| 
 | ||||
| 		/// Add a unary constraint, allowing only a single value
 | ||||
| 		void addSingleValue(const DiscreteKey& dkey, size_t value) { | ||||
|  | @ -71,19 +51,28 @@ namespace gtsam { | |||
| 			push_back(factor); | ||||
| 		} | ||||
| 
 | ||||
| //		/** return product of all factors as a single factor */
 | ||||
| //		DecisionTreeFactor product() const {
 | ||||
| //			DecisionTreeFactor result;
 | ||||
| //			BOOST_FOREACH(const sharedFactor& factor, *this)
 | ||||
| //				if (factor) result = (*factor) * result;
 | ||||
| //			return result;
 | ||||
| //		}
 | ||||
| 
 | ||||
| 		/// Find the best total assignment - can be expensive
 | ||||
| 		sharedValues optimalAssignment() const; | ||||
| 
 | ||||
| 		/*
 | ||||
| 		 * Perform loopy belief propagation | ||||
| 		 * True belief propagation would check for each value in domain | ||||
| 		 * whether any satisfying separator assignment can be found. | ||||
| 		 * This corresponds to hyper-arc consistency in CSP speak. | ||||
| 		 * This can be done by creating a mini-factor graph and search. | ||||
| 		 * For a nine-by-nine Sudoku, the search tree will be 8+6+6=20 levels deep. | ||||
| 		 * It will be very expensive to exclude values that way. | ||||
| 		 */ | ||||
| 		// void applyBeliefPropagation(size_t nrIterations = 10) const;
 | ||||
| //		/*
 | ||||
| //		 * Perform loopy belief propagation
 | ||||
| //		 * True belief propagation would check for each value in domain
 | ||||
| //		 * whether any satisfying separator assignment can be found.
 | ||||
| //		 * This corresponds to hyper-arc consistency in CSP speak.
 | ||||
| //		 * This can be done by creating a mini-factor graph and search.
 | ||||
| //		 * For a nine-by-nine Sudoku, the search tree will be 8+6+6=20 levels deep.
 | ||||
| //		 * It will be very expensive to exclude values that way.
 | ||||
| //		 */
 | ||||
| //		 void applyBeliefPropagation(size_t nrIterations = 10) const;
 | ||||
| 
 | ||||
| 		/*
 | ||||
| 		 * Apply arc-consistency ~ Approximate loopy belief propagation | ||||
| 		 * We need to give the domains to a constraint, and it returns | ||||
|  | @ -92,7 +81,7 @@ namespace gtsam { | |||
| 		 */ | ||||
| 		void runArcConsistency(size_t cardinality, size_t nrIterations = 10, | ||||
| 				bool print = false) const; | ||||
| 	}; | ||||
| 	}; // CSP
 | ||||
| 
 | ||||
| } // gtsam
 | ||||
| 
 | ||||
|  |  | |||
|  | @ -105,7 +105,6 @@ namespace gtsam { | |||
| 
 | ||||
| 	/** Add student-specific constraints to the graph */ | ||||
| 	void Scheduler::addStudentSpecificConstraints(size_t i, boost::optional<size_t> slot) { | ||||
| #ifdef BROKEN | ||||
| 		bool debug = ISDEBUG("Scheduler::buildGraph"); | ||||
| 
 | ||||
| 		assert(i<nrStudents()); | ||||
|  | @ -134,7 +133,7 @@ namespace gtsam { | |||
| 				DiscreteKey dummy(0, nrTimeSlots()); | ||||
| 				Potentials::ADT p(dummy & areaKey, available_); | ||||
| 				Potentials::ADT q = p.choose(0, *slot); | ||||
| 				Constraint::shared_ptr f(new DecisionTreeFactor(areaKey, q)); | ||||
| 				DiscreteFactor::shared_ptr f(new DecisionTreeFactor(areaKey, q)); | ||||
| 				CSP::push_back(f); | ||||
| 			} else { | ||||
| 				CSP::add(s.key_, areaKey, available_); | ||||
|  | @ -144,15 +143,11 @@ namespace gtsam { | |||
| 		// add mutex
 | ||||
| 		if (debug) cout << "Mutex for faculty" << endl; | ||||
| 		addAllDiff(s.keys_[0] & s.keys_[1] & s.keys_[2]); | ||||
| #else | ||||
| 				throw runtime_error("addStudentSpecificConstraints is broken"); | ||||
| #endif | ||||
| 	} | ||||
| 
 | ||||
| 
 | ||||
| 	/** Main routine that builds factor graph */ | ||||
| 	void Scheduler::buildGraph(size_t mutexBound) { | ||||
| #ifdef BROKEN | ||||
| 		bool debug = ISDEBUG("Scheduler::buildGraph"); | ||||
| 
 | ||||
| 		if (debug) cout << "Adding student-specific constraints" << endl; | ||||
|  | @ -178,10 +173,6 @@ namespace gtsam { | |||
| 				} | ||||
| 			} | ||||
| 		} | ||||
| #else | ||||
| 				throw runtime_error("buildGraph is broken"); | ||||
| #endif | ||||
| 
 | ||||
| 	} // buildGraph
 | ||||
| 
 | ||||
| 	/** print */ | ||||
|  |  | |||
|  | @ -17,8 +17,8 @@ namespace gtsam { | |||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
| 	void SingleValue::print(const string& s) const { | ||||
| 		cout << s << ": SingleValue on " << keys_[0] << " (j=" << keys_[0] | ||||
| 				<< ") with value " << value_ << endl; | ||||
| 		cout << s << "SingleValue on " << "j=" << keys_[0] | ||||
| 				<< " with value " << value_ << endl; | ||||
| 	} | ||||
| 
 | ||||
| 	/* ************************************************************************* */ | ||||
|  |  | |||
|  | @ -54,17 +54,17 @@ TEST_UNSAFE( CSP, allInOne) | |||
| 	invalid[ID.first] = 0; | ||||
| 	invalid[UT.first] = 0; | ||||
| 	invalid[AZ.first] = 0; | ||||
| 	EXPECT_DOUBLES_EQUAL(0, csp(invalid), 1e-9); // FIXME: fails due to lack of operator() interface
 | ||||
| 	EXPECT_DOUBLES_EQUAL(0, csp(invalid), 1e-9); | ||||
| 
 | ||||
| 	// Check a valid combination
 | ||||
| 	DiscreteFactor::Values valid; | ||||
| 	valid[ID.first] = 0; | ||||
| 	valid[UT.first] = 1; | ||||
| 	valid[AZ.first] = 0; | ||||
| 	EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9); // FIXME: fails due to lack of operator() interface
 | ||||
| 	EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9); | ||||
| 
 | ||||
| 	// Just for fun, create the product and check it
 | ||||
| 	DecisionTreeFactor product = csp.product(); // FIXME: fails due to lack of product()
 | ||||
| 	DecisionTreeFactor product = csp.product(); | ||||
| 	// product.dot("product");
 | ||||
| 	DecisionTreeFactor expectedProduct(ID & AZ & UT, "0 1 0 0 0 0 1 0"); | ||||
| 	EXPECT(assert_equal(expectedProduct,product)); | ||||
|  | @ -74,7 +74,7 @@ TEST_UNSAFE( CSP, allInOne) | |||
| 	CSP::Values expected; | ||||
| 	insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 1); | ||||
| 	EXPECT(assert_equal(expected,*mpe)); | ||||
| 	EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); // FIXME: fails due to lack of operator() interface
 | ||||
| 	EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
|  | @ -122,7 +122,7 @@ TEST_UNSAFE( CSP, WesternUS) | |||
| 	(MT.first,1)(WY.first,0)(NM.first,3)(CO.first,2) | ||||
| 	(ID.first,2)(UT.first,1)(AZ.first,0); | ||||
| 	EXPECT(assert_equal(expected,*mpe)); | ||||
| 	EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); // FIXME: fails due to lack of operator() interface
 | ||||
| 	EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); | ||||
| 
 | ||||
| 	// Write out the dual graph for hmetis
 | ||||
| #ifdef DUAL | ||||
|  | @ -146,7 +146,7 @@ TEST_UNSAFE( CSP, AllDiff) | |||
| 	dkeys += ID,UT,AZ; | ||||
| 	csp.addAllDiff(dkeys); | ||||
| 	csp.addSingleValue(AZ,2); | ||||
| 	//GTSAM_PRINT(csp);
 | ||||
| //	GTSAM_PRINT(csp);
 | ||||
| 
 | ||||
| 	// Check construction and conversion
 | ||||
| 	SingleValue s(AZ,2); | ||||
|  | @ -167,21 +167,21 @@ TEST_UNSAFE( CSP, AllDiff) | |||
| 	invalid[ID.first] = 0; | ||||
| 	invalid[UT.first] = 1; | ||||
| 	invalid[AZ.first] = 0; | ||||
| 	EXPECT_DOUBLES_EQUAL(0, csp(invalid), 1e-9); // FIXME: fails due to lack of operator() interface
 | ||||
| 	EXPECT_DOUBLES_EQUAL(0, csp(invalid), 1e-9); | ||||
| 
 | ||||
| 	// Check a valid combination
 | ||||
| 	DiscreteFactor::Values valid; | ||||
| 	valid[ID.first] = 0; | ||||
| 	valid[UT.first] = 1; | ||||
| 	valid[AZ.first] = 2; | ||||
| 	EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9); // FIXME: fails due to lack of operator() interface
 | ||||
| 	EXPECT_DOUBLES_EQUAL(1, csp(valid), 1e-9); | ||||
| 
 | ||||
| 	// Solve
 | ||||
| 	CSP::sharedValues mpe = csp.optimalAssignment(); | ||||
| 	CSP::Values expected; | ||||
| 	insert(expected)(ID.first, 1)(UT.first, 0)(AZ.first, 2); | ||||
| 	EXPECT(assert_equal(expected,*mpe)); | ||||
| 	EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); // FIXME: fails due to lack of operator() interface
 | ||||
| 	EXPECT_DOUBLES_EQUAL(1, csp(*mpe), 1e-9); | ||||
| 
 | ||||
| 	// Arc-consistency
 | ||||
| 	vector<Domain> domains; | ||||
|  |  | |||
|  | @ -14,6 +14,8 @@ | |||
| using namespace std; | ||||
| using namespace gtsam; | ||||
| 
 | ||||
| #define PRINT false | ||||
| 
 | ||||
| class Sudoku: public CSP { | ||||
| 
 | ||||
| 	/// sudoku size
 | ||||
|  | @ -119,7 +121,7 @@ TEST_UNSAFE( Sudoku, small) | |||
| 			0,1, 0,0); | ||||
| 
 | ||||
| 	// Do BP
 | ||||
| 	csp.runArcConsistency(4); | ||||
| 	csp.runArcConsistency(4,10,PRINT); | ||||
| 
 | ||||
| 	// optimize and check
 | ||||
| 	CSP::sharedValues solution = csp.optimalAssignment(); | ||||
|  | @ -150,7 +152,7 @@ TEST_UNSAFE( Sudoku, easy) | |||
| 			5,0,0, 0,3,0, 7,0,0); | ||||
| 
 | ||||
| 	// Do BP
 | ||||
| 	sudoku.runArcConsistency(4); | ||||
| 	sudoku.runArcConsistency(4,10,PRINT); | ||||
| 
 | ||||
| 	// sudoku.printSolution(); // don't do it
 | ||||
| } | ||||
|  | @ -172,7 +174,7 @@ TEST_UNSAFE( Sudoku, extreme) | |||
| 			0,0,0, 2,7,5, 9,0,0); | ||||
| 
 | ||||
| 	// Do BP
 | ||||
| 	sudoku.runArcConsistency(9,10,false); | ||||
| 	sudoku.runArcConsistency(9,10,PRINT); | ||||
| 
 | ||||
| #ifdef METIS | ||||
| 	VariableIndex index(sudoku); | ||||
|  | @ -201,7 +203,7 @@ TEST_UNSAFE( Sudoku, AJC_3star_Feb8_2012) | |||
| 			0,0,0, 1,0,0, 0,3,7); | ||||
| 
 | ||||
| 	// Do BP
 | ||||
| 	sudoku.runArcConsistency(9,10,true); | ||||
| 	sudoku.runArcConsistency(9,10,PRINT); | ||||
| 
 | ||||
| 	//sudoku.printSolution(); // don't do it
 | ||||
| } | ||||
|  |  | |||
|  | @ -23,4 +23,4 @@ set (dynamics_excluded_tests "") | |||
| 
 | ||||
| # Add all tests | ||||
| gtsam_add_subdir_tests(dynamics_unstable "${dynamics_local_libs}" "${dynamics_full_libs}" "${dynamics_excluded_tests}")  | ||||
| 
 | ||||
| add_dependencies(check.unstable check.dynamics_unstable) | ||||
|  |  | |||
|  | @ -23,4 +23,4 @@ set (slam_excluded_tests "") | |||
| 
 | ||||
| # Add all tests | ||||
| gtsam_add_subdir_tests(slam_unstable "${slam_local_libs}" "${slam_full_libs}" "${slam_excluded_tests}")  | ||||
| 
 | ||||
| add_dependencies(check.unstable check.slam_unstable) | ||||
|  |  | |||
|  | @ -107,6 +107,43 @@ TEST( Graph, linearize ) | |||
| 	CHECK(assert_equal(expected,*linearized)); // Needs correct linearizations
 | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( Graph, clone ) | ||||
| { | ||||
| 	Graph fg = createNonlinearFactorGraph(); | ||||
| 	Graph actClone = fg.clone(); | ||||
| 	EXPECT(assert_equal(fg, actClone)); | ||||
| 	for (size_t i=0; i<fg.size(); ++i) | ||||
| 		EXPECT(fg[i] != actClone[i]); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST( Graph, rekey ) | ||||
| { | ||||
| 	Graph init = createNonlinearFactorGraph(); | ||||
| 	map<Key,Key> rekey_mapping; | ||||
| 	rekey_mapping.insert(make_pair(kl(1), kl(4))); | ||||
| 	Graph actRekey = init.rekey(rekey_mapping); | ||||
| 
 | ||||
| 	// ensure deep clone
 | ||||
| 	LONGS_EQUAL(init.size(), actRekey.size()); | ||||
| 	for (size_t i=0; i<init.size(); ++i) | ||||
| 			EXPECT(init[i] != actRekey[i]); | ||||
| 
 | ||||
| 	Graph expRekey; | ||||
| 	// original measurements
 | ||||
| 	expRekey.push_back(init[0]); | ||||
| 	expRekey.push_back(init[1]); | ||||
| 
 | ||||
| 	// updated measurements
 | ||||
| 	Point2 z3(0, -1),  z4(-1.5, -1.); | ||||
| 	SharedDiagonal sigma0_2 = noiseModel::Isotropic::Sigma(2,0.2); | ||||
| 	expRekey.add(simulated2D::Measurement(z3, sigma0_2, kx(1), kl(4))); | ||||
| 	expRekey.add(simulated2D::Measurement(z4, sigma0_2, kx(2), kl(4))); | ||||
| 
 | ||||
| 	EXPECT(assert_equal(expRekey, actRekey)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| int main() { | ||||
| 	TestResult tr; | ||||
|  |  | |||
|  | @ -149,9 +149,9 @@ TEST( NonlinearOptimizer, SimpleDLOptimizer ) | |||
| TEST( NonlinearOptimizer, optimization_method ) | ||||
| { | ||||
|   LevenbergMarquardtParams paramsQR; | ||||
|   paramsQR.factorization = LevenbergMarquardtParams::QR; | ||||
|   paramsQR.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_QR; | ||||
|   LevenbergMarquardtParams paramsChol; | ||||
|   paramsChol.factorization = LevenbergMarquardtParams::CHOLESKY; | ||||
|   paramsChol.linearSolverType = LevenbergMarquardtParams::MULTIFRONTAL_CHOLESKY; | ||||
| 
 | ||||
| 	example::Graph fg = example::createReallyNonlinearFactorGraph(); | ||||
| 
 | ||||
|  |  | |||
|  | @ -47,6 +47,17 @@ add_custom_target(wrap_gtsam ALL COMMAND | |||
|     ${EXECUTABLE_OUTPUT_PATH}/wrap ${GTSAM_MEX_BIN_EXTENSION} ${CMAKE_CURRENT_SOURCE_DIR}/../ ${moduleName} ${toolbox_path} "${mexFlags}" | ||||
|     DEPENDS wrap) | ||||
| 
 | ||||
| # Build command | ||||
| # Experimental: requires matlab to be on your path | ||||
| if (GTSAM_ENABLE_BUILD_MEX_BINARIES) | ||||
|     # Actually compile the mex files when building the library | ||||
|     set(TOOLBOX_MAKE_FLAGS "-j2")     | ||||
|     add_custom_target(wrap_gtsam_build  | ||||
|         COMMAND make ${TOOLBOX_MAKE_FLAGS}  | ||||
|         WORKING_DIRECTORY ${toolbox_path}  | ||||
|         DEPENDS wrap_gtsam) | ||||
| endif (GTSAM_ENABLE_BUILD_MEX_BINARIES)     | ||||
| 
 | ||||
| set(GTSAM_TOOLBOX_INSTALL_PATH ${CMAKE_INSTALL_PREFIX}/borg/toolbox CACHE DOCSTRING "Path to install matlab toolbox") | ||||
| 
 | ||||
| if (GTSAM_INSTALL_MATLAB_TOOLBOX) | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue