diff --git a/.cproject b/.cproject
index d5a6ca4d4..1783efa98 100644
--- a/.cproject
+++ b/.cproject
@@ -568,7 +568,6 @@
make
-
tests/testBayesTree.run
true
false
@@ -576,7 +575,6 @@
make
-
testBinaryBayesNet.run
true
false
@@ -624,7 +622,6 @@
make
-
testSymbolicBayesNet.run
true
false
@@ -632,7 +629,6 @@
make
-
tests/testSymbolicFactor.run
true
false
@@ -640,7 +636,6 @@
make
-
testSymbolicFactorGraph.run
true
false
@@ -656,12 +651,19 @@
make
-
tests/testBayesTree
true
false
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j5
@@ -678,6 +680,142 @@
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
+ timeCalibratedCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ timePinholeCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ timeStereoCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testCal3Unified.run
+ true
+ true
+ true
+
make
-j5
@@ -718,6 +856,62 @@
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
@@ -734,6 +928,30 @@
true
true
+
+ make
+ -j2
+ all
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean all
+ true
+ true
+ true
+
make
-j2
@@ -806,6 +1024,46 @@
true
true
+
+ make
+ -j5
+ testWrap.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
@@ -974,6 +1232,307 @@
true
true
+
+ make
+ -j5
+ testDiscreteFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j1
+ testDiscreteBayesTree.run
+ true
+ false
+ true
+
+
+ make
+ -j5
+ testDiscreteFactorGraph.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testDiscreteConditional.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testDiscreteMarginals.run
+ true
+ 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
+ testInvDepthCamera3.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testTriangulation.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
@@ -1080,7 +1639,6 @@
make
-
testErrors.run
true
false
@@ -1126,6 +1684,14 @@
true
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j5
@@ -1310,6 +1876,54 @@
true
true
+
+ make
+ -j2
+ install
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ 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
@@ -1318,6 +1932,70 @@
true
true
+
+ make
+ -j5
+ testAntiFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testBetweenFactor.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
@@ -1416,6 +2094,7 @@
make
+
testSimulated2DOriented.run
true
false
@@ -1455,6 +2134,7 @@
make
+
testSimulated2D.run
true
false
@@ -1462,6 +2142,7 @@
make
+
testSimulated3D.run
true
false
@@ -1515,1140 +2196,6 @@
false
true
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- tests/testLieConfig.run
- true
- true
- true
-
-
- make
- -j3
- install
- true
- false
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j1
- check
- true
- false
- true
-
-
- make
- -j5
- all
- true
- true
- true
-
-
- cmake
- ..
- true
- false
- true
-
-
- make
- -j5
- gtsam-shared
- true
- true
- true
-
-
- make
- -j5
- gtsam-static
- true
- true
- true
-
-
- make
- -j5
- timing
- true
- true
- true
-
-
- make
- -j5
- examples
- true
- true
- true
-
-
- make
- -j5
- VERBOSE=1 all
- true
- true
- true
-
-
- make
- -j5
- VERBOSE=1 check
- true
- true
- true
-
-
- make
- -j5
- check.base
- true
- true
- true
-
-
- make
- -j5
- timing.base
- true
- true
- true
-
-
- make
- -j2 VERBOSE=1
- check.geometry
- true
- false
- true
-
-
- make
- -j5
- timing.geometry
- true
- true
- true
-
-
- make
- -j2 VERBOSE=1
- check.inference
- true
- false
- true
-
-
- make
- -j5
- timing.inference
- true
- true
- true
-
-
- make
- -j2 VERBOSE=1
- check.linear
- true
- false
- true
-
-
- make
- -j5
- timing.linear
- true
- true
- true
-
-
- make
- -j2 VERBOSE=1
- check.nonlinear
- true
- false
- true
-
-
- make
- -j5
- timing.nonlinear
- true
- true
- true
-
-
- make
- -j2 VERBOSE=1
- check.slam
- true
- false
- true
-
-
- make
- -j5
- timing.slam
- true
- true
- true
-
-
- make
- -j5
- wrap_gtsam
- true
- true
- true
-
-
- make
- VERBOSE=1
- wrap_gtsam
- true
- false
- true
-
-
- cpack
-
- -G DEB
- true
- false
- true
-
-
- cpack
-
- -G RPM
- true
- false
- true
-
-
- cpack
-
- -G TGZ
- true
- false
- true
-
-
- cpack
-
- --config CPackSourceConfig.cmake
- true
- false
- true
-
-
- make
- -j5
- check.discrete
- true
- true
- true
-
-
- make
- -j5
- check.discrete_unstable
- true
- true
- true
-
-
- make
- -j5
- check.base_unstable
- true
- true
- true
-
-
- make
- -j5
- check.dynamics_unstable
- true
- true
- true
-
-
- make
- -j5
- check.slam_unstable
- true
- true
- true
-
-
- make
- -j5
- check.unstable
- true
- true
- true
-
-
- make
- -j5
- wrap_gtsam_build
- true
- true
- true
-
-
- make
- -j5
- wrap_gtsam_unstable_build
- true
- true
- true
-
-
- make
- -j5
- wrap_gtsam_unstable
- true
- true
- true
-
-
- make
- -j5
- wrap
- true
- true
- true
-
-
- make
- -j5
- wrap_gtsam_distclean
- true
- true
- true
-
-
- make
- -j5
- wrap_gtsam_unstable_distclean
- true
- true
- true
-
-
- make
- -j5
- doc
- true
- true
- true
-
-
- make
- -j5
- doc_clean
- true
- true
- true
-
-
- make
- -j5
- check
- true
- true
- true
-
-
- make
- -j5
- check.geometry_unstable
- true
- true
- true
-
-
- make
- -j5
- check.linear_unstable
- true
- true
- true
-
-
- make
- -j6 -j8
- gtsam_unstable-shared
- true
- true
- true
-
-
- make
- -j6 -j8
- gtsam_unstable-static
- true
- true
- true
-
-
- make
- -j6 -j8
- check.nonlinear_unstable
- true
- true
- true
-
-
- make
- -j5
- check.tests
- 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
- testGaussianFactor.run
- true
- 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
- timeCalibratedCamera.run
- true
- true
- true
-
-
- make
- -j5
- timePinholeCamera.run
- true
- true
- true
-
-
- make
- -j5
- timeStereoCamera.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
- -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
- testDiscreteFactor.run
- true
- true
- true
-
-
- make
- -j1
- testDiscreteBayesTree.run
- true
- false
- true
-
-
- make
- -j5
- testDiscreteFactorGraph.run
- true
- true
- true
-
-
- make
- -j5
- testDiscreteConditional.run
- true
- true
- true
-
-
- make
- -j5
- testDiscreteMarginals.run
- true
- true
- true
-
-
- make
- -j2
- vSFMexample.run
- true
- true
- true
-
-
- make
- -j2
- testVSLAMGraph
- true
- true
- true
-
-
- make
- -j5
- testInvDepthCamera3.run
- true
- true
- true
-
-
- make
- -j5
- testTriangulation.run
- true
- true
- true
-
-
- make
- -j5
- testMatrix.run
- true
- true
- true
-
-
- make
- -j5
- testVector.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
- -j2
- testGaussianFactor.run
- true
- true
- true
-
-
- make
- -j2
- install
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- 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
- -j5
- testAntiFactor.run
- true
- true
- true
-
-
- make
- -j5
- testBetweenFactor.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
-j2
@@ -2883,11 +2430,28 @@
make
+
tests/testGaussianISAM2
true
false
true
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testLieConfig.run
+ true
+ true
+ true
+
make
-j2
@@ -2984,6 +2548,401 @@
true
true
+
+ make
+ -j3
+ install
+ true
+ false
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j1
+ check
+ true
+ false
+ true
+
+
+ make
+ -j5
+ all
+ true
+ true
+ true
+
+
+ cmake
+ ..
+ true
+ false
+ true
+
+
+ make
+ -j5
+ gtsam-shared
+ true
+ true
+ true
+
+
+ make
+ -j5
+ gtsam-static
+ true
+ true
+ true
+
+
+ make
+ -j5
+ timing
+ true
+ true
+ true
+
+
+ make
+ -j5
+ examples
+ true
+ true
+ true
+
+
+ make
+ -j5
+ VERBOSE=1 all
+ true
+ true
+ true
+
+
+ make
+ -j5
+ VERBOSE=1 check
+ true
+ true
+ true
+
+
+ make
+ -j5
+ check.base
+ true
+ true
+ true
+
+
+ make
+ -j5
+ timing.base
+ true
+ true
+ true
+
+
+ make
+ -j2 VERBOSE=1
+ check.geometry
+ true
+ false
+ true
+
+
+ make
+ -j5
+ timing.geometry
+ true
+ true
+ true
+
+
+ make
+ -j2 VERBOSE=1
+ check.inference
+ true
+ false
+ true
+
+
+ make
+ -j5
+ timing.inference
+ true
+ true
+ true
+
+
+ make
+ -j2 VERBOSE=1
+ check.linear
+ true
+ false
+ true
+
+
+ make
+ -j5
+ timing.linear
+ true
+ true
+ true
+
+
+ make
+ -j2 VERBOSE=1
+ check.nonlinear
+ true
+ false
+ true
+
+
+ make
+ -j5
+ timing.nonlinear
+ true
+ true
+ true
+
+
+ make
+ -j2 VERBOSE=1
+ check.slam
+ true
+ false
+ true
+
+
+ make
+ -j5
+ timing.slam
+ true
+ true
+ true
+
+
+ make
+ -j5
+ wrap_gtsam
+ true
+ true
+ true
+
+
+ make
+ VERBOSE=1
+ wrap_gtsam
+ true
+ false
+ true
+
+
+ cpack
+ -G DEB
+ true
+ false
+ true
+
+
+ cpack
+ -G RPM
+ true
+ false
+ true
+
+
+ cpack
+ -G TGZ
+ true
+ false
+ true
+
+
+ cpack
+ --config CPackSourceConfig.cmake
+ true
+ false
+ true
+
+
+ make
+ -j5
+ check.discrete
+ true
+ true
+ true
+
+
+ make
+ -j5
+ check.discrete_unstable
+ true
+ true
+ true
+
+
+ make
+ -j5
+ check.base_unstable
+ true
+ true
+ true
+
+
+ make
+ -j5
+ check.dynamics_unstable
+ true
+ true
+ true
+
+
+ make
+ -j5
+ check.slam_unstable
+ true
+ true
+ true
+
+
+ make
+ -j5
+ check.unstable
+ true
+ true
+ true
+
+
+ make
+ -j5
+ wrap_gtsam_build
+ true
+ true
+ true
+
+
+ make
+ -j5
+ wrap_gtsam_unstable_build
+ true
+ true
+ true
+
+
+ make
+ -j5
+ wrap_gtsam_unstable
+ true
+ true
+ true
+
+
+ make
+ -j5
+ wrap
+ true
+ true
+ true
+
+
+ make
+ -j5
+ wrap_gtsam_distclean
+ true
+ true
+ true
+
+
+ make
+ -j5
+ wrap_gtsam_unstable_distclean
+ true
+ true
+ true
+
+
+ make
+ -j5
+ doc
+ true
+ true
+ true
+
+
+ make
+ -j5
+ doc_clean
+ true
+ true
+ true
+
+
+ make
+ -j5
+ check
+ true
+ true
+ true
+
+
+ make
+ -j5
+ check.geometry_unstable
+ true
+ true
+ true
+
+
+ make
+ -j5
+ check.linear_unstable
+ true
+ true
+ true
+
+
+ make
+ -j6 -j8
+ gtsam_unstable-shared
+ true
+ true
+ true
+
+
+ make
+ -j6 -j8
+ gtsam_unstable-static
+ true
+ true
+ true
+
+
+ make
+ -j6 -j8
+ check.nonlinear_unstable
+ true
+ true
+ true
+
+
+ make
+ -j5
+ check.tests
+ true
+ true
+ true
+
make
-j5
@@ -3008,6 +2967,45 @@
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
+
diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp
index e6c97cb1a..fa2495615 100644
--- a/gtsam/geometry/Cal3DS2.cpp
+++ b/gtsam/geometry/Cal3DS2.cpp
@@ -25,7 +25,7 @@ namespace gtsam {
/* ************************************************************************* */
Cal3DS2::Cal3DS2(const Vector &v):
- fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), k3_(v[7]), k4_(v[8]){}
+ fx_(v[0]), fy_(v[1]), s_(v[2]), u0_(v[3]), v0_(v[4]), k1_(v[5]), k2_(v[6]), p1_(v[7]), p2_(v[8]){}
/* ************************************************************************* */
Matrix Cal3DS2::K() const {
@@ -34,20 +34,20 @@ Matrix Cal3DS2::K() const {
/* ************************************************************************* */
Vector Cal3DS2::vector() const {
- return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_);
+ return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_);
}
/* ************************************************************************* */
-void Cal3DS2::print(const std::string& s) const {
- gtsam::print(K(), s + ".K");
- gtsam::print(Vector(k()), s + ".k");
+void Cal3DS2::print(const std::string& s_) const {
+ gtsam::print(K(), s_ + ".K");
+ gtsam::print(Vector(k()), s_ + ".k");
}
/* ************************************************************************* */
bool Cal3DS2::equals(const Cal3DS2& K, double tol) const {
if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
- fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol)
+ fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol)
return false;
return true;
}
@@ -57,10 +57,6 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
boost::optional H1,
boost::optional H2) const {
- // parameters
- const double fx = fx_, fy = fy_, s = s_;
- const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
-
// rr = x^2 + y^2;
// g = (1 + k(1)*rr + k(2)*rr^2);
// dp = [2*k(3)*x*y + k(4)*(rr + 2*x^2); 2*k(4)*x*y + k(3)*(rr + 2*y^2)];
@@ -68,40 +64,44 @@ Point2 Cal3DS2::uncalibrate(const Point2& p,
const double x = p.x(), y = p.y(), xy = x * y, xx = x * x, yy = y * y;
const double rr = xx + yy;
const double r4 = rr * rr;
- const double g = 1. + k1 * rr + k2 * r4;
- const double dx = 2. * k3 * xy + k4 * (rr + 2. * xx);
- const double dy = 2. * k4 * xy + k3 * (rr + 2. * yy);
+ const double g = 1. + k1_ * rr + k2_ * r4; // scaling factor
+ // tangential component
+ const double dx = 2. * p1_ * xy + p2_ * (rr + 2. * xx);
+ const double dy = 2. * p2_ * xy + p1_ * (rr + 2. * yy);
+
+ // Radial and tangential distortion applied
const double pnx = g*x + dx;
const double pny = g*y + dy;
// Inlined derivative for calibration
if (H1) {
- *H1 = (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr,
- fx * x * r4 + s * y * r4, fx * 2. * xy + s * (rr + 2. * yy),
- fx * (rr + 2. * xx) + s * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0,
- fy * y * rr, fy * y * r4, fy * (rr + 2. * yy), fy * (2. * xy));
+ *H1 = (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx_ * x * rr + s_ * y * rr,
+ fx_ * x * r4 + s_ * y * r4, fx_ * 2. * xy + s_ * (rr + 2. * yy),
+ fx_ * (rr + 2. * xx) + s_ * (2. * xy), 0.0, pny, 0.0, 0.0, 1.0,
+ fy_ * y * rr, fy_ * y * r4, fy_ * (rr + 2. * yy), fy_ * (2. * xy));
}
// Inlined derivative for points
if (H2) {
const double dr_dx = 2. * x;
const double dr_dy = 2. * y;
- const double dg_dx = k1 * dr_dx + k2 * 2. * rr * dr_dx;
- const double dg_dy = k1 * dr_dy + k2 * 2. * rr * dr_dy;
+ const double dg_dx = k1_ * dr_dx + k2_ * 2. * rr * dr_dx;
+ const double dg_dy = k1_ * dr_dy + k2_ * 2. * rr * dr_dy;
- const double dDx_dx = 2. * k3 * y + k4 * (dr_dx + 4. * x);
- const double dDx_dy = 2. * k3 * x + k4 * dr_dy;
- const double dDy_dx = 2. * k4 * y + k3 * dr_dx;
- const double dDy_dy = 2. * k4 * x + k3 * (dr_dy + 4. * y);
+ const double dDx_dx = 2. * p1_ * y + p2_ * (dr_dx + 4. * x);
+ const double dDx_dy = 2. * p1_ * x + p2_ * dr_dy;
+ const double dDy_dx = 2. * p2_ * y + p1_ * dr_dx;
+ const double dDy_dy = 2. * p2_ * x + p1_ * (dr_dy + 4. * y);
- Matrix DK = (Matrix(2, 2) << fx, s_, 0.0, fy);
+ Matrix DK = (Matrix(2, 2) << fx_, s_, 0.0, fy_);
Matrix DR = (Matrix(2, 2) << g + x * dg_dx + dDx_dx, x * dg_dy + dDx_dy,
y * dg_dx + dDy_dx, g + y * dg_dy + dDy_dy);
*H2 = DK * DR;
}
- return Point2(fx * pnx + s * pny + u0_, fy * pny + v0_);
+ // Regular uncalibrate after distortion
+ return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_);
}
/* ************************************************************************* */
@@ -123,8 +123,8 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
const double x = pn.x(), y = pn.y(), xy = x*y, xx = x*x, yy = y*y;
const double rr = xx + yy;
const double g = (1+k1_*rr+k2_*rr*rr);
- const double dx = 2*k3_*xy + k4_*(rr+2*xx);
- const double dy = 2*k4_*xy + k3_*(rr+2*yy);
+ const double dx = 2*p1_*xy + p2_*(rr+2*xx);
+ const double dy = 2*p2_*xy + p1_*(rr+2*yy);
pn = (invKPi - Point2(dx,dy))/g;
}
@@ -136,24 +136,21 @@ Point2 Cal3DS2::calibrate(const Point2& pi, const double tol) const {
/* ************************************************************************* */
Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const {
- //const double fx = fx_, fy = fy_, s = s_;
- const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_;
- //const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y;
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y;
const double rr = xx + yy;
const double dr_dx = 2*x;
const double dr_dy = 2*y;
const double r4 = rr*rr;
- const double g = 1 + k1*rr + k2*r4;
- const double dg_dx = k1*dr_dx + k2*2*rr*dr_dx;
- const double dg_dy = k1*dr_dy + k2*2*rr*dr_dy;
+ const double g = 1 + k1_*rr + k2_*r4;
+ const double dg_dx = k1_*dr_dx + k2_*2*rr*dr_dx;
+ const double dg_dy = k1_*dr_dy + k2_*2*rr*dr_dy;
- // Dx = 2*k3*xy + k4*(rr+2*xx);
- // Dy = 2*k4*xy + k3*(rr+2*yy);
- const double dDx_dx = 2*k3*y + k4*(dr_dx + 4*x);
- const double dDx_dy = 2*k3*x + k4*dr_dy;
- const double dDy_dx = 2*k4*y + k3*dr_dx;
- const double dDy_dy = 2*k4*x + k3*(dr_dy + 4*y);
+ // Dx = 2*p1_*xy + p2_*(rr+2*xx);
+ // Dy = 2*p2_*xy + p1_*(rr+2*yy);
+ const double dDx_dx = 2*p1_*y + p2_*(dr_dx + 4*x);
+ const double dDx_dy = 2*p1_*x + p2_*dr_dy;
+ const double dDy_dx = 2*p2_*y + p1_*dr_dx;
+ const double dDy_dy = 2*p2_*x + p1_*(dr_dy + 4*y);
Matrix DK = (Matrix(2, 2) << fx_, s_, 0.0, fy_);
Matrix DR = (Matrix(2, 2) << g + x*dg_dx + dDx_dx, x*dg_dy + dDx_dy,
@@ -167,16 +164,15 @@ Matrix Cal3DS2::D2d_calibration(const Point2& p) const {
const double x = p.x(), y = p.y(), xx = x*x, yy = y*y, xy = x*y;
const double rr = xx + yy;
const double r4 = rr*rr;
- const double fx = fx_, fy = fy_, s = s_;
const double g = (1+k1_*rr+k2_*r4);
- const double dx = 2*k3_*xy + k4_*(rr+2*xx);
- const double dy = 2*k4_*xy + k3_*(rr+2*yy);
+ const double dx = 2*p1_*xy + p2_*(rr+2*xx);
+ const double dy = 2*p2_*xy + p1_*(rr+2*yy);
const double pnx = g*x + dx;
const double pny = g*y + dy;
return (Matrix(2, 9) <<
- pnx, 0.0, pny, 1.0, 0.0, fx*x*rr + s*y*rr, fx*x*r4 + s*y*r4, fx*2*xy + s*(rr+2*yy), fx*(rr+2*xx) + s*(2*xy),
- 0.0, pny, 0.0, 0.0, 1.0, fy*y*rr , fy*y*r4 , fy*(rr+2*yy) , fy*(2*xy) );
+ pnx, 0.0, pny, 1.0, 0.0, fx_*x*rr + s_*y*rr, fx_*x*r4 + s_*y*r4, fx_*2*xy + s_*(rr+2*yy), fx_*(rr+2*xx) + s_*(2*xy),
+ 0.0, pny, 0.0, 0.0, 1.0, fy_*y*rr , fy_*y*r4 , fy_*(rr+2*yy) , fy_*(2*xy) );
}
/* ************************************************************************* */
diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h
index ced05086b..f796b708b 100644
--- a/gtsam/geometry/Cal3DS2.h
+++ b/gtsam/geometry/Cal3DS2.h
@@ -27,6 +27,15 @@ namespace gtsam {
* @brief Calibration of a camera with radial distortion
* @addtogroup geometry
* \nosubgrouping
+ *
+ * Uses same distortionmodel as OpenCV, with
+ * http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
+ * but using only k1,k2,p1, and p2 coefficients.
+ * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
+ * rr = Pn.x^2 + Pn.y^2
+ * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
+ * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
+ * pi = K*pn
*/
class GTSAM_EXPORT Cal3DS2 : public DerivedValue {
@@ -34,28 +43,22 @@ protected:
double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point
double k1_, k2_ ; // radial 2nd-order and 4th-order
- double k3_, k4_ ; // tangential distortion
-
- // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
- // rr = Pn.x^2 + Pn.y^2
- // \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
- // k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
- // pi = K*pn
+ double p1_, p2_ ; // tangential distortion
public:
Matrix K() const ;
- Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, k3_, k4_); }
+ Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, p1_, p2_); }
Vector vector() const ;
/// @name Standard Constructors
/// @{
/// Default Constructor with only unit focal length
- Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {}
+ Cal3DS2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {}
Cal3DS2(double fx, double fy, double s, double u0, double v0,
- double k1, double k2, double k3 = 0.0, double k4 = 0.0) :
- fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {}
+ double k1, double k2, double p1 = 0.0, double p2 = 0.0) :
+ fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), p1_(p1), p2_(p2) {}
/// @}
/// @name Advanced Constructors
@@ -92,18 +95,30 @@ public:
/// image center in y
inline double py() const { return v0_;}
+ /// image center in x
+ inline double k1() const { return k1_;}
+
+ /// image center in y
+ inline double k2() const { return k2_;}
+
+ /// image center in x
+ inline double p1() const { return p1_;}
+
+ /// image center in y
+ inline double p2() const { return p2_;}
+
/**
- * convert intrinsic coordinates xy to image coordinates uv
+ * convert intrinsic coordinates xy to (distorted) image coordinates uv
* @param p point in intrinsic coordinates
* @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters
* @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates
- * @return point in image coordinates
+ * @return point in (distorted) image coordinates
*/
Point2 uncalibrate(const Point2& p,
boost::optional Dcal = boost::none,
boost::optional Dp = boost::none) const ;
- /// Conver a pixel coordinate to ideal coordinate
+ /// Convert (distorted) image coordinates uv to intrinsic coordinates xy
Point2 calibrate(const Point2& p, const double tol=1e-5) const;
/// Derivative of uncalibrate wrpt intrinsic coordinates
@@ -148,8 +163,8 @@ private:
ar & BOOST_SERIALIZATION_NVP(v0_);
ar & BOOST_SERIALIZATION_NVP(k1_);
ar & BOOST_SERIALIZATION_NVP(k2_);
- ar & BOOST_SERIALIZATION_NVP(k3_);
- ar & BOOST_SERIALIZATION_NVP(k4_);
+ ar & BOOST_SERIALIZATION_NVP(p1_);
+ ar & BOOST_SERIALIZATION_NVP(p2_);
}
diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp
index 808cb84a4..e7b408982 100644
--- a/gtsam/geometry/Cal3Unified.cpp
+++ b/gtsam/geometry/Cal3Unified.cpp
@@ -43,7 +43,7 @@ void Cal3Unified::print(const std::string& s) const {
bool Cal3Unified::equals(const Cal3Unified& K, double tol) const {
if (fabs(fx_ - K.fx_) > tol || fabs(fy_ - K.fy_) > tol || fabs(s_ - K.s_) > tol ||
fabs(u0_ - K.u0_) > tol || fabs(v0_ - K.v0_) > tol || fabs(k1_ - K.k1_) > tol ||
- fabs(k2_ - K.k2_) > tol || fabs(k3_ - K.k3_) > tol || fabs(k4_ - K.k4_) > tol ||
+ fabs(k2_ - K.k2_) > tol || fabs(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol ||
fabs(xi_ - K.xi_) > tol)
return false;
return true;
diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h
index a1d47332a..58e024c27 100644
--- a/gtsam/geometry/Cal3Unified.h
+++ b/gtsam/geometry/Cal3Unified.h
@@ -31,6 +31,14 @@ namespace gtsam {
* @brief Calibration of a omni-directional camera with mirror + lens radial distortion
* @addtogroup geometry
* \nosubgrouping
+ *
+ * Similar to Cal3DS2, does distortion but has additional mirror parameter xi
+ * K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
+ * Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})]
+ * rr = Pn.x^2 + Pn.y^2
+ * \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
+ * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
+ * pi = K*pn
*/
class GTSAM_EXPORT Cal3Unified : public Cal3DS2 {
@@ -41,13 +49,6 @@ private:
double xi_; // mirror parameter
- // K = [ fx s u0 ; 0 fy v0 ; 0 0 1 ]
- // Pn = [ P.x / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1}), P.y / (1 + xi * \sqrt{P.x^2 + P.y^2 + 1})]
- // rr = Pn.x^2 + Pn.y^2
- // \hat{pn} = (1 + k1*rr + k2*rr^2 ) pn + [ 2*k3 pn.x pn.y + k4 (rr + 2 Pn.x^2) ;
- // k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ]
- // pi = K*pn
-
public:
//Matrix K() const ;
//Eigen::Vector4d k() const { return Base::k(); }
@@ -60,8 +61,8 @@ public:
Cal3Unified() : Base(), xi_(0) {}
Cal3Unified(double fx, double fy, double s, double u0, double v0,
- double k1, double k2, double k3 = 0.0, double k4 = 0.0, double xi = 0.0) :
- Base(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {}
+ double k1, double k2, double p1 = 0.0, double p2 = 0.0, double xi = 0.0) :
+ Base(fx, fy, s, u0, v0, k1, k2, p1, p2), xi_(xi) {}
/// @}
/// @name Advanced Constructors