From 1e76082888d316503174e12738b76c47ceb11f3e Mon Sep 17 00:00:00 2001 From: jing Date: Sun, 9 Mar 2014 00:40:02 -0500 Subject: [PATCH 01/10] add unified projection model: a calibration class for omni-directional camera. Not test yet --- gtsam/geometry/Cal3Unify.cpp | 201 +++++++++++++++++++++++++++++++++++ gtsam/geometry/Cal3Unify.h | 168 +++++++++++++++++++++++++++++ 2 files changed, 369 insertions(+) create mode 100644 gtsam/geometry/Cal3Unify.cpp create mode 100644 gtsam/geometry/Cal3Unify.h diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp new file mode 100644 index 000000000..d49417492 --- /dev/null +++ b/gtsam/geometry/Cal3Unify.cpp @@ -0,0 +1,201 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Cal3Unify.cpp + * @date Mar 8, 2014 + * @author Jing Dong + */ + +#include +#include +#include +#include + +#include + +namespace gtsam { + +/* ************************************************************************* */ +Cal3Unify::Cal3Unify(const Vector &v): + xi_(v[0]), fx_(v[1]), fy_(v[2]), s_(v[3]), u0_(v[4]), v0_(v[5]), k1_(v[6]), k2_(v[7]), k3_(v[8]), k4_(v[9]){} + +/* ************************************************************************* */ +Matrix Cal3Unify::K() const { + return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); +} + +/* ************************************************************************* */ +Vector Cal3Unify::vector() const { + return (Vector(10) << xi_, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); +} + +/* ************************************************************************* */ +void Cal3Unify::print(const std::string& s) const { + gtsam::print(K(), s + ".K"); + gtsam::print(Vector(k()), s + ".k"); + gtsam::print(Vector(xi_), s + ".xi"); +} + +/* ************************************************************************* */ +bool Cal3Unify::equals(const Cal3Unify& 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(xi_ - K.xi_) > tol) + return false; + return true; +} + +/* ************************************************************************* */ +Point2 Cal3Unify::uncalibrate(const Point2& p, + boost::optional H1, + boost::optional H2) const { + + // this part of code is modified from Cal3DS2, + // since the second part of this model (after project to normalized plane) + // is same as Cal3DS2 + + // parameters + const double xi = xi_, fx = fx_, fy = fy_, s = s_; + const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_; + + // Part1: project 3D space to NPlane + const double xs = p.x(), ys = p.y(); // normalized points in 3D space + const double sqrt_nx = sqrt(xs * xs + ys * ys + 1.0); + const double xi_sqrt_nx = 1 + xi * sqrt_nx; + const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx; + const double x = xs / xi_sqrt_nx, y = ys / xi_sqrt_nx; // points on NPlane + + // Part2: project NPlane point to pixel plane: same as Cal3DS2 + const double 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 pnx = g*x + dx; + const double pny = g*y + dy; + + // DDS2 will be used both in H1 and H2 + Matrix DDS2; + if (H1 || H2) { + // part2 + 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 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); + + 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); + + DDS2 = DK * DR; + } + + // Inlined derivative for calibration + if (H1) { + // part1 + Matrix DU = (Matrix(2,1) << xs * sqrt_nx / xi_sqrt_nx2, + ys * sqrt_nx / xi_sqrt_nx2); + Matrix DDS2U = DDS2 * DU; + // part2 + Matrix DDS2V = (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 = collect(2, &DDS2U, &DDS2V); + } + // Inlined derivative for points + if (H2) { + // part1 + Matrix DU = (Matrix(2, 2) << (xi_sqrt_nx - xs * xs / sqrt_nx) / xi_sqrt_nx2, + -(ys * ys / (sqrt_nx * xi_sqrt_nx2)), + -(xs * xs / (sqrt_nx * xi_sqrt_nx2)), + (xi_sqrt_nx - ys * ys / sqrt_nx) / xi_sqrt_nx2); + + *H2 = DDS2 * DU; + } + + + return Point2(fx * pnx + s * pny + u0_, fy * pny + v0_); +} + +/* ************************************************************************* */ +Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { + // Use the following fixed point iteration to invert the radial distortion. + // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t}) + + // point on the normalized plane, input for DS2 + Point2 pnpl = this->imageToNPlane(pi); + double px = pnpl.x(); + double py = pnpl.y(); + const Point2 invKPi ((1 / fx_) * (px - u0_ - (s_ / fy_) * (py - v0_)), + (1 / fy_) * (py - v0_)); + + // initialize by ignoring the distortion at all, might be problematic for pixels around boundary + Point2 pn = invKPi; + + // iterate until the uncalibrate is close to the actual pixel coordinate + const int maxIterations = 10; + int iteration; + for ( iteration = 0; iteration < maxIterations; ++iteration ) { + + if ( uncalibrate(pn).distance(pi) <= tol ) break; + + // part1: image -> normalized plane + pnpl = this->imageToNPlane(pn); + // part2: normalized plane -> 3D space + px = pnpl.x(), py = pnpl.y(); + const double xy = px*py, xx = px*px, yy = py*py; + 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); + pn = (invKPi - Point2(dx,dy))/g; + } + + if ( iteration >= maxIterations ) + throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization"); + + return pn; +} +/* ************************************************************************* */ +Point2 Cal3Unify::imageToNPlane(const Point2& p) const { + + const double x = p.x(), y = p.y(); + const double xy2 = x * x + y * y; + const double sq_xy = (xi_ + sqrt(1 + (1 - xi_ * xi_) * xy2)) / (xy2 + 1); + + return Point2((sq_xy * x / (sq_xy - xi_)), (sq_xy * y / (sq_xy - xi_))); +} + +/* ************************************************************************* */ +Cal3Unify Cal3Unify::retract(const Vector& d) const { + return Cal3Unify(vector() + d); +} + +/* ************************************************************************* */ +Vector Cal3Unify::localCoordinates(const Cal3Unify& T2) const { + return T2.vector() - vector(); +} + +} +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/Cal3Unify.h b/gtsam/geometry/Cal3Unify.h new file mode 100644 index 000000000..58f19ece5 --- /dev/null +++ b/gtsam/geometry/Cal3Unify.h @@ -0,0 +1,168 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file Cal3Unify.h + * @brief Unified Calibration Model, see Mei07icra for details + * @date Mar 8, 2014 + * @author Jing Dong + */ + +/** + * @addtogroup geometry + */ + +#pragma once + +#include +#include + +namespace gtsam { + +/** + * @brief Calibration of a omni-directional camera with mirror + lens radial distortion + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Cal3Unify : public DerivedValue { + +private: + + double xi_; // mirror parameter + 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 ] + // 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 Eigen::Vector4d(k1_, k2_, k3_, k4_); } + Vector vector() const ; + + /// @name Standard Constructors + /// @{ + + /// Default Constructor with only unit focal length + Cal3Unify() : xi_(0), fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {} + + Cal3Unify(double xi, double fx, double fy, double s, double u0, double v0, + double k1, double k2, double k3 = 0.0, double k4 = 0.0) : + xi_(xi), fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {} + + /// @} + /// @name Advanced Constructors + /// @{ + + Cal3Unify(const Vector &v) ; + + /// @} + /// @name Testable + /// @{ + + /// print with optional string + void print(const std::string& s = "") const ; + + /// assert equality up to a tolerance + bool equals(const Cal3Unify& K, double tol = 10e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// mirror parameter + inline double xi() const { return xi_;} + + /// focal length x + inline double fx() const { return fx_;} + + /// focal length y + inline double fy() const { return fy_;} + + /// skew + inline double skew() const { return s_;} + + /// image center in x + inline double px() const { return u0_;} + + /// image center in y + inline double py() const { return v0_;} + + /** + * convert intrinsic coordinates xy to 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 + */ + Point2 uncalibrate(const Point2& p, + boost::optional Dcal = boost::none, + boost::optional Dp = boost::none) const ; + + /// Conver a pixel coordinate to ideal coordinate + Point2 calibrate(const Point2& p, const double tol=1e-5) const; + + /// Convert a image point to normalized unit plane + Point2 imageToNPlane(const Point2& p) const; + + /// @} + /// @name Manifold + /// @{ + + /// Given delta vector, update calibration + Cal3Unify retract(const Vector& d) const ; + + /// Given a different calibration, calculate update to obtain it + Vector localCoordinates(const Cal3Unify& T2) const ; + + /// Return dimensions of calibration manifold object + virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) + + /// Return dimensions of calibration manifold object + static size_t Dim() { return 10; } //TODO: make a final dimension variable + +private: + + /// @} + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + ar & boost::serialization::make_nvp("Cal3Unify", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(xi_); + ar & BOOST_SERIALIZATION_NVP(fx_); + ar & BOOST_SERIALIZATION_NVP(fy_); + ar & BOOST_SERIALIZATION_NVP(s_); + ar & BOOST_SERIALIZATION_NVP(u0_); + 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_); + } + + + /// @} + +}; + +} + From e311e993ad8956ca64f3d48d1b3fd8f9745544e5 Mon Sep 17 00:00:00 2001 From: jing Date: Sun, 9 Mar 2014 01:18:06 -0500 Subject: [PATCH 02/10] add unit test --- gtsam/geometry/tests/testCal3Unify.cpp | 91 ++++++++++++++++++++++++++ 1 file changed, 91 insertions(+) create mode 100644 gtsam/geometry/tests/testCal3Unify.cpp diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp new file mode 100644 index 000000000..775f95159 --- /dev/null +++ b/gtsam/geometry/tests/testCal3Unify.cpp @@ -0,0 +1,91 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testCal3Unify.cpp + * @brief Unit tests for transform derivatives + */ + +#include +#include +#include +#include + +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(Cal3Unify) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unify) + +/* ground truth data will from matlab, code : +X = [2 3 1]'; +V = [0.01, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; +[P, J] = spaceToImgPlane(X, V); +matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox */ + +static Cal3Unify K(0.01, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); +static Point2 p(2,3); + +/* ************************************************************************* */ +TEST( Cal3Unify, uncalibrate) +{ + Point2 p_i(582.5228344366194, 649.6685266099726) ; + Point2 q = K.uncalibrate(p); + CHECK(assert_equal(q,p_i)); +} + +TEST( Cal3Unify, calibrate ) +{ + Point2 pi = K.uncalibrate(p); + Point2 pn_hat = K.calibrate(pi); + CHECK( p.equals(pn_hat, 1e-5)); +} + +Point2 uncalibrate_(const Cal3Unify& k, const Point2& pt) { return k.uncalibrate(pt); } + +/* ************************************************************************* */ +TEST( Cal3Unify, Duncalibrate1) +{ + Matrix computed; + K.uncalibrate(p, computed, boost::none); + Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); + CHECK(assert_equal(numerical,computed,1e-5)); +} + +/* ************************************************************************* */ +TEST( Cal3Unify, Duncalibrate2) +{ + Matrix computed; + K.uncalibrate(p, boost::none, computed); + Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); + CHECK(assert_equal(numerical,computed,1e-5)); +} + +/* ************************************************************************* */ +TEST( Cal3Unify, assert_equal) +{ + CHECK(assert_equal(K,K,1e-5)); +} + +/* ************************************************************************* */ +TEST( Cal3Unify, retract) +{ + Cal3Unify expected(0.01 + 1, 100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10); + Vector d(10); + d << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10; + Cal3Unify actual = K.retract(d); + CHECK(assert_equal(expected,actual,1e-7)); + CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); +} + +/* ************************************************************************* */ +int main() { TestResult tr; return TestRegistry::runAllTests(tr); } +/* ************************************************************************* */ From b43b84b59a20e2fc98907e613f1ca13c0ae56b3d Mon Sep 17 00:00:00 2001 From: jing Date: Sun, 9 Mar 2014 03:34:57 -0400 Subject: [PATCH 03/10] some bug fixed, still not pass unit test: calibrate() converge so slow, and Jacobians are buggy --- gtsam/geometry/Cal3Unify.cpp | 31 +++++++++++++++++--------- gtsam/geometry/Cal3Unify.h | 7 ++++-- gtsam/geometry/tests/testCal3Unify.cpp | 22 +++++++++++++----- 3 files changed, 42 insertions(+), 18 deletions(-) diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp index d49417492..1ddbcd2b2 100644 --- a/gtsam/geometry/Cal3Unify.cpp +++ b/gtsam/geometry/Cal3Unify.cpp @@ -142,24 +142,24 @@ Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t}) // point on the normalized plane, input for DS2 - Point2 pnpl = this->imageToNPlane(pi); - double px = pnpl.x(); - double py = pnpl.y(); + double px = pi.x(); + double py = pi.y(); const Point2 invKPi ((1 / fx_) * (px - u0_ - (s_ / fy_) * (py - v0_)), (1 / fy_) * (py - v0_)); // initialize by ignoring the distortion at all, might be problematic for pixels around boundary - Point2 pn = invKPi; + Point2 pn = nPlaneToSpace(invKPi); // iterate until the uncalibrate is close to the actual pixel coordinate - const int maxIterations = 10; + const int maxIterations = 100; int iteration; for ( iteration = 0; iteration < maxIterations; ++iteration ) { - if ( uncalibrate(pn).distance(pi) <= tol ) break; + pn.print(); + if ( pn.distance(pi) <= tol ) break; - // part1: image -> normalized plane - pnpl = this->imageToNPlane(pn); + // part1: 3D space -> normalized plane + Point2 pnpl = spaceToNPlane(pn); // part2: normalized plane -> 3D space px = pnpl.x(), py = pnpl.y(); const double xy = px*py, xx = px*px, yy = py*py; @@ -167,16 +167,16 @@ Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { 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); - pn = (invKPi - Point2(dx,dy))/g; + pn = nPlaneToSpace((invKPi - Point2(dx,dy))/g); } if ( iteration >= maxIterations ) - throw std::runtime_error("Cal3DS2::calibrate fails to converge. need a better initialization"); + throw std::runtime_error("Cal3Unify::calibrate fails to converge. need a better initialization"); return pn; } /* ************************************************************************* */ -Point2 Cal3Unify::imageToNPlane(const Point2& p) const { +Point2 Cal3Unify::nPlaneToSpace(const Point2& p) const { const double x = p.x(), y = p.y(); const double xy2 = x * x + y * y; @@ -185,6 +185,15 @@ Point2 Cal3Unify::imageToNPlane(const Point2& p) const { return Point2((sq_xy * x / (sq_xy - xi_)), (sq_xy * y / (sq_xy - xi_))); } +/* ************************************************************************* */ +Point2 Cal3Unify::spaceToNPlane(const Point2& p) const { + + const double x = p.x(), y = p.y(); + const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1); + + return Point2((x / sq_xy), (y / sq_xy)); +} + /* ************************************************************************* */ Cal3Unify Cal3Unify::retract(const Vector& d) const { return Cal3Unify(vector() + d); diff --git a/gtsam/geometry/Cal3Unify.h b/gtsam/geometry/Cal3Unify.h index 58f19ece5..4d70db051 100644 --- a/gtsam/geometry/Cal3Unify.h +++ b/gtsam/geometry/Cal3Unify.h @@ -115,8 +115,11 @@ public: /// Conver a pixel coordinate to ideal coordinate Point2 calibrate(const Point2& p, const double tol=1e-5) const; - /// Convert a image point to normalized unit plane - Point2 imageToNPlane(const Point2& p) const; + /// Convert a 3D point to normalized unit plane + Point2 spaceToNPlane(const Point2& p) const; + + /// Convert a normalized unit plane point to 3D space + Point2 nPlaneToSpace(const Point2& p) const; /// @} /// @name Manifold diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp index 775f95159..9ff9d7646 100644 --- a/gtsam/geometry/tests/testCal3Unify.cpp +++ b/gtsam/geometry/tests/testCal3Unify.cpp @@ -19,16 +19,20 @@ #include #include -using namespace gtsam; +#include +using namespace gtsam; +using namespace std; GTSAM_CONCEPT_TESTABLE_INST(Cal3Unify) GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unify) -/* ground truth data will from matlab, code : +/* +ground truth from matlab, code : X = [2 3 1]'; V = [0.01, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; [P, J] = spaceToImgPlane(X, V); -matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox */ +matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox +*/ static Cal3Unify K(0.01, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); static Point2 p(2,3); @@ -41,10 +45,18 @@ TEST( Cal3Unify, uncalibrate) CHECK(assert_equal(q,p_i)); } -TEST( Cal3Unify, calibrate ) +/* ************************************************************************* */ +TEST( Cal3Unify, spaceNplane) +{ + Point2 q = K.spaceToNPlane(p); + CHECK(assert_equal(p, K.nPlaneToSpace(q))); +} + +/* ************************************************************************* */ +TEST( Cal3Unify, calibrate) { Point2 pi = K.uncalibrate(p); - Point2 pn_hat = K.calibrate(pi); + Point2 pn_hat = K.calibrate(pi, 1); CHECK( p.equals(pn_hat, 1e-5)); } From 4affb9a0d2d9a5ecc0141f6f0f4e7ce6d3cae4b4 Mon Sep 17 00:00:00 2001 From: jing Date: Mon, 10 Mar 2014 16:27:41 -0400 Subject: [PATCH 04/10] fix jacobians, calibration still buggy --- gtsam/geometry/Cal3Unify.cpp | 19 ++++++++++--------- gtsam/geometry/tests/testCal3Unify.cpp | 6 +++--- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp index 1ddbcd2b2..5f2e330bd 100644 --- a/gtsam/geometry/Cal3Unify.cpp +++ b/gtsam/geometry/Cal3Unify.cpp @@ -42,7 +42,7 @@ Vector Cal3Unify::vector() const { void Cal3Unify::print(const std::string& s) const { gtsam::print(K(), s + ".K"); gtsam::print(Vector(k()), s + ".k"); - gtsam::print(Vector(xi_), s + ".xi"); + gtsam::print((Vector)(Vector(1) << xi_), s + ".xi"); } /* ************************************************************************* */ @@ -71,9 +71,9 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, // Part1: project 3D space to NPlane const double xs = p.x(), ys = p.y(); // normalized points in 3D space const double sqrt_nx = sqrt(xs * xs + ys * ys + 1.0); - const double xi_sqrt_nx = 1 + xi * sqrt_nx; + const double xi_sqrt_nx = 1.0 / (1 + xi * sqrt_nx); const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx; - const double x = xs / xi_sqrt_nx, y = ys / xi_sqrt_nx; // points on NPlane + const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane // Part2: project NPlane point to pixel plane: same as Cal3DS2 const double xy = x * y, xx = x * x, yy = y * y; @@ -110,8 +110,8 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, // Inlined derivative for calibration if (H1) { // part1 - Matrix DU = (Matrix(2,1) << xs * sqrt_nx / xi_sqrt_nx2, - ys * sqrt_nx / xi_sqrt_nx2); + Matrix DU = (Matrix(2,1) << -xs * sqrt_nx * xi_sqrt_nx2, + -ys * sqrt_nx * xi_sqrt_nx2); Matrix DDS2U = DDS2 * DU; // part2 Matrix DDS2V = (Matrix(2, 9) << pnx, 0.0, pny, 1.0, 0.0, fx * x * rr + s * y * rr, @@ -124,10 +124,11 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, // Inlined derivative for points if (H2) { // part1 - Matrix DU = (Matrix(2, 2) << (xi_sqrt_nx - xs * xs / sqrt_nx) / xi_sqrt_nx2, - -(ys * ys / (sqrt_nx * xi_sqrt_nx2)), - -(xs * xs / (sqrt_nx * xi_sqrt_nx2)), - (xi_sqrt_nx - ys * ys / sqrt_nx) / xi_sqrt_nx2); + const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx; + const double mid = -(xi * xs*ys) * denom; + Matrix DU = (Matrix(2, 2) << + (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, + mid, (sqrt_nx + xi*(xs*xs + 1)) * denom); *H2 = DDS2 * DU; } diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp index 9ff9d7646..3995e03b2 100644 --- a/gtsam/geometry/tests/testCal3Unify.cpp +++ b/gtsam/geometry/tests/testCal3Unify.cpp @@ -35,7 +35,7 @@ matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox */ static Cal3Unify K(0.01, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); -static Point2 p(2,3); +static Point2 p(2, 3); /* ************************************************************************* */ TEST( Cal3Unify, uncalibrate) @@ -68,7 +68,7 @@ TEST( Cal3Unify, Duncalibrate1) Matrix computed; K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical,computed,1e-5)); + CHECK(assert_equal(numerical,computed,1e-6)); } /* ************************************************************************* */ @@ -77,7 +77,7 @@ TEST( Cal3Unify, Duncalibrate2) Matrix computed; K.uncalibrate(p, boost::none, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical,computed,1e-5)); + CHECK(assert_equal(numerical,computed,1e-6)); } /* ************************************************************************* */ From 7ff7482792b9586682269a486a0612c380ffa86e Mon Sep 17 00:00:00 2001 From: jing Date: Mon, 10 Mar 2014 17:09:56 -0400 Subject: [PATCH 05/10] all unit test failure fixed --- gtsam/geometry/Cal3Unify.cpp | 5 ++--- gtsam/geometry/tests/testCal3Unify.cpp | 23 +++++++++++------------ 2 files changed, 13 insertions(+), 15 deletions(-) diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp index 5f2e330bd..8e200ff2a 100644 --- a/gtsam/geometry/Cal3Unify.cpp +++ b/gtsam/geometry/Cal3Unify.cpp @@ -152,12 +152,11 @@ Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { Point2 pn = nPlaneToSpace(invKPi); // iterate until the uncalibrate is close to the actual pixel coordinate - const int maxIterations = 100; + const int maxIterations = 20; int iteration; for ( iteration = 0; iteration < maxIterations; ++iteration ) { - pn.print(); - if ( pn.distance(pi) <= tol ) break; + if ( uncalibrate(pn).distance(pi) <= tol ) break; // part1: 3D space -> normalized plane Point2 pnpl = spaceToNPlane(pn); diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp index 3995e03b2..370877ef3 100644 --- a/gtsam/geometry/tests/testCal3Unify.cpp +++ b/gtsam/geometry/tests/testCal3Unify.cpp @@ -19,28 +19,26 @@ #include #include - -#include using namespace gtsam; -using namespace std; + GTSAM_CONCEPT_TESTABLE_INST(Cal3Unify) GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unify) /* ground truth from matlab, code : -X = [2 3 1]'; -V = [0.01, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; +X = [0.5 0.7 1]'; +V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; [P, J] = spaceToImgPlane(X, V); matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox */ -static Cal3Unify K(0.01, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); -static Point2 p(2, 3); +static Cal3Unify K(0.1, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); +static Point2 p(0.5, 0.7); /* ************************************************************************* */ TEST( Cal3Unify, uncalibrate) { - Point2 p_i(582.5228344366194, 649.6685266099726) ; + Point2 p_i(364.7791831734982, 305.6677211952602) ; Point2 q = K.uncalibrate(p); CHECK(assert_equal(q,p_i)); } @@ -49,6 +47,7 @@ TEST( Cal3Unify, uncalibrate) TEST( Cal3Unify, spaceNplane) { Point2 q = K.spaceToNPlane(p); + CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q)); CHECK(assert_equal(p, K.nPlaneToSpace(q))); } @@ -56,7 +55,7 @@ TEST( Cal3Unify, spaceNplane) TEST( Cal3Unify, calibrate) { Point2 pi = K.uncalibrate(p); - Point2 pn_hat = K.calibrate(pi, 1); + Point2 pn_hat = K.calibrate(pi); CHECK( p.equals(pn_hat, 1e-5)); } @@ -68,7 +67,7 @@ TEST( Cal3Unify, Duncalibrate1) Matrix computed; K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical,computed,1e-6)); + CHECK(assert_equal(numerical,computed,1e-5)); } /* ************************************************************************* */ @@ -77,7 +76,7 @@ TEST( Cal3Unify, Duncalibrate2) Matrix computed; K.uncalibrate(p, boost::none, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical,computed,1e-6)); + CHECK(assert_equal(numerical,computed,1e-5)); } /* ************************************************************************* */ @@ -89,7 +88,7 @@ TEST( Cal3Unify, assert_equal) /* ************************************************************************* */ TEST( Cal3Unify, retract) { - Cal3Unify expected(0.01 + 1, 100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + Cal3Unify expected(0.1 + 1, 100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10); Vector d(10); d << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10; From 6d356fbf8e278513932ad71f4d66e20ed2bf7d2e Mon Sep 17 00:00:00 2001 From: jing Date: Thu, 27 Mar 2014 14:42:53 -0400 Subject: [PATCH 06/10] midterm commit, now Cal3Unify live on Cal3DS2, jacobian sequnce changes --- gtsam/geometry/Cal3DS2.h | 2 +- gtsam/geometry/Cal3Unify.cpp | 11 +++++------ gtsam/geometry/Cal3Unify.h | 22 +++++++++++----------- gtsam/geometry/tests/testCal3Unify.cpp | 12 ++++++------ 4 files changed, 23 insertions(+), 24 deletions(-) diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 5453e1481..ced05086b 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -30,7 +30,7 @@ namespace gtsam { */ class GTSAM_EXPORT Cal3DS2 : public DerivedValue { -private: +protected: double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point double k1_, k2_ ; // radial 2nd-order and 4th-order diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp index 8e200ff2a..86a82ff4c 100644 --- a/gtsam/geometry/Cal3Unify.cpp +++ b/gtsam/geometry/Cal3Unify.cpp @@ -26,22 +26,21 @@ namespace gtsam { /* ************************************************************************* */ Cal3Unify::Cal3Unify(const Vector &v): - xi_(v[0]), fx_(v[1]), fy_(v[2]), s_(v[3]), u0_(v[4]), v0_(v[5]), k1_(v[6]), k2_(v[7]), k3_(v[8]), k4_(v[9]){} + Cal3DS2(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} /* ************************************************************************* */ Matrix Cal3Unify::K() const { - return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); + return Base::K(); } /* ************************************************************************* */ Vector Cal3Unify::vector() const { - return (Vector(10) << xi_, fx_, fy_, s_, u0_, v0_, k1_, k2_, k3_, k4_); + return (Vector(10) << Base::vector(), xi_); } /* ************************************************************************* */ void Cal3Unify::print(const std::string& s) const { - gtsam::print(K(), s + ".K"); - gtsam::print(Vector(k()), s + ".k"); + Base::print(s); gtsam::print((Vector)(Vector(1) << xi_), s + ".xi"); } @@ -119,7 +118,7 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, 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 = collect(2, &DDS2U, &DDS2V); + *H1 = collect(2, &DDS2V, &DDS2U); } // Inlined derivative for points if (H2) { diff --git a/gtsam/geometry/Cal3Unify.h b/gtsam/geometry/Cal3Unify.h index 4d70db051..37b291994 100644 --- a/gtsam/geometry/Cal3Unify.h +++ b/gtsam/geometry/Cal3Unify.h @@ -22,7 +22,7 @@ #pragma once -#include +#include #include namespace gtsam { @@ -32,14 +32,14 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3Unify : public DerivedValue { +class GTSAM_EXPORT Cal3Unify : protected Cal3DS2 { + + typedef Cal3Unify This; + typedef Cal3DS2 Base; private: double xi_; // mirror parameter - 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 ] // 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})] @@ -50,18 +50,18 @@ private: public: Matrix K() const ; - Eigen::Vector4d k() const { return Eigen::Vector4d(k1_, k2_, k3_, k4_); } + Eigen::Vector4d k() const { return Base::k(); } Vector vector() const ; /// @name Standard Constructors /// @{ /// Default Constructor with only unit focal length - Cal3Unify() : xi_(0), fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), k3_(0), k4_(0) {} + Cal3Unify() : Cal3DS2(), xi_(0) {} - Cal3Unify(double xi, double fx, double fy, double s, double u0, double v0, - double k1, double k2, double k3 = 0.0, double k4 = 0.0) : - xi_(xi), fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0), k1_(k1), k2_(k2), k3_(k3), k4_(k4) {} + Cal3Unify(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) : + Cal3DS2(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {} /// @} /// @name Advanced Constructors @@ -150,7 +150,6 @@ private: { ar & boost::serialization::make_nvp("Cal3Unify", boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(xi_); ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); ar & BOOST_SERIALIZATION_NVP(s_); @@ -160,6 +159,7 @@ private: ar & BOOST_SERIALIZATION_NVP(k2_); ar & BOOST_SERIALIZATION_NVP(k3_); ar & BOOST_SERIALIZATION_NVP(k4_); + ar & BOOST_SERIALIZATION_NVP(xi_); } diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp index 370877ef3..015cff237 100644 --- a/gtsam/geometry/tests/testCal3Unify.cpp +++ b/gtsam/geometry/tests/testCal3Unify.cpp @@ -32,7 +32,7 @@ V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox */ -static Cal3Unify K(0.1, 100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3); +static Cal3Unify K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1); static Point2 p(0.5, 0.7); /* ************************************************************************* */ @@ -67,7 +67,7 @@ TEST( Cal3Unify, Duncalibrate1) Matrix computed; K.uncalibrate(p, computed, boost::none); Matrix numerical = numericalDerivative21(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical,computed,1e-5)); + CHECK(assert_equal(numerical,computed,1e-6)); } /* ************************************************************************* */ @@ -76,7 +76,7 @@ TEST( Cal3Unify, Duncalibrate2) Matrix computed; K.uncalibrate(p, boost::none, computed); Matrix numerical = numericalDerivative22(uncalibrate_, K, p, 1e-7); - CHECK(assert_equal(numerical,computed,1e-5)); + CHECK(assert_equal(numerical,computed,1e-6)); } /* ************************************************************************* */ @@ -88,10 +88,10 @@ TEST( Cal3Unify, assert_equal) /* ************************************************************************* */ TEST( Cal3Unify, retract) { - Cal3Unify expected(0.1 + 1, 100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, - 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10); + Cal3Unify expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1); Vector d(10); - d << 1, 2, 3, 4, 5, 6, 7, 8, 9, 10; + d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1; Cal3Unify actual = K.retract(d); CHECK(assert_equal(expected,actual,1e-7)); CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); From 3f5bbe3fd238394d87d2c951bb7a97db891dc912 Mon Sep 17 00:00:00 2001 From: jing Date: Thu, 27 Mar 2014 15:03:06 -0400 Subject: [PATCH 07/10] jacobian fixed, now use base class jacobians and chain rule --- gtsam/geometry/Cal3Unify.cpp | 52 +++++++----------------------------- 1 file changed, 9 insertions(+), 43 deletions(-) diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp index 86a82ff4c..eefd33801 100644 --- a/gtsam/geometry/Cal3Unify.cpp +++ b/gtsam/geometry/Cal3Unify.cpp @@ -64,8 +64,7 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, // is same as Cal3DS2 // parameters - const double xi = xi_, fx = fx_, fy = fy_, s = s_; - const double k1 = k1_, k2 = k2_, k3 = k3_, k4 = k4_; + const double xi = xi_; // Part1: project 3D space to NPlane const double xs = p.x(), ys = p.y(); // normalized points in 3D space @@ -74,51 +73,19 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, const double xi_sqrt_nx2 = xi_sqrt_nx * xi_sqrt_nx; const double x = xs * xi_sqrt_nx, y = ys * xi_sqrt_nx; // points on NPlane - // Part2: project NPlane point to pixel plane: same as Cal3DS2 - const double 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 pnx = g*x + dx; - const double pny = g*y + dy; - - // DDS2 will be used both in H1 and H2 - Matrix DDS2; - if (H1 || H2) { - // part2 - 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 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); - - 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); - - DDS2 = DK * DR; - } + // Part2: project NPlane point to pixel plane: use Cal3DS2 + Point2 m(x,y); + Matrix H1base, H2base; // jacobians from Base class + Point2 puncalib = Base::uncalibrate(m, H1base, H2base); // Inlined derivative for calibration if (H1) { // part1 Matrix DU = (Matrix(2,1) << -xs * sqrt_nx * xi_sqrt_nx2, -ys * sqrt_nx * xi_sqrt_nx2); - Matrix DDS2U = DDS2 * DU; - // part2 - Matrix DDS2V = (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)); + Matrix DDS2U = H2base * DU; - *H1 = collect(2, &DDS2V, &DDS2U); + *H1 = collect(2, &H1base, &DDS2U); } // Inlined derivative for points if (H2) { @@ -129,11 +96,10 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, mid, (sqrt_nx + xi*(xs*xs + 1)) * denom); - *H2 = DDS2 * DU; + *H2 = H2base * DU; } - - return Point2(fx * pnx + s * pny + u0_, fy * pny + v0_); + return puncalib; } /* ************************************************************************* */ From 77f494b341dadbf4b9cdcf54a7c9f55afccfe241 Mon Sep 17 00:00:00 2001 From: jing Date: Thu, 27 Mar 2014 15:09:26 -0400 Subject: [PATCH 08/10] calibrate() done by using base class --- gtsam/geometry/Cal3Unify.cpp | 37 +++----------------------- gtsam/geometry/tests/testCal3Unify.cpp | 8 +++--- 2 files changed, 8 insertions(+), 37 deletions(-) diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unify.cpp index eefd33801..e837eff5d 100644 --- a/gtsam/geometry/Cal3Unify.cpp +++ b/gtsam/geometry/Cal3Unify.cpp @@ -104,41 +104,12 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, /* ************************************************************************* */ Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { - // Use the following fixed point iteration to invert the radial distortion. - // pn_{t+1} = (inv(K)*pi - dp(pn_{t})) / g(pn_{t}) - // point on the normalized plane, input for DS2 - double px = pi.x(); - double py = pi.y(); - const Point2 invKPi ((1 / fx_) * (px - u0_ - (s_ / fy_) * (py - v0_)), - (1 / fy_) * (py - v0_)); + // calibrate point to Nplane use base class::calibrate() + Point2 pnplane = Base::calibrate(pi, tol); - // initialize by ignoring the distortion at all, might be problematic for pixels around boundary - Point2 pn = nPlaneToSpace(invKPi); - - // iterate until the uncalibrate is close to the actual pixel coordinate - const int maxIterations = 20; - int iteration; - for ( iteration = 0; iteration < maxIterations; ++iteration ) { - - if ( uncalibrate(pn).distance(pi) <= tol ) break; - - // part1: 3D space -> normalized plane - Point2 pnpl = spaceToNPlane(pn); - // part2: normalized plane -> 3D space - px = pnpl.x(), py = pnpl.y(); - const double xy = px*py, xx = px*px, yy = py*py; - 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); - pn = nPlaneToSpace((invKPi - Point2(dx,dy))/g); - } - - if ( iteration >= maxIterations ) - throw std::runtime_error("Cal3Unify::calibrate fails to converge. need a better initialization"); - - return pn; + // call nplane to space + return this->nPlaneToSpace(pnplane); } /* ************************************************************************* */ Point2 Cal3Unify::nPlaneToSpace(const Point2& p) const { diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp index 015cff237..c36b756e3 100644 --- a/gtsam/geometry/tests/testCal3Unify.cpp +++ b/gtsam/geometry/tests/testCal3Unify.cpp @@ -56,7 +56,7 @@ TEST( Cal3Unify, calibrate) { Point2 pi = K.uncalibrate(p); Point2 pn_hat = K.calibrate(pi); - CHECK( p.equals(pn_hat, 1e-5)); + CHECK( p.equals(pn_hat, 1e-8)); } Point2 uncalibrate_(const Cal3Unify& k, const Point2& pt) { return k.uncalibrate(pt); } @@ -82,7 +82,7 @@ TEST( Cal3Unify, Duncalibrate2) /* ************************************************************************* */ TEST( Cal3Unify, assert_equal) { - CHECK(assert_equal(K,K,1e-5)); + CHECK(assert_equal(K,K,1e-9)); } /* ************************************************************************* */ @@ -93,8 +93,8 @@ TEST( Cal3Unify, retract) Vector d(10); d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1; Cal3Unify actual = K.retract(d); - CHECK(assert_equal(expected,actual,1e-7)); - CHECK(assert_equal(d,K.localCoordinates(actual),1e-7)); + CHECK(assert_equal(expected,actual,1e-9)); + CHECK(assert_equal(d,K.localCoordinates(actual),1e-9)); } /* ************************************************************************* */ From 8eeb7e8cfb175699be8d7eb83031dae8432186b0 Mon Sep 17 00:00:00 2001 From: jing Date: Thu, 27 Mar 2014 15:15:30 -0400 Subject: [PATCH 09/10] change name from Cal3Unify to Cal3Unified --- .../{Cal3Unify.cpp => Cal3Unified.cpp} | 28 +++++++++---------- gtsam/geometry/{Cal3Unify.h => Cal3Unified.h} | 20 ++++++------- gtsam/geometry/tests/testCal3Unify.cpp | 28 +++++++++---------- 3 files changed, 38 insertions(+), 38 deletions(-) rename gtsam/geometry/{Cal3Unify.cpp => Cal3Unified.cpp} (84%) rename gtsam/geometry/{Cal3Unify.h => Cal3Unified.h} (89%) diff --git a/gtsam/geometry/Cal3Unify.cpp b/gtsam/geometry/Cal3Unified.cpp similarity index 84% rename from gtsam/geometry/Cal3Unify.cpp rename to gtsam/geometry/Cal3Unified.cpp index e837eff5d..d2a2be287 100644 --- a/gtsam/geometry/Cal3Unify.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -18,34 +18,34 @@ #include #include #include -#include +#include #include namespace gtsam { /* ************************************************************************* */ -Cal3Unify::Cal3Unify(const Vector &v): - Cal3DS2(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} +Cal3Unified::Cal3Unified(const Vector &v): + Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} /* ************************************************************************* */ -Matrix Cal3Unify::K() const { +Matrix Cal3Unified::K() const { return Base::K(); } /* ************************************************************************* */ -Vector Cal3Unify::vector() const { +Vector Cal3Unified::vector() const { return (Vector(10) << Base::vector(), xi_); } /* ************************************************************************* */ -void Cal3Unify::print(const std::string& s) const { +void Cal3Unified::print(const std::string& s) const { Base::print(s); gtsam::print((Vector)(Vector(1) << xi_), s + ".xi"); } /* ************************************************************************* */ -bool Cal3Unify::equals(const Cal3Unify& K, double tol) 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 || @@ -55,7 +55,7 @@ bool Cal3Unify::equals(const Cal3Unify& K, double tol) const { } /* ************************************************************************* */ -Point2 Cal3Unify::uncalibrate(const Point2& p, +Point2 Cal3Unified::uncalibrate(const Point2& p, boost::optional H1, boost::optional H2) const { @@ -103,7 +103,7 @@ Point2 Cal3Unify::uncalibrate(const Point2& p, } /* ************************************************************************* */ -Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { +Point2 Cal3Unified::calibrate(const Point2& pi, const double tol) const { // calibrate point to Nplane use base class::calibrate() Point2 pnplane = Base::calibrate(pi, tol); @@ -112,7 +112,7 @@ Point2 Cal3Unify::calibrate(const Point2& pi, const double tol) const { return this->nPlaneToSpace(pnplane); } /* ************************************************************************* */ -Point2 Cal3Unify::nPlaneToSpace(const Point2& p) const { +Point2 Cal3Unified::nPlaneToSpace(const Point2& p) const { const double x = p.x(), y = p.y(); const double xy2 = x * x + y * y; @@ -122,7 +122,7 @@ Point2 Cal3Unify::nPlaneToSpace(const Point2& p) const { } /* ************************************************************************* */ -Point2 Cal3Unify::spaceToNPlane(const Point2& p) const { +Point2 Cal3Unified::spaceToNPlane(const Point2& p) const { const double x = p.x(), y = p.y(); const double sq_xy = 1 + xi_ * sqrt(x * x + y * y + 1); @@ -131,12 +131,12 @@ Point2 Cal3Unify::spaceToNPlane(const Point2& p) const { } /* ************************************************************************* */ -Cal3Unify Cal3Unify::retract(const Vector& d) const { - return Cal3Unify(vector() + d); +Cal3Unified Cal3Unified::retract(const Vector& d) const { + return Cal3Unified(vector() + d); } /* ************************************************************************* */ -Vector Cal3Unify::localCoordinates(const Cal3Unify& T2) const { +Vector Cal3Unified::localCoordinates(const Cal3Unified& T2) const { return T2.vector() - vector(); } diff --git a/gtsam/geometry/Cal3Unify.h b/gtsam/geometry/Cal3Unified.h similarity index 89% rename from gtsam/geometry/Cal3Unify.h rename to gtsam/geometry/Cal3Unified.h index 37b291994..d8c0251f0 100644 --- a/gtsam/geometry/Cal3Unify.h +++ b/gtsam/geometry/Cal3Unified.h @@ -32,9 +32,9 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3Unify : protected Cal3DS2 { +class GTSAM_EXPORT Cal3Unified : protected Cal3DS2 { - typedef Cal3Unify This; + typedef Cal3Unified This; typedef Cal3DS2 Base; private: @@ -57,17 +57,17 @@ public: /// @{ /// Default Constructor with only unit focal length - Cal3Unify() : Cal3DS2(), xi_(0) {} + Cal3Unified() : Base(), xi_(0) {} - Cal3Unify(double fx, double fy, double s, double u0, double v0, + 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) : - Cal3DS2(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {} + Base(fx, fy, s, u0, v0, k1, k2, k3, k4), xi_(xi) {} /// @} /// @name Advanced Constructors /// @{ - Cal3Unify(const Vector &v) ; + Cal3Unified(const Vector &v) ; /// @} /// @name Testable @@ -77,7 +77,7 @@ public: void print(const std::string& s = "") const ; /// assert equality up to a tolerance - bool equals(const Cal3Unify& K, double tol = 10e-9) const; + bool equals(const Cal3Unified& K, double tol = 10e-9) const; /// @} /// @name Standard Interface @@ -126,10 +126,10 @@ public: /// @{ /// Given delta vector, update calibration - Cal3Unify retract(const Vector& d) const ; + Cal3Unified retract(const Vector& d) const ; /// Given a different calibration, calculate update to obtain it - Vector localCoordinates(const Cal3Unify& T2) const ; + Vector localCoordinates(const Cal3Unified& T2) const ; /// Return dimensions of calibration manifold object virtual size_t dim() const { return 10 ; } //TODO: make a final dimension variable (also, usually size_t in other classes e.g. Pose2) @@ -148,7 +148,7 @@ private: template void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Cal3Unify", + ar & boost::serialization::make_nvp("Cal3Unified", boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(fx_); ar & BOOST_SERIALIZATION_NVP(fy_); diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unify.cpp index c36b756e3..b260415f1 100644 --- a/gtsam/geometry/tests/testCal3Unify.cpp +++ b/gtsam/geometry/tests/testCal3Unify.cpp @@ -17,12 +17,12 @@ #include #include #include -#include +#include using namespace gtsam; -GTSAM_CONCEPT_TESTABLE_INST(Cal3Unify) -GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unify) +GTSAM_CONCEPT_TESTABLE_INST(Cal3Unified) +GTSAM_CONCEPT_MANIFOLD_INST(Cal3Unified) /* ground truth from matlab, code : @@ -32,11 +32,11 @@ V = [0.1, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0, 0, 100, 105, 320, 240]; matlab toolbox available at http://homepages.laas.fr/~cmei/index.php/Toolbox */ -static Cal3Unify K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1); +static Cal3Unified K(100, 105, 0.0, 320, 240, 1e-3, 2.0*1e-3, 3.0*1e-3, 4.0*1e-3, 0.1); static Point2 p(0.5, 0.7); /* ************************************************************************* */ -TEST( Cal3Unify, uncalibrate) +TEST( Cal3Unified, uncalibrate) { Point2 p_i(364.7791831734982, 305.6677211952602) ; Point2 q = K.uncalibrate(p); @@ -44,7 +44,7 @@ TEST( Cal3Unify, uncalibrate) } /* ************************************************************************* */ -TEST( Cal3Unify, spaceNplane) +TEST( Cal3Unified, spaceNplane) { Point2 q = K.spaceToNPlane(p); CHECK(assert_equal(Point2(0.441731600049497, 0.618424240069295), q)); @@ -52,17 +52,17 @@ TEST( Cal3Unify, spaceNplane) } /* ************************************************************************* */ -TEST( Cal3Unify, calibrate) +TEST( Cal3Unified, calibrate) { Point2 pi = K.uncalibrate(p); Point2 pn_hat = K.calibrate(pi); CHECK( p.equals(pn_hat, 1e-8)); } -Point2 uncalibrate_(const Cal3Unify& k, const Point2& pt) { return k.uncalibrate(pt); } +Point2 uncalibrate_(const Cal3Unified& k, const Point2& pt) { return k.uncalibrate(pt); } /* ************************************************************************* */ -TEST( Cal3Unify, Duncalibrate1) +TEST( Cal3Unified, Duncalibrate1) { Matrix computed; K.uncalibrate(p, computed, boost::none); @@ -71,7 +71,7 @@ TEST( Cal3Unify, Duncalibrate1) } /* ************************************************************************* */ -TEST( Cal3Unify, Duncalibrate2) +TEST( Cal3Unified, Duncalibrate2) { Matrix computed; K.uncalibrate(p, boost::none, computed); @@ -80,19 +80,19 @@ TEST( Cal3Unify, Duncalibrate2) } /* ************************************************************************* */ -TEST( Cal3Unify, assert_equal) +TEST( Cal3Unified, assert_equal) { CHECK(assert_equal(K,K,1e-9)); } /* ************************************************************************* */ -TEST( Cal3Unify, retract) +TEST( Cal3Unified, retract) { - Cal3Unify expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, + Cal3Unified expected(100 + 2, 105 + 3, 0.0 + 4, 320 + 5, 240 + 6, 1e-3 + 7, 2.0*1e-3 + 8, 3.0*1e-3 + 9, 4.0*1e-3 + 10, 0.1 + 1); Vector d(10); d << 2, 3, 4, 5, 6, 7, 8, 9, 10, 1; - Cal3Unify actual = K.retract(d); + Cal3Unified actual = K.retract(d); CHECK(assert_equal(expected,actual,1e-9)); CHECK(assert_equal(d,K.localCoordinates(actual),1e-9)); } From f27cb3f317e7ff57e915ffca0fc2cea7d5e10fc4 Mon Sep 17 00:00:00 2001 From: jing Date: Sat, 3 May 2014 17:04:07 -0400 Subject: [PATCH 10/10] few style bugs fixed --- gtsam/geometry/Cal3Unified.cpp | 5 --- gtsam/geometry/Cal3Unified.h | 33 +++---------------- ...{testCal3Unify.cpp => testCal3Unified.cpp} | 0 3 files changed, 4 insertions(+), 34 deletions(-) rename gtsam/geometry/tests/{testCal3Unify.cpp => testCal3Unified.cpp} (100%) diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index d2a2be287..39ad221cb 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -28,11 +28,6 @@ namespace gtsam { Cal3Unified::Cal3Unified(const Vector &v): Base(v[0], v[1], v[2], v[3], v[4], v[5], v[6], v[7], v[8]), xi_(v[9]) {} -/* ************************************************************************* */ -Matrix Cal3Unified::K() const { - return Base::K(); -} - /* ************************************************************************* */ Vector Cal3Unified::vector() const { return (Vector(10) << Base::vector(), xi_); diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index d8c0251f0..6961757bd 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -32,7 +32,7 @@ namespace gtsam { * @addtogroup geometry * \nosubgrouping */ -class GTSAM_EXPORT Cal3Unified : protected Cal3DS2 { +class GTSAM_EXPORT Cal3Unified : public Cal3DS2 { typedef Cal3Unified This; typedef Cal3DS2 Base; @@ -49,8 +49,8 @@ private: // pi = K*pn public: - Matrix K() const ; - Eigen::Vector4d k() const { return Base::k(); } + //Matrix K() const ; + //Eigen::Vector4d k() const { return Base::k(); } Vector vector() const ; /// @name Standard Constructors @@ -86,21 +86,6 @@ public: /// mirror parameter inline double xi() const { return xi_;} - /// focal length x - inline double fx() const { return fx_;} - - /// focal length y - inline double fy() const { return fy_;} - - /// skew - inline double skew() const { return s_;} - - /// image center in x - inline double px() const { return u0_;} - - /// image center in y - inline double py() const { return v0_;} - /** * convert intrinsic coordinates xy to image coordinates uv * @param p point in intrinsic coordinates @@ -149,20 +134,10 @@ private: void serialize(Archive & ar, const unsigned int version) { ar & boost::serialization::make_nvp("Cal3Unified", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(fx_); - ar & BOOST_SERIALIZATION_NVP(fy_); - ar & BOOST_SERIALIZATION_NVP(s_); - ar & BOOST_SERIALIZATION_NVP(u0_); - 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_); + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(xi_); } - /// @} }; diff --git a/gtsam/geometry/tests/testCal3Unify.cpp b/gtsam/geometry/tests/testCal3Unified.cpp similarity index 100% rename from gtsam/geometry/tests/testCal3Unify.cpp rename to gtsam/geometry/tests/testCal3Unified.cpp