diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index fb4fb191e..8765fee76 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -90,14 +90,14 @@ Point2 Cal3_S2::calibrate(const Point2& p) const { } /* ************************************************************************* */ -Point2 Cal3_S2::calibrate(const Point2& p, OptionalJacobian<2,5> Dcal, - OptionalJacobian<2,2> Dp) const { +Point2 Cal3_S2::calibrate2(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_; if(Dcal) - *Dcal << - inv_fx * inv_fx * (delta_u - s_ * inv_fy * delta_v) , - inv_fx * (s_ * inv_fy * inv_fy) * delta_v, -inv_fx * inv_fy * delta_v, + *Dcal << - inv_fx * inv_fx * (delta_u - s_ * inv_fy * delta_v) , + inv_fx * (s_ * inv_fy * inv_fy) * delta_v, -inv_fx * inv_fy * delta_v, -inv_fx, inv_fx * s_ * inv_fy, 0, -inv_fy * inv_fy * delta_v, 0, 0, -inv_fy; if(Dp) diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index c448cb0b1..9347c563c 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -167,7 +167,7 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in intrinsic coordinates */ - Point2 calibrate(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, + Point2 calibrate2(const Point2& p, OptionalJacobian<2,5> Dcal = boost::none, OptionalJacobian<2,2> Dp = boost::none) const; /**