From cd6c5ca0bd32a18899efd0510cd12303790c1180 Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 19 Jun 2015 18:09:39 -0400 Subject: [PATCH] using parameters in smart projection factors constructors.. breaking the API, but now is way more elegant --- .cproject | 3534 ++++++++--------- gtsam/geometry/triangulation.h | 8 +- gtsam/slam/SmartProjectionFactor.h | 71 +- gtsam/slam/SmartProjectionPoseFactor.h | 25 +- gtsam/slam/tests/smartFactorScenarios.h | 1 + .../tests/testSmartProjectionCameraFactor.cpp | 81 +- .../tests/testSmartProjectionPoseFactor.cpp | 152 +- 7 files changed, 1945 insertions(+), 1927 deletions(-) diff --git a/.cproject b/.cproject index fa20e5794..2051b74fb 100644 --- a/.cproject +++ b/.cproject @@ -1,17 +1,19 @@ - + + + + + - - @@ -60,13 +62,13 @@ + + - - @@ -116,13 +118,13 @@ + + - - @@ -484,341 +486,6 @@ - - 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 - -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 - - - make - -j4 - testSimilarity3.run - true - true - true - - - make - -j5 - testInvDepthCamera3.run - true - true - true - - - make - -j5 - testTriangulation.run - true - true - true - - - make - -j4 - testEvent.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 -j5 @@ -867,183 +534,39 @@ 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 + testSimilarity3.run true true true - + + make + -j5 + testInvDepthCamera3.run + true + true + true + + + make + -j5 + testTriangulation.run + true + true + true + + make -j4 - testQuaternion.run + testEvent.run true true true - - make - -j4 - testOrientedPlane3.run - true - true - true - - - make - -j4 - testPinholePose.run - true - true - true - - - make - -j4 - testCyclic.run - true - true - true - - + make -j2 all @@ -1051,7 +574,7 @@ true true - + make -j2 clean @@ -1059,23 +582,137 @@ true true - + + make + -k + check + true + false + true + + + make + tests/testBayesTree.run + true + false + true + + + make + testBinaryBayesNet.run + true + false + true + + make -j2 - clean all + testFactorGraph.run true true true - + make -j2 - install + testISAM.run 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 @@ -1227,143 +864,154 @@ true true - + make - -j2 - all + -j5 + testGaussianFactorGraphUnordered.run true true true - + make - -j2 - check + -j5 + testGaussianBayesNetUnordered.run true true true - + make - -j2 + -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 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 + -j5 + all true true true @@ -1458,112 +1106,479 @@ make - testErrors.run true false true - + make - -j5 - testAntiFactor.run + -j2 + check true true true - + make - -j5 - testPriorFactor.run + -j2 + tests/testGaussianJunctionTree.run true true true - + make - -j5 - testDataset.run + -j2 + tests/testGaussianFactor.run true true true - + make - -j5 - testEssentialMatrixFactor.run + -j2 + tests/testGaussianConditional.run true true true - + make - -j5 - testGeneralSFMFactor_Cal3Bundler.run + -j2 + tests/timeSLAMlike.run true true true - + make - -j5 - testGeneralSFMFactor.run + -j2 + check true true true - + make - -j5 - testProjectionFactor.run + -j2 + clean true true true - + make - -j5 - testRotateFactor.run + -j2 + testBTree.run true true true - + make - -j5 - testPoseRotationPrior.run + -j2 + testDSF.run true true true - + make - -j5 - testImplicitSchurFactor.run + -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 + -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 + 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 + -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 - testRangeFactor.run + testLabeledSymbol.run true true true - + make - -j4 - testOrientedPlane3Factor.run + -j2 + check true true true - + make - -j4 - testSmartProjectionPoseFactor.run + -j2 + tests/testLieConfig.run true true true @@ -1769,7 +1784,6 @@ cpack - -G DEB true false @@ -1777,7 +1791,6 @@ cpack - -G RPM true false @@ -1785,7 +1798,6 @@ cpack - -G TGZ true false @@ -1793,7 +1805,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -1975,7 +1986,7 @@ false true - + make -j2 check @@ -1983,54 +1994,38 @@ true true - + make - tests/testGaussianISAM2 + -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/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 @@ -2038,10 +2033,442 @@ true true - + make -j5 - testParticleFactor.run + 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 + -j4 + testPinholePose.run + true + true + true + + + make + -j4 + testCyclic.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 + -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 + -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 + -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 true true true @@ -2094,7 +2521,332 @@ true true - + + make + -j2 + vSFMexample.run + 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 + -j4 + testGroup.run + true + true + true + + + make + -j2 + testVSLAMGraph + 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 + -j4 + testLie.run + true + true + true + + + make + -j5 + testParticleFactor.run + true + true + true + + + make + -j2 + testGaussianFactor.run + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + make -j2 all @@ -2102,7 +2854,7 @@ true true - + make -j2 clean @@ -2110,135 +2862,15 @@ 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 + all true true true - - make - -j2 - testISAM.run - 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 - check - true - true - true - - - make - -j2 - timeCalibratedCamera.run - true - true - true - - - make - -j2 - timeRot3.run - true - true - true - - + make -j2 clean @@ -2246,18 +2878,114 @@ true true - + make - -j2 - tests/testPose2.run + -j5 + testAntiFactor.run true true true - + make - -j2 - tests/testPose3.run + -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 + true + true + true + + + make + -j4 + testOrientedPlane3Factor.run + true + true + true + + + make + -j4 + testSmartProjectionPoseFactor.run + true + true + true + + + make + -j4 + testSmartProjectionCameraFactor.run true true true @@ -2462,6 +3190,190 @@ 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 + -j4 + testExecutionTrace.run + true + true + true + + + make + -j4 + testImuFactor.run + true + true + true + + + make + -j2 + check + true + true + true + + + make + + tests/testGaussianISAM2 + true + false + 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 @@ -2558,922 +3470,10 @@ 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 - -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 - -j4 - testExecutionTrace.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 - -j4 - testGroup.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 - -j4 - testLie.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 + wrap true true true diff --git a/gtsam/geometry/triangulation.h b/gtsam/geometry/triangulation.h index adaa3dbaf..4ac634f03 100644 --- a/gtsam/geometry/triangulation.h +++ b/gtsam/geometry/triangulation.h @@ -320,21 +320,21 @@ Point3 triangulatePoint3( struct TriangulationParameters { - const double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate - const bool enableEPI; ///< if set to true, will refine triangulation using LM + double rankTolerance; ///< threshold to decide whether triangulation is result.degenerate + bool enableEPI; ///< if set to true, will refine triangulation using LM /** * if the landmark is triangulated at distance larger than this, * result is flagged as degenerate. */ - const double landmarkDistanceThreshold; // + double landmarkDistanceThreshold; // /** * If this is nonnegative the we will check if the average reprojection error * is smaller than this threshold after triangulation, otherwise result is * flagged as degenerate. */ - const double dynamicOutlierRejectionThreshold; + double dynamicOutlierRejectionThreshold; /** * Constructor diff --git a/gtsam/slam/SmartProjectionFactor.h b/gtsam/slam/SmartProjectionFactor.h index 7ac2a03be..3ad2d05a5 100644 --- a/gtsam/slam/SmartProjectionFactor.h +++ b/gtsam/slam/SmartProjectionFactor.h @@ -48,12 +48,12 @@ class GTSAM_EXPORT SmartProjectionParams { public: - const LinearizationMode linearizationMode; ///< How to linearize the factor - const DegeneracyMode degeneracyMode; ///< How to linearize the factor + LinearizationMode linearizationMode; ///< How to linearize the factor + DegeneracyMode degeneracyMode; ///< How to linearize the factor /// @name Parameters governing the triangulation /// @{ - const TriangulationParameters triangulationParameters; + mutable TriangulationParameters triangulationParameters; const double retriangulationThreshold; ///< threshold to decide whether to re-triangulate /// @} @@ -64,10 +64,10 @@ public: /// @} // Constructor - SmartProjectionParams(LinearizationMode linMode = HESSIAN, - DegeneracyMode degMode = IGNORE_DEGENERACY, double rankTol = 1, - bool enableEPI = false, double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1): + SmartProjectionParams(const LinearizationMode linMode = HESSIAN, + const DegeneracyMode degMode = IGNORE_DEGENERACY, const double rankTol = 1, + const bool enableEPI = false, const double landmarkDistanceThreshold = 1e10, + const double dynamicOutlierRejectionThreshold = -1): linearizationMode(linMode), degeneracyMode(degMode), triangulationParameters(rankTol, enableEPI, landmarkDistanceThreshold, dynamicOutlierRejectionThreshold), @@ -89,15 +89,36 @@ public: inline LinearizationMode getLinearizationMode() const { return linearizationMode; } - /** return cheirality verbosity */ + inline DegeneracyMode getDegeneracyMode() const { + return degeneracyMode; + } + inline TriangulationParameters getTriangulationParameters() const { + return triangulationParameters; + } inline bool getVerboseCheirality() const { return verboseCheirality; } - /** return flag for throwing cheirality exceptions */ inline bool getThrowCheirality() const { return throwCheirality; } - + inline void setLinearizationMode(LinearizationMode linMode) { + linearizationMode = linMode; + } + inline void setDegeneracyMode(DegeneracyMode degMode) { + degeneracyMode = degMode; + } + inline void setRankTolerance(double rankTol) { + triangulationParameters.rankTolerance = rankTol; + } + inline void setEnableEPI(bool enableEPI) { + triangulationParameters.enableEPI = enableEPI; + } + inline void setLandmarkDistanceThreshold(bool landmarkDistanceThreshold) { + triangulationParameters.landmarkDistanceThreshold = landmarkDistanceThreshold; + } + inline void setDynamicOutlierRejectionThreshold(bool dynOutRejectionThreshold) { + triangulationParameters.dynamicOutlierRejectionThreshold = dynOutRejectionThreshold; + } }; /** @@ -134,36 +155,16 @@ public: /// shorthand for a set of cameras typedef CameraSet Cameras; - /** - * Constructor - * @param rankTol tolerance used to check if point triangulation is degenerate - * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization) - * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, - * otherwise the factor is simply neglected - * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations - */ - SmartProjectionFactor(LinearizationMode linearizationMode = HESSIAN, - double rankTolerance = 1, DegeneracyMode degeneracyMode = - IGNORE_DEGENERACY, bool enableEPI = false, - double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1, - boost::optional body_P_sensor = boost::none) : - Base(body_P_sensor), - params_(linearizationMode, degeneracyMode, - rankTolerance, enableEPI, landmarkDistanceThreshold, - dynamicOutlierRejectionThreshold), // - result_(TriangulationResult::Degenerate()) {} - /** * Constructor * @param body_P_sensor pose of the camera in the body frame * @param params internal parameters of the smart factors */ -// SmartProjectionFactor(const boost::optional body_P_sensor = boost::none, -// const SmartProjectionParams params = SmartProjectionParams()) : -// Base(body_P_sensor), -// params_(params), // -// result_(TriangulationResult::Degenerate()) {} + SmartProjectionFactor(const boost::optional body_P_sensor = boost::none, + const SmartProjectionParams params = SmartProjectionParams()) : + Base(body_P_sensor), + params_(params), // + result_(TriangulationResult::Degenerate()) {} /** Virtual destructor */ virtual ~SmartProjectionFactor() { diff --git a/gtsam/slam/SmartProjectionPoseFactor.h b/gtsam/slam/SmartProjectionPoseFactor.h index b428d7b05..322c3790f 100644 --- a/gtsam/slam/SmartProjectionPoseFactor.h +++ b/gtsam/slam/SmartProjectionPoseFactor.h @@ -55,33 +55,16 @@ public: /// shorthand for a smart pointer to a factor typedef boost::shared_ptr shared_ptr; - /** - * Constructor - * @param rankTol tolerance used to check if point triangulation is degenerate - * @param linThreshold threshold on relative pose changes used to decide whether to relinearize (selective relinearization) - * @param manageDegeneracy is true, in presence of degenerate triangulation, the factor is converted to a rotation-only constraint, - * otherwise the factor is simply neglected (this functionality is deprecated) - * @param enableEPI if set to true linear triangulation is refined with embedded LM iterations - * @param body_P_sensor is the transform from sensor to body frame (default identity) - */ - SmartProjectionPoseFactor(const boost::shared_ptr K, const double rankTol = 1, - const double linThreshold = -1, const DegeneracyMode manageDegeneracy = IGNORE_DEGENERACY, - const bool enableEPI = false, boost::optional body_P_sensor = boost::none, - LinearizationMode linearizeTo = HESSIAN, double landmarkDistanceThreshold = 1e10, - double dynamicOutlierRejectionThreshold = -1) : - Base(linearizeTo, rankTol, manageDegeneracy, enableEPI, landmarkDistanceThreshold, - dynamicOutlierRejectionThreshold, body_P_sensor), K_(K) {} - /** * Constructor * @param K (fixed) calibration, assumed to be the same for all cameras * @param body_P_sensor pose of the camera in the body frame * @param params internal parameters of the smart factors */ -// SmartProjectionPoseFactor(const boost::shared_ptr K, -// const boost::optional body_P_sensor = boost::none, -// const SmartProjectionParams params = SmartProjectionParams()) : -// Base(body_P_sensor, params), K_(K) {} + SmartProjectionPoseFactor(const boost::shared_ptr K, + const boost::optional body_P_sensor = boost::none, + const SmartProjectionParams params = SmartProjectionParams()) : + Base(body_P_sensor, params), K_(K) {} /** Virtual destructor */ virtual ~SmartProjectionPoseFactor() {} diff --git a/gtsam/slam/tests/smartFactorScenarios.h b/gtsam/slam/tests/smartFactorScenarios.h index e566146d0..8e83ec503 100644 --- a/gtsam/slam/tests/smartFactorScenarios.h +++ b/gtsam/slam/tests/smartFactorScenarios.h @@ -61,6 +61,7 @@ Camera cam1(level_pose, K2); Camera cam2(pose_right, K2); Camera cam3(pose_above, K2); typedef GeneralSFMFactor SFMFactor; +SmartProjectionParams params; } /* ************************************************************************* */ diff --git a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp index 636e69b4b..533e16bec 100644 --- a/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionCameraFactor.cpp @@ -30,8 +30,6 @@ using namespace boost::assign; static bool isDebugTest = false; -static double rankTol = 1.0; - // Convenience for named keys using symbol_shorthand::X; using symbol_shorthand::L; @@ -42,6 +40,8 @@ Key c1 = 1, c2 = 2, c3 = 3; static Point2 measurement1(323.0, 240.0); +static double rankTol = 1.0; + template PinholeCamera perturbCameraPoseAndCalibration( const PinholeCamera& camera) { @@ -78,7 +78,8 @@ TEST( SmartProjectionCameraFactor, Constructor) { /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor2) { using namespace vanilla; - SmartFactor factor1(gtsam::HESSIAN, rankTol); + params.setRankTolerance(rankTol); + SmartFactor factor1(boost::none, params); } /* ************************************************************************* */ @@ -91,7 +92,8 @@ TEST( SmartProjectionCameraFactor, Constructor3) { /* ************************************************************************* */ TEST( SmartProjectionCameraFactor, Constructor4) { using namespace vanilla; - SmartFactor factor1(gtsam::HESSIAN, rankTol); + params.setRankTolerance(rankTol); + SmartFactor factor1(boost::none, params); factor1.add(measurement1, x1, unit2); } @@ -257,12 +259,12 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimize ) { EXPECT(assert_equal(expected, actual, 1)); // Optimize - LevenbergMarquardtParams params; + LevenbergMarquardtParams lmParams; if (isDebugTest) { - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; - params.verbosity = NonlinearOptimizerParams::ERROR; + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + lmParams.verbosity = NonlinearOptimizerParams::ERROR; } - LevenbergMarquardtOptimizer optimizer(graph, initial, params); + LevenbergMarquardtOptimizer optimizer(graph, initial, lmParams); Values result = optimizer.optimize(); EXPECT(assert_equal(landmark1, *smartFactor1->point(), 1e-7)); @@ -338,14 +340,14 @@ TEST( SmartProjectionCameraFactor, perturbPoseAndOptimizeFromSfM_tracks ) { if (isDebugTest) values.at(c3).print("Smart: Pose3 before optimization: "); - LevenbergMarquardtParams params; + LevenbergMarquardtParams lmParams; if (isDebugTest) - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; if (isDebugTest) - params.verbosity = NonlinearOptimizerParams::ERROR; + lmParams.verbosity = NonlinearOptimizerParams::ERROR; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); @@ -415,17 +417,17 @@ TEST( SmartProjectionCameraFactor, perturbCamerasAndOptimize ) { if (isDebugTest) values.at(c3).print("Smart: Pose3 before optimization: "); - LevenbergMarquardtParams params; - params.relativeErrorTol = 1e-8; - params.absoluteErrorTol = 0; - params.maxIterations = 20; + LevenbergMarquardtParams lmParams; + lmParams.relativeErrorTol = 1e-8; + lmParams.absoluteErrorTol = 0; + lmParams.maxIterations = 20; if (isDebugTest) - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; if (isDebugTest) - params.verbosity = NonlinearOptimizerParams::ERROR; + lmParams.verbosity = NonlinearOptimizerParams::ERROR; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); // GaussianFactorGraph::shared_ptr GFG = graph.linearize(values); @@ -494,17 +496,17 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler ) { if (isDebugTest) values.at(c3).print("Smart: Pose3 before optimization: "); - LevenbergMarquardtParams params; - params.relativeErrorTol = 1e-8; - params.absoluteErrorTol = 0; - params.maxIterations = 20; + LevenbergMarquardtParams lmParams; + lmParams.relativeErrorTol = 1e-8; + lmParams.absoluteErrorTol = 0; + lmParams.maxIterations = 20; if (isDebugTest) - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; if (isDebugTest) - params.verbosity = NonlinearOptimizerParams::ERROR; + lmParams.verbosity = NonlinearOptimizerParams::ERROR; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); if (isDebugTest) @@ -570,17 +572,17 @@ TEST( SmartProjectionCameraFactor, Cal3Bundler2 ) { if (isDebugTest) values.at(c3).print("Smart: Pose3 before optimization: "); - LevenbergMarquardtParams params; - params.relativeErrorTol = 1e-8; - params.absoluteErrorTol = 0; - params.maxIterations = 20; + LevenbergMarquardtParams lmParams; + lmParams.relativeErrorTol = 1e-8; + lmParams.absoluteErrorTol = 0; + lmParams.maxIterations = 20; if (isDebugTest) - params.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; + lmParams.verbosityLM = LevenbergMarquardtParams::TRYLAMBDA; if (isDebugTest) - params.verbosity = NonlinearOptimizerParams::ERROR; + lmParams.verbosity = NonlinearOptimizerParams::ERROR; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); if (isDebugTest) @@ -805,9 +807,14 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { bool useEPI = false; // Hessian version + SmartProjectionParams params; + params.setRankTolerance(rankTol); + params.setLinearizationMode(gtsam::HESSIAN); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setEnableEPI(useEPI); + SmartFactor::shared_ptr explicitFactor( - new SmartFactor(gtsam::HESSIAN, rankTol, - gtsam::IGNORE_DEGENERACY, useEPI)); + new SmartFactor(boost::none, params)); explicitFactor->add(level_uv, c1, unit2); explicitFactor->add(level_uv_right, c2, unit2); @@ -817,9 +824,9 @@ TEST( SmartProjectionCameraFactor, implicitJacobianFactor ) { dynamic_cast(*gaussianHessianFactor); // Implicit Schur version + params.setLinearizationMode(gtsam::IMPLICIT_SCHUR); SmartFactor::shared_ptr implicitFactor( - new SmartFactor(gtsam::IMPLICIT_SCHUR, rankTol, - gtsam::IGNORE_DEGENERACY, useEPI)); + new SmartFactor(boost::none, params)); implicitFactor->add(level_uv, c1, unit2); implicitFactor->add(level_uv_right, c2, unit2); GaussianFactor::shared_ptr gaussianImplicitSchurFactor = diff --git a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp index 5f6c2fee3..72147ff35 100644 --- a/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp +++ b/gtsam/slam/tests/testSmartProjectionPoseFactor.cpp @@ -46,7 +46,7 @@ static Symbol x3('X', 3); static Point2 measurement1(323.0, 240.0); -LevenbergMarquardtParams params; +LevenbergMarquardtParams lmParams; // Make more verbose like so (in tests): // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; @@ -59,7 +59,9 @@ TEST( SmartProjectionPoseFactor, Constructor) { /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor2) { using namespace vanillaPose; - SmartFactor factor1(sharedK, rankTol); + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor1(sharedK, boost::none, params); } /* ************************************************************************* */ @@ -72,7 +74,9 @@ TEST( SmartProjectionPoseFactor, Constructor3) { /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, Constructor4) { using namespace vanillaPose; - SmartFactor factor1(sharedK, rankTol); + SmartProjectionParams params; + params.setRankTolerance(rankTol); + SmartFactor factor1(sharedK, boost::none, params); factor1.add(measurement1, x1, model); } @@ -229,15 +233,18 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ views.push_back(x2); views.push_back(x3); - bool enableEPI = false; + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setEnableEPI(false); - SmartProjectionPoseFactor smartFactor1(K, 1.0, -1.0, gtsam::IGNORE_DEGENERACY, enableEPI, sensor_to_body); + SmartProjectionPoseFactor smartFactor1(K, sensor_to_body, params); smartFactor1.add(measurements_cam1, views, model); - SmartProjectionPoseFactor smartFactor2(K, 1.0, -1.0, gtsam::IGNORE_DEGENERACY, enableEPI, sensor_to_body); + SmartProjectionPoseFactor smartFactor2(K, sensor_to_body, params); smartFactor2.add(measurements_cam2, views, model); - SmartProjectionPoseFactor smartFactor3(K, 1.0, -1.0, gtsam::IGNORE_DEGENERACY, enableEPI, sensor_to_body); + SmartProjectionPoseFactor smartFactor3(K, sensor_to_body, params); smartFactor3.add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -266,9 +273,9 @@ TEST( SmartProjectionPoseFactor, smartFactorWithSensorBodyTransform ){ // initialize third pose with some noise, we expect it to move back to original pose3 values.insert(x3, bodyPose3*noise_pose); - LevenbergMarquardtParams params; + LevenbergMarquardtParams lmParams; Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); EXPECT(assert_equal(bodyPose3,result.at(x3))); } @@ -329,7 +336,7 @@ TEST( SmartProjectionPoseFactor, 3poses_smart_projection_factor ) { Point3(0.1, -0.1, 1.9)), values.at(x3))); Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); } @@ -552,7 +559,7 @@ TEST( SmartProjectionPoseFactor, 3poses_iterative_smart_projection_factor ) { values.at(x3))); Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); EXPECT(assert_equal(pose_above, result.at(x3), 1e-7)); } @@ -574,16 +581,23 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setEnableEPI(false); + SmartFactor factor1(sharedK, boost::none, params); + SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_SVD)); + new SmartFactor(sharedK, boost::none, params)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_SVD)); + new SmartFactor(sharedK, boost::none, params)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), gtsam::JACOBIAN_SVD)); + new SmartFactor(sharedK, boost::none, params)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -604,7 +618,7 @@ TEST( SmartProjectionPoseFactor, jacobianSVD ) { values.insert(x3, pose_above * noise_pose); Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); } @@ -628,19 +642,23 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + SmartProjectionParams params; + params.setRankTolerance(1.0); + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setDegeneracyMode(gtsam::IGNORE_DEGENERACY); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setEnableEPI(false); + SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), - gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + new SmartFactor(sharedK, boost::none, params)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), - gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + new SmartFactor(sharedK, boost::none, params)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), - gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist)); + new SmartFactor(sharedK, boost::none, params)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -662,7 +680,7 @@ TEST( SmartProjectionPoseFactor, landmarkDistance ) { // All factors are disabled and pose should remain where it is Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); EXPECT(assert_equal(values.at(x3), result.at(x3))); } @@ -693,24 +711,25 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { projectToMultipleCameras(cam1, cam2, cam3, landmark4, measurements_cam4); measurements_cam4.at(0) = measurements_cam4.at(0) + Point2(10, 10); // add outlier + SmartProjectionParams params; + params.setLinearizationMode(gtsam::JACOBIAN_SVD); + params.setLandmarkDistanceThreshold(excludeLandmarksFutherThanDist); + params.setDynamicOutlierRejectionThreshold(dynamicOutlierRejectionThreshold); + SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), - gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + new SmartFactor(sharedK, boost::none, params)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), - gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + new SmartFactor(sharedK, boost::none, params)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), - gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + new SmartFactor(sharedK, boost::none, params)); smartFactor3->add(measurements_cam3, views, model); SmartFactor::shared_ptr smartFactor4( - new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, false, Pose3(), - gtsam::JACOBIAN_SVD, excludeLandmarksFutherThanDist, dynamicOutlierRejectionThreshold)); + new SmartFactor(sharedK, boost::none, params)); smartFactor4->add(measurements_cam4, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -730,7 +749,7 @@ TEST( SmartProjectionPoseFactor, dynamicOutlierRejection ) { // All factors are disabled and pose should remain where it is Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); EXPECT(assert_equal(cam3.pose(), result.at(x3))); } @@ -752,19 +771,19 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); + SmartProjectionParams params; + params.setLinearizationMode(gtsam::JACOBIAN_Q); + SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, - false, Pose3(), gtsam::JACOBIAN_Q)); + new SmartFactor(sharedK, boost::none, params)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, - false, Pose3(), gtsam::JACOBIAN_Q)); + new SmartFactor(sharedK, boost::none, params)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, 1, -1, gtsam::IGNORE_DEGENERACY, - false, Pose3(), gtsam::JACOBIAN_Q)); + new SmartFactor(sharedK, boost::none, params)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -784,7 +803,7 @@ TEST( SmartProjectionPoseFactor, jacobianQ ) { values.insert(x3, pose_above * noise_pose); Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); EXPECT(assert_equal(pose_above, result.at(x3), 1e-8)); } @@ -840,7 +859,7 @@ TEST( SmartProjectionPoseFactor, 3poses_projection_factor ) { DOUBLES_EQUAL(48406055, graph.error(values), 1); - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); Values result = optimizer.optimize(); DOUBLES_EQUAL(0, graph.error(result), 1e-9); @@ -872,18 +891,19 @@ TEST( SmartProjectionPoseFactor, CheckHessian) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - double rankTol = 10; + SmartProjectionParams params; + params.setRankTolerance(10); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, rankTol)); // HESSIAN, by default + new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, rankTol)); // HESSIAN, by default + new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, rankTol)); // HESSIAN, by default + new SmartFactor(sharedK, boost::none, params)); // HESSIAN, by default smartFactor3->add(measurements_cam3, views, model); NonlinearFactorGraph graph; @@ -953,13 +973,16 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac projectToMultipleCameras(cam1, cam2, cam3, landmark1, measurements_cam1); projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); - double rankTol = 50; + SmartProjectionParams params; + params.setRankTolerance(50); + params.setDegeneracyMode(gtsam::HANDLE_INFINITY); + SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK2, rankTol, -1.0, gtsam::HANDLE_INFINITY)); + new SmartFactor(sharedK2, boost::none, params)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK2, rankTol, -1.0, gtsam::HANDLE_INFINITY)); + new SmartFactor(sharedK2, boost::none, params)); smartFactor2->add(measurements_cam2, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -983,7 +1006,7 @@ TEST( SmartProjectionPoseFactor, 3poses_2land_rotation_only_smart_projection_fac values.insert(x3, pose3 * noise_pose); // params.verbosityLM = LevenbergMarquardtParams::SUMMARY; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); Values result = optimizer.optimize(); EXPECT(assert_equal(pose3, result.at(x3))); } @@ -1012,18 +1035,20 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - double rankTol = 10; + SmartProjectionParams params; + params.setRankTolerance(10); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY)); + new SmartFactor(sharedK, boost::none, params)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY)); + new SmartFactor(sharedK, boost::none, params)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY)); + new SmartFactor(sharedK, boost::none, params)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1058,7 +1083,7 @@ TEST( SmartProjectionPoseFactor, 3poses_rotation_only_smart_projection_factor ) values.at(x3))); Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); // Since we do not do anything on degenerate instances (ZERO_ON_DEGENERACY) @@ -1212,7 +1237,9 @@ TEST( SmartProjectionPoseFactor, HessianWithRotationDegenerate ) { /* ************************************************************************* */ TEST( SmartProjectionPoseFactor, ConstructorWithCal3Bundler) { using namespace bundlerPose; - SmartFactor factor(sharedBundlerK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY, false, Pose3(), gtsam::HESSIAN); + SmartProjectionParams params; + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); + SmartFactor factor(sharedBundlerK, boost::none, params); factor.add(measurement1, x1, model); } @@ -1221,7 +1248,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { using namespace bundlerPose; - // three landmarks ~5 meters infront of camera + // three landmarks ~5 meters in front of camera Point3 landmark3(3, 0, 3.0); vector measurements_cam1, measurements_cam2, measurements_cam3; @@ -1270,7 +1297,7 @@ TEST( SmartProjectionPoseFactor, Cal3Bundler ) { Point3(0.1, -0.1, 1.9)), values.at(x3))); Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); EXPECT(assert_equal(cam3.pose(), result.at(x3), 1e-6)); } @@ -1302,21 +1329,20 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { projectToMultipleCameras(cam1, cam2, cam3, landmark2, measurements_cam2); projectToMultipleCameras(cam1, cam2, cam3, landmark3, measurements_cam3); - double rankTol = 10; + SmartProjectionParams params; + params.setRankTolerance(10); + params.setDegeneracyMode(gtsam::ZERO_ON_DEGENERACY); SmartFactor::shared_ptr smartFactor1( - new SmartFactor(sharedBundlerK, rankTol, -1.0, gtsam::ZERO_ON_DEGENERACY, - false, Pose3(), gtsam::HESSIAN)); + new SmartFactor(sharedBundlerK, boost::none, params)); smartFactor1->add(measurements_cam1, views, model); SmartFactor::shared_ptr smartFactor2( - new SmartFactor(sharedBundlerK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY, - false, Pose3(), gtsam::HESSIAN)); + new SmartFactor(sharedBundlerK, boost::none, params)); smartFactor2->add(measurements_cam2, views, model); SmartFactor::shared_ptr smartFactor3( - new SmartFactor(sharedBundlerK, rankTol, -1, gtsam::ZERO_ON_DEGENERACY, - false, Pose3(), gtsam::HESSIAN)); + new SmartFactor(sharedBundlerK, boost::none, params)); smartFactor3->add(measurements_cam3, views, model); const SharedDiagonal noisePrior = noiseModel::Isotropic::Sigma(6, 0.10); @@ -1352,7 +1378,7 @@ TEST( SmartProjectionPoseFactor, Cal3BundlerRotationOnly ) { values.at(x3))); Values result; - LevenbergMarquardtOptimizer optimizer(graph, values, params); + LevenbergMarquardtOptimizer optimizer(graph, values, lmParams); result = optimizer.optimize(); EXPECT(