diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 0ee19ef76..1b9285100 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -286,9 +286,10 @@ Matrix4 Pose3::matrix() const { } /* ************************************************************************* */ -Pose3 Pose3::transformPoseFrom(const Pose3& aTb) const { +Pose3 Pose3::transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself, + OptionalJacobian<6, 6> HaTb) const { const Pose3& wTa = *this; - return wTa * aTb; + return wTa.compose(aTb, Hself, HaTb); } /* ************************************************************************* */ @@ -310,28 +311,28 @@ Pose3 Pose3::transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself, } /* ************************************************************************* */ -Point3 Pose3::transformFrom(const Point3& p, OptionalJacobian<3, 6> Hself, +Point3 Pose3::transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself, OptionalJacobian<3, 3> Hpoint) const { // Only get matrix once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 R = R_.matrix(); if (Hself) { - Hself->leftCols<3>() = R * skewSymmetric(-p.x(), -p.y(), -p.z()); + Hself->leftCols<3>() = R * skewSymmetric(-point.x(), -point.y(), -point.z()); Hself->rightCols<3>() = R; } if (Hpoint) { *Hpoint = R; } - return R_ * p + t_; + return R_ * point + t_; } /* ************************************************************************* */ -Point3 Pose3::transformTo(const Point3& p, OptionalJacobian<3, 6> Hself, +Point3 Pose3::transformTo(const Point3& point, OptionalJacobian<3, 6> Hself, OptionalJacobian<3, 3> Hpoint) const { // Only get transpose once, to avoid multiple allocations, // as well as multiple conversions in the Quaternion case const Matrix3 Rt = R_.transpose(); - const Point3 q(Rt*(p - t_)); + const Point3 q(Rt*(point - t_)); if (Hself) { const double wx = q.x(), wy = q.y(), wz = q.z(); (*Hself) << diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index f56e6903a..3825b6241 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -201,27 +201,27 @@ public: /** * @brief takes point in Pose coordinates and transforms it to world coordinates - * @param p point in Pose coordinates + * @param point point in Pose coordinates * @param Hself optional 3*6 Jacobian wrpt this pose * @param Hpoint optional 3*3 Jacobian wrpt point * @return point in world coordinates */ - Point3 transformFrom(const Point3& p, OptionalJacobian<3, 6> Hself = + Point3 transformFrom(const Point3& point, OptionalJacobian<3, 6> Hself = boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const; /** syntactic sugar for transformFrom */ - inline Point3 operator*(const Point3& p) const { - return transformFrom(p); + inline Point3 operator*(const Point3& point) const { + return transformFrom(point); } /** * @brief takes point in world coordinates and transforms it to Pose coordinates - * @param p point in world coordinates + * @param point point in world coordinates * @param Hself optional 3*6 Jacobian wrpt this pose * @param Hpoint optional 3*3 Jacobian wrpt point * @return point in Pose coordinates */ - Point3 transformTo(const Point3& p, OptionalJacobian<3, 6> Hself = + Point3 transformTo(const Point3& point, OptionalJacobian<3, 6> Hself = boost::none, OptionalJacobian<3, 3> Hpoint = boost::none) const; /// @} @@ -252,12 +252,20 @@ public: /** convert to 4*4 matrix */ Matrix4 matrix() const; - /** receives a pose in local coordinates and transforms it to world coordinates */ - Pose3 transformPoseFrom(const Pose3& pose) const; + /** + * Assuming self == wTa, takes a pose aTb in local coordinates + * and transforms it to world coordinates wTb = wTa * aTb. + * This is identical to compose. + */ + Pose3 transformPoseFrom(const Pose3& aTb, OptionalJacobian<6, 6> Hself = boost::none, + OptionalJacobian<6, 6> HaTb = boost::none) const; - /** receives a pose in world coordinates and transforms it to local coordinates */ - Pose3 transformPoseTo(const Pose3& pose, OptionalJacobian<6, 6> Hself = boost::none, - OptionalJacobian<6, 6> Hpose = boost::none) const; + /** + * Assuming self == wTa, takes a pose wTb in world coordinates + * and transforms it to local coordinates aTb = inv(wTa) * wTb + */ + Pose3 transformPoseTo(const Pose3& wTb, OptionalJacobian<6, 6> Hself = boost::none, + OptionalJacobian<6, 6> HwTb = boost::none) const; /** * Calculate range to a landmark