diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 8dcf14d4b..ef82e9369 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -1,198 +1,209 @@ -/* ---------------------------------------------------------------------------- - - * 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 -#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 Point3& w, double theta) { - return rodriguez((Vector)w.vector(),theta); -} - -/* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Sphere2& w, double theta) { - return rodriguez(w.point3(),theta); -} - -/* ************************************************************************* */ -Rot3 Rot3::Random(boost::random::mt19937 & rng) { - // TODO allow any engine without including all of boost :-( - Sphere2 w = Sphere2::Random(rng); - boost::random::uniform_real_distribution randomAngle(-M_PI,M_PI); - double angle = randomAngle(rng); - return rodriguez(w,angle); -} - -/* ************************************************************************* */ -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 - +/* ---------------------------------------------------------------------------- + + * 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 +#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 Point3& w, double theta) { + return rodriguez((Vector)w.vector(),theta); +} + +/* ************************************************************************* */ +Rot3 Rot3::rodriguez(const Sphere2& w, double theta) { + return rodriguez(w.point3(),theta); +} + +/* ************************************************************************* */ +Rot3 Rot3::Random(boost::random::mt19937 & rng) { + // TODO allow any engine without including all of boost :-( + Sphere2 w = Sphere2::Random(rng); + boost::random::uniform_real_distribution randomAngle(-M_PI,M_PI); + double angle = randomAngle(rng); + return rodriguez(w,angle); +} + +/* ************************************************************************* */ +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::unrotate(const Sphere2& p, + boost::optional HR, boost::optional Hp) const { + Sphere2 q = unrotate(p.point3(Hp)); + if (Hp) + (*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp); + if (HR) + (*HR) = q.basis().transpose() * q.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/Rot3.h b/gtsam/geometry/Rot3.h index ea59fab17..d302a3502 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -331,6 +331,10 @@ namespace gtsam { Sphere2 rotate(const Sphere2& p, boost::optional HR = boost::none, boost::optional Hp = boost::none) const; + /// unrotate 3D direction from world frame to rotated coordinate frame + Sphere2 unrotate(const Sphere2& p, boost::optional HR = boost::none, + boost::optional Hp = boost::none) const; + /// rotate 3D direction from rotated coordinate frame to world frame Sphere2 operator*(const Sphere2& p) const; diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 99c5c619e..6257d8400 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -1,308 +1,308 @@ -/* ---------------------------------------------------------------------------- - - * 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 Rot3M.cpp - * @brief Rotation (internal: 3*3 matrix representation*) - * @author Alireza Fathi - * @author Christian Potthast - * @author Frank Dellaert - * @author Richard Roberts - */ - -#include // Get GTSAM_USE_QUATERNIONS macro - -#ifndef GTSAM_USE_QUATERNIONS - -#include -#include -#include - -using namespace std; - -namespace gtsam { - -static const Matrix3 I3 = Matrix3::Identity(); - -/* ************************************************************************* */ -Rot3::Rot3() : rot_(Matrix3::Identity()) {} - -/* ************************************************************************* */ -Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { - rot_.col(0) = col1.vector(); - rot_.col(1) = col2.vector(); - rot_.col(2) = col3.vector(); -} - -/* ************************************************************************* */ -Rot3::Rot3(double R11, double R12, double R13, - double R21, double R22, double R23, - double R31, double R32, double R33) { - rot_ << R11, R12, R13, - R21, R22, R23, - R31, R32, R33; -} - -/* ************************************************************************* */ -Rot3::Rot3(const Matrix3& R) { - rot_ = R; -} - -/* ************************************************************************* */ -Rot3::Rot3(const Matrix& R) { - if (R.rows()!=3 || R.cols()!=3) - throw invalid_argument("Rot3 constructor expects 3*3 matrix"); - rot_ = R; -} - -///* ************************************************************************* */ -//Rot3::Rot3(const Matrix3& R) : rot_(R) {} - -/* ************************************************************************* */ -Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {} - -/* ************************************************************************* */ -Rot3 Rot3::Rx(double t) { - double st = sin(t), ct = cos(t); - return Rot3( - 1, 0, 0, - 0, ct,-st, - 0, st, ct); -} - -/* ************************************************************************* */ -Rot3 Rot3::Ry(double t) { - double st = sin(t), ct = cos(t); - return Rot3( - ct, 0, st, - 0, 1, 0, - -st, 0, ct); -} - -/* ************************************************************************* */ -Rot3 Rot3::Rz(double t) { - double st = sin(t), ct = cos(t); - return Rot3( - ct,-st, 0, - st, ct, 0, - 0, 0, 1); -} - -/* ************************************************************************* */ -// Considerably faster than composing matrices above ! -Rot3 Rot3::RzRyRx(double x, double y, double z) { - double cx=cos(x),sx=sin(x); - double cy=cos(y),sy=sin(y); - double cz=cos(z),sz=sin(z); - double ss_ = sx * sy; - double cs_ = cx * sy; - double sc_ = sx * cy; - double cc_ = cx * cy; - double c_s = cx * sz; - double s_s = sx * sz; - double _cs = cy * sz; - double _cc = cy * cz; - double s_c = sx * cz; - double c_c = cx * cz; - double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; - return Rot3( - _cc,- c_s + ssc, s_s + csc, - _cs, c_c + sss, -s_c + css, - -sy, sc_, cc_ - ); -} - -/* ************************************************************************* */ -Rot3 Rot3::rodriguez(const Vector& w, double theta) { - // get components of axis \omega - double wx = w(0), wy=w(1), wz=w(2); - double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; -#ifndef NDEBUG - double l_n = wwTxx + wwTyy + wwTzz; - if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); -#endif - - double c = cos(theta), s = sin(theta), c_1 = 1 - c; - - double swx = wx * s, swy = wy * s, swz = wz * s; - double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; - double C11 = c_1*wwTyy, C12 = c_1*wy*wz; - double C22 = c_1*wwTzz; - - return Rot3( - c + C00, -swz + C01, swy + C02, - swz + C01, c + C11, -swx + C12, - -swy + C02, swx + C12, c + C22); -} - -/* ************************************************************************* */ -Rot3 Rot3::compose (const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = R2.transpose(); - if (H2) *H2 = I3; - return *this * R2; -} - -/* ************************************************************************* */ -Rot3 Rot3::operator*(const Rot3& R2) const { - return Rot3(Matrix3(rot_*R2.rot_)); -} - -/* ************************************************************************* */ -Rot3 Rot3::inverse(boost::optional H1) const { - if (H1) *H1 = -rot_; - return Rot3(Matrix3(rot_.transpose())); -} - -/* ************************************************************************* */ -Rot3 Rot3::between (const Rot3& R2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = -(R2.transpose()*rot_); - if (H2) *H2 = I3; - return Rot3(Matrix3(rot_.transpose()*R2.rot_)); -} - -/* ************************************************************************* */ -Point3 Rot3::rotate(const Point3& p, - boost::optional H1, boost::optional H2) const { - if (H1 || H2) { - if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); - if (H2) *H2 = rot_; - } - return Point3(rot_ * p.vector()); -} - -/* ************************************************************************* */ -// Log map at identity - return the canonical coordinates of this rotation -Vector3 Rot3::Logmap(const Rot3& R) { - - static const double PI = boost::math::constants::pi(); - - const Matrix3& rot = R.rot_; - // Get trace(R) - double tr = rot.trace(); - - // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. - // we do something special - if (std::abs(tr+1.0) < 1e-10) { - if(std::abs(rot(2,2)+1.0) > 1e-10) - return (PI / sqrt(2.0+2.0*rot(2,2) )) * - Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2)); - else if(std::abs(rot(1,1)+1.0) > 1e-10) - return (PI / sqrt(2.0+2.0*rot(1,1))) * - Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1)); - else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit - return (PI / sqrt(2.0+2.0*rot(0,0))) * - Vector3(1.0+rot(0,0), rot(1,0), rot(2,0)); - } else { - double magnitude; - double tr_3 = tr-3.0; // always negative - if (tr_3<-1e-7) { - double theta = acos((tr-1.0)/2.0); - magnitude = theta/(2.0*sin(theta)); - } else { - // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) - // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2) - magnitude = 0.5 - tr_3*tr_3/12.0; - } - return magnitude*Vector3( - rot(2,1)-rot(1,2), - rot(0,2)-rot(2,0), - rot(1,0)-rot(0,1)); - } -} - -/* ************************************************************************* */ -Rot3 Rot3::retractCayley(const Vector& omega) const { - const double x = omega(0), y = omega(1), z = omega(2); - const double x2 = x * x, y2 = y * y, z2 = z * z; - const double xy = x * y, xz = x * z, yz = y * z; - const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f; - return (*this) - * Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, - (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f, - (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f); -} - -/* ************************************************************************* */ -Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::EXPMAP) { - return (*this)*Expmap(omega); - } else if(mode == Rot3::CAYLEY) { - return retractCayley(omega); - } else if(mode == Rot3::SLOW_CAYLEY) { - Matrix Omega = skewSymmetric(omega); - return (*this)*Cayley<3>(-Omega/2); - } else { - assert(false); - exit(1); - } -} - -/* ************************************************************************* */ -Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { - if(mode == Rot3::EXPMAP) { - return Logmap(between(T)); - } else if(mode == Rot3::CAYLEY) { - // Create a fixed-size matrix - Eigen::Matrix3d A(between(T).matrix()); - // Mathematica closed form optimization (procrastination?) gone wild: - const double a=A(0,0),b=A(0,1),c=A(0,2); - const double d=A(1,0),e=A(1,1),f=A(1,2); - const double g=A(2,0),h=A(2,1),i=A(2,2); - const double di = d*i, ce = c*e, cd = c*d, fg=f*g; - const double M = 1 + e - f*h + i + e*i; - const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); - const double x = (a * f - cd + f) * K; - const double y = (b * f - ce - c) * K; - const double z = (fg - di - d) * K; - return -2 * Vector3(x, y, z); - } else if(mode == Rot3::SLOW_CAYLEY) { - // Create a fixed-size matrix - Eigen::Matrix3d A(between(T).matrix()); - // using templated version of Cayley - Eigen::Matrix3d Omega = Cayley<3>(A); - return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); - } else { - assert(false); - exit(1); - } -} - -/* ************************************************************************* */ -Matrix3 Rot3::matrix() const { - return rot_; -} - -/* ************************************************************************* */ -Matrix3 Rot3::transpose() const { - return rot_.transpose(); -} - -/* ************************************************************************* */ -Point3 Rot3::r1() const { return Point3(rot_.col(0)); } - -/* ************************************************************************* */ -Point3 Rot3::r2() const { return Point3(rot_.col(1)); } - -/* ************************************************************************* */ -Point3 Rot3::r3() const { return Point3(rot_.col(2)); } - -/* ************************************************************************* */ -Quaternion Rot3::toQuaternion() const { - return Quaternion(rot_); -} - -/* ************************************************************************* */ - -} // namespace gtsam - -#endif +/* ---------------------------------------------------------------------------- + + * 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 Rot3M.cpp + * @brief Rotation (internal: 3*3 matrix representation*) + * @author Alireza Fathi + * @author Christian Potthast + * @author Frank Dellaert + * @author Richard Roberts + */ + +#include // Get GTSAM_USE_QUATERNIONS macro + +#ifndef GTSAM_USE_QUATERNIONS + +#include +#include +#include + +using namespace std; + +namespace gtsam { + +static const Matrix3 I3 = Matrix3::Identity(); + +/* ************************************************************************* */ +Rot3::Rot3() : rot_(Matrix3::Identity()) {} + +/* ************************************************************************* */ +Rot3::Rot3(const Point3& col1, const Point3& col2, const Point3& col3) { + rot_.col(0) = col1.vector(); + rot_.col(1) = col2.vector(); + rot_.col(2) = col3.vector(); +} + +/* ************************************************************************* */ +Rot3::Rot3(double R11, double R12, double R13, + double R21, double R22, double R23, + double R31, double R32, double R33) { + rot_ << R11, R12, R13, + R21, R22, R23, + R31, R32, R33; +} + +/* ************************************************************************* */ +Rot3::Rot3(const Matrix3& R) { + rot_ = R; +} + +/* ************************************************************************* */ +Rot3::Rot3(const Matrix& R) { + if (R.rows()!=3 || R.cols()!=3) + throw invalid_argument("Rot3 constructor expects 3*3 matrix"); + rot_ = R; +} + +///* ************************************************************************* */ +//Rot3::Rot3(const Matrix3& R) : rot_(R) {} + +/* ************************************************************************* */ +Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {} + +/* ************************************************************************* */ +Rot3 Rot3::Rx(double t) { + double st = sin(t), ct = cos(t); + return Rot3( + 1, 0, 0, + 0, ct,-st, + 0, st, ct); +} + +/* ************************************************************************* */ +Rot3 Rot3::Ry(double t) { + double st = sin(t), ct = cos(t); + return Rot3( + ct, 0, st, + 0, 1, 0, + -st, 0, ct); +} + +/* ************************************************************************* */ +Rot3 Rot3::Rz(double t) { + double st = sin(t), ct = cos(t); + return Rot3( + ct,-st, 0, + st, ct, 0, + 0, 0, 1); +} + +/* ************************************************************************* */ +// Considerably faster than composing matrices above ! +Rot3 Rot3::RzRyRx(double x, double y, double z) { + double cx=cos(x),sx=sin(x); + double cy=cos(y),sy=sin(y); + double cz=cos(z),sz=sin(z); + double ss_ = sx * sy; + double cs_ = cx * sy; + double sc_ = sx * cy; + double cc_ = cx * cy; + double c_s = cx * sz; + double s_s = sx * sz; + double _cs = cy * sz; + double _cc = cy * cz; + double s_c = sx * cz; + double c_c = cx * cz; + double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz; + return Rot3( + _cc,- c_s + ssc, s_s + csc, + _cs, c_c + sss, -s_c + css, + -sy, sc_, cc_ + ); +} + +/* ************************************************************************* */ +Rot3 Rot3::rodriguez(const Vector& w, double theta) { + // get components of axis \omega + double wx = w(0), wy=w(1), wz=w(2); + double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz; +#ifndef NDEBUG + double l_n = wwTxx + wwTyy + wwTzz; + if (std::abs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); +#endif + + double c = cos(theta), s = sin(theta), c_1 = 1 - c; + + double swx = wx * s, swy = wy * s, swz = wz * s; + double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz; + double C11 = c_1*wwTyy, C12 = c_1*wy*wz; + double C22 = c_1*wwTzz; + + return Rot3( + c + C00, -swz + C01, swy + C02, + swz + C01, c + C11, -swx + C12, + -swy + C02, swx + C12, c + C22); +} + +/* ************************************************************************* */ +Rot3 Rot3::compose (const Rot3& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = R2.transpose(); + if (H2) *H2 = I3; + return *this * R2; +} + +/* ************************************************************************* */ +Rot3 Rot3::operator*(const Rot3& R2) const { + return Rot3(Matrix3(rot_*R2.rot_)); +} + +/* ************************************************************************* */ +Rot3 Rot3::inverse(boost::optional H1) const { + if (H1) *H1 = -rot_; + return Rot3(Matrix3(rot_.transpose())); +} + +/* ************************************************************************* */ +Rot3 Rot3::between (const Rot3& R2, + boost::optional H1, boost::optional H2) const { + if (H1) *H1 = -(R2.transpose()*rot_); + if (H2) *H2 = I3; + return Rot3(Matrix3(rot_.transpose()*R2.rot_)); +} + +/* ************************************************************************* */ +Point3 Rot3::rotate(const Point3& p, + boost::optional H1, boost::optional H2) const { + if (H1 || H2) { + if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z()); + if (H2) *H2 = rot_; + } + return Point3(rot_ * p.vector()); +} + +/* ************************************************************************* */ +// Log map at identity - return the canonical coordinates of this rotation +Vector3 Rot3::Logmap(const Rot3& R) { + + static const double PI = boost::math::constants::pi(); + + const Matrix3& rot = R.rot_; + // Get trace(R) + double tr = rot.trace(); + + // when trace == -1, i.e., when theta = +-pi, +-3pi, +-5pi, etc. + // we do something special + if (std::abs(tr+1.0) < 1e-10) { + if(std::abs(rot(2,2)+1.0) > 1e-10) + return (PI / sqrt(2.0+2.0*rot(2,2) )) * + Vector3(rot(0,2), rot(1,2), 1.0+rot(2,2)); + else if(std::abs(rot(1,1)+1.0) > 1e-10) + return (PI / sqrt(2.0+2.0*rot(1,1))) * + Vector3(rot(0,1), 1.0+rot(1,1), rot(2,1)); + else // if(std::abs(R.r1_.x()+1.0) > 1e-10) This is implicit + return (PI / sqrt(2.0+2.0*rot(0,0))) * + Vector3(1.0+rot(0,0), rot(1,0), rot(2,0)); + } else { + double magnitude; + double tr_3 = tr-3.0; // always negative + if (tr_3<-1e-7) { + double theta = acos((tr-1.0)/2.0); + magnitude = theta/(2.0*sin(theta)); + } else { + // when theta near 0, +-2pi, +-4pi, etc. (trace near 3.0) + // use Taylor expansion: magnitude \approx 1/2-(t-3)/12 + O((t-3)^2) + magnitude = 0.5 - tr_3*tr_3/12.0; + } + return magnitude*Vector3( + rot(2,1)-rot(1,2), + rot(0,2)-rot(2,0), + rot(1,0)-rot(0,1)); + } +} + +/* ************************************************************************* */ +Rot3 Rot3::retractCayley(const Vector& omega) const { + const double x = omega(0), y = omega(1), z = omega(2); + const double x2 = x * x, y2 = y * y, z2 = z * z; + const double xy = x * y, xz = x * z, yz = y * z; + const double f = 1.0 / (4.0 + x2 + y2 + z2), _2f = 2.0 * f; + return (*this) + * Rot3((4 + x2 - y2 - z2) * f, (xy - 2 * z) * _2f, (xz + 2 * y) * _2f, + (xy + 2 * z) * _2f, (4 - x2 + y2 - z2) * f, (yz - 2 * x) * _2f, + (xz - 2 * y) * _2f, (yz + 2 * x) * _2f, (4 - x2 - y2 + z2) * f); +} + +/* ************************************************************************* */ +Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const { + if(mode == Rot3::EXPMAP) { + return (*this)*Expmap(omega); + } else if(mode == Rot3::CAYLEY) { + return retractCayley(omega); + } else if(mode == Rot3::SLOW_CAYLEY) { + Matrix Omega = skewSymmetric(omega); + return (*this)*Cayley<3>(-Omega/2); + } else { + assert(false); + exit(1); + } +} + +/* ************************************************************************* */ +Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const { + if(mode == Rot3::EXPMAP) { + return Logmap(between(T)); + } else if(mode == Rot3::CAYLEY) { + // Create a fixed-size matrix + Eigen::Matrix3d A(between(T).matrix()); + // Mathematica closed form optimization (procrastination?) gone wild: + const double a=A(0,0),b=A(0,1),c=A(0,2); + const double d=A(1,0),e=A(1,1),f=A(1,2); + const double g=A(2,0),h=A(2,1),i=A(2,2); + const double di = d*i, ce = c*e, cd = c*d, fg=f*g; + const double M = 1 + e - f*h + i + e*i; + const double K = 2.0 / (cd*h + M + a*M -g*(c + ce) - b*(d + di - fg)); + const double x = (a * f - cd + f) * K; + const double y = (b * f - ce - c) * K; + const double z = (fg - di - d) * K; + return -2 * Vector3(x, y, z); + } else if(mode == Rot3::SLOW_CAYLEY) { + // Create a fixed-size matrix + Eigen::Matrix3d A(between(T).matrix()); + // using templated version of Cayley + Eigen::Matrix3d Omega = Cayley<3>(A); + return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0)); + } else { + assert(false); + exit(1); + } +} + +/* ************************************************************************* */ +Matrix3 Rot3::matrix() const { + return rot_; +} + +/* ************************************************************************* */ +Matrix3 Rot3::transpose() const { + return rot_.transpose(); +} + +/* ************************************************************************* */ +Point3 Rot3::r1() const { return Point3(rot_.col(0)); } + +/* ************************************************************************* */ +Point3 Rot3::r2() const { return Point3(rot_.col(1)); } + +/* ************************************************************************* */ +Point3 Rot3::r3() const { return Point3(rot_.col(2)); } + +/* ************************************************************************* */ +Quaternion Rot3::toQuaternion() const { + return Quaternion(rot_); +} + +/* ************************************************************************* */ + +} // namespace gtsam + +#endif diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 26890c370..c5990153a 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())); } diff --git a/gtsam/geometry/Sphere2.cpp b/gtsam/geometry/Sphere2.cpp index 29a170c4d..2069fe0ee 100644 --- a/gtsam/geometry/Sphere2.cpp +++ b/gtsam/geometry/Sphere2.cpp @@ -14,6 +14,7 @@ * @date Feb 02, 2011 * @author Can Erdogan * @author Frank Dellaert + * @author Alex Trevor * @brief The Sphere2 class - basically a point on a unit sphere */ @@ -113,7 +114,7 @@ double Sphere2::distance(const Sphere2& q, boost::optional H) const { } /* ************************************************************************* */ -Sphere2 Sphere2::retract(const Vector& v) const { +Sphere2 Sphere2::retract(const Vector& v, Sphere2::CoordinatesMode mode) const { // Get the vector form of the point and the basis matrix Vector p = Point3::Logmap(p_); @@ -121,35 +122,75 @@ Sphere2 Sphere2::retract(const Vector& v) const { // Compute the 3D xi_hat vector Vector xi_hat = v(0) * B.col(0) + v(1) * B.col(1); - Vector newPoint = p + xi_hat; + + if (mode == Sphere2::EXPMAP) { + double xi_hat_norm = xi_hat.norm(); - // Project onto the manifold, i.e. the closest point on the circle to the new location; - // same as putting it onto the unit circle - Vector projected = newPoint / newPoint.norm(); + // Avoid nan + if (xi_hat_norm == 0.0) { + if (v.norm () == 0.0) + return Sphere2 (point3 ()); + else + return Sphere2 (-point3 ()); + } + + Vector exp_p_xi_hat = cos (xi_hat_norm) * p + sin(xi_hat_norm) * (xi_hat / xi_hat_norm); + return Sphere2(exp_p_xi_hat); + } else if (mode == Sphere2::RENORM) { + // Project onto the manifold, i.e. the closest point on the circle to the new location; + // same as putting it onto the unit circle + Vector newPoint = p + xi_hat; + Vector projected = newPoint / newPoint.norm(); - return Sphere2(Point3::Expmap(projected)); + return Sphere2(Point3::Expmap(projected)); + } else { + assert (false); + exit (1); + } } /* ************************************************************************* */ -Vector Sphere2::localCoordinates(const Sphere2& y) const { + Vector Sphere2::localCoordinates(const Sphere2& y, Sphere2::CoordinatesMode mode) const { - // Make sure that the angle different between x and y is less than 90. Otherwise, - // we can project x + xi_hat from the tangent space at x to y. - assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y."); + if (mode == Sphere2::EXPMAP) { + Matrix B = basis(); + + Vector p = Point3::Logmap(p_); + Vector q = Point3::Logmap(y.p_); + double theta = acos(p.transpose() * q); - // Get the basis matrix - Matrix B = basis(); - - // Create the vector forms of p and q (the Point3 of y). - Vector p = Point3::Logmap(p_); - Vector q = Point3::Logmap(y.p_); - - // Compute the basis coefficients [v0,v1] = (B'q)/(p'q). - double alpha = p.transpose() * q; - assert(alpha != 0.0); - Matrix coeffs = (B.transpose() * q) / alpha; - Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0)); - return result; + // the below will be nan if theta == 0.0 + if (p == q) + return (Vector (2) << 0, 0); + else if (p == (Vector)-q) + return (Vector (2) << M_PI, 0); + + Vector result_hat = (theta / sin(theta)) * (q - p * cos(theta)); + Vector result = B.transpose() * result_hat; + + return result; + } else if (mode == Sphere2::RENORM) { + // Make sure that the angle different between x and y is less than 90. Otherwise, + // we can project x + xi_hat from the tangent space at x to y. + assert(y.p_.dot(p_) > 0.0 && "Can not retract from x to y."); + + // Get the basis matrix + Matrix B = basis(); + + // Create the vector forms of p and q (the Point3 of y). + Vector p = Point3::Logmap(p_); + Vector q = Point3::Logmap(y.p_); + + // Compute the basis coefficients [v0,v1] = (B'q)/(p'q). + double alpha = p.transpose() * q; + assert(alpha != 0.0); + Matrix coeffs = (B.transpose() * q) / alpha; + Vector result = Vector_(2, coeffs(0, 0), coeffs(1, 0)); + return result; + } else { + assert (false); + exit (1); + } } /* ************************************************************************* */ diff --git a/gtsam/geometry/Sphere2.h b/gtsam/geometry/Sphere2.h index 8555b5d30..ac8124139 100644 --- a/gtsam/geometry/Sphere2.h +++ b/gtsam/geometry/Sphere2.h @@ -14,6 +14,7 @@ * @date Feb 02, 2011 * @author Can Erdogan * @author Frank Dellaert + * @author Alex Trevor * @brief Develop a Sphere2 class - basically a point on a unit sphere */ @@ -22,6 +23,10 @@ #include #include +#ifndef SPHERE2_DEFAULT_COORDINATES_MODE + #define SPHERE2_DEFAULT_COORDINATES_MODE Sphere2::RENORM +#endif + // (Cumbersome) forward declaration for random generator namespace boost { namespace random { @@ -106,6 +111,13 @@ public: return p_; } + /// Return unit-norm Vector + Vector unitVector(boost::optional H = boost::none) const { + if (H) + *H = basis(); + return (p_.vector ()); + } + /// Signed, vector-valued error between two directions Vector error(const Sphere2& q, boost::optional H = boost::none) const; @@ -129,11 +141,16 @@ public: return 2; } + enum CoordinatesMode { + EXPMAP, ///< Use the exponential map to retract + RENORM ///< Retract with vector addtion and renormalize. + }; + /// The retract function - Sphere2 retract(const Vector& v) const; + Sphere2 retract(const Vector& v, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const; /// The local coordinates function - Vector localCoordinates(const Sphere2& s) const; + Vector localCoordinates(const Sphere2& s, Sphere2::CoordinatesMode mode = SPHERE2_DEFAULT_COORDINATES_MODE) const; /// @} }; diff --git a/gtsam/geometry/tests/testEssentialMatrix.cpp b/gtsam/geometry/tests/testEssentialMatrix.cpp index 59cc15581..865f27f81 100644 --- a/gtsam/geometry/tests/testEssentialMatrix.cpp +++ b/gtsam/geometry/tests/testEssentialMatrix.cpp @@ -89,10 +89,7 @@ TEST (EssentialMatrix, rotate) { // Derivatives Matrix actH1, actH2; - try { - bodyE.rotate(cRb, actH1, actH2); - } catch (exception e) { - } // avoid exception + try { bodyE.rotate(cRb, actH1, actH2);} catch(exception e) {} // avoid exception Matrix expH1 = numericalDerivative21(rotate_, bodyE, cRb), // expH2 = numericalDerivative22(rotate_, bodyE, cRb); EXPECT(assert_equal(expH1, actH1, 1e-8)); diff --git a/gtsam/geometry/tests/testSphere2.cpp b/gtsam/geometry/tests/testSphere2.cpp index b9aa84949..706aa8fb4 100644 --- a/gtsam/geometry/tests/testSphere2.cpp +++ b/gtsam/geometry/tests/testSphere2.cpp @@ -13,6 +13,8 @@ * @file testSphere2.cpp * @date Feb 03, 2012 * @author Can Erdogan + * @author Frank Dellaert + * @author Alex Trevor * @brief Tests the Sphere2 class */ @@ -76,10 +78,35 @@ TEST(Sphere2, rotate) { } } +//******************************************************************************* +static Sphere2 unrotate_(const Rot3& R, const Sphere2& p) { + return R.unrotate (p); +} + +TEST(Sphere2, unrotate) { + Rot3 R = Rot3::yaw(-M_PI/4.0); + Sphere2 p(1, 0, 0); + Sphere2 expected = Sphere2(1, 1, 0); + Sphere2 actual = R.unrotate (p); + EXPECT(assert_equal(expected, actual, 1e-8)); + Matrix actualH, expectedH; + // Use numerical derivatives to calculate the expected Jacobian + { + expectedH = numericalDerivative21(unrotate_, R, p); + R.unrotate(p, actualH, boost::none); + EXPECT(assert_equal(expectedH, actualH, 1e-9)); + } + { + expectedH = numericalDerivative22(unrotate_, R, p); + R.unrotate(p, boost::none, actualH); + EXPECT(assert_equal(expectedH, actualH, 1e-9)); + } +} + //******************************************************************************* TEST(Sphere2, error) { - Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // - r = p.retract((Vector(2) << 0.8, 0)); + Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), // + r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM); EXPECT(assert_equal((Vector(2) << 0, 0), p.error(p), 1e-8)); EXPECT(assert_equal((Vector(2) << 0.447214, 0), p.error(q), 1e-5)); EXPECT(assert_equal((Vector(2) << 0.624695, 0), p.error(r), 1e-5)); @@ -102,8 +129,8 @@ TEST(Sphere2, error) { //******************************************************************************* TEST(Sphere2, distance) { - Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0)), // - r = p.retract((Vector(2) << 0.8, 0)); + Sphere2 p(1, 0, 0), q = p.retract((Vector(2) << 0.5, 0), Sphere2::RENORM), // + r = p.retract((Vector(2) << 0.8, 0), Sphere2::RENORM); EXPECT_DOUBLES_EQUAL(0, p.distance(p), 1e-8); EXPECT_DOUBLES_EQUAL(0.44721359549995798, p.distance(q), 1e-8); EXPECT_DOUBLES_EQUAL(0.6246950475544244, p.distance(r), 1e-8); @@ -147,9 +174,20 @@ TEST(Sphere2, retract) { Vector v(2); v << 0.5, 0; Sphere2 expected(Point3(1, 0, 0.5)); - Sphere2 actual = p.retract(v); + Sphere2 actual = p.retract(v, Sphere2::RENORM); EXPECT(assert_equal(expected, actual, 1e-8)); - EXPECT(assert_equal(v, p.localCoordinates(actual), 1e-8)); + EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::RENORM), 1e-8)); +} + +//******************************************************************************* +TEST(Sphere2, retract_expmap) { + Sphere2 p; + Vector v(2); + v << (M_PI/2.0), 0; + Sphere2 expected(Point3(0, 0, 1)); + Sphere2 actual = p.retract(v, Sphere2::EXPMAP); + EXPECT(assert_equal(expected, actual, 1e-8)); + EXPECT(assert_equal(v, p.localCoordinates(actual, Sphere2::EXPMAP), 1e-8)); } //******************************************************************************* @@ -199,6 +237,39 @@ TEST(Sphere2, localCoordinates_retract) { } } +//******************************************************************************* +// Let x and y be two Sphere2's. +// The equality x.localCoordinates(x.retract(v)) == v should hold. +TEST(Sphere2, localCoordinates_retract_expmap) { + + size_t numIterations = 10000; + Vector minSphereLimit = Vector_(3, -1.0, -1.0, -1.0), maxSphereLimit = + Vector_(3, 1.0, 1.0, 1.0); + Vector minXiLimit = Vector_(2, -M_PI, -M_PI), maxXiLimit = Vector_(2, M_PI, M_PI); + for (size_t i = 0; i < numIterations; i++) { + + // Sleep for the random number generator (TODO?: Better create all of them first). + sleep(0); + + // Create the two Sphere2s. + // Unlike the above case, we can use any two sphers. + Sphere2 s1(Point3(randomVector(minSphereLimit, maxSphereLimit))); +// Sphere2 s2 (Point3(randomVector(minSphereLimit, maxSphereLimit))); + Vector v12 = randomVector(minXiLimit, maxXiLimit); + + // Magnitude of the rotation can be at most pi + if (v12.norm () > M_PI) + v12 = v12 / M_PI; + Sphere2 s2 = s1.retract(v12); + + // Check if the local coordinates and retract return the same results. + Vector actual_v12 = s1.localCoordinates(s2); + EXPECT(assert_equal(v12, actual_v12, 1e-3)); + Sphere2 actual_s2 = s1.retract(actual_v12); + EXPECT(assert_equal(s2, actual_s2, 1e-3)); + } +} + //******************************************************************************* //TEST( Pose2, between ) //{ @@ -245,13 +316,13 @@ TEST(Sphere2, Random) { // Check that is deterministic given same random seed Point3 expected(-0.667578, 0.671447, 0.321713); Point3 actual = Sphere2::Random(rng).point3(); - EXPECT(assert_equal(expected, actual, 1e-5)); + EXPECT(assert_equal(expected,actual,1e-5)); // Check that means are all zero at least Point3 expectedMean, actualMean; for (size_t i = 0; i < 100; i++) actualMean = actualMean + Sphere2::Random(rng).point3(); - actualMean = actualMean / 100; - EXPECT(assert_equal(expectedMean, actualMean, 0.1)); + actualMean = actualMean/100; + EXPECT(assert_equal(expectedMean,actualMean,0.1)); } //************************************************************************* diff --git a/gtsam/geometry/tests/timePose3.cpp b/gtsam/geometry/tests/timePose3.cpp index 9538ad4b4..13e630706 100644 --- a/gtsam/geometry/tests/timePose3.cpp +++ b/gtsam/geometry/tests/timePose3.cpp @@ -17,23 +17,23 @@ #include -#include +#include #include using namespace std; using namespace gtsam; -/* ************************************************************************* */ +/* ************************************************************************* */ #define TEST(TITLE,STATEMENT) \ - gttic_(TITLE); \ - for(int i = 0; i < n; i++) \ - STATEMENT; \ - gttoc_(TITLE); + gttic_(TITLE); \ + for(int i = 0; i < n; i++) \ + STATEMENT; \ + gttoc_(TITLE); int main() { - int n = 5000000; - cout << "NOTE: Times are reported for " << n << " calls" << endl; + int n = 5000000; + cout << "NOTE: Times are reported for " << n << " calls" << endl; double norm=sqrt(1.0+16.0+4.0); double x=1.0/norm, y=4.0/norm, z=2.0/norm; @@ -47,9 +47,9 @@ int main() TEST(between, T.between(T2)) TEST(between_derivatives, T.between(T2,H1,H2)) TEST(Logmap, Pose3::Logmap(T.between(T2))) - - // Print timings - tictoc_print_(); + + // Print timings + tictoc_print_(); return 0; }