diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index c8c0255ea..c82b36a83 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -75,10 +75,14 @@ bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, boost::optional Dp) const { const double x = p.x(), y = p.y(); - if (Dcal) - *Dcal = (Matrix(2, 5) << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0); - if (Dp) - *Dp = (Matrix(2, 2) << fx_, s_, 0.000, fy_); + if (Dcal) { + Dcal->resize(2,5); + *Dcal << x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0; + } + if (Dp) { + Dp->resize(2,2); + *Dp << fx_, s_, 0.0, fy_; + } return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); } diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index 709b95181..4f81750a5 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -278,7 +278,8 @@ public: double d = 1.0 / P.z(); const double u = P.x() * d, v = P.y() * d; if (Dpoint) { - *Dpoint = (Matrix(2, 3) << d, 0.0, -u * d, 0.0, d, -v * d); + Dpoint->resize(2,3); + *Dpoint << d, 0.0, -u * d, 0.0, d, -v * d; } return Point2(u, v); }