diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index c2c0b7d79..c131d46f7 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -83,26 +83,20 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, OptionalJacobian<2, 5> Dcal, } /* ************************************************************************* */ -Point2 Cal3_S2::calibrate(const Point2& p) const { - const double u = p.x(), v = p.y(); - return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)), - (1 / fy_) * (v - v0_)); -} - -/* ************************************************************************* */ -Point2 Cal3_S2::calibrate2(const Point2& p, OptionalJacobian<2,5> Dcal, +Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal, OptionalJacobian<2,2> Dp) const { const double u = p.x(), v = p.y(); double delta_u = u - u0_, delta_v = v - v0_; double inv_fx = 1/ fx_, inv_fy = 1/fy_; - Point2 point(inv_fx * (delta_u - s_ * inv_fy * delta_v), - inv_fy * delta_v); + double inv_fy_delta_v = inv_fy * delta_v, inv_fx_s_inv_fy = inv_fx * s_ * inv_fy; + Point2 point(inv_fx * (delta_u - s_ * inv_fy_delta_v), + inv_fy_delta_v); if(Dcal) - *Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy * delta_v, -inv_fx * point.y(), - -inv_fx, inv_fx * s_ * inv_fy, + *Dcal << - inv_fx * point.x(), inv_fx * s_ * inv_fy * inv_fy_delta_v, -inv_fx * point.y(), + -inv_fx, inv_fx_s_inv_fy, 0, -inv_fy * point.y(), 0, 0, -inv_fy; if(Dp) - *Dp << inv_fx, -inv_fx * s_ * inv_fy, 0, inv_fy; + *Dp << inv_fx, -inv_fx_s_inv_fy, 0, inv_fy; return point; } diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index 9347c563c..4e62ca7e9 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -153,13 +153,6 @@ public: Point2 uncalibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, OptionalJacobian<2,2> Dp = boost::none) const; - /** - * convert image coordinates uv to intrinsic coordinates xy - * @param p point in image coordinates - * @return point in intrinsic coordinates - */ - Point2 calibrate(const Point2& p) const; - /** * convert image coordinates uv to intrinsic coordinates xy * @param p point in image coordinates @@ -167,7 +160,7 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in intrinsic coordinates */ - Point2 calibrate2(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, + Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, OptionalJacobian<2,2> Dp = boost::none) const; /**