From 55bda50235607bdecfaea46f686a3902278449a8 Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Fri, 17 Oct 2014 13:30:43 -0400 Subject: [PATCH 1/6] move calculation of Cal3DS2 to base class, all unit tests passed and hopefully fix issue of DerivedValue --- gtsam/geometry/Cal3DS2.cpp | 146 +------------------------ gtsam/geometry/Cal3DS2.h | 84 ++------------ gtsam/geometry/Cal3DS2_Base.cpp | 187 ++++++++++++++++++++++++++++++++ gtsam/geometry/Cal3DS2_Base.h | 157 +++++++++++++++++++++++++++ gtsam/geometry/Cal3Unified.h | 12 +- 5 files changed, 361 insertions(+), 225 deletions(-) create mode 100644 gtsam/geometry/Cal3DS2_Base.cpp create mode 100644 gtsam/geometry/Cal3DS2_Base.h diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index c75eff022..044d47de1 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -23,24 +23,9 @@ 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]), p1_(v[7]), p2_(v[8]){} - -/* ************************************************************************* */ -Matrix Cal3DS2::K() const { - return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); -} - -/* ************************************************************************* */ -Vector Cal3DS2::vector() const { - 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"); + Base::print(s_); } /* ************************************************************************* */ @@ -52,135 +37,6 @@ bool Cal3DS2::equals(const Cal3DS2& K, double tol) const { return true; } -/* ************************************************************************* */ -static Eigen::Matrix D2dcalibration(double x, double y, double xx, - double yy, double xy, double rr, double r4, double pnx, double pny, - const Eigen::Matrix& DK) { - Eigen::Matrix DR1; - DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; - Eigen::Matrix DR2; - DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // - y * rr, y * r4, rr + 2 * yy, 2 * xy; - Eigen::Matrix D; - D << DR1, DK * DR2; - return D; -} - -/* ************************************************************************* */ -static Eigen::Matrix D2dintrinsic(double x, double y, double rr, - double g, double k1, double k2, double p1, double p2, - const Eigen::Matrix& DK) { - const double drdx = 2. * x; - const double drdy = 2. * y; - const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; - const double dgdy = k1 * drdy + k2 * 2. * rr * drdy; - - // Dx = 2*p1*xy + p2*(rr+2*xx); - // Dy = 2*p2*xy + p1*(rr+2*yy); - const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x); - const double dDxdy = 2. * p1 * x + p2 * drdy; - const double dDydx = 2. * p2 * y + p1 * drdx; - const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); - - Eigen::Matrix DR; - DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // - y * dgdx + dDydx, g + y * dgdy + dDydy; - - return DK * DR; -} - -/* ************************************************************************* */ -Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional H1, - boost::optional H2) const { - - // 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)]; - // pi(:,i) = g * pn(:,i) + dp; - 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; // 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; - - Eigen::Matrix DK; - if (H1 || H2) DK << fx_, s_, 0.0, fy_; - - // Derivative for calibration - if (H1) - *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); - - // Derivative for points - if (H2) - *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); - - // Regular uncalibrate after distortion - return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); -} - -/* ************************************************************************* */ -Point2 Cal3DS2::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}) - - const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)), - (1 / fy_) * (pi.y() - 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; - 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 * p1_ * xy + p2_ * (rr + 2 * xx); - const double dy = 2 * p2_ * xy + p1_ * (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; -} - -/* ************************************************************************* */ -Matrix Cal3DS2::D2d_intrinsic(const Point2& p) const { - const double x = p.x(), y = p.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); - Eigen::Matrix DK; - DK << fx_, s_, 0.0, fy_; - return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); -} - -/* ************************************************************************* */ -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 g = (1 + k1_ * rr + k2_ * r4); - 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; - Eigen::Matrix DK; - DK << fx_, s_, 0.0, fy_; - return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); -} - /* ************************************************************************* */ Cal3DS2 Cal3DS2::retract(const Vector& d) const { return Cal3DS2(vector() + d); diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index 51fe958d6..dbd50f450 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -11,7 +11,7 @@ /** * @file Cal3DS2.h - * @brief Calibration of a camera with radial distortion + * @brief Calibration of a camera with radial distortion, calculations in base class Cal3DS2_Base * @date Feb 28, 2010 * @author ydjian */ @@ -19,7 +19,7 @@ #pragma once #include -#include +#include namespace gtsam { @@ -37,34 +37,27 @@ namespace gtsam { * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * pi = K*pn */ -class GTSAM_EXPORT Cal3DS2 : public DerivedValue { +class GTSAM_EXPORT Cal3DS2 : public Cal3DS2_Base, public DerivedValue { -protected: - - double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point - double k1_, k2_ ; // radial 2nd-order and 4th-order - double p1_, p2_ ; // tangential distortion + typedef Cal3DS2_Base Base; public: - Matrix K() const ; - 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), p1_(0), p2_(0) {} + Cal3DS2() : Base() {} Cal3DS2(double fx, double fy, double s, double u0, double v0, 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) {} + Base(fx, fy, s, u0, v0, k1, k2, p1, p2) {} /// @} /// @name Advanced Constructors /// @{ - Cal3DS2(const Vector &v) ; + Cal3DS2(const Vector &v) : Base(v) {} /// @} /// @name Testable @@ -76,57 +69,6 @@ public: /// assert equality up to a tolerance bool equals(const Cal3DS2& K, double tol = 10e-9) const; - /// @} - /// @name Standard Interface - /// @{ - - /// focal length x - inline double fx() const { return fx_;} - - /// focal length x - 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_;} - - /// First distortion coefficient - inline double k1() const { return k1_;} - - /// Second distortion coefficient - inline double k2() const { return k2_;} - - /// First tangential distortion coefficient - inline double p1() const { return p1_;} - - /// Second tangential distortion coefficient - inline double p2() const { return p2_;} - - /** - * 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 (distorted) image coordinates - */ - Point2 uncalibrate(const Point2& p, - boost::optional Dcal = boost::none, - boost::optional Dp = boost::none) const ; - - /// 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 - Matrix D2d_intrinsic(const Point2& p) const ; - - /// Derivative of uncalibrate wrpt the calibration parameters - Matrix D2d_calibration(const Point2& p) const ; - /// @} /// @name Manifold /// @{ @@ -156,18 +98,10 @@ private: { ar & boost::serialization::make_nvp("Cal3DS2", 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(p1_); - ar & BOOST_SERIALIZATION_NVP(p2_); + ar & boost::serialization::make_nvp("Cal3DS2", + boost::serialization::base_object(*this)); } - /// @} }; diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp new file mode 100644 index 000000000..b8181ab4d --- /dev/null +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -0,0 +1,187 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3DS2_Base.cpp + * @date Feb 28, 2010 + * @author ydjian + */ + +#include +#include +#include +#include +#include + +namespace gtsam { + +/* ************************************************************************* */ +Cal3DS2_Base::Cal3DS2_Base(const Vector &v): + 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_Base::K() const { + return (Matrix(3, 3) << fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); +} + +/* ************************************************************************* */ +Vector Cal3DS2_Base::vector() const { + return (Vector(9) << fx_, fy_, s_, u0_, v0_, k1_, k2_, p1_, p2_); +} + +/* ************************************************************************* */ +void Cal3DS2_Base::print(const std::string& s_) const { + gtsam::print(K(), s_ + ".K"); + gtsam::print(Vector(k()), s_ + ".k"); +} + +/* ************************************************************************* */ +bool Cal3DS2_Base::equals(const Cal3DS2_Base& 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(p1_ - K.p1_) > tol || fabs(p2_ - K.p2_) > tol) + return false; + return true; +} + +/* ************************************************************************* */ +static Eigen::Matrix D2dcalibration(double x, double y, double xx, + double yy, double xy, double rr, double r4, double pnx, double pny, + const Eigen::Matrix& DK) { + Eigen::Matrix DR1; + DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; + Eigen::Matrix DR2; + DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // + y * rr, y * r4, rr + 2 * yy, 2 * xy; + Eigen::Matrix D; + D << DR1, DK * DR2; + return D; +} + +/* ************************************************************************* */ +static Eigen::Matrix D2dintrinsic(double x, double y, double rr, + double g, double k1, double k2, double p1, double p2, + const Eigen::Matrix& DK) { + const double drdx = 2. * x; + const double drdy = 2. * y; + const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; + const double dgdy = k1 * drdy + k2 * 2. * rr * drdy; + + // Dx = 2*p1*xy + p2*(rr+2*xx); + // Dy = 2*p2*xy + p1*(rr+2*yy); + const double dDxdx = 2. * p1 * y + p2 * (drdx + 4. * x); + const double dDxdy = 2. * p1 * x + p2 * drdy; + const double dDydx = 2. * p2 * y + p1 * drdx; + const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); + + Eigen::Matrix DR; + DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // + y * dgdx + dDydx, g + y * dgdy + dDydy; + + return DK * DR; +} + +/* ************************************************************************* */ +Point2 Cal3DS2_Base::uncalibrate(const Point2& p, boost::optional H1, + boost::optional H2) const { + + // 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)]; + // pi(:,i) = g * pn(:,i) + dp; + 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; // 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; + + Eigen::Matrix DK; + if (H1 || H2) DK << fx_, s_, 0.0, fy_; + + // Derivative for calibration + if (H1) + *H1 = D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); + + // Derivative for points + if (H2) + *H2 = D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); + + // Regular uncalibrate after distortion + return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); +} + +/* ************************************************************************* */ +Point2 Cal3DS2_Base::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}) + + const Point2 invKPi ((1 / fx_) * (pi.x() - u0_ - (s_ / fy_) * (pi.y() - v0_)), + (1 / fy_) * (pi.y() - 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; + 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 * p1_ * xy + p2_ * (rr + 2 * xx); + const double dy = 2 * p2_ * xy + p1_ * (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; +} + +/* ************************************************************************* */ +Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { + const double x = p.x(), y = p.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); + Eigen::Matrix DK; + DK << fx_, s_, 0.0, fy_; + return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); +} + +/* ************************************************************************* */ +Matrix Cal3DS2_Base::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 g = (1 + k1_ * rr + k2_ * r4); + 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; + Eigen::Matrix DK; + DK << fx_, s_, 0.0, fy_; + return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); +} + +} +/* ************************************************************************* */ + + diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h new file mode 100644 index 000000000..fc5f5d7c1 --- /dev/null +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -0,0 +1,157 @@ +/* ---------------------------------------------------------------------------- + + * 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 Cal3DS2_Base.h + * @brief Base class of calibration of a camera with radial distortion, used for Cal3DS2 and Cal3Unified + * @date Feb 28, 2010 + * @author ydjian + */ + +#pragma once + +#include + +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_Base { + +protected: + + double fx_, fy_, s_, u0_, v0_ ; // focal length, skew and principal point + double k1_, k2_ ; // radial 2nd-order and 4th-order + double p1_, p2_ ; // tangential distortion + +public: + Matrix K() const ; + 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_Base() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0), k1_(0), k2_(0), p1_(0), p2_(0) {} + + Cal3DS2_Base(double fx, double fy, double s, double u0, double v0, + 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 + /// @{ + + Cal3DS2_Base(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 Cal3DS2_Base& K, double tol = 10e-9) const; + + /// @} + /// @name Standard Interface + /// @{ + + /// focal length x + inline double fx() const { return fx_;} + + /// focal length x + 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_;} + + /// First distortion coefficient + inline double k1() const { return k1_;} + + /// Second distortion coefficient + inline double k2() const { return k2_;} + + /// First tangential distortion coefficient + inline double p1() const { return p1_;} + + /// Second tangential distortion coefficient + inline double p2() const { return p2_;} + + /** + * 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 (distorted) image coordinates + */ + Point2 uncalibrate(const Point2& p, + boost::optional Dcal = boost::none, + boost::optional Dp = boost::none) const ; + + /// 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 + Matrix D2d_intrinsic(const Point2& p) const ; + + /// Derivative of uncalibrate wrpt the calibration parameters + Matrix D2d_calibration(const Point2& p) const ; + + +private: + + /// @} + /// @name Advanced Interface + /// @{ + + /** Serialization function */ + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) + { + 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(p1_); + ar & BOOST_SERIALIZATION_NVP(p2_); + } + + /// @} + +}; + +} + diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 58e024c27..750d2ed73 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -22,8 +22,8 @@ #pragma once -#include -#include +#include +#include namespace gtsam { @@ -40,10 +40,10 @@ namespace gtsam { * k3 (rr + 2 Pn.y^2) + 2*k4 pn.x pn.y ] * pi = K*pn */ -class GTSAM_EXPORT Cal3Unified : public Cal3DS2 { +class GTSAM_EXPORT Cal3Unified : public Cal3DS2_Base, public DerivedValue { typedef Cal3Unified This; - typedef Cal3DS2 Base; + typedef Cal3DS2_Base Base; private: @@ -135,7 +135,9 @@ private: void serialize(Archive & ar, const unsigned int version) { ar & boost::serialization::make_nvp("Cal3Unified", - boost::serialization::base_object(*this)); + boost::serialization::base_object(*this)); + ar & boost::serialization::make_nvp("Cal3Unified", + boost::serialization::base_object(*this)); ar & BOOST_SERIALIZATION_NVP(xi_); } From cdc121cf7da30408aed14a05f24924ad98c07a7e Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Fri, 17 Oct 2014 14:25:42 -0400 Subject: [PATCH 2/6] add in testSerialization --- gtsam/geometry/Cal3Unified.h | 2 +- gtsam/geometry/tests/testSerializationGeometry.cpp | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/gtsam/geometry/Cal3Unified.h b/gtsam/geometry/Cal3Unified.h index 750d2ed73..03db6af9a 100644 --- a/gtsam/geometry/Cal3Unified.h +++ b/gtsam/geometry/Cal3Unified.h @@ -90,7 +90,7 @@ public: /** * convert intrinsic coordinates xy to image coordinates uv * @param p point in intrinsic coordinates - * @param Dcal optional 2*9 Jacobian wrpt Cal3DS2 parameters + * @param Dcal optional 2*10 Jacobian wrpt Cal3Unified parameters * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ diff --git a/gtsam/geometry/tests/testSerializationGeometry.cpp b/gtsam/geometry/tests/testSerializationGeometry.cpp index 95001a033..a7e792cb8 100644 --- a/gtsam/geometry/tests/testSerializationGeometry.cpp +++ b/gtsam/geometry/tests/testSerializationGeometry.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #include #include @@ -46,6 +47,7 @@ static Cal3Bundler cal3(1.0, 2.0, 3.0); static Cal3_S2Stereo cal4(1.0, 2.0, 3.0, 4.0, 5.0, 6.0); static Cal3_S2Stereo::shared_ptr cal4ptr(new Cal3_S2Stereo(cal4)); static CalibratedCamera cal5(Pose3(rt3, pt3)); +static Cal3Unified cal6(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0); static PinholeCamera cam1(pose3, cal1); static StereoCamera cam2(pose3, cal4ptr); @@ -66,6 +68,7 @@ TEST (Serialization, text_geometry) { EXPECT(equalsObj(cal3)); EXPECT(equalsObj(cal4)); EXPECT(equalsObj(cal5)); + EXPECT(equalsObj(cal6)); EXPECT(equalsObj(cam1)); EXPECT(equalsObj(cam2)); From f258bfe0443e192a6bfc341229dc641f249e139d Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Fri, 17 Oct 2014 14:44:37 -0400 Subject: [PATCH 3/6] add DerivedValue test for Cal3Unified --- gtsam/geometry/tests/testCal3Unified.cpp | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/gtsam/geometry/tests/testCal3Unified.cpp b/gtsam/geometry/tests/testCal3Unified.cpp index b260415f1..de9a8b739 100644 --- a/gtsam/geometry/tests/testCal3Unified.cpp +++ b/gtsam/geometry/tests/testCal3Unified.cpp @@ -19,6 +19,9 @@ #include #include +#include +#include + using namespace gtsam; GTSAM_CONCEPT_TESTABLE_INST(Cal3Unified) @@ -97,6 +100,19 @@ TEST( Cal3Unified, retract) CHECK(assert_equal(d,K.localCoordinates(actual),1e-9)); } +/* ************************************************************************* */ +TEST( Cal3Unified, DerivedValue) +{ + Values values; + Cal3Unified cal(1, 2, 3, 4, 5, 6, 7, 8, 9, 10); + Key key = 1; + values.insert(key, cal); + + Cal3Unified calafter = values.at(key); + + CHECK(assert_equal(cal,calafter,1e-9)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr); } /* ************************************************************************* */ From c10993a69035f147e593d0b4a7421ca93ebdbc4f Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 16 Oct 2014 14:39:09 -0400 Subject: [PATCH 4/6] displaying nr of iterations for verbosity = TERMINATION --- gtsam/nonlinear/NonlinearOptimizer.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/gtsam/nonlinear/NonlinearOptimizer.cpp b/gtsam/nonlinear/NonlinearOptimizer.cpp index b4498bee4..80407a372 100644 --- a/gtsam/nonlinear/NonlinearOptimizer.cpp +++ b/gtsam/nonlinear/NonlinearOptimizer.cpp @@ -77,9 +77,11 @@ void NonlinearOptimizer::defaultOptimize() { params.errorTol, currentError, this->error(), params.verbosity)); // Printing if verbose - if (params.verbosity >= NonlinearOptimizerParams::TERMINATION && - this->iterations() >= params.maxIterations) - cout << "Terminating because reached maximum iterations" << endl; + if (params.verbosity >= NonlinearOptimizerParams::TERMINATION) { + cout << "iterations: " << this->iterations() << " >? " << params.maxIterations << endl; + if (this->iterations() >= params.maxIterations) + cout << "Terminating because reached maximum iterations" << endl; + } } /* ************************************************************************* */ From e96ceb2b4f6fb86e2d3df29857844feca8a9c566 Mon Sep 17 00:00:00 2001 From: Luca Date: Thu, 16 Oct 2014 14:39:23 -0400 Subject: [PATCH 5/6] extended example for robust kernels --- examples/Pose2SLAMExample_g2o.cpp | 48 ++++++++++++++++++++++++------- 1 file changed, 37 insertions(+), 11 deletions(-) diff --git a/examples/Pose2SLAMExample_g2o.cpp b/examples/Pose2SLAMExample_g2o.cpp index 8f1a53a66..8d8f2edc1 100644 --- a/examples/Pose2SLAMExample_g2o.cpp +++ b/examples/Pose2SLAMExample_g2o.cpp @@ -26,30 +26,53 @@ using namespace std; using namespace gtsam; +// HOWTO: ./Pose2SLAMExample_g2o inputFile outputFile (maxIterations) (tukey/huber) int main(const int argc, const char *argv[]) { - // Read graph from file - string g2oFile; - if (argc < 2) - g2oFile = findExampleDataFile("noisyToyGraph.txt"); - else - g2oFile = argv[1]; + string kernelType = "none"; + int maxIterations = 100; // default + string g2oFile = findExampleDataFile("noisyToyGraph.txt"); // default + // Parse user's inputs + if (argc > 1){ + g2oFile = argv[1]; // input dataset filename + // outputFile = g2oFile = argv[2]; // done later + } + if (argc > 3){ + maxIterations = atoi(argv[3]); // user can specify either tukey or huber + } + if (argc > 4){ + kernelType = argv[4]; // user can specify either tukey or huber + } + + // reading file and creating factor graph NonlinearFactorGraph::shared_ptr graph; Values::shared_ptr initial; - boost::tie(graph, initial) = readG2o(g2oFile); + bool is3D = false; + if(kernelType.compare("none") == 0){ + boost::tie(graph, initial) = readG2o(g2oFile,is3D); + } + if(kernelType.compare("huber") == 0){ + std::cout << "Using robust kernel: huber " << std::endl; + boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeHUBER); + } + if(kernelType.compare("tukey") == 0){ + std::cout << "Using robust kernel: tukey " << std::endl; + boost::tie(graph, initial) = readG2o(g2oFile,is3D, KernelFunctionTypeTUKEY); + } // Add prior on the pose having index (key) = 0 NonlinearFactorGraph graphWithPrior = *graph; noiseModel::Diagonal::shared_ptr priorModel = // noiseModel::Diagonal::Variances((Vector(3) << 1e-6, 1e-6, 1e-8)); graphWithPrior.add(PriorFactor(0, Pose2(), priorModel)); + std::cout << "Adding prior on pose 0 " << std::endl; GaussNewtonParams params; params.setVerbosity("TERMINATION"); - if (argc == 4) { - params.maxIterations = atoi(argv[3]); - std::cout << "User required to perform " << params.maxIterations << " iterations "<< std::endl; + if (argc > 3) { + params.maxIterations = maxIterations; + std::cout << "User required to perform maximum " << params.maxIterations << " iterations "<< std::endl; } std::cout << "Optimizing the factor graph" << std::endl; @@ -65,7 +88,10 @@ int main(const int argc, const char *argv[]) { } else { const string outputFile = argv[2]; std::cout << "Writing results to file: " << outputFile << std::endl; - writeG2o(*graph, result, outputFile); + NonlinearFactorGraph::shared_ptr graphNoKernel; + Values::shared_ptr initial2; + boost::tie(graphNoKernel, initial2) = readG2o(g2oFile); + writeG2o(*graphNoKernel, result, outputFile); std::cout << "done! " << std::endl; } return 0; From 5995b20ebe9deb55c540ee2cbc64922b62146d95 Mon Sep 17 00:00:00 2001 From: Luca Date: Fri, 17 Oct 2014 15:48:20 -0400 Subject: [PATCH 6/6] fixed test --- gtsam/geometry/tests/testPinholeCamera.cpp | 43 ++++++++++++---------- 1 file changed, 24 insertions(+), 19 deletions(-) diff --git a/gtsam/geometry/tests/testPinholeCamera.cpp b/gtsam/geometry/tests/testPinholeCamera.cpp index 6ed49d0d9..101070940 100644 --- a/gtsam/geometry/tests/testPinholeCamera.cpp +++ b/gtsam/geometry/tests/testPinholeCamera.cpp @@ -183,25 +183,30 @@ TEST( PinholeCamera, Dproject) } /* ************************************************************************* */ -//static Point2 projectInfinity3(const Pose3& pose, const Point2& point2D, const Cal3_S2& cal) { -// Point3 point(point2D.x(), point2D.y(), 1.0); -// return Camera(pose,cal).projectPointAtInfinity(point); -//} -// -//TEST( PinholeCamera, Dproject_Infinity) -//{ -// Matrix Dpose, Dpoint, Dcal; -// Point2 point2D(-0.08,-0.08); -// Point3 point3D(point1.x(), point1.y(), 1.0); -// Point2 result = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); -// Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point2D, K); -// Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point2D, K); -// Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point2D, K); -// CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); -// CHECK(assert_equal(numerical_point, Dpoint, 1e-7)); -// CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); -//} -// +static Point2 projectInfinity3(const Pose3& pose, const Point3& point3D, const Cal3_S2& cal) { + return Camera(pose,cal).projectPointAtInfinity(point3D); +} + +TEST( PinholeCamera, Dproject_Infinity) +{ + Matrix Dpose, Dpoint, Dcal; + Point3 point3D(point1.x(), point1.y(), -10.0); // a point in front of the camera + + // test Projection + Point2 actual = camera.projectPointAtInfinity(point3D, Dpose, Dpoint, Dcal); + Point2 expected(-5.0, 5.0); + CHECK(assert_equal(actual, expected, 1e-7)); + + // test Jacobians + Matrix numerical_pose = numericalDerivative31(projectInfinity3, pose1, point3D, K); + Matrix numerical_point = numericalDerivative32(projectInfinity3, pose1, point3D, K); + Matrix numerical_point2x2 = numerical_point.block(0,0,2,2); // only the direction to the point matters + Matrix numerical_cal = numericalDerivative33(projectInfinity3, pose1, point3D, K); + CHECK(assert_equal(numerical_pose, Dpose, 1e-7)); + CHECK(assert_equal(numerical_point2x2, Dpoint, 1e-7)); + CHECK(assert_equal(numerical_cal, Dcal, 1e-7)); +} + /* ************************************************************************* */ static Point2 project4(const Camera& camera, const Point3& point) { return camera.project2(point);