diff --git a/.cproject b/.cproject index 756116cfb..ab1d0cdfc 100644 --- a/.cproject +++ b/.cproject @@ -1,7 +1,5 @@ - - - + @@ -486,26 +484,265 @@ - + make -j5 - SmartProjectionFactorExample_kitti_nonbatch.run + testCombinedImuFactor.run true true true - + make -j5 - SmartProjectionFactorExample_kitti.run + testImuFactor.run true true true - + make -j5 - SmartProjectionFactorTesting.run + testAHRSFactor.run + true + true + true + + + make + -j8 + testAttitudeFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + testNonlinearConstraint.run + true + true + true + + + make + -j2 + testLieConfig.run + true + true + true + + + make + -j2 + testConstraintOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + testBTree.run + true + true + true + + + make + -j2 + testDSF.run + true + true + true + + + make + -j2 + testDSFVector.run + true + true + true + + + make + -j2 + testMatrix.run + true + true + true + + + make + -j2 + testSPQRUtil.run + true + true + true + + + make + -j2 + testVector.run + true + true + true + + + make + -j2 + timeMatrix.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + wrap + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + all + true + true + true + + + cmake + .. + true + false + true + + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testLieConfig.run true true true @@ -534,6 +771,70 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + all + true + false + true + + + make + -j5 + check + true + false + true + + + make + -j5 + SmartProjectionFactorExample_kitti_nonbatch.run + true + true + true + + + make + -j5 + SmartProjectionFactorExample_kitti.run + true + true + true + + + make + -j5 + SmartProjectionFactorTesting.run + true + true + true + make -j2 @@ -558,7 +859,167 @@ true true - + + make + -j5 + testCal3Bundler.run + true + true + true + + + make + -j5 + testCal3DS2.run + true + true + true + + + make + -j5 + testCalibratedCamera.run + true + true + true + + + make + -j5 + testEssentialMatrix.run + true + true + true + + + make + -j1 VERBOSE=1 + testHomography2.run + true + false + true + + + make + -j5 + testPinholeCamera.run + true + true + true + + + make + -j5 + testPoint2.run + true + true + true + + + make + -j5 + testPoint3.run + true + true + true + + + make + -j5 + testPose2.run + true + true + true + + + make + -j5 + testPose3.run + true + true + true + + + make + -j5 + testRot3M.run + true + true + true + + + make + -j5 + testSphere2.run + true + true + true + + + make + -j5 + testStereoCamera.run + true + true + true + + + make + -j5 + testCal3Unified.run + true + true + true + + + make + -j5 + testRot2.run + true + true + true + + + make + -j5 + testRot3Q.run + true + true + true + + + make + -j5 + testRot3.run + true + true + true + + + make + -j4 + testSO3.run + true + true + true + + + make + -j4 + testQuaternion.run + true + true + true + + + make + -j4 + testOrientedPlane3.run + true + true + true + + make -j2 all @@ -566,7 +1027,7 @@ true true - + make -j2 clean @@ -574,143 +1035,23 @@ true true - - make - -k - check - true - false - true - - - make - - tests/testBayesTree.run - true - false - true - - - make - - testBinaryBayesNet.run - true - false - true - - + make -j2 - testFactorGraph.run + clean all true true true - + make -j2 - testISAM.run + install true true true - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - testKey.run - true - true - true - - - make - -j2 - testOrdering.run - true - true - true - - - make - - testSymbolicBayesNet.run - true - false - true - - - make - - tests/testSymbolicFactor.run - true - false - true - - - make - - testSymbolicFactorGraph.run - true - false - true - - - make - -j2 - timeSymbolMaps.run - true - true - true - - - make - - tests/testBayesTree - true - false - true - - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - + make -j2 clean @@ -862,154 +1203,146 @@ true true - + make - -j5 - testGaussianFactorGraphUnordered.run + -j2 + all true true true - + make - -j5 - testGaussianBayesNetUnordered.run + -j2 + check true true true - + make - -j5 - testGaussianConditional.run - true - true - true - - - make - -j5 - testGaussianDensity.run - true - true - true - - - make - -j5 - testGaussianJunctionTree.run - true - true - true - - - make - -j5 - testHessianFactor.run - true - true - true - - - make - -j5 - testJacobianFactor.run - true - true - true - - - make - -j5 - testKalmanFilter.run - true - true - true - - - make - -j5 - testNoiseModel.run - true - true - true - - - make - -j5 - testSampler.run - true - true - true - - - make - -j5 - testSerializationLinear.run - true - true - true - - - make - -j5 - testVectorValues.run - true - true - true - - - make - -j5 - testGaussianBayesTree.run - true - true - true - - - make - -j5 - testCombinedImuFactor.run - true - true - true - - - make - -j5 - testImuFactor.run - true - true - true - - - make - -j5 - testAHRSFactor.run - true - true - true - - - make - -j8 - testAttitudeFactor.run - true - true - true - - - make - -j5 + -j2 clean true true true - + make - -j5 - all + -j2 + testPlanarSLAM.run + true + true + true + + + make + -j2 + testPose2Config.run + true + true + true + + + make + -j2 + testPose2Factor.run + true + true + true + + + make + -j2 + testPose2Prior.run + true + true + true + + + make + -j2 + testPose2SLAM.run + true + true + true + + + make + -j2 + testPose3Config.run + true + true + true + + + make + -j2 + testPose3SLAM.run + true + true + true + + + make + + testSimulated2DOriented.run + true + false + true + + + make + -j2 + testVSLAMConfig.run + true + true + true + + + make + -j2 + testVSLAMFactor.run + true + true + true + + + make + -j2 + testVSLAMGraph.run + true + true + true + + + make + -j2 + testPose3Factor.run + true + true + true + + + make + + testSimulated2D.run + true + false + true + + + make + + testSimulated3D.run + true + false + true + + + make + -j2 + tests/testGaussianISAM2 true true true @@ -1104,477 +1437,103 @@ make - testErrors.run true false true - - make - -j2 - check - true - true - true - - - make - -j2 - tests/testGaussianJunctionTree.run - true - true - true - - - make - -j2 - tests/testGaussianFactor.run - true - true - true - - - make - -j2 - tests/testGaussianConditional.run - true - true - true - - - make - -j2 - tests/timeSLAMlike.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testBTree.run - true - true - true - - - make - -j2 - testDSF.run - true - true - true - - - make - -j2 - testDSFVector.run - true - true - true - - - make - -j2 - testMatrix.run - true - true - true - - - make - -j2 - testSPQRUtil.run - true - true - true - - - make - -j2 - testVector.run - true - true - true - - - make - -j2 - timeMatrix.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - testClusterTree.run - true - true - true - - - make - -j2 - testJunctionTree.run - true - true - true - - - make - -j2 - tests/testEliminationTree.run - true - true - true - - - make - -j2 - tests/testSymbolicFactor.run - true - true - true - - - make - -j2 - tests/testVariableSlots.run - true - true - true - - - make - -j2 - tests/testConditional.run - true - true - true - - - make - -j2 - tests/testSymbolicFactorGraph.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - testNonlinearConstraint.run - true - true - true - - - make - -j2 - testLieConfig.run - true - true - true - - - make - -j2 - testConstraintOptimizer.run - true - true - true - - - make - -j2 - check - true - true - true - - + make -j5 - testBTree.run + testAntiFactor.run true true true - + make -j5 - testDSF.run + testPriorFactor.run true true true - + make -j5 - testDSFMap.run + testDataset.run true true true - + make -j5 - testDSFVector.run + testEssentialMatrixFactor.run true true true - + make -j5 - testFixedVector.run + testGeneralSFMFactor_Cal3Bundler.run true true true - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - testPlanarSLAM.run - true - true - true - - - make - -j2 - testPose2Config.run - true - true - true - - - make - -j2 - testPose2Factor.run - true - true - true - - - make - -j2 - testPose2Prior.run - true - true - true - - - make - -j2 - testPose2SLAM.run - true - true - true - - - make - -j2 - testPose3Config.run - true - true - true - - - make - -j2 - testPose3SLAM.run - true - true - true - - - make - testSimulated2DOriented.run - true - false - true - - - make - -j2 - testVSLAMConfig.run - true - true - true - - - make - -j2 - testVSLAMFactor.run - true - true - true - - - make - -j2 - testVSLAMGraph.run - true - true - true - - - make - -j2 - testPose3Factor.run - true - true - true - - - make - testSimulated2D.run - true - false - true - - - make - testSimulated3D.run - true - false - true - - - make - -j2 - tests/testGaussianISAM2 - true - true - true - - + make -j5 - testEliminationTree.run + testGeneralSFMFactor.run true true true - + make -j5 - testInference.run + testProjectionFactor.run true true true - + make -j5 - testKey.run + testRotateFactor.run true true true - + make - -j1 - testSymbolicBayesTree.run + -j5 + testPoseRotationPrior.run true - false + true true - + make - -j1 - testSymbolicSequentialSolver.run + -j5 + testImplicitSchurFactor.run true - false + true true - + make -j4 - testLabeledSymbol.run + testRangeFactor.run true true true - + make - -j2 - check - true - true - true - - - make - -j2 - tests/testLieConfig.run + -j4 + testOrientedPlane3Factor.run true true true @@ -1780,7 +1739,6 @@ cpack - -G DEB true false @@ -1788,7 +1746,6 @@ cpack - -G RPM true false @@ -1796,7 +1753,6 @@ cpack - -G TGZ true false @@ -1804,7 +1760,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -1986,7 +1941,7 @@ false true - + make -j2 check @@ -1994,38 +1949,55 @@ true true - + make - -j2 - clean - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - all - true - true - true - - - cmake - .. + + tests/testGaussianISAM2 true false true - + + make + -j2 + check + true + true + true + + + make + -j2 + tests/testGaussianJunctionTree.run + true + true + true + + + make + -j2 + tests/testGaussianFactor.run + true + true + true + + + make + -j2 + tests/testGaussianConditional.run + true + true + true + + + make + -j2 + tests/timeSLAMlike.run + true + true + true + + make -j2 testGaussianFactor.run @@ -2033,418 +2005,26 @@ true true - + make -j5 - testCal3Bundler.run + testParticleFactor.run true true true - + make -j5 - testCal3DS2.run + testExpressionMeta.run true true true - - make - -j5 - testCalibratedCamera.run - true - true - true - - - make - -j5 - testEssentialMatrix.run - true - true - true - - - make - -j1 VERBOSE=1 - testHomography2.run - true - false - true - - - make - -j5 - testPinholeCamera.run - true - true - true - - - make - -j5 - testPoint2.run - true - true - true - - - make - -j5 - testPoint3.run - true - true - true - - - make - -j5 - testPose2.run - true - true - true - - - make - -j5 - testPose3.run - true - true - true - - - make - -j5 - testRot3M.run - true - true - true - - - make - -j5 - testSphere2.run - true - true - true - - - make - -j5 - testStereoCamera.run - true - true - true - - - make - -j5 - testCal3Unified.run - true - true - true - - - make - -j5 - testRot2.run - true - true - true - - - make - -j5 - testRot3Q.run - true - true - true - - - make - -j5 - testRot3.run - true - true - true - - + make -j4 - testSO3.run - true - true - true - - - make - -j4 - testQuaternion.run - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - all - true - false - true - - - make - -j5 - check - true - false - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - clean all - true - true - true - - - make - -j5 - testIMUSystem.run - true - true - true - - - make - -j5 - testPoseRTV.run - true - true - true - - - make - -j5 - testVelocityConstraint.run - true - true - true - - - make - -j5 - testVelocityConstraint3.run - true - true - true - - - make - -j1 - testDiscreteBayesTree.run - true - false - true - - - make - -j5 - testDiscreteConditional.run - true - true - true - - - make - -j5 - testDiscreteFactor.run - true - true - true - - - make - -j5 - testDiscreteFactorGraph.run - true - true - true - - - make - -j5 - testDiscreteMarginals.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testWrap.run - true - true - true - - - make - -j5 - testSpirit.run - true - true - true - - - make - -j5 - check.wrap - true - true - true - - - make - -j5 - testMethod.run - true - true - true - - - make - -j5 - testClass.run - true - true - true - - - make - -j4 - testType.run - true - true - true - - - make - -j4 - testArgument.run - true - true - true - - - make - -j4 - testReturnValue.run - true - true - true - - - make - -j4 - testTemplate.run - true - true - true - - - make - -j4 - testGlobalFunction.run + testCustomChartExpression.run true true true @@ -2497,327 +2077,145 @@ true true - + make -j2 - vSFMexample.run + all true true true - + make -j2 - testVSLAMGraph - true - true - true - - - make - -j5 - testMatrix.run - true - true - true - - - make - -j5 - testVector.run - true - true - true - - - make - -j5 - testNumericalDerivative.run - true - true - true - - - make - -j5 - testVerticalBlockMatrix.run - true - true - true - - - make - -j4 - testOptionalJacobian.run - true - true - true - - - make - -j5 - check.tests - true - true - true - - - make - -j2 - timeGaussianFactorGraph.run - true - true - true - - - make - -j5 - testMarginals.run - true - true - true - - - make - -j5 - testGaussianISAM2.run - true - true - true - - - make - -j5 - testSymbolicFactorGraphB.run - true - true - true - - - make - -j2 - timeSequentialOnDataset.run - true - true - true - - - make - -j5 - testGradientDescentOptimizer.run - true - true - true - - - make - -j2 - testGaussianFactor.run - true - true - true - - - make - -j2 - testNonlinearOptimizer.run - true - true - true - - - make - -j2 - testGaussianBayesNet.run - true - true - true - - - make - -j2 - testNonlinearISAM.run - true - true - true - - - make - -j2 - testNonlinearEquality.run - true - true - true - - - make - -j2 - testExtendedKalmanFilter.run - true - true - true - - - make - -j5 - timing.tests - true - true - true - - - make - -j5 - testNonlinearFactor.run - true - true - true - - - make - -j5 clean true true true - + make - -j5 - testGaussianJunctionTreeB.run - true - true - true - - - make - - testGraph.run + -k + check true false true - + make - + tests/testBayesTree.run + true + false + true + + + make + testBinaryBayesNet.run + true + false + true + + + make + -j2 + testFactorGraph.run + true + true + true + + + make + -j2 + testISAM.run + true + true + true + + + make + -j2 testJunctionTree.run true - false + true true - + make - - testSymbolicBayesNetB.run + -j2 + testKey.run + true + true + true + + + make + -j2 + testOrdering.run + true + true + true + + + make + testSymbolicBayesNet.run true false true - + make - -j5 - testGaussianISAM.run + tests/testSymbolicFactor.run true - true + false true - + make - -j5 - testDoglegOptimizer.run + testSymbolicFactorGraph.run true - true + false true - - make - -j5 - testNonlinearFactorGraph.run - true - true - true - - - make - -j5 - testIterative.run - true - true - true - - - make - -j5 - testSubgraphSolver.run - true - true - true - - - make - -j5 - testGaussianFactorGraphB.run - true - true - true - - - make - -j5 - testSummarization.run - true - true - true - - - make - -j5 - testManifold.run - true - true - true - - - make - -j5 - testParticleFactor.run - true - true - true - - - make - -j5 - testExpressionMeta.run - true - true - true - - - make - -j4 - testCustomChartExpression.run - true - true - true - - + make -j2 - testGaussianFactor.run + timeSymbolMaps.run true true true - + + make + tests/testBayesTree + true + false + true + + make -j2 - install + check true true true - + + make + -j2 + timeCalibratedCamera.run + true + true + true + + + make + -j2 + timeRot3.run + true + true + true + + make -j2 clean @@ -2825,122 +2223,18 @@ true true - + make -j2 - all + tests/testPose2.run true true true - + make -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j5 - testAntiFactor.run - true - true - true - - - make - -j5 - testPriorFactor.run - true - true - true - - - make - -j5 - testDataset.run - true - true - true - - - make - -j5 - testEssentialMatrixFactor.run - true - true - true - - - make - -j5 - testGeneralSFMFactor_Cal3Bundler.run - true - true - true - - - make - -j5 - testGeneralSFMFactor.run - true - true - true - - - make - -j5 - testProjectionFactor.run - true - true - true - - - make - -j5 - testRotateFactor.run - true - true - true - - - make - -j5 - testPoseRotationPrior.run - true - true - true - - - make - -j5 - testImplicitSchurFactor.run - true - true - true - - - make - -j4 - testRangeFactor.run + tests/testPose3.run true true true @@ -3137,181 +2431,14 @@ true true - - make - -j5 - testLago.run - true - true - true - - - make - -j5 - testLinearContainerFactor.run - true - true - true - - - make - -j5 - testOrdering.run - true - true - true - - - make - -j5 - testValues.run - true - true - true - - - make - -j5 - testWhiteNoiseFactor.run - true - true - true - - + make -j4 - testExpression.run + SFMExampleExpressions_bal.run true true true - - make - -j4 - testAdaptAutoDiff.run - true - true - true - - - make - -j4 - testCallRecord.run - true - true - true - - - make - -j4 - testExpressionFactor.run - true - true - true - - - make - -j4 - testImuFactor.run - true - true - true - - - make - -j5 - timeCalibratedCamera.run - true - true - true - - - make - -j5 - timePinholeCamera.run - true - true - true - - - make - -j5 - timeStereoCamera.run - true - true - true - - - make - -j5 - timeLago.run - true - true - true - - - make - -j5 - timePose3.run - true - true - true - - - make - -j4 - timeAdaptAutoDiff.run - true - true - true - - - make - -j4 - timeCameraExpression.run - true - true - true - - - make - -j4 - timeOneCameraExpression.run - true - true - true - - - make - -j4 - timeSFMExpressions.run - true - true - true - - - make - -j4 - timeIncremental.run - true - true - true - - - make - -j2 - check - true - true - true - - - make - tests/testGaussianISAM2 - true - false - true - make -j2 @@ -3408,10 +2535,895 @@ true true - + + make + -j1 + testDiscreteBayesTree.run + true + false + true + + make -j5 - wrap + testDiscreteConditional.run + true + true + true + + + make + -j5 + testDiscreteFactor.run + true + true + true + + + make + -j5 + testDiscreteFactorGraph.run + true + true + true + + + make + -j5 + testDiscreteMarginals.run + true + true + true + + + make + -j5 + testEliminationTree.run + true + true + true + + + make + -j5 + testInference.run + true + true + true + + + make + -j5 + testKey.run + true + true + true + + + make + -j1 + testSymbolicBayesTree.run + true + false + true + + + make + -j1 + testSymbolicSequentialSolver.run + true + false + true + + + make + -j4 + testLabeledSymbol.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + testClusterTree.run + true + true + true + + + make + -j2 + testJunctionTree.run + true + true + true + + + make + -j2 + tests/testEliminationTree.run + true + true + true + + + make + -j2 + tests/testSymbolicFactor.run + true + true + true + + + make + -j2 + tests/testVariableSlots.run + true + true + true + + + make + -j2 + tests/testConditional.run + true + true + true + + + make + -j2 + tests/testSymbolicFactorGraph.run + true + true + true + + + make + -j5 + testLago.run + true + true + true + + + make + -j5 + testLinearContainerFactor.run + true + true + true + + + make + -j5 + testOrdering.run + true + true + true + + + make + -j5 + testValues.run + true + true + true + + + make + -j5 + testWhiteNoiseFactor.run + true + true + true + + + make + -j4 + testExpression.run + true + true + true + + + make + -j4 + testAdaptAutoDiff.run + true + true + true + + + make + -j4 + testCallRecord.run + true + true + true + + + make + -j4 + testExpressionFactor.run + true + true + true + + + make + -j5 + testIMUSystem.run + true + true + true + + + make + -j5 + testPoseRTV.run + true + true + true + + + make + -j5 + testVelocityConstraint.run + true + true + true + + + make + -j5 + testVelocityConstraint3.run + true + true + true + + + make + -j5 + timeCalibratedCamera.run + true + true + true + + + make + -j5 + timePinholeCamera.run + true + true + true + + + make + -j5 + timeStereoCamera.run + true + true + true + + + make + -j5 + timeLago.run + true + true + true + + + make + -j5 + timePose3.run + true + true + true + + + make + -j4 + timeAdaptAutoDiff.run + true + true + true + + + make + -j4 + timeCameraExpression.run + true + true + true + + + make + -j4 + timeOneCameraExpression.run + true + true + true + + + make + -j4 + timeSFMExpressions.run + true + true + true + + + make + -j4 + timeIncremental.run + true + true + true + + + make + -j5 + testBTree.run + true + true + true + + + make + -j5 + testDSF.run + true + true + true + + + make + -j5 + testDSFMap.run + true + true + true + + + make + -j5 + testDSFVector.run + true + true + true + + + make + -j5 + testFixedVector.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testGaussianFactorGraphUnordered.run + true + true + true + + + make + -j5 + testGaussianBayesNetUnordered.run + true + true + true + + + make + -j5 + testGaussianConditional.run + true + true + true + + + make + -j5 + testGaussianDensity.run + true + true + true + + + make + -j5 + testGaussianJunctionTree.run + true + true + true + + + make + -j5 + testHessianFactor.run + true + true + true + + + make + -j5 + testJacobianFactor.run + true + true + true + + + make + -j5 + testKalmanFilter.run + true + true + true + + + make + -j5 + testNoiseModel.run + true + true + true + + + make + -j5 + testSampler.run + true + true + true + + + make + -j5 + testSerializationLinear.run + true + true + true + + + make + -j5 + testVectorValues.run + true + true + true + + + make + -j5 + testGaussianBayesTree.run + true + true + true + + + make + -j5 + testWrap.run + true + true + true + + + make + -j5 + testSpirit.run + true + true + true + + + make + -j5 + check.wrap + true + true + true + + + make + -j5 + testMethod.run + true + true + true + + + make + -j5 + testClass.run + true + true + true + + + make + -j4 + testType.run + true + true + true + + + make + -j4 + testArgument.run + true + true + true + + + make + -j4 + testReturnValue.run + true + true + true + + + make + -j4 + testTemplate.run + true + true + true + + + make + -j4 + testGlobalFunction.run + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j5 + testMatrix.run + true + true + true + + + make + -j5 + testVector.run + true + true + true + + + make + -j5 + testNumericalDerivative.run + true + true + true + + + make + -j5 + testVerticalBlockMatrix.run + true + true + true + + + make + -j4 + testOptionalJacobian.run + true + true + true + + + make + -j5 + check.tests + true + true + true + + + make + -j2 + timeGaussianFactorGraph.run + true + true + true + + + make + -j5 + testMarginals.run + true + true + true + + + make + -j5 + testGaussianISAM2.run + true + true + true + + + make + -j5 + testSymbolicFactorGraphB.run + true + true + true + + + make + -j2 + timeSequentialOnDataset.run + true + true + true + + + make + -j5 + testGradientDescentOptimizer.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + testNonlinearOptimizer.run + true + true + true + + + make + -j2 + testGaussianBayesNet.run + true + true + true + + + make + -j2 + testNonlinearISAM.run + true + true + true + + + make + -j2 + testNonlinearEquality.run + true + true + true + + + make + -j2 + testExtendedKalmanFilter.run + true + true + true + + + make + -j5 + timing.tests + true + true + true + + + make + -j5 + testNonlinearFactor.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + testGaussianJunctionTreeB.run + true + true + true + + + make + testGraph.run + true + false + true + + + make + testJunctionTree.run + true + false + true + + + make + testSymbolicBayesNetB.run + true + false + true + + + make + -j5 + testGaussianISAM.run + true + true + true + + + make + -j5 + testDoglegOptimizer.run + true + true + true + + + make + -j5 + testNonlinearFactorGraph.run + true + true + true + + + make + -j5 + testIterative.run + true + true + true + + + make + -j5 + testSubgraphSolver.run + true + true + true + + + make + -j5 + testGaussianFactorGraphB.run + true + true + true + + + make + -j5 + testSummarization.run + true + true + true + + + make + -j5 + testManifold.run + true + true + true + + + make + -j2 + vSFMexample.run + true + true + true + + + make + -j5 + clean + true + true + true + + + make + -j5 + all + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + testVSLAMGraph true true true diff --git a/gtsam/geometry/tests/testOrientedPlane3.cpp b/gtsam/geometry/tests/testOrientedPlane3.cpp index 14a387102..b2b4ecc43 100644 --- a/gtsam/geometry/tests/testOrientedPlane3.cpp +++ b/gtsam/geometry/tests/testOrientedPlane3.cpp @@ -16,53 +16,47 @@ * @brief Tests the OrientedPlane3 class */ -#include #include -#include -#include -#include -#include -#include -#include -#include -#include -#include #include #include -#include -#include #include using namespace boost::assign; using namespace gtsam; using namespace std; +using boost::none; GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) //******************************************************************************* -TEST (OrientedPlane3, transform) -{ +TEST (OrientedPlane3, transform) { // Test transforming a plane to a pose - gtsam::Pose3 pose(gtsam::Rot3::ypr (-M_PI/4.0, 0.0, 0.0), gtsam::Point3(2.0, 3.0, 4.0)); - OrientedPlane3 plane (-1 , 0, 0, 5); - OrientedPlane3 expected_meas (-sqrt (2.0)/2.0, -sqrt (2.0)/2.0, 0.0, 3); - OrientedPlane3 transformed_plane = OrientedPlane3::Transform (plane, pose, boost::none, boost::none); - EXPECT (assert_equal (expected_meas, transformed_plane, 1e-9)); + gtsam::Pose3 pose(gtsam::Rot3::ypr(-M_PI / 4.0, 0.0, 0.0), + gtsam::Point3(2.0, 3.0, 4.0)); + OrientedPlane3 plane(-1, 0, 0, 5); + OrientedPlane3 expected_meas(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, 0.0, 3); + OrientedPlane3 transformed_plane = OrientedPlane3::Transform(plane, pose, + none, none); + EXPECT(assert_equal(expected_meas, transformed_plane, 1e-9)); // Test the jacobians of transform Matrix actualH1, expectedH1, actualH2, expectedH2; { - expectedH1 = numericalDerivative11(boost::bind (&OrientedPlane3::Transform, plane, _1, boost::none, boost::none), pose); + expectedH1 = numericalDerivative11( + boost::bind(&OrientedPlane3::Transform, plane, _1, none, none), pose); - OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, actualH1, boost::none); - EXPECT (assert_equal (expectedH1, actualH1, 1e-9)); + OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, actualH1, + none); + EXPECT(assert_equal(expectedH1, actualH1, 1e-9)); } { - expectedH2 = numericalDerivative11 (boost::bind (&OrientedPlane3::Transform, _1, pose, boost::none, boost::none), plane); + expectedH2 = numericalDerivative11( + boost::bind(&OrientedPlane3::Transform, _1, pose, none, none), plane); - OrientedPlane3 tformed = OrientedPlane3::Transform (plane, pose, boost::none, actualH2); - EXPECT (assert_equal (expectedH2, actualH2, 1e-9)); + OrientedPlane3 tformed = OrientedPlane3::Transform(plane, pose, none, + actualH2); + EXPECT(assert_equal(expectedH2, actualH2, 1e-9)); } } @@ -78,23 +72,23 @@ inline static Vector randomVector(const Vector& minLimits, // Create the random vector for (size_t i = 0; i < numDims; i++) { - double range = maxLimits(i) - minLimits(i); - vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i); - } + double range = maxLimits(i) - minLimits(i); + vector(i) = (((double) rand()) / RAND_MAX) * range + minLimits(i); + } return vector; } //******************************************************************************* TEST(OrientedPlane3, localCoordinates_retract) { - + size_t numIterations = 10000; gtsam::Vector minPlaneLimit(4), maxPlaneLimit(4); minPlaneLimit << -1.0, -1.0, -1.0, 0.01; maxPlaneLimit << 1.0, 1.0, 1.0, 10.0; - Vector minXiLimit(3),maxXiLimit(3); - minXiLimit << -M_PI, -M_PI, -10.0; - maxXiLimit << M_PI, M_PI, 10.0; + Vector minXiLimit(3), maxXiLimit(3); + minXiLimit << -M_PI, -M_PI, -10.0; + maxXiLimit << M_PI, M_PI, 10.0; for (size_t i = 0; i < numIterations; i++) { sleep(0); @@ -104,7 +98,7 @@ TEST(OrientedPlane3, localCoordinates_retract) { Vector v12 = randomVector(minXiLimit, maxXiLimit); // Magnitude of the rotation can be at most pi - if (v12.segment<3>(0).norm () > M_PI) + if (v12.segment<3>(0).norm() > M_PI) v12.segment<3>(0) = v12.segment<3>(0) / M_PI; OrientedPlane3 p2 = p1.retract(v12); diff --git a/gtsam/slam/OrientedPlane3Factor.cpp b/gtsam/slam/OrientedPlane3Factor.cpp index 7ec72825b..3728e53b1 100644 --- a/gtsam/slam/OrientedPlane3Factor.cpp +++ b/gtsam/slam/OrientedPlane3Factor.cpp @@ -5,7 +5,6 @@ * Author: Natesh Srinivasan */ - #include "OrientedPlane3Factor.h" using namespace std; @@ -13,19 +12,27 @@ using namespace std; namespace gtsam { //*************************************************************************** +void OrientedPlane3Factor::print(const string& s, + const KeyFormatter& keyFormatter) const { + cout << "OrientedPlane3Factor Factor on " << landmarkKey_ << "\n"; + measured_p_.print("Measured Plane"); + this->noiseModel_->print(" noise model: "); +} -void OrientedPlane3DirectionPrior::print(const string& s) const { +//*************************************************************************** +void OrientedPlane3DirectionPrior::print(const string& s, + const KeyFormatter& keyFormatter) const { cout << "Prior Factor on " << landmarkKey_ << "\n"; measured_p_.print("Measured Plane"); this->noiseModel_->print(" noise model: "); } //*************************************************************************** - bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, - double tol) const { + double tol) const { const This* e = dynamic_cast(&expected); - return e != NULL && Base::equals(*e, tol) && this->measured_p_.equals(e->measured_p_, tol); + return e != NULL && Base::equals(*e, tol) + && this->measured_p_.equals(e->measured_p_, tol); } //*************************************************************************** @@ -33,21 +40,21 @@ bool OrientedPlane3DirectionPrior::equals(const NonlinearFactor& expected, Vector OrientedPlane3DirectionPrior::evaluateError(const OrientedPlane3& plane, boost::optional H) const { -if(H) { - Matrix H_p; - Unit3 n_hat_p = measured_p_.normal(); - Unit3 n_hat_q = plane.normal(); - Vector e = n_hat_p.error(n_hat_q,H_p); - H->resize(2,3); - H->block <2,2>(0,0) << H_p; - H->block <2,1>(0,2) << Matrix::Zero(2, 1); - return e; -} else { - Unit3 n_hat_p = measured_p_.normal(); - Unit3 n_hat_q = plane.normal(); - Vector e = n_hat_p.error(n_hat_q); - return e; -} + if (H) { + Matrix H_p; + Unit3 n_hat_p = measured_p_.normal(); + Unit3 n_hat_q = plane.normal(); + Vector e = n_hat_p.error(n_hat_q, H_p); + H->resize(2, 3); + H->block<2, 2>(0, 0) << H_p; + H->block<2, 1>(0, 2) << Matrix::Zero(2, 1); + return e; + } else { + Unit3 n_hat_p = measured_p_.normal(); + Unit3 n_hat_q = plane.normal(); + Vector e = n_hat_p.error(n_hat_q); + return e; + } } } diff --git a/gtsam/slam/OrientedPlane3Factor.h b/gtsam/slam/OrientedPlane3Factor.h index 7827a5c2c..ab77ec612 100644 --- a/gtsam/slam/OrientedPlane3Factor.h +++ b/gtsam/slam/OrientedPlane3Factor.h @@ -7,57 +7,53 @@ #pragma once -#include -#include #include -#include -#include -#include +#include namespace gtsam { - /** +/** * Factor to measure a planar landmark from a given pose */ class OrientedPlane3Factor: public NoiseModelFactor2 { protected: - Symbol poseSymbol_; - Symbol landmarkSymbol_; + Key poseKey_; + Key landmarkKey_; Vector measured_coeffs_; OrientedPlane3 measured_p_; - typedef NoiseModelFactor2 Base; + typedef NoiseModelFactor2 Base; public: /// Constructor - OrientedPlane3Factor () - {} + OrientedPlane3Factor() { + } /// Constructor with measured plane coefficients (a,b,c,d), noise model, pose symbol - OrientedPlane3Factor (const Vector&z, const SharedGaussian& noiseModel, - const Symbol& pose, - const Symbol& landmark) - : Base (noiseModel, pose, landmark), - poseSymbol_ (pose), - landmarkSymbol_ (landmark), - measured_coeffs_ (z) - { - measured_p_ = OrientedPlane3 (Unit3 (z (0), z (1), z (2)), z (3)); + OrientedPlane3Factor(const Vector&z, const SharedGaussian& noiseModel, + const Key& pose, const Key& landmark) : + Base(noiseModel, pose, landmark), poseKey_(pose), landmarkKey_(landmark), measured_coeffs_( + z) { + measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3)); } /// print - void print(const std::string& s="PlaneFactor") const; + virtual void print(const std::string& s = "OrientedPlane3Factor", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// evaluateError virtual Vector evaluateError(const Pose3& pose, const OrientedPlane3& plane, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const - { - OrientedPlane3 predicted_plane = OrientedPlane3::Transform (plane, pose, H1, H2); + boost::optional H1 = boost::none, boost::optional H2 = + boost::none) const { + OrientedPlane3 predicted_plane = OrientedPlane3::Transform(plane, pose, H1, + H2); Vector err(3); - err << predicted_plane.error (measured_p_); + err << predicted_plane.error(measured_p_); return (err); - }; + } + ; }; // TODO: Convert this factor to dimension two, three dimensions is redundant for direction prior @@ -65,30 +61,30 @@ class OrientedPlane3DirectionPrior: public NoiseModelFactor1 { protected: OrientedPlane3 measured_p_; /// measured plane parameters Key landmarkKey_; - typedef NoiseModelFactor1 Base; + typedef NoiseModelFactor1 Base; public: typedef OrientedPlane3DirectionPrior This; /// Constructor - OrientedPlane3DirectionPrior () - {} + OrientedPlane3DirectionPrior() { + } /// Constructor with measured plane coefficients (a,b,c,d), noise model, landmark symbol - OrientedPlane3DirectionPrior (Key key, const Vector&z, - const SharedGaussian& noiseModel) - : Base (noiseModel, key), - landmarkKey_ (key) - { - measured_p_ = OrientedPlane3 (Unit3 (z (0), z (1), z (2)), z (3)); + OrientedPlane3DirectionPrior(Key key, const Vector&z, + const SharedGaussian& noiseModel) : + Base(noiseModel, key), landmarkKey_(key) { + measured_p_ = OrientedPlane3(Unit3(z(0), z(1), z(2)), z(3)); } - /// print - void print(const std::string& s) const; - /** equals */ + /// print + virtual void print(const std::string& s = "OrientedPlane3DirectionPrior", + const KeyFormatter& keyFormatter = DefaultKeyFormatter) const; + + /// equals virtual bool equals(const NonlinearFactor& expected, double tol = 1e-9) const; virtual Vector evaluateError(const OrientedPlane3& plane, - boost::optional H = boost::none) const; + boost::optional H = boost::none) const; }; } // gtsam diff --git a/gtsam/slam/tests/testOrientedPlane3Factor.cpp b/gtsam/slam/tests/testOrientedPlane3Factor.cpp index fa16a0f91..4a7b4c3fe 100644 --- a/gtsam/slam/tests/testOrientedPlane3Factor.cpp +++ b/gtsam/slam/tests/testOrientedPlane3Factor.cpp @@ -16,21 +16,12 @@ * @brief Tests the OrientedPlane3Factor class */ -#include -#include #include -#include -#include -#include -#include #include -#include -#include -#include -#include #include -#include +#include #include + #include #include #include @@ -43,92 +34,100 @@ using namespace std; GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3) GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3) -TEST (OrientedPlane3, lm_translation_error) -{ +// ************************************************************************* +TEST (OrientedPlane3Factor, lm_translation_error) { // Tests one pose, two measurements of the landmark that differ in range only. // Normal along -x, 3m away - gtsam::Symbol lm_sym ('p', 0); - gtsam::OrientedPlane3 test_lm0 (-1.0, 0.0, 0.0, 3.0); - - gtsam::ISAM2 isam2; - gtsam::Values new_values; - gtsam::NonlinearFactorGraph new_graph; + Symbol lm_sym('p', 0); + OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); + + ISAM2 isam2; + Values new_values; + NonlinearFactorGraph new_graph; // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose - gtsam::Symbol init_sym ('x', 0); - gtsam::Pose3 init_pose (gtsam::Rot3::ypr (0.0, 0.0, 0.0), - gtsam::Point3 (0.0, 0.0, 0.0)); - gtsam::Vector sigmas(6); + Symbol init_sym('x', 0); + Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); + Vector sigmas(6); sigmas << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001; - gtsam::PriorFactor pose_prior (init_sym, init_pose, gtsam::noiseModel::Diagonal::Sigmas (sigmas) ); - new_values.insert (init_sym, init_pose); - new_graph.add (pose_prior); - + PriorFactor pose_prior(init_sym, init_pose, + noiseModel::Diagonal::Sigmas(sigmas)); + new_values.insert(init_sym, init_pose); + new_graph.add(pose_prior); + // Add two landmark measurements, differing in range - new_values.insert (lm_sym, test_lm0); - gtsam::Vector sigmas3(3); + new_values.insert(lm_sym, test_lm0); + Vector sigmas3(3); sigmas3 << 0.1, 0.1, 0.1; - gtsam::Vector test_meas0_mean(4); - test_meas0_mean << -1.0, 0.0, 0.0, 3.0; - gtsam::OrientedPlane3Factor test_meas0 (test_meas0_mean, gtsam::noiseModel::Diagonal::Sigmas (sigmas3), init_sym, lm_sym); - new_graph.add (test_meas0); - gtsam::Vector test_meas1_mean(4); - test_meas1_mean << -1.0, 0.0, 0.0, 1.0; - gtsam::OrientedPlane3Factor test_meas1 (test_meas1_mean, gtsam::noiseModel::Diagonal::Sigmas (sigmas3), init_sym, lm_sym); - new_graph.add (test_meas1); - - // Optimize - gtsam::ISAM2Result result = isam2.update (new_graph, new_values); - gtsam::Values result_values = isam2.calculateEstimate (); - gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at(lm_sym); - - // Given two noisy measurements of equal weight, expect result between the two - gtsam::OrientedPlane3 expected_plane_landmark (-1.0, 0.0, 0.0, 2.0); - EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark)); -} - -TEST (OrientedPlane3, lm_rotation_error) -{ - // Tests one pose, two measurements of the landmark that differ in angle only. - // Normal along -x, 3m away - gtsam::Symbol lm_sym ('p', 0); - gtsam::OrientedPlane3 test_lm0 (-1.0, 0.0, 0.0, 3.0); - - gtsam::ISAM2 isam2; - gtsam::Values new_values; - gtsam::NonlinearFactorGraph new_graph; - - // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose - gtsam::Symbol init_sym ('x', 0); - gtsam::Pose3 init_pose (gtsam::Rot3::ypr (0.0, 0.0, 0.0), - gtsam::Point3 (0.0, 0.0, 0.0)); - gtsam::PriorFactor pose_prior (init_sym, init_pose, gtsam::noiseModel::Diagonal::Sigmas ((Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); - new_values.insert (init_sym, init_pose); - new_graph.add (pose_prior); - -// // Add two landmark measurements, differing in angle - new_values.insert (lm_sym, test_lm0); Vector test_meas0_mean(4); test_meas0_mean << -1.0, 0.0, 0.0, 3.0; - gtsam::OrientedPlane3Factor test_meas0 (test_meas0_mean, gtsam::noiseModel::Diagonal::Sigmas(Vector3( 0.1, 0.1, 0.1)), init_sym, lm_sym); - new_graph.add (test_meas0); + OrientedPlane3Factor test_meas0(test_meas0_mean, + noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym); + new_graph.add(test_meas0); Vector test_meas1_mean(4); - test_meas1_mean << 0.0, -1.0, 0.0, 3.0; - gtsam::OrientedPlane3Factor test_meas1 (test_meas1_mean, gtsam::noiseModel::Diagonal::Sigmas (Vector3( 0.1, 0.1, 0.1)), init_sym, lm_sym); - new_graph.add (test_meas1); - + test_meas1_mean << -1.0, 0.0, 0.0, 1.0; + OrientedPlane3Factor test_meas1(test_meas1_mean, + noiseModel::Diagonal::Sigmas(sigmas3), init_sym, lm_sym); + new_graph.add(test_meas1); + // Optimize - gtsam::ISAM2Result result = isam2.update (new_graph, new_values); - gtsam::Values result_values = isam2.calculateEstimate (); - gtsam::OrientedPlane3 optimized_plane_landmark = result_values.at(lm_sym); + ISAM2Result result = isam2.update(new_graph, new_values); + Values result_values = isam2.calculateEstimate(); + OrientedPlane3 optimized_plane_landmark = result_values.at( + lm_sym); // Given two noisy measurements of equal weight, expect result between the two - gtsam::OrientedPlane3 expected_plane_landmark (-sqrt (2.0)/2.0, -sqrt (2.0)/2.0, 0.0, 3.0); - EXPECT (assert_equal (optimized_plane_landmark, expected_plane_landmark)); + OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0); + EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); } // ************************************************************************* -TEST( OrientedPlane3DirectionPriorFactor, Constructor ) { +TEST (OrientedPlane3Factor, lm_rotation_error) { + // Tests one pose, two measurements of the landmark that differ in angle only. + // Normal along -x, 3m away + Symbol lm_sym('p', 0); + OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0); + + ISAM2 isam2; + Values new_values; + NonlinearFactorGraph new_graph; + + // Init pose and prior. Pose Prior is needed since a single plane measurement does not fully constrain the pose + Symbol init_sym('x', 0); + Pose3 init_pose(Rot3::ypr(0.0, 0.0, 0.0), Point3(0.0, 0.0, 0.0)); + PriorFactor pose_prior(init_sym, init_pose, + noiseModel::Diagonal::Sigmas( + (Vector(6) << 0.001, 0.001, 0.001, 0.001, 0.001, 0.001).finished())); + new_values.insert(init_sym, init_pose); + new_graph.add(pose_prior); + +// // Add two landmark measurements, differing in angle + new_values.insert(lm_sym, test_lm0); + Vector test_meas0_mean(4); + test_meas0_mean << -1.0, 0.0, 0.0, 3.0; + OrientedPlane3Factor test_meas0(test_meas0_mean, + noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym); + new_graph.add(test_meas0); + Vector test_meas1_mean(4); + test_meas1_mean << 0.0, -1.0, 0.0, 3.0; + OrientedPlane3Factor test_meas1(test_meas1_mean, + noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 0.1)), init_sym, lm_sym); + new_graph.add(test_meas1); + + // Optimize + ISAM2Result result = isam2.update(new_graph, new_values); + Values result_values = isam2.calculateEstimate(); + OrientedPlane3 optimized_plane_landmark = result_values.at( + lm_sym); + + // Given two noisy measurements of equal weight, expect result between the two + OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0, + 0.0, 3.0); + EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark)); +} + +// ************************************************************************* +TEST( OrientedPlane3DirectionPrior, Constructor ) { // Example: pitch and roll of aircraft in an ENU Cartesian frame. // If pitch and roll are zero for an aerospace frame, @@ -138,29 +137,30 @@ TEST( OrientedPlane3DirectionPriorFactor, Constructor ) { // Factor Key key(1); - SharedGaussian model = gtsam::noiseModel::Diagonal::Sigmas (Vector3(0.1, 0.1, 10.0)); + SharedGaussian model = noiseModel::Diagonal::Sigmas(Vector3(0.1, 0.1, 10.0)); OrientedPlane3DirectionPrior factor(key, planeOrientation, model); // Create a linearization point at the zero-error point Vector theta1 = Vector4(0.0, 0.02, -1.2, 10.0); - Vector theta2 = Vector4(0.0, 0.1, - 0.8, 10.0); - Vector theta3 = Vector4(0.0, 0.2, -0.9, 10.0); - + Vector theta2 = Vector4(0.0, 0.1, -0.8, 10.0); + Vector theta3 = Vector4(0.0, 0.2, -0.9, 10.0); OrientedPlane3 T1(theta1); OrientedPlane3 T2(theta2); OrientedPlane3 T3(theta3); - // Calculate numerical derivatives - Matrix expectedH1 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T1); + Matrix expectedH1 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + boost::none), T1); - Matrix expectedH2 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T2); + Matrix expectedH2 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + boost::none), T2); - Matrix expectedH3 = numericalDerivative11( - boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, boost::none), T3); + Matrix expectedH3 = numericalDerivative11( + boost::bind(&OrientedPlane3DirectionPrior::evaluateError, &factor, _1, + boost::none), T3); // Use the factor to calculate the derivative Matrix actualH1, actualH2, actualH3;