diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 8765fee76..c2c0b7d79 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -95,15 +95,15 @@ Point2 Cal3_S2::calibrate2(const Point2& p, OptionalJacobian<2,5> Dcal, 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); 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 * 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 * inv_fy * delta_v, 0, 0, -inv_fy; + 0, -inv_fy * point.y(), 0, 0, -inv_fy; if(Dp) *Dp << inv_fx, -inv_fx * s_ * inv_fy, 0, inv_fy; - return Point2(inv_fx * (delta_u - s_ * inv_fy * delta_v), - inv_fy * delta_v); + return point; } /* ************************************************************************* */