diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp new file mode 100644 index 000000000..34b86cd80 --- /dev/null +++ b/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 +#include +#include + +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 HR, boost::optional 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 H1, boost::optional 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 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 + diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 327d5feb0..03e7c0e0d 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/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 H1, boost::optional 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 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 HR, boost::optional 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 H1, boost::optional 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 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 diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 8e0f46e92..837972805 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/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 H1, boost::optional H2) const { @@ -108,9 +91,8 @@ namespace gtsam { } /* ************************************************************************* */ - Point3 Rot3::operator*(const Point3& p) const { - Eigen::Vector3d r = quaternion_ * Eigen::Vector3d(p.x(), p.y(), p.z()); - return Point3(r(0), r(1), r(2)); + Rot3 Rot3::operator*(const Rot3& R2) const { + return Rot3(quaternion_ * R2.quaternion_); } /* ************************************************************************* */ @@ -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 H1, boost::optional 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 H1, boost::optional 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) { @@ -175,22 +141,10 @@ namespace gtsam { } /* ************************************************************************* */ - Matrix3 Rot3::matrix() const { return quaternion_.toRotationMatrix(); } + Matrix3 Rot3::matrix() const {return quaternion_.toRotationMatrix();} /* ************************************************************************* */ - 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"); - } + Matrix3 Rot3::transpose() const {return quaternion_.toRotationMatrix().transpose();} /* ************************************************************************* */ 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 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 diff --git a/gtsam_unstable/geometry/triangulation.cpp b/gtsam_unstable/geometry/triangulation.cpp index b6a90c83c..559871059 100644 --- a/gtsam_unstable/geometry/triangulation.cpp +++ b/gtsam_unstable/geometry/triangulation.cpp @@ -16,12 +16,8 @@ * @author Chris Beall */ -#pragma once - #include - - namespace gtsam { /**