From ea40de6758fa8c1d8096a402c5f4539b43b885ef Mon Sep 17 00:00:00 2001 From: dellaert Date: Mon, 6 Oct 2014 21:37:05 +0200 Subject: [PATCH] Fixed Jacobian versions --- gtsam/geometry/Cal3_S2.cpp | 6 ++++++ gtsam/geometry/Cal3_S2.h | 10 ++++++---- gtsam/geometry/PinholeCamera.h | 2 +- gtsam/geometry/Pose2.cpp | 21 +++++++++++++++++++++ gtsam/geometry/Pose2.h | 12 ++++++++++-- gtsam/geometry/Pose3.cpp | 28 ++++++++++++++++++++++++++++ gtsam/geometry/Pose3.h | 7 ++++++- gtsam/geometry/Rot3.h | 11 +++++++++-- gtsam/geometry/Rot3M.cpp | 13 +++++++++++++ gtsam/geometry/Rot3Q.cpp | 13 +++++++++++++ 10 files changed, 113 insertions(+), 10 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 133f1821c..aece1ded1 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -95,6 +95,12 @@ Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); } +/* ************************************************************************* */ +Point2 Cal3_S2::uncalibrate(const Point2& p) const { + const double x = p.x(), y = p.y(); + return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); +} + /* ************************************************************************* */ Point2 Cal3_S2::calibrate(const Point2& p) const { const double u = p.x(), v = p.y(); diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index c7996f5d2..45ef782d7 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -150,11 +150,13 @@ public: * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates * @return point in image coordinates */ - Point2 uncalibrate(const Point2& p, boost::optional Dcal = - boost::none, boost::optional Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, boost::optional Dcal, + boost::optional Dp) const; - Point2 uncalibrate(const Point2& p, boost::optional Dcal = - boost::none, boost::optional Dp = boost::none) const; + Point2 uncalibrate(const Point2& p, boost::optional Dcal, + boost::optional Dp) const; + + Point2 uncalibrate(const Point2& p) const; /** * convert image coordinates uv to intrinsic coordinates xy diff --git a/gtsam/geometry/PinholeCamera.h b/gtsam/geometry/PinholeCamera.h index baf450365..095da4daa 100644 --- a/gtsam/geometry/PinholeCamera.h +++ b/gtsam/geometry/PinholeCamera.h @@ -325,7 +325,7 @@ public: } return pi; } else - return K_.uncalibrate(pn, Dcal); + return K_.uncalibrate(pn, Dcal, boost::none); } /** project a point at infinity from world coordinate to the image diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index 85307e322..99c81b488 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -123,6 +123,27 @@ Pose2 Pose2::inverse(boost::optional H1) const { return Pose2(r_.inverse(), r_.unrotate(Point2(-t_.x(), -t_.y()))); } +/* ************************************************************************* */ +// see doc/math.lyx, SE(2) section +Point2 Pose2::transform_to(const Point2& point) const { + Point2 d = point - t_; + return r_.unrotate(d); +} + +/* ************************************************************************* */ +// see doc/math.lyx, SE(2) section +Point2 Pose2::transform_to(const Point2& point, + boost::optional H1, boost::optional H2) const { + Point2 d = point - t_; + Point2 q = r_.unrotate(d); + if (!H1 && !H2) return q; + if (H1) *H1 << + -1.0, 0.0, q.y(), + 0.0, -1.0, -q.x(); + if (H2) *H2 = r_.transpose(); + return q; +} + /* ************************************************************************* */ // see doc/math.lyx, SE(2) section Point2 Pose2::transform_to(const Point2& point, diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 26244877b..91b131bcb 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -182,10 +182,18 @@ public: /// @name Group Action on Point2 /// @{ + /** Return point coordinates in pose coordinate frame */ + Point2 transform_to(const Point2& point) const; + /** Return point coordinates in pose coordinate frame */ Point2 transform_to(const Point2& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + boost::optional H1, + boost::optional H2) const; + + /** Return point coordinates in pose coordinate frame */ + Point2 transform_to(const Point2& point, + boost::optional H1, + boost::optional H2) const; /** Return point coordinates in global frame */ Point2 transform_from(const Point2& point, diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index ea04c1d44..f82c8e588 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -253,6 +253,34 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, return R_ * p + t_; } +/* ************************************************************************* */ +Point3 Pose3::transform_to(const Point3& p) const { + // Only get transpose once, to avoid multiple allocations, + // as well as multiple conversions in the Quaternion case + return R_.unrotate(p - t_); +} + +/* ************************************************************************* */ +Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, + boost::optional Dpoint) const { + // Only get transpose once, to avoid multiple allocations, + // as well as multiple conversions in the Quaternion case + Matrix3 Rt(R_.transpose()); + const Point3 q(Rt*(p - t_).vector()); + if (Dpose) { + double wx = q.x(); + double wy = q.y(); + double wz = q.z(); + (*Dpose) << + 0.0, -wz, +wy,-1.0, 0.0, 0.0, + +wz, 0.0, -wx, 0.0,-1.0, 0.0, + -wy, +wx, 0.0, 0.0, 0.0,-1.0; + } + if (Dpoint) + *Dpoint = Rt; + return q; +} + /* ************************************************************************* */ Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, boost::optional Dpoint) const { diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 825389243..55cda05a8 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -250,8 +250,13 @@ public: * @param Dpoint optional 3*3 Jacobian wrpt point * @return point in Pose coordinates */ + Point3 transform_to(const Point3& p) const; + Point3 transform_to(const Point3& p, - boost::optional Dpose=boost::none, boost::optional Dpoint=boost::none) const; + boost::optional Dpose, boost::optional Dpoint) const; + + Point3 transform_to(const Point3& p, + boost::optional Dpose, boost::optional Dpoint) const; /// @} /// @name Standard Interface diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index c8aeae51b..fa9787076 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -219,8 +219,15 @@ namespace gtsam { Rot3 inverse(boost::optional H1=boost::none) const; /// Compose two rotations i.e., R= (*this) * R2 - Rot3 compose(const Rot3& R2, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + Rot3 compose(const Rot3& R2) const; + + /// Compose two rotations i.e., R= (*this) * R2 + Rot3 compose(const Rot3& R2, boost::optional H1, + boost::optional H2) const; + + /// Compose two rotations i.e., R= (*this) * R2 + Rot3 compose(const Rot3& R2, boost::optional H1, + boost::optional H2) const; /** compose two rotations */ Rot3 operator*(const Rot3& R2) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 118d8546e..7db3acaa2 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -143,6 +143,19 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) { -swy + C02, swx + C12, c + C22); } +/* ************************************************************************* */ +Rot3 Rot3::compose (const Rot3& R2) const { + return *this * R2; +} + +/* ************************************************************************* */ +Rot3 Rot3::compose (const Rot3& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I3; + return *this * R2; +} + /* ************************************************************************* */ Rot3 Rot3::compose (const Rot3& R2, boost::optional H1, boost::optional H2) const { diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index c5990153a..dd0d39ffa 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -82,6 +82,19 @@ namespace gtsam { Rot3 Rot3::rodriguez(const Vector& w, double theta) { return Quaternion(Eigen::AngleAxisd(theta, w)); } + /* ************************************************************************* */ + Rot3 Rot3::compose(const Rot3& R2) const { + return Rot3(quaternion_ * R2.quaternion_); + } + + /* ************************************************************************* */ + Rot3 Rot3::compose(const Rot3& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I3; + return Rot3(quaternion_ * R2.quaternion_); + } + /* ************************************************************************* */ Rot3 Rot3::compose(const Rot3& R2, boost::optional H1, boost::optional H2) const {