Added Pose3::align() for labeled correspondences. Fixed depreciated make targets.
							parent
							
								
									e4a9c398c8
								
							
						
					
					
						commit
						26f5f93c60
					
				
							
								
								
									
										420
									
								
								.cproject
								
								
								
								
							
							
						
						
									
										420
									
								
								.cproject
								
								
								
								
							|  | @ -307,14 +307,6 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testGaussianFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="all" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -341,6 +333,7 @@ | |||
| 			</target> | ||||
| 			<target name="tests/testBayesTree.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>tests/testBayesTree.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -348,6 +341,7 @@ | |||
| 			</target> | ||||
| 			<target name="testBinaryBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testBinaryBayesNet.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -395,6 +389,7 @@ | |||
| 			</target> | ||||
| 			<target name="testSymbolicBayesNet.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSymbolicBayesNet.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -402,6 +397,7 @@ | |||
| 			</target> | ||||
| 			<target name="tests/testSymbolicFactor.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>tests/testSymbolicFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -409,6 +405,7 @@ | |||
| 			</target> | ||||
| 			<target name="testSymbolicFactorGraph.run" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSymbolicFactorGraph.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -424,11 +421,20 @@ | |||
| 			</target> | ||||
| 			<target name="tests/testBayesTree" path="inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>tests/testBayesTree</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testGaussianFactor.run" path="linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testGaussianFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testPoseRTV.run" path="build/gtsam_unstable/dynamics" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
|  | @ -517,22 +523,6 @@ | |||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>all</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="clean" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>clean</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="tests/testPose2.run" path="build_retract/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -549,6 +539,22 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="all" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>all</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="clean" path="CppUnitLite" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>clean</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="all" path="spqr_mini" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -573,34 +579,26 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testValues.run</buildTarget> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>all</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testOrdering.run</buildTarget> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>check</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testKey.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testLinearContainerFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testLinearContainerFactor.run</buildTarget> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>clean</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
|  | @ -685,26 +683,34 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="all" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="testValues.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>all</buildTarget> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testValues.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="check" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="testOrdering.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>check</buildTarget> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testOrdering.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="clean" path="build_wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="testKey.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>clean</buildTarget> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testKey.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testLinearContainerFactor.run" path="build/gtsam/nonlinear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testLinearContainerFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
|  | @ -1055,6 +1061,7 @@ | |||
| 			</target> | ||||
| 			<target name="testGraph.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testGraph.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1062,6 +1069,7 @@ | |||
| 			</target> | ||||
| 			<target name="testJunctionTree.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testJunctionTree.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1069,6 +1077,7 @@ | |||
| 			</target> | ||||
| 			<target name="testSymbolicBayesNetB.run" path="build/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSymbolicBayesNetB.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1228,6 +1237,7 @@ | |||
| 			</target> | ||||
| 			<target name="testErrors.run" path="linear" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testErrors.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1273,14 +1283,6 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testGaussianFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="check" path="base" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -1361,6 +1363,14 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testGaussianFactor.run" path="build/linear/tests" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testGaussianFactor.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="check" path="build/inference" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
|  | @ -1513,98 +1523,74 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="tests/testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="testStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/testStereoCamera.run</buildTarget> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testStereoCamera.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="tests/testRot3M.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="testRot3M.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/testRot3M.run</buildTarget> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testRot3M.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="tests/testPoint3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="testPoint3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/testPoint3.run</buildTarget> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testPoint3.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="tests/testCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="testCalibratedCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/testCalibratedCamera.run</buildTarget> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testCalibratedCamera.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="tests/timeRot3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="timeStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/timeRot3.run</buildTarget> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>timeStereoCamera.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="tests/timePose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/timePose3.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="tests/timeStereoCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>tests/timeStereoCamera.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="geometry.testHomography2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="testHomography2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j1 VERBOSE=1</buildArguments> | ||||
| 				<buildTarget>geometry.testHomography2.run</buildTarget> | ||||
| 				<buildTarget>testHomography2.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="geometry.testPoint2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="testPoint2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>geometry.testPoint2.run</buildTarget> | ||||
| 				<buildTarget>testPoint2.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="geometry.testPose2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="testPose2.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>geometry.testPose2.run</buildTarget> | ||||
| 				<buildTarget>testPose2.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="geometry.testPose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="testPose3.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>geometry.testPose3.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testSimpleCamera.run" path="build/gtsam/geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testSimpleCamera.run</buildTarget> | ||||
| 				<buildTarget>testPose3.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
|  | @ -1691,7 +1677,6 @@ | |||
| 			</target> | ||||
| 			<target name="testSimulated2DOriented.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSimulated2DOriented.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1731,7 +1716,6 @@ | |||
| 			</target> | ||||
| 			<target name="testSimulated2D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSimulated2D.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1739,7 +1723,6 @@ | |||
| 			</target> | ||||
| 			<target name="testSimulated3D.run" path="slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>testSimulated3D.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1939,7 +1922,6 @@ | |||
| 			</target> | ||||
| 			<target name="tests/testGaussianISAM2" path="build/slam" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>tests/testGaussianISAM2</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -1961,102 +1943,6 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testRot3.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testRot2.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testPose3.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>timeRot3.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testPose2.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testCal3_S2.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testSimpleCamera.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testHomography2.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testCalibratedCamera.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>check</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>clean</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testPoint2.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="install" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j3</buildArguments> | ||||
|  | @ -2258,6 +2144,7 @@ | |||
| 			</target> | ||||
| 			<target name="Generate DEB Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>cpack</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>-G DEB</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -2265,6 +2152,7 @@ | |||
| 			</target> | ||||
| 			<target name="Generate RPM Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>cpack</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>-G RPM</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -2272,6 +2160,7 @@ | |||
| 			</target> | ||||
| 			<target name="Generate TGZ Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>cpack</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>-G TGZ</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -2279,6 +2168,7 @@ | |||
| 			</target> | ||||
| 			<target name="Generate TGZ Source Package" path="" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>cpack</buildCommand> | ||||
| 				<buildArguments/> | ||||
| 				<buildTarget>--config CPackSourceConfig.cmake</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
|  | @ -2420,34 +2310,98 @@ | |||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="testRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testSpirit.run</buildTarget> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testRot3.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="testRot2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testWrap.run</buildTarget> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testRot2.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="testPose3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>check.wrap</buildTarget> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testPose3.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 			<target name="timeRot3.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>wrap</buildTarget> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>timeRot3.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testPose2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testPose2.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testCal3_S2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testCal3_S2.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testSimpleCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testSimpleCamera.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testHomography2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testHomography2.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testCalibratedCamera.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testCalibratedCamera.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="check" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>check</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="clean" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>clean</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testPoint2.run" path="geometry" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j2</buildArguments> | ||||
| 				<buildTarget>testPoint2.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
|  | @ -2491,6 +2445,38 @@ | |||
| 				<useDefaultCommand>false</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="testSpirit.run" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>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"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>testWrap.run</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="check.wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>check.wrap</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 			<target name="wrap" path="build/wrap" targetID="org.eclipse.cdt.build.MakeTargetBuilder"> | ||||
| 				<buildCommand>make</buildCommand> | ||||
| 				<buildArguments>-j5</buildArguments> | ||||
| 				<buildTarget>wrap</buildTarget> | ||||
| 				<stopOnError>true</stopOnError> | ||||
| 				<useDefaultCommand>true</useDefaultCommand> | ||||
| 				<runAllBuilders>true</runAllBuilders> | ||||
| 			</target> | ||||
| 		</buildTargets> | ||||
| 	</storageModule> | ||||
| </cproject> | ||||
|  |  | |||
|  | @ -18,6 +18,7 @@ | |||
| #include <gtsam/geometry/Pose2.h> | ||||
| #include <gtsam/geometry/concepts.h> | ||||
| #include <gtsam/base/Lie-inl.h> | ||||
| #include <boost/foreach.hpp> | ||||
| #include <iostream> | ||||
| #include <cmath> | ||||
| 
 | ||||
|  | @ -283,4 +284,40 @@ namespace gtsam { | |||
|      } | ||||
|      return r; | ||||
|   } | ||||
| 
 | ||||
|   /* ************************************************************************* */ | ||||
|   boost::optional<Pose3> align(const vector<Point3Pair>& pairs) { | ||||
|     const size_t n = pairs.size(); | ||||
|     if (n<3) return boost::none; // we need at least three pairs
 | ||||
| 
 | ||||
|     // calculate centroids
 | ||||
|     Vector cp = zero(3),cq = zero(3); | ||||
|     BOOST_FOREACH(const Point3Pair& pair, pairs) { | ||||
|       cp += pair.first.vector(); | ||||
|       cq += pair.second.vector(); | ||||
|     } | ||||
|     double f = 1.0/n; | ||||
|     cp *= f; cq *= f; | ||||
| 
 | ||||
|     // Add to form H matrix
 | ||||
|     Matrix H = zeros(3,3); | ||||
|     BOOST_FOREACH(const Point3Pair& pair, pairs) { | ||||
|       Vector dp = pair.first.vector()  - cp; | ||||
|       Vector dq = pair.second.vector() - cq; | ||||
|       H += dp * dq.transpose(); | ||||
|     } | ||||
| 
 | ||||
|     // Compute SVD
 | ||||
|     Matrix U,V; | ||||
|     Vector S; | ||||
|     svd(H,U,S,V); | ||||
| 
 | ||||
|     // Recover transform with correction from Eggert97machinevisionandapplications
 | ||||
|     Matrix UVtranspose = U * V.transpose(); | ||||
|     Matrix detWeighting = eye(3,3); | ||||
|     detWeighting(2,2) = UVtranspose.determinant(); | ||||
|     Rot3 R(Matrix(V * detWeighting * U.transpose())); | ||||
|     Point3 t = Point3(cq) - R * Point3(cp); | ||||
|     return Pose3(R, t); | ||||
|   } | ||||
| } // namespace gtsam
 | ||||
|  |  | |||
|  | @ -179,7 +179,7 @@ namespace gtsam { | |||
|      * as detailed in [Kobilarov09siggraph] eq. (15) and C_TLN. | ||||
|      * The full formula is documented in [Celledoni99cmame] | ||||
|      *    Elena Celledoni and Brynjulf Owren. Lie group methods for rigid body dynamics and | ||||
|      *    time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421Ð 438, 2003. | ||||
|      *    time integration on manifolds. Comput. meth. in Appl. Mech. and Eng., 19(3,4):421<EFBFBD> 438, 2003. | ||||
|      */ | ||||
|     static Matrix6 dExpInv_TLN(const Vector&  xi); | ||||
| 
 | ||||
|  | @ -300,4 +300,11 @@ namespace gtsam { | |||
|     return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); | ||||
|   } | ||||
| 
 | ||||
|   /**
 | ||||
|    * Calculate pose between a vector of 3D point correspondences (p,q) | ||||
|    * where q = Pose3::transform_from(p) = t + R*p | ||||
|    */ | ||||
|   typedef std::pair<Point3,Point3> Point3Pair; | ||||
|   GTSAM_EXPORT boost::optional<Pose3> align(const std::vector<Point3Pair>& pairs); | ||||
| 
 | ||||
| } // namespace gtsam
 | ||||
|  |  | |||
|  | @ -19,6 +19,10 @@ | |||
| #include <gtsam/base/lieProxies.h> | ||||
| #include <gtsam/base/Testable.h> | ||||
| #include <gtsam/base/numericalDerivative.h> | ||||
| 
 | ||||
| #include <boost/assign/std/vector.hpp> // for operator += | ||||
| using namespace boost::assign; | ||||
| 
 | ||||
| #include <CppUnitLite/TestHarness.h> | ||||
| #include <cmath> | ||||
| 
 | ||||
|  | @ -633,6 +637,40 @@ TEST( Pose3, adjoint) { | |||
|   EXPECT(assert_equal(expected,res,1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(Pose3, align_1) { | ||||
|   Pose3 expected(Rot3(), Point3(10,10,0)); | ||||
| 
 | ||||
|   vector<Point3Pair> correspondences; | ||||
|   Point3Pair pq1(make_pair(Point3(0,0,0), Point3(10,10,0))); | ||||
|   Point3Pair pq2(make_pair(Point3(20,10,0), Point3(30,20,0))); | ||||
|   Point3Pair pq3(make_pair(Point3(10,20,0), Point3(20,30,0))); | ||||
|   correspondences += pq1, pq2, pq3; | ||||
| 
 | ||||
|   boost::optional<Pose3> actual = align(correspondences); | ||||
|   EXPECT(assert_equal(expected, *actual)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| TEST(Pose3, align_2) { | ||||
|   Point3 t(20,10,5); | ||||
|   Rot3 R = Rot3::RzRyRx(0.3, 0.2, 0.1); | ||||
|   Pose3 expected(R, t); | ||||
| 
 | ||||
|   vector<Point3Pair> correspondences; | ||||
|   Point3 p1(0,0,1), p2(10,0,2), p3(20,-10,30); | ||||
|   Point3 q1 = expected.transform_from(p1), | ||||
|          q2 = expected.transform_from(p2), | ||||
|          q3 = expected.transform_from(p3); | ||||
|   Point3Pair pq1(make_pair(p1, q1)); | ||||
|   Point3Pair pq2(make_pair(p2, q2)); | ||||
|   Point3Pair pq3(make_pair(p3, q3)); | ||||
|   correspondences += pq1, pq2, pq3; | ||||
| 
 | ||||
|   boost::optional<Pose3> actual = align(correspondences); | ||||
|   EXPECT(assert_equal(expected, *actual, 1e-5)); | ||||
| } | ||||
| 
 | ||||
| /* ************************************************************************* */ | ||||
| /// exp(xi) exp(y) = exp(xi + x)
 | ||||
| /// Hence, y = log (exp(-xi)*exp(xi+x))
 | ||||
|  |  | |||
		Loading…
	
		Reference in New Issue