From 30508264d5a1a8ea9bb345bd7ef91455b1f83473 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Mon, 2 Jan 2012 16:17:27 +0000 Subject: [PATCH] Reordered functions to be in the same order in the header and cpp files --- gtsam/geometry/Rot3.h | 48 +++++------ gtsam/geometry/Rot3M.cpp | 172 +++++++++++++++++++-------------------- gtsam/geometry/Rot3Q.cpp | 130 ++++++++++++++--------------- 3 files changed, 175 insertions(+), 175 deletions(-) diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 7f65cacee..4aeadb0ed 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -199,6 +199,30 @@ namespace gtsam { /// derivative of inverse rotation R^T s.t. inverse(R)*R = identity Rot3 inverse(boost::optional H1=boost::none) const; + /** + * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' + */ + Rot3 between(const Rot3& R2, + boost::optional H1=boost::none, + boost::optional H2=boost::none) const; + + /** compose two rotations */ + Rot3 operator*(const Rot3& R2) const; + + /** + * rotate point from rotated coordinate frame to + * world = R*p + */ + Point3 rotate(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + + /** + * rotate point from world to rotated + * frame = R'*p + */ + Point3 unrotate(const Point3& p, + boost::optional H1=boost::none, boost::optional H2=boost::none) const; + /// @} /// @name Manifold /// @{ @@ -294,30 +318,6 @@ namespace gtsam { */ Quaternion toQuaternion() const; - /** - * Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1' - */ - Rot3 between(const Rot3& R2, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; - - /** compose two rotations */ - Rot3 operator*(const Rot3& R2) const; - - /** - * rotate point from rotated coordinate frame to - * world = R*p - */ - Point3 rotate(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; - - /** - * rotate point from world to rotated - * frame = R'*p - */ - Point3 unrotate(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; - private: /** Serialization function */ friend class boost::serialization::access; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index f83764565..9b0d93f1c 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -147,6 +147,92 @@ bool Rot3M::equals(const Rot3M & R, double tol) const { return equal_with_abs_tol(matrix(), R.matrix(), tol); } +/* ************************************************************************* */ +Rot3M Rot3M::compose (const Rot3M& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I3; + return *this * R2; +} + +/* ************************************************************************* */ +Point3 Rot3M::operator*(const Point3& p) const { return rotate(p); } + +/* ************************************************************************* */ +Rot3M Rot3M::inverse(boost::optional H1) const { + if (H1) *H1 = -matrix(); + return Rot3M( + r1_.x(), r1_.y(), r1_.z(), + r2_.x(), r2_.y(), r2_.z(), + r3_.x(), r3_.y(), r3_.z()); +} + +/* ************************************************************************* */ +Rot3M Rot3M::between (const Rot3M& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = -(R2.transpose()*matrix()); + if (H2) *H2 = I3; + return between_default(*this, R2); +} + +/* ************************************************************************* */ +Rot3M Rot3M::operator*(const Rot3M& R2) const { + return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_)); +} + +/* ************************************************************************* */ +Point3 Rot3M::rotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = matrix(); + return r1_ * p.x() + r2_ * p.y() + r3_ * p.z(); +} + +/* ************************************************************************* */ +// see doc/math.lyx, SO(3) section +Point3 Rot3M::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 +Vector Rot3M::Logmap(const Rot3M& R) { + double tr = R.r1().x()+R.r2().y()+R.r3().z(); + // FIXME should tr in statement below be absolute value? + if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10) + return zero(3); + } else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3) + double theta = acos((tr-1.0)/2.0); + // Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4) + return (0.5 + theta*theta/12)*Vector_(3, + R.r2().z()-R.r3().y(), + R.r3().x()-R.r1().z(), + R.r1().y()-R.r2().x()); + // FIXME: in statement below, is this the right comparision? + } else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. + if(fabs(R.r3().z() - -1.0) > 1e-10) + return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r3().z())) * + Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z()); + else if(fabs(R.r2().y() - -1.0) > 1e-10) + return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r2().y())) * + Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z()); + else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit + return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r1().x())) * + Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z()); + } else { + double theta = acos((tr-1.0)/2.0); + return (theta/2.0/sin(theta))*Vector_(3, + R.r2().z()-R.r3().y(), + R.r3().x()-R.r1().z(), + R.r1().y()-R.r2().x()); + } +} + /* ************************************************************************* */ Matrix Rot3M::matrix() const { Matrix R(3,3); @@ -207,79 +293,6 @@ Vector Rot3M::rpy() const { return Vector_(3,q(0),q(1),q(2)); } -/* ************************************************************************* */ -// Log map at identity - return the canonical coordinates of this rotation -Vector Rot3M::Logmap(const Rot3M& R) { - double tr = R.r1().x()+R.r2().y()+R.r3().z(); - // FIXME should tr in statement below be absolute value? - if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10) - return zero(3); - } else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3) - double theta = acos((tr-1.0)/2.0); - // Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4) - return (0.5 + theta*theta/12)*Vector_(3, - R.r2().z()-R.r3().y(), - R.r3().x()-R.r1().z(), - R.r1().y()-R.r2().x()); - // FIXME: in statement below, is this the right comparision? - } else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. - if(fabs(R.r3().z() - -1.0) > 1e-10) - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r3().z())) * - Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z()); - else if(fabs(R.r2().y() - -1.0) > 1e-10) - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r2().y())) * - Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z()); - else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit - return (boost::math::constants::pi() / sqrt(2.0+2.0*R.r1().x())) * - Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z()); - } else { - double theta = acos((tr-1.0)/2.0); - return (theta/2.0/sin(theta))*Vector_(3, - R.r2().z()-R.r3().y(), - R.r3().x()-R.r1().z(), - R.r1().y()-R.r2().x()); - } -} - -/* ************************************************************************* */ -Point3 Rot3M::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z()); - if (H2) *H2 = matrix(); - return r1_ * p.x() + r2_ * p.y() + r3_ * p.z(); -} - -/* ************************************************************************* */ -// see doc/math.lyx, SO(3) section -Point3 Rot3M::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; -} - -/* ************************************************************************* */ -Rot3M Rot3M::compose (const Rot3M& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return *this * R2; -} - -/* ************************************************************************* */ -Point3 Rot3M::operator*(const Point3& p) const { return rotate(p); } - -/* ************************************************************************* */ -Rot3M Rot3M::inverse(boost::optional H1) const { - if (H1) *H1 = -matrix(); - return Rot3M( - r1_.x(), r1_.y(), r1_.z(), - r2_.x(), r2_.y(), r2_.z(), - r3_.x(), r3_.y(), r3_.z()); -} - /* ************************************************************************* */ Quaternion Rot3M::toQuaternion() const { return Quaternion((Eigen::Matrix3d() << @@ -288,19 +301,6 @@ Quaternion Rot3M::toQuaternion() const { r1_.z(), r2_.z(), r3_.z()).finished()); } -/* ************************************************************************* */ -Rot3M Rot3M::between (const Rot3M& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = -(R2.transpose()*matrix()); - if (H2) *H2 = I3; - return between_default(*this, R2); -} - -/* ************************************************************************* */ -Rot3M Rot3M::operator*(const Rot3M& R2) const { - return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_)); -} - /* ************************************************************************* */ pair RQ(const Matrix& A) { diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 232e896f3..082a8c403 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -87,6 +87,71 @@ namespace gtsam { return equal_with_abs_tol(matrix(), R.matrix(), tol); } + /* ************************************************************************* */ + Rot3Q Rot3Q::compose(const Rot3Q& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I3; + return Rot3Q(quaternion_ * R2.quaternion_); + } + + /* ************************************************************************* */ + Point3 Rot3Q::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)); + } + + /* ************************************************************************* */ + Rot3Q Rot3Q::inverse(boost::optional H1) const { + if (H1) *H1 = -matrix(); + return Rot3Q(quaternion_.inverse()); + } + + /* ************************************************************************* */ + Rot3Q Rot3Q::between(const Rot3Q& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = -(R2.transpose()*matrix()); + if (H2) *H2 = I3; + return between_default(*this, R2); + } + + /* ************************************************************************* */ + Rot3Q Rot3Q::operator*(const Rot3Q& R2) const { + return Rot3Q(quaternion_ * R2.quaternion_); + } + + /* ************************************************************************* */ + Point3 Rot3Q::rotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + Matrix R = matrix(); + if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = R; + Eigen::Vector3d r = R * p.vector(); + return Point3(r.x(), r.y(), r.z()); + } + + /* ************************************************************************* */ + // see doc/math.lyx, SO(3) section + Point3 Rot3Q::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 + Vector Rot3Q::Logmap(const Rot3Q& R) { + Eigen::AngleAxisd angleAxis(R.quaternion_); + if(angleAxis.angle() > M_PI) // Important: use the smallest possible + angleAxis.angle() -= 2.0*M_PI; // angle, e.g. no more than PI, to keep + if(angleAxis.angle() < -M_PI) // error continuous. + angleAxis.angle() += 2.0*M_PI; + return angleAxis.axis() * angleAxis.angle(); + } + /* ************************************************************************* */ Matrix Rot3Q::matrix() const { return quaternion_.toRotationMatrix(); } @@ -133,74 +198,9 @@ namespace gtsam { return Vector_(3,q(0),q(1),q(2)); } - /* ************************************************************************* */ - // Log map at identity - return the canonical coordinates of this rotation - Vector Rot3Q::Logmap(const Rot3Q& R) { - Eigen::AngleAxisd angleAxis(R.quaternion_); - if(angleAxis.angle() > M_PI) // Important: use the smallest possible - angleAxis.angle() -= 2.0*M_PI; // angle, e.g. no more than PI, to keep - if(angleAxis.angle() < -M_PI) // error continuous. - angleAxis.angle() += 2.0*M_PI; - return angleAxis.axis() * angleAxis.angle(); - } - - /* ************************************************************************* */ - Point3 Rot3Q::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - Matrix R = matrix(); - if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z()); - if (H2) *H2 = R; - Eigen::Vector3d r = R * p.vector(); - return Point3(r.x(), r.y(), r.z()); - } - - /* ************************************************************************* */ - // see doc/math.lyx, SO(3) section - Point3 Rot3Q::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; - } - - /* ************************************************************************* */ - Rot3Q Rot3Q::compose(const Rot3Q& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return Rot3Q(quaternion_ * R2.quaternion_); - } - - /* ************************************************************************* */ - Point3 Rot3Q::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)); - } - - /* ************************************************************************* */ - Rot3Q Rot3Q::inverse(boost::optional H1) const { - if (H1) *H1 = -matrix(); - return Rot3Q(quaternion_.inverse()); - } - /* ************************************************************************* */ Quaternion Rot3Q::toQuaternion() const { return quaternion_; } /* ************************************************************************* */ - Rot3Q Rot3Q::between(const Rot3Q& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = -(R2.transpose()*matrix()); - if (H2) *H2 = I3; - return between_default(*this, R2); - } - - /* ************************************************************************* */ - Rot3Q Rot3Q::operator*(const Rot3Q& R2) const { - return Rot3Q(quaternion_ * R2.quaternion_); - } - - /* ************************************************************************* */ } // namespace gtsam