From ea0dbf05ef919c343de91e2058309eb85a5107dd Mon Sep 17 00:00:00 2001 From: dellaert Date: Wed, 18 Jun 2014 14:36:09 -0400 Subject: [PATCH] Name change from k3,k4 to p1,p2 as in OpenCV --- .cproject | 2280 ++++++++++++++++---------------- gtsam/geometry/Cal3DS2.cpp | 84 +- gtsam/geometry/Cal3DS2.h | 47 +- gtsam/geometry/Cal3Unified.cpp | 2 +- gtsam/geometry/Cal3Unified.h | 19 +- 5 files changed, 1221 insertions(+), 1211 deletions(-) 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