Faster versions

gtsam/geometry/Pose3.cpp

@@ -255,8 +255,6 @@ Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose,
 
 /* ************************************************************************* */
 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_);
 }
 
@@ -268,9 +266,7 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
   Matrix3 Rt(R_.transpose());
   const Point3 q(Rt*(p - t_).vector());
   if (Dpose) {
-    double wx = q.x();
+    const double wx = q.x(), wy = q.y(), wz = q.z();
-    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,
@@ -284,14 +280,10 @@ Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix36&> Dpose,
 /* ************************************************************************* */
 Point3 Pose3::transform_to(const Point3& p, boost::optional<Matrix&> Dpose,
     boost::optional<Matrix&> 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();
+    const double wx = q.x(), wy = q.y(), wz = q.z();
-    double wy = q.y();
-    double wz = q.z();
     Dpose->resize(3, 6);
     (*Dpose) <<
         0.0, -wz, +wy,-1.0, 0.0, 0.0,

gtsam/geometry/Rot3.cpp

@@ -97,13 +97,33 @@ Unit3 Rot3::operator*(const Unit3& p) const {
   return rotate(p);
 }
 
+/* ************************************************************************* */
+// see doc/math.lyx, SO(3) section
+Point3 Rot3::unrotate(const Point3& p, boost::optional<Matrix3&> H1,
+    boost::optional<Matrix3&> H2) const {
+  Matrix3 Rt(transpose());
+  Point3 q(Rt * p.vector()); // q = Rt*p
+  const double wx = q.x(), wy = q.y(), wz = q.z();
+  if (H1)
+    *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
+  if (H2)
+    *H2 = Rt;
+  return q;
+}
+
 /* ************************************************************************* */
 // see doc/math.lyx, SO(3) section
 Point3 Rot3::unrotate(const Point3& p,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-  Point3 q(transpose()*p.vector()); // q = Rt*p
+  Matrix3 Rt(transpose());
-  if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
+  Point3 q(Rt * p.vector()); // q = Rt*p
-  if (H2) *H2 = transpose();
+  const double wx = q.x(), wy = q.y(), wz = q.z();
+  if (H1) {
+    H1->resize(3,3);
+    *H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
+  }
+  if (H2)
+    *H2 = Rt;
   return q;
 }
 

gtsam/geometry/Rot3.h

@@ -335,11 +335,16 @@ namespace gtsam {
     /// rotate point from rotated coordinate frame to world = R*p
     Point3 operator*(const Point3& p) const;
 
-    /**
+    /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
-     * rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
+    Point3 unrotate(const Point3& p) const;
-     */
+
-    Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1 = boost::none,
+    /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
-        boost::optional<Matrix&> H2 = boost::none) const;
+    Point3 unrotate(const Point3& p, boost::optional<Matrix3&> H1,
+        boost::optional<Matrix3&> H2) const;
+
+    /// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
+    Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1,
+        boost::optional<Matrix&> H2) const;
 
     /// @}
     /// @name Group Action on Unit3

gtsam/geometry/Rot3M.cpp

@@ -314,6 +314,12 @@ Quaternion Rot3::toQuaternion() const {
   return Quaternion(rot_);
 }
 
+/* ************************************************************************* */
+Point3 Rot3::unrotate(const Point3& p) const {
+  // Eigen expression
+  return Point3(rot_.transpose()*p.vector()); // q = Rt*p
+}
+
 /* ************************************************************************* */
 
 } // namespace gtsam

gtsam/geometry/Rot3Q.cpp

@@ -172,6 +172,11 @@ namespace gtsam {
   Quaternion Rot3::toQuaternion() const { return quaternion_; }
 
   /* ************************************************************************* */
+  Point3 Rot3::unrotate(const Point3& p) const {
+    return Point3(transpose()*p.vector()); // q = Rt*p
+  }
+
+ /* ************************************************************************* */
 
 } // namespace gtsam