From b4c62969d160f6c7b7174816d84cc39ab2be9e3f Mon Sep 17 00:00:00 2001 From: Jing Dong Date: Fri, 17 Oct 2014 13:30:43 -0400 Subject: [PATCH] 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 | 82 ++------------ gtsam/geometry/Cal3DS2_Base.cpp | 187 ++++++++++++++++++++++++++++++++ gtsam/geometry/Cal3DS2_Base.h | 158 +++++++++++++++++++++++++++ gtsam/geometry/Cal3Unified.h | 12 +- 5 files changed, 361 insertions(+), 224 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..ad8080fa3 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -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..7be5a6874 --- /dev/null +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -0,0 +1,158 @@ +/* ---------------------------------------------------------------------------- + + * 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.h + * @brief Calibration of a camera with radial distortion + * @date Feb 28, 2010 + * @author ydjian + */ + +#pragma once + +#include +#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_); }