diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index 4f2349e1a..044d47de1 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -47,19 +47,6 @@ Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const { return T2.vector() - vector(); } -/* ************************************************************************* */ -Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional H1, - boost::optional H2) const { - Matrix29 H1f; - Matrix2 H2f; - Point2 u = Base::uncalibrate(p,H1f,H2f); - if (H1) - *H1 = H1f; - if (H2) - *H2 = H2f; - return u; -} - } /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3DS2.h b/gtsam/geometry/Cal3DS2.h index a035cc29a..95524e115 100644 --- a/gtsam/geometry/Cal3DS2.h +++ b/gtsam/geometry/Cal3DS2.h @@ -87,21 +87,6 @@ public: /// Return dimensions of calibration manifold object static size_t Dim() { return 9; } //TODO: make a final dimension variable - /// @} - /// @name Standard Interface - /// @{ - - /** - * 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 ; private: diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index 24ab80690..e4397a449 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -91,8 +91,8 @@ static Matrix2 D2dintrinsic(double x, double y, double rr, /* ************************************************************************* */ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, - boost::optional&> H1, - boost::optional&> H2) const { + boost::optional H1, + boost::optional H2) const { // rr = x^2 + y^2; // g = (1 + k(1)*rr + k(2)*rr^2); @@ -126,6 +126,25 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, return Point2(fx_ * pnx + s_ * pny + u0_, fy_ * pny + v0_); } +/* ************************************************************************* */ +Point2 Cal3DS2_Base::uncalibrate(const Point2& p, + boost::optional H1, + boost::optional H2) const { + + if (H1 || H2) { + Matrix29 D1; + Matrix2 D2; + Point2 pu = uncalibrate(p, D1, D2); + if (H1) + *H1 = D1; + if (H2) + *H2 = D2; + return pu; + + } else { + return uncalibrate(p); + } +} /* ************************************************************************* */ Point2 Cal3DS2_Base::calibrate(const Point2& pi, const double tol) const { diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 349b73f2d..61c01e349 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -18,7 +18,6 @@ #pragma once -#include #include namespace gtsam { @@ -114,10 +113,15 @@ public: * @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 ; + Point2 uncalibrate(const Point2& p, + boost::optional Dcal, + boost::optional Dp) const ; + /// Convert (distorted) image coordinates uv to intrinsic coordinates xy Point2 calibrate(const Point2& p, const double tol=1e-5) const;