diff --git a/gtsam/geometry/Cal3DS2.cpp b/gtsam/geometry/Cal3DS2.cpp index fbc81da7e..4f2349e1a 100644 --- a/gtsam/geometry/Cal3DS2.cpp +++ b/gtsam/geometry/Cal3DS2.cpp @@ -50,8 +50,8 @@ Vector Cal3DS2::localCoordinates(const Cal3DS2& T2) const { /* ************************************************************************* */ Point2 Cal3DS2::uncalibrate(const Point2& p, boost::optional H1, boost::optional H2) const { - Eigen::Matrix H1f; - Eigen::Matrix H2f; + Matrix29 H1f; + Matrix2 H2f; Point2 u = Base::uncalibrate(p,H1f,H2f); if (H1) *H1 = H1f; diff --git a/gtsam/geometry/Cal3DS2_Base.cpp b/gtsam/geometry/Cal3DS2_Base.cpp index f88fce4a6..24ab80690 100644 --- a/gtsam/geometry/Cal3DS2_Base.cpp +++ b/gtsam/geometry/Cal3DS2_Base.cpp @@ -53,23 +53,23 @@ bool Cal3DS2_Base::equals(const Cal3DS2_Base& K, double tol) const { } /* ************************************************************************* */ -static Eigen::Matrix D2dcalibration(double x, double y, double xx, +static Matrix29 D2dcalibration(double x, double y, double xx, double yy, double xy, double rr, double r4, double pnx, double pny, - const Eigen::Matrix& DK) { - Eigen::Matrix DR1; + const Matrix2& DK) { + Matrix25 DR1; DR1 << pnx, 0.0, pny, 1.0, 0.0, 0.0, pny, 0.0, 0.0, 1.0; - Eigen::Matrix DR2; + Matrix24 DR2; DR2 << x * rr, x * r4, 2 * xy, rr + 2 * xx, // y * rr, y * r4, rr + 2 * yy, 2 * xy; - Eigen::Matrix D; + Matrix29 D; D << DR1, DK * DR2; return D; } /* ************************************************************************* */ -static Eigen::Matrix D2dintrinsic(double x, double y, double rr, +static Matrix2 D2dintrinsic(double x, double y, double rr, double g, double k1, double k2, double p1, double p2, - const Eigen::Matrix& DK) { + const Matrix2& DK) { const double drdx = 2. * x; const double drdy = 2. * y; const double dgdx = k1 * drdx + k2 * 2. * rr * drdx; @@ -82,7 +82,7 @@ static Eigen::Matrix D2dintrinsic(double x, double y, double rr, const double dDydx = 2. * p2 * y + p1 * drdx; const double dDydy = 2. * p2 * x + p1 * (drdy + 4. * y); - Eigen::Matrix DR; + Matrix2 DR; DR << g + x * dgdx + dDxdx, x * dgdy + dDxdy, // y * dgdx + dDydx, g + y * dgdy + dDydy; @@ -111,7 +111,7 @@ Point2 Cal3DS2_Base::uncalibrate(const Point2& p, const double pnx = g * x + dx; const double pny = g * y + dy; - Eigen::Matrix DK; + Matrix2 DK; if (H1 || H2) DK << fx_, s_, 0.0, fy_; // Derivative for calibration @@ -163,7 +163,7 @@ Matrix Cal3DS2_Base::D2d_intrinsic(const Point2& p) const { const double rr = xx + yy; const double r4 = rr * rr; const double g = (1 + k1_ * rr + k2_ * r4); - Eigen::Matrix DK; + Matrix2 DK; DK << fx_, s_, 0.0, fy_; return D2dintrinsic(x, y, rr, g, k1_, k2_, p1_, p2_, DK); } @@ -178,7 +178,7 @@ Matrix Cal3DS2_Base::D2d_calibration(const Point2& p) const { const double dy = 2 * p2_ * xy + p1_ * (rr + 2 * yy); const double pnx = g * x + dx; const double pny = g * y + dy; - Eigen::Matrix DK; + Matrix2 DK; DK << fx_, s_, 0.0, fy_; return D2dcalibration(x, y, xx, yy, xy, rr, r4, pnx, pny, DK); } diff --git a/gtsam/geometry/Cal3DS2_Base.h b/gtsam/geometry/Cal3DS2_Base.h index 8886d4636..349b73f2d 100644 --- a/gtsam/geometry/Cal3DS2_Base.h +++ b/gtsam/geometry/Cal3DS2_Base.h @@ -115,8 +115,8 @@ public: * @return point in (distorted) image coordinates */ Point2 uncalibrate(const Point2& p, - boost::optional&> Dcal = boost::none, - boost::optional&> Dp = boost::none) const ; + boost::optional Dcal = boost::none, + boost::optional Dp = boost::none) const ; /// Convert (distorted) image coordinates uv to intrinsic coordinates xy Point2 calibrate(const Point2& p, const double tol=1e-5) const; diff --git a/gtsam/geometry/Cal3Unified.cpp b/gtsam/geometry/Cal3Unified.cpp index 6a998a5dc..d8b85747d 100644 --- a/gtsam/geometry/Cal3Unified.cpp +++ b/gtsam/geometry/Cal3Unified.cpp @@ -71,17 +71,16 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, // Part2: project NPlane point to pixel plane: use Cal3DS2 Point2 m(x,y); - Eigen::Matrix H1base; - Eigen::Matrix H2base; // jacobians from Base class + Matrix29 H1base; + Matrix2 H2base; // jacobians from Base class Point2 puncalib = Base::uncalibrate(m, H1base, H2base); // Inlined derivative for calibration if (H1) { // part1 - Eigen::Matrix DU; + Vector2 DU, DDS2U; DU << -xs * sqrt_nx * xi_sqrt_nx2, // -ys * sqrt_nx * xi_sqrt_nx2; - Eigen::Matrix DDS2U; DDS2U = H2base * DU; //*H1 = collect(2, &H1base, &DDS2U); @@ -93,7 +92,7 @@ Point2 Cal3Unified::uncalibrate(const Point2& p, // part1 const double denom = 1.0 * xi_sqrt_nx2 / sqrt_nx; const double mid = -(xi * xs*ys) * denom; - Eigen::Matrix DU; + Matrix2 DU; DU << (sqrt_nx + xi*(ys*ys + 1)) * denom, mid, // mid, (sqrt_nx + xi*(xs*xs + 1)) * denom;