Moved all common methods in new file Rot3.cpp

git-svn-id: https://svn.cc.gatech.edu/borg/gtsam/trunk@20416 898a188c-9671-0410-8e00-e3fd810bbb7f

gtsam/geometry/Rot3.cpp

@@ -0,0 +1,178 @@
+/* ----------------------------------------------------------------------------
+
+ * GTSAM Copyright 2010, Georgia Tech Research Corporation, 
+ * Atlanta, Georgia 30332-0415
+ * All Rights Reserved
+ * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
+
+ * See LICENSE for the license information
+
+ * -------------------------------------------------------------------------- */
+
+/**
+ * @file    Rot3.cpp
+ * @brief   Rotation, common code between Rotation matrix and Quaternion
+ * @author  Alireza Fathi
+ * @author  Christian Potthast
+ * @author  Frank Dellaert
+ * @author  Richard Roberts
+ */
+
+#include <gtsam/geometry/Rot3.h>
+#include <boost/math/constants/constants.hpp>
+#include <cmath>
+
+using namespace std;
+
+namespace gtsam {
+
+static const Matrix3 I3 = Matrix3::Identity();
+
+/* ************************************************************************* */
+void Rot3::print(const std::string& s) const {
+  gtsam::print((Matrix)matrix(), s);
+}
+
+/* ************************************************************************* */
+Rot3 Rot3::rodriguez(const Vector& w) {
+  double t = w.norm();
+  if (t < 1e-10) return Rot3();
+  return rodriguez(w/t, t);
+}
+
+/* ************************************************************************* */
+bool Rot3::equals(const Rot3 & R, double tol) const {
+  return equal_with_abs_tol(matrix(), R.matrix(), tol);
+}
+
+/* ************************************************************************* */
+Point3 Rot3::operator*(const Point3& p) const {
+  return rotate(p);
+}
+
+/* ************************************************************************* */
+Sphere2 Rot3::rotate(const Sphere2& p,
+    boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
+  Sphere2 q = rotate(p.point3(Hp));
+  if (Hp)
+    (*Hp) = q.basis().transpose() * matrix() * (*Hp);
+  if (HR)
+    (*HR) = -q.basis().transpose() * matrix() * p.skew();
+  return q;
+}
+
+/* ************************************************************************* */
+Sphere2 Rot3::operator*(const Sphere2& p) const {
+  return rotate(p);
+}
+
+/* ************************************************************************* */
+// see doc/math.lyx, SO(3) section
+Point3 Rot3::unrotate(const Point3& p,
+    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
+  const Matrix Rt(transpose());
+  Point3 q(Rt*p.vector()); // q = Rt*p
+  if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
+  if (H2) *H2 = Rt;
+  return q;
+}
+
+/* ************************************************************************* */
+/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
+Matrix3 Rot3::dexpL(const Vector3& v) {
+  if(zero(v)) return eye(3);
+  Matrix x = skewSymmetric(v);
+  Matrix x2 = x*x;
+  double theta = v.norm(), vi = theta/2.0;
+  double s1 = sin(vi)/vi;
+  double s2 = (theta - sin(theta))/(theta*theta*theta);
+  Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
+  return res;
+}
+
+/* ************************************************************************* */
+/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
+Matrix3 Rot3::dexpInvL(const Vector3& v) {
+  if(zero(v)) return eye(3);
+  Matrix x = skewSymmetric(v);
+  Matrix x2 = x*x;
+  double theta = v.norm(), vi = theta/2.0;
+  double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
+  Matrix res = eye(3) + 0.5*x - s2*x2;
+  return res;
+}
+
+
+/* ************************************************************************* */
+Point3 Rot3::column(int index) const{
+  if(index == 3)
+    return r3();
+  else if(index == 2)
+    return r2();
+  else if(index == 1)
+    return r1(); // default returns r1
+  else
+    throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
+}
+
+/* ************************************************************************* */
+Vector3 Rot3::xyz() const {
+  Matrix I;Vector3 q;
+  boost::tie(I,q)=RQ(matrix());
+  return q;
+}
+
+/* ************************************************************************* */
+Vector3 Rot3::ypr() const {
+  Vector3 q = xyz();
+  return Vector3(q(2),q(1),q(0));
+}
+
+/* ************************************************************************* */
+Vector3 Rot3::rpy() const {
+  return xyz();
+}
+
+/* ************************************************************************* */
+Vector Rot3::quaternion() const {
+  Quaternion q = toQuaternion();
+  Vector v(4);
+  v(0) = q.w();
+  v(1) = q.x();
+  v(2) = q.y();
+  v(3) = q.z();
+  return v;
+}
+
+/* ************************************************************************* */
+pair<Matrix3, Vector3> RQ(const Matrix3& A) {
+
+  double x = -atan2(-A(2, 1), A(2, 2));
+  Rot3 Qx = Rot3::Rx(-x);
+  Matrix3 B = A * Qx.matrix();
+
+  double y = -atan2(B(2, 0), B(2, 2));
+  Rot3 Qy = Rot3::Ry(-y);
+  Matrix3 C = B * Qy.matrix();
+
+  double z = -atan2(-C(1, 0), C(1, 1));
+  Rot3 Qz = Rot3::Rz(-z);
+  Matrix3 R = C * Qz.matrix();
+
+  Vector xyz = Vector3(x, y, z);
+  return make_pair(R, xyz);
+}
+
+/* ************************************************************************* */
+ostream &operator<<(ostream &os, const Rot3& R) {
+  os << "\n";
+  os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
+  os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
+  os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
+  return os;
+}
+
+/* ************************************************************************* */
+
+} // namespace gtsam
+

gtsam/geometry/Rot3M.cpp

@@ -69,11 +69,6 @@ Rot3::Rot3(const Matrix& R) {
 /* ************************************************************************* */
 Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {}
 
-/* ************************************************************************* */
-void Rot3::print(const std::string& s) const {
-  gtsam::print((Matrix)matrix(), s);
-}
-
 /* ************************************************************************* */
 Rot3 Rot3::Rx(double t) {
   double st = sin(t), ct = cos(t);
@@ -148,18 +143,6 @@ Rot3 Rot3::rodriguez(const Vector& w, double theta) {
      -swy + C02,  swx + C12,    c + C22);
 }
 
-/* ************************************************************************* */
-Rot3 Rot3::rodriguez(const Vector& w) {
-  double t = w.norm();
-  if (t < 1e-10) return Rot3();
-  return rodriguez(w/t, t);
-}
-
-/* ************************************************************************* */
-bool Rot3::equals(const Rot3 & R, double tol) const {
-  return equal_with_abs_tol(matrix(), R.matrix(), tol);
-}
-
 /* ************************************************************************* */
 Rot3 Rot3::compose (const Rot3& R2,
     boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@@ -169,7 +152,9 @@ Rot3 Rot3::compose (const Rot3& R2,
 }
 
 /* ************************************************************************* */
-Point3 Rot3::operator*(const Point3& p) const { return rotate(p); }
+Rot3 Rot3::operator*(const Rot3& R2) const {
+  return Rot3(Matrix3(rot_*R2.rot_));
+}
 
 /* ************************************************************************* */
 Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
@@ -183,12 +168,6 @@ Rot3 Rot3::between (const Rot3& R2,
   if (H1) *H1 = -(R2.transpose()*rot_);
   if (H2) *H2 = I3;
   return Rot3(Matrix3(rot_.transpose()*R2.rot_));
-  //return between_default(*this, R2);
-}
-
-/* ************************************************************************* */
-Rot3 Rot3::operator*(const Rot3& R2) const {
-  return Rot3(Matrix3(rot_*R2.rot_));
 }
 
 /* ************************************************************************* */
@@ -201,32 +180,6 @@ Point3 Rot3::rotate(const Point3& p,
   return Point3(rot_ * p.vector());
 }
 
-/* ************************************************************************* */
-Sphere2 Rot3::rotate(const Sphere2& p,
-    boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
-  Sphere2 q(rotate(p.point3(Hp)));
-  if (Hp)
-    (*Hp) = q.basis().transpose() * matrix() * (*Hp);
-  if (HR)
-    (*HR) = -q.basis().transpose() * matrix() * p.skew();
-  return q;
-}
-
-/* ************************************************************************* */
-Sphere2 Rot3::operator*(const Sphere2& p) const {
-  return rotate(p);
-}
-
-/* ************************************************************************* */
-// see doc/math.lyx, SO(3) section
-Point3 Rot3::unrotate(const Point3& p,
-    boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-  Point3 q(rot_.transpose()*p.vector()); // q = Rt*p
-  if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
-  if (H2) *H2 = transpose();
-  return q;
-}
-
 /* ************************************************************************* */
 // Log map at identity - return the canonical coordinates of this rotation
 Vector3 Rot3::Logmap(const Rot3& R) {
@@ -324,32 +277,6 @@ Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const
   }
 }
 
-/* ************************************************************************* */
-/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
-Matrix3 Rot3::dexpL(const Vector3& v) {
-  if(zero(v)) return eye(3);
-  Matrix x = skewSymmetric(v);
-  Matrix x2 = x*x;
-  double theta = v.norm(), vi = theta/2.0;
-  double s1 = sin(vi)/vi;
-  double s2 = (theta - sin(theta))/(theta*theta*theta);
-  Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
-  return res;
-}
-
-/* ************************************************************************* */
-/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
-Matrix3 Rot3::dexpInvL(const Vector3& v) {
-  if(zero(v)) return eye(3);
-  Matrix x = skewSymmetric(v);
-  Matrix x2 = x*x;
-  double theta = v.norm(), vi = theta/2.0;
-  double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
-  Matrix res = eye(3) + 0.5*x - s2*x2;
-  return res;
-}
-
-
 /* ************************************************************************* */
 Matrix3 Rot3::matrix() const {
   return rot_;
@@ -360,18 +287,6 @@ Matrix3 Rot3::transpose() const {
   return rot_.transpose();
 }
 
-/* ************************************************************************* */
-Point3 Rot3::column(int index) const{
-  if(index == 3)
-    return r3();
-  else if(index == 2)
-    return r2();
-  else if(index == 1)
-    return r1(); // default returns r1
-  else
-    throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
-}
-
 /* ************************************************************************* */
 Point3 Rot3::r1() const { return Point3(rot_.col(0)); }
 
@@ -381,68 +296,11 @@ Point3 Rot3::r2() const { return Point3(rot_.col(1)); }
 /* ************************************************************************* */
 Point3 Rot3::r3() const { return Point3(rot_.col(2)); }
 
-/* ************************************************************************* */
-Vector3 Rot3::xyz() const {
-  Matrix3 I;Vector3 q;
-  boost::tie(I,q)=RQ(rot_);
-  return q;
-}
-
-/* ************************************************************************* */
-Vector3 Rot3::ypr() const {
-  Vector3 q = xyz();
-  return Vector3(q(2),q(1),q(0));
-}
-
-/* ************************************************************************* */
-Vector3 Rot3::rpy() const {
-  return xyz();
-}
-
 /* ************************************************************************* */
 Quaternion Rot3::toQuaternion() const {
   return Quaternion(rot_);
 }
 
-/* ************************************************************************* */
-Vector Rot3::quaternion() const {
-  Quaternion q = toQuaternion();
-  Vector v(4);
-  v(0) = q.w();
-  v(1) = q.x();
-  v(2) = q.y();
-  v(3) = q.z();
-  return v;
-}
-
-/* ************************************************************************* */
-pair<Matrix3, Vector3> RQ(const Matrix3& A) {
-
-  double x = -atan2(-A(2, 1), A(2, 2));
-  Rot3 Qx = Rot3::Rx(-x);
-  Matrix3 B = A * Qx.matrix();
-
-  double y = -atan2(B(2, 0), B(2, 2));
-  Rot3 Qy = Rot3::Ry(-y);
-  Matrix3 C = B * Qy.matrix();
-
-  double z = -atan2(-C(1, 0), C(1, 1));
-  Rot3 Qz = Rot3::Rz(-z);
-  Matrix3 R = C * Qz.matrix();
-
-  Vector xyz = Vector3(x, y, z);
-  return make_pair(R, xyz);
-}
-
-/* ************************************************************************* */
-ostream &operator<<(ostream &os, const Rot3& R) {
-  os << "\n";
-  os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
-  os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
-  os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
-  return os;
-}
-
 /* ************************************************************************* */
 
 } // namespace gtsam

gtsam/geometry/Rot3Q.cpp

@@ -62,11 +62,6 @@ namespace gtsam {
   /* ************************************************************************* */
   Rot3::Rot3(const Quaternion& q) : quaternion_(q) {}
 
-  /* ************************************************************************* */
-  void Rot3::print(const std::string& s) const {
-    gtsam::print((Matrix)matrix(), s);
-  }
-
   /* ************************************************************************* */
   Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); }
 
@@ -87,18 +82,6 @@ namespace gtsam {
   Rot3 Rot3::rodriguez(const Vector& w, double theta) {
     return Quaternion(Eigen::AngleAxisd(theta, w)); }
 
-  /* ************************************************************************* */
-  Rot3 Rot3::rodriguez(const Vector& w) {
-    double t = w.norm();
-    if (t < 1e-10) return Rot3();
-    return rodriguez(w/t, t);
-  }
-
-  /* ************************************************************************* */
-  bool Rot3::equals(const Rot3 & R, double tol) const {
-    return equal_with_abs_tol(matrix(), R.matrix(), tol);
-  }
-
   /* ************************************************************************* */
   Rot3 Rot3::compose(const Rot3& R2,
   boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
@@ -108,9 +91,8 @@ namespace gtsam {
   }
 
   /* ************************************************************************* */
-  Point3 Rot3::operator*(const Point3& p) const {
+  Rot3 Rot3::operator*(const Rot3& R2) const {
-    Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z());
+    return Rot3(quaternion_ * R2.quaternion_);
-    return Point3(r(0), r(1), r(2));
   }
 
   /* ************************************************************************* */
@@ -127,11 +109,6 @@ namespace gtsam {
     return between_default(*this, R2);
   }
 
-  /* ************************************************************************* */
-  Rot3 Rot3::operator*(const Rot3& R2) const {
-    return Rot3(quaternion_ * R2.quaternion_);
-  }
-
   /* ************************************************************************* */
   Point3 Rot3::rotate(const Point3& p,
         boost::optional<Matrix&> H1,  boost::optional<Matrix&> H2) const {
@@ -142,17 +119,6 @@ namespace gtsam {
     return Point3(r.x(), r.y(), r.z());
   }
 
-  /* ************************************************************************* */
-  // see doc/math.lyx, SO(3) section
-  Point3 Rot3::unrotate(const Point3& p,
-      boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
-    const Matrix Rt(transpose());
-    Point3 q(Rt*p.vector()); // q = Rt*p
-    if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
-    if (H2) *H2 = Rt;
-    return q;
-  }
-
   /* ************************************************************************* */
   // Log map at identity - return the canonical coordinates of this rotation
   Vector3 Rot3::Logmap(const Rot3& R) {
@@ -180,18 +146,6 @@ namespace gtsam {
   /* ************************************************************************* */
   Matrix3 Rot3::transpose() const {return quaternion_.toRotationMatrix().transpose();}
 
-  /* ************************************************************************* */
-  Point3 Rot3::column(int index) const{
-    if(index == 3)
-      return r3();
-    else if(index == 2)
-      return r2();
-    else if(index == 1)
-      return r1(); // default returns r1
-    else
-      throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
-  }
-
   /* ************************************************************************* */
   Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); }
 
@@ -201,55 +155,10 @@ namespace gtsam {
   /* ************************************************************************* */
   Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); }
 
-  /* ************************************************************************* */
-  Vector3 Rot3::xyz() const {
-    Matrix I;Vector3 q;
-    boost::tie(I,q)=RQ(matrix());
-    return q;
-  }
-
-  /* ************************************************************************* */
-  Vector3 Rot3::ypr() const {
-    Vector3 q = xyz();
-    return Vector3(q(2),q(1),q(0));
-  }
-
-  /* ************************************************************************* */
-  Vector3 Rot3::rpy() const {
-    Vector3 q = xyz();
-    return Vector3(q(0),q(1),q(2));
-  }
-
   /* ************************************************************************* */
   Quaternion Rot3::toQuaternion() const { return quaternion_; }
 
  /* ************************************************************************* */
-  pair<Matrix3, Vector3> RQ(const Matrix3& A) {
-
-    double x = -atan2(-A(2, 1), A(2, 2));
-    Rot3 Qx = Rot3::Rx(-x);
-    Matrix3 B = A * Qx.matrix();
-
-    double y = -atan2(B(2, 0), B(2, 2));
-    Rot3 Qy = Rot3::Ry(-y);
-    Matrix3 C = B * Qy.matrix();
-
-    double z = -atan2(-C(1, 0), C(1, 1));
-    Rot3 Qz = Rot3::Rz(-z);
-    Matrix3 R = C * Qz.matrix();
-
-    Vector xyz = Vector3(x, y, z);
-    return make_pair(R, xyz);
-  }
-
-  /* ************************************************************************* */
-  ostream &operator<<(ostream &os, const Rot3& R) {
-    os << "\n";
-    os << '|' << R.r1().x() << ", " << R.r2().x() << ", " << R.r3().x() << "|\n";
-    os << '|' << R.r1().y() << ", " << R.r2().y() << ", " << R.r3().y() << "|\n";
-    os << '|' << R.r1().z() << ", " << R.r2().z() << ", " << R.r3().z() << "|\n";
-    return os;
-  }
 
 } // namespace gtsam
 

gtsam_unstable/geometry/triangulation.cpp

@@ -16,12 +16,8 @@
  * @author Chris Beall
  */
 
-#pragma once
-
 #include <gtsam_unstable/geometry/triangulation.h>
 
-
-
 namespace gtsam {
 
 /**