Simplified Rot3 by having only one implementation available at a time
Richard Roberts
parent
4a9cfbc98a
commit
b9bd2e61d8
|
|
@ -16,20 +16,13 @@
|
|||
*/
|
||||
// \callgraph
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/3rdparty/Eigen/Eigen/Geometry>
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Below is the class definition of Rot3. By the macros at the end of this
|
||||
// file, both Rot3M and Rot3Q are actually defined with this interface.
|
||||
#if defined Rot3 || defined __DOXYGEN
|
||||
|
||||
namespace gtsam {
|
||||
|
||||
// Forward declarations;
|
||||
class Rot3M;
|
||||
class Rot3Q;
|
||||
|
||||
/// Typedef to an Eigen Quaternion<double>, we disable alignment because
|
||||
/// geometry objects are stored in boost pool allocators, in Values
|
||||
/// containers, and and these pool allocators do not support alignment.
|
||||
|
|
@ -47,10 +40,10 @@ namespace gtsam {
|
|||
static const size_t dimension = 3;
|
||||
|
||||
private:
|
||||
#if defined ROT3_IS_MATRIX
|
||||
#ifndef GTSAM_DEFAULT_QUATERNIONS
|
||||
/** We store columns ! */
|
||||
Point3 r1_, r2_, r3_;
|
||||
#elif defined ROT3_IS_QUATERNION
|
||||
#else
|
||||
/** Internal Eigen Quaternion */
|
||||
Quaternion quaternion_;
|
||||
#endif
|
||||
|
|
@ -85,9 +78,6 @@ namespace gtsam {
|
|||
*/
|
||||
Rot3(const Quaternion& q);
|
||||
|
||||
/** Constructor from a rotation matrix in a Rot3M */
|
||||
Rot3(const Rot3M& r);
|
||||
|
||||
/* Static member function to generate some well known rotations */
|
||||
|
||||
/// Rotation around X axis as in http://en.wikipedia.org/wiki/Rotation_matrix, counterclockwise when looking from unchanging axis.
|
||||
|
|
@ -324,11 +314,11 @@ namespace gtsam {
|
|||
template<class ARCHIVE>
|
||||
void serialize(ARCHIVE & ar, const unsigned int version)
|
||||
{
|
||||
#if defined ROT3_IS_MATRIX
|
||||
#ifndef GTSAM_DEFAULT_QUATERNIONS
|
||||
ar & BOOST_SERIALIZATION_NVP(r1_);
|
||||
ar & BOOST_SERIALIZATION_NVP(r2_);
|
||||
ar & BOOST_SERIALIZATION_NVP(r3_);
|
||||
#elif defined ROT3_IS_QUATERNION
|
||||
#else
|
||||
ar & BOOST_SERIALIZATION_NVP(quaternion_);
|
||||
#endif
|
||||
}
|
||||
|
|
@ -346,48 +336,3 @@ namespace gtsam {
|
|||
*/
|
||||
std::pair<Matrix,Vector> RQ(const Matrix& A);
|
||||
}
|
||||
|
||||
#endif // if defined Rot3 || defined __DOXYGEN
|
||||
|
||||
|
||||
/* ************************************************************************* */
|
||||
// This block of code defines both Rot3Q and Rot3M, by self-including Rot3.h
|
||||
// twice and using preprocessor definitions of Rot3 to be Rot3M and Rot3Q. It
|
||||
// then creates a typedef of Rot3 to either Rot3M or Rot3Q, depending on
|
||||
// whether GTSAM_DEFAULT_QUATERNIONS is defined.
|
||||
#if !defined __ROT3_H
|
||||
#define __ROT3_H
|
||||
|
||||
// Define Rot3M
|
||||
#define Rot3 Rot3M
|
||||
#define ROT3_IS_MATRIX
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#undef Rot3
|
||||
#undef ROT3_IS_MATRIX
|
||||
|
||||
// Define Rot3Q
|
||||
#define Rot3 Rot3Q
|
||||
#define ROT3_IS_QUATERNION
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
#undef Rot3
|
||||
#undef ROT3_IS_QUATERNION
|
||||
|
||||
// Create Rot3 typedef
|
||||
namespace gtsam {
|
||||
/**
|
||||
* Typedef to the main 3D rotation implementation, which is Rot3M by default,
|
||||
* or Rot3Q if GTSAM_DEFAULT_QUATERNIONS is defined. Depending on whether
|
||||
* GTSAM_DEFAULT_QUATERNIONS is defined, Rot3M (the rotation matrix
|
||||
* implementation) or Rot3Q (the quaternion implementation) will used in all
|
||||
* built-in gtsam geometry types that involve 3D rotations, such as Pose3,
|
||||
* SimpleCamera, CalibratedCamera, StereoCamera, etc.
|
||||
*/
|
||||
#ifdef GTSAM_DEFAULT_QUATERNIONS
|
||||
typedef Rot3Q Rot3;
|
||||
#else
|
||||
typedef Rot3M Rot3;
|
||||
#endif
|
||||
}
|
||||
|
||||
#endif // if !defined Rot3
|
||||
|
||||
|
|
|
|||
|
|
@ -10,13 +10,15 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Rot3M.cpp
|
||||
* @file Rot3.cpp
|
||||
* @brief Rotation (internal: 3*3 matrix representation*)
|
||||
* @author Alireza Fathi
|
||||
* @author Christian Potthast
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#ifndef GTSAM_DEFAULT_QUATERNIONS
|
||||
|
||||
#include <boost/math/constants/constants.hpp>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
|
||||
|
|
@ -27,17 +29,17 @@ namespace gtsam {
|
|||
static const Matrix I3 = eye(3);
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M::Rot3M() :
|
||||
Rot3::Rot3() :
|
||||
r1_(Point3(1.0,0.0,0.0)),
|
||||
r2_(Point3(0.0,1.0,0.0)),
|
||||
r3_(Point3(0.0,0.0,1.0)) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M::Rot3M(const Point3& r1, const Point3& r2, const Point3& r3) :
|
||||
Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
|
||||
r1_(r1), r2_(r2), r3_(r3) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M::Rot3M(double R11, double R12, double R13,
|
||||
Rot3::Rot3(double R11, double R12, double R13,
|
||||
double R21, double R22, double R23,
|
||||
double R31, double R32, double R33) :
|
||||
r1_(Point3(R11, R21, R31)),
|
||||
|
|
@ -45,13 +47,13 @@ Rot3M::Rot3M(double R11, double R12, double R13,
|
|||
r3_(Point3(R13, R23, R33)) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M::Rot3M(const Matrix& R):
|
||||
Rot3::Rot3(const Matrix& R):
|
||||
r1_(Point3(R(0,0), R(1,0), R(2,0))),
|
||||
r2_(Point3(R(0,1), R(1,1), R(2,1))),
|
||||
r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M::Rot3M(const Quaternion& q) {
|
||||
Rot3::Rot3(const Quaternion& q) {
|
||||
Eigen::Matrix3d R = q.toRotationMatrix();
|
||||
r1_ = Point3(R.col(0));
|
||||
r2_ = Point3(R.col(1));
|
||||
|
|
@ -59,30 +61,27 @@ Rot3M::Rot3M(const Quaternion& q) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M::Rot3M(const Rot3M& r) : r1_(r.r1_), r2_(r.r2_), r3_(r.r3_) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::Rx(double t) {
|
||||
Rot3 Rot3::Rx(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3M(
|
||||
return Rot3(
|
||||
1, 0, 0,
|
||||
0, ct,-st,
|
||||
0, st, ct);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::Ry(double t) {
|
||||
Rot3 Rot3::Ry(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3M(
|
||||
return Rot3(
|
||||
ct, 0, st,
|
||||
0, 1, 0,
|
||||
-st, 0, ct);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::Rz(double t) {
|
||||
Rot3 Rot3::Rz(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3M(
|
||||
return Rot3(
|
||||
ct,-st, 0,
|
||||
st, ct, 0,
|
||||
0, 0, 1);
|
||||
|
|
@ -90,7 +89,7 @@ Rot3M Rot3M::Rz(double t) {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Considerably faster than composing matrices above !
|
||||
Rot3M Rot3M::RzRyRx(double x, double y, double z) {
|
||||
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);
|
||||
|
|
@ -105,7 +104,7 @@ Rot3M Rot3M::RzRyRx(double x, double y, double z) {
|
|||
double s_c = sx * cz;
|
||||
double c_c = cx * cz;
|
||||
double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
|
||||
return Rot3M(
|
||||
return Rot3(
|
||||
_cc,- c_s + ssc, s_s + csc,
|
||||
_cs, c_c + sss, -s_c + css,
|
||||
-sy, sc_, cc_
|
||||
|
|
@ -113,7 +112,7 @@ Rot3M Rot3M::RzRyRx(double x, double y, double z) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::rodriguez(const Vector& w, double theta) {
|
||||
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;
|
||||
|
|
@ -129,26 +128,26 @@ Rot3M Rot3M::rodriguez(const Vector& w, double theta) {
|
|||
double C11 = c_1*wwTyy, C12 = c_1*wy*wz;
|
||||
double C22 = c_1*wwTzz;
|
||||
|
||||
return Rot3M(
|
||||
return Rot3(
|
||||
c + C00, -swz + C01, swy + C02,
|
||||
swz + C01, c + C11, -swx + C12,
|
||||
-swy + C02, swx + C12, c + C22);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::rodriguez(const Vector& w) {
|
||||
Rot3 Rot3::rodriguez(const Vector& w) {
|
||||
double t = w.norm();
|
||||
if (t < 1e-10) return Rot3M();
|
||||
if (t < 1e-10) return Rot3();
|
||||
return rodriguez(w/t, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Rot3M::equals(const Rot3M & R, double tol) const {
|
||||
bool Rot3::equals(const Rot3 & R, double tol) const {
|
||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::compose (const Rot3M& R2,
|
||||
Rot3 Rot3::compose (const Rot3& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = R2.transpose();
|
||||
if (H2) *H2 = I3;
|
||||
|
|
@ -156,19 +155,19 @@ Rot3M Rot3M::compose (const Rot3M& R2,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3M::operator*(const Point3& p) const { return rotate(p); }
|
||||
Point3 Rot3::operator*(const Point3& p) const { return rotate(p); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::inverse(boost::optional<Matrix&> H1) const {
|
||||
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
|
||||
if (H1) *H1 = -matrix();
|
||||
return Rot3M(
|
||||
return Rot3(
|
||||
r1_.x(), r1_.y(), r1_.z(),
|
||||
r2_.x(), r2_.y(), r2_.z(),
|
||||
r3_.x(), r3_.y(), r3_.z());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::between (const Rot3M& R2,
|
||||
Rot3 Rot3::between (const Rot3& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = -(R2.transpose()*matrix());
|
||||
if (H2) *H2 = I3;
|
||||
|
|
@ -176,12 +175,12 @@ Rot3M Rot3M::between (const Rot3M& R2,
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::operator*(const Rot3M& R2) const {
|
||||
return Rot3M(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
|
||||
Rot3 Rot3::operator*(const Rot3& R2) const {
|
||||
return Rot3(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3M::rotate(const Point3& p,
|
||||
Point3 Rot3::rotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
if (H2) *H2 = matrix();
|
||||
|
|
@ -190,7 +189,7 @@ Point3 Rot3M::rotate(const Point3& p,
|
|||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SO(3) section
|
||||
Point3 Rot3M::unrotate(const Point3& p,
|
||||
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
|
||||
|
|
@ -201,7 +200,7 @@ Point3 Rot3M::unrotate(const Point3& p,
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
Vector Rot3M::Logmap(const Rot3M& R) {
|
||||
Vector Rot3::Logmap(const Rot3& 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)
|
||||
|
|
@ -234,7 +233,7 @@ Vector Rot3M::Logmap(const Rot3M& R) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3M::matrix() const {
|
||||
Matrix Rot3::matrix() const {
|
||||
Matrix R(3,3);
|
||||
R <<
|
||||
r1_.x(), r2_.x(), r3_.x(),
|
||||
|
|
@ -244,7 +243,7 @@ Matrix Rot3M::matrix() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3M::transpose() const {
|
||||
Matrix Rot3::transpose() const {
|
||||
Matrix Rt(3,3);
|
||||
Rt <<
|
||||
r1_.x(), r1_.y(), r1_.z(),
|
||||
|
|
@ -254,7 +253,7 @@ Matrix Rot3M::transpose() const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3M::column(int index) const{
|
||||
Point3 Rot3::column(int index) const{
|
||||
if(index == 3)
|
||||
return r3_;
|
||||
else if(index == 2)
|
||||
|
|
@ -266,35 +265,35 @@ Point3 Rot3M::column(int index) const{
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3M::r1() const { return r1_; }
|
||||
Point3 Rot3::r1() const { return r1_; }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3M::r2() const { return r2_; }
|
||||
Point3 Rot3::r2() const { return r2_; }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3M::r3() const { return r3_; }
|
||||
Point3 Rot3::r3() const { return r3_; }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3M::xyz() const {
|
||||
Vector Rot3::xyz() const {
|
||||
Matrix I;Vector q;
|
||||
boost::tie(I,q)=RQ(matrix());
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3M::ypr() const {
|
||||
Vector Rot3::ypr() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(2),q(1),q(0));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3M::rpy() const {
|
||||
Vector Rot3::rpy() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(0),q(1),q(2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Quaternion Rot3M::toQuaternion() const {
|
||||
Quaternion Rot3::toQuaternion() const {
|
||||
return Quaternion((Eigen::Matrix3d() <<
|
||||
r1_.x(), r2_.x(), r3_.x(),
|
||||
r1_.y(), r2_.y(), r3_.y(),
|
||||
|
|
@ -305,15 +304,15 @@ Quaternion Rot3M::toQuaternion() const {
|
|||
pair<Matrix, Vector> RQ(const Matrix& A) {
|
||||
|
||||
double x = -atan2(-A(2, 1), A(2, 2));
|
||||
Rot3M Qx = Rot3M::Rx(-x);
|
||||
Rot3 Qx = Rot3::Rx(-x);
|
||||
Matrix B = A * Qx.matrix();
|
||||
|
||||
double y = -atan2(B(2, 0), B(2, 2));
|
||||
Rot3M Qy = Rot3M::Ry(-y);
|
||||
Rot3 Qy = Rot3::Ry(-y);
|
||||
Matrix C = B * Qy.matrix();
|
||||
|
||||
double z = -atan2(-C(1, 0), C(1, 1));
|
||||
Rot3M Qz = Rot3M::Rz(-z);
|
||||
Rot3 Qz = Rot3::Rz(-z);
|
||||
Matrix R = C * Qz.matrix();
|
||||
|
||||
Vector xyz = Vector_(3, x, y, z);
|
||||
|
|
@ -323,3 +322,5 @@ pair<Matrix, Vector> RQ(const Matrix& A) {
|
|||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -10,13 +10,15 @@
|
|||
* -------------------------------------------------------------------------- */
|
||||
|
||||
/**
|
||||
* @file Rot3Q.cpp
|
||||
* @file Rot3.cpp
|
||||
* @brief Rotation (internal: 3*3 matrix representation*)
|
||||
* @author Alireza Fathi
|
||||
* @author Christian Potthast
|
||||
* @author Frank Dellaert
|
||||
*/
|
||||
|
||||
#ifdef GTSAM_DEFAULT_QUATERNIONS
|
||||
|
||||
#include <boost/math/constants/constants.hpp>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
|
||||
|
|
@ -27,17 +29,17 @@ namespace gtsam {
|
|||
static const Matrix I3 = eye(3);
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3Q::Rot3Q() : quaternion_(Quaternion::Identity()) {}
|
||||
Rot3::Rot3() : quaternion_(Quaternion::Identity()) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3Q::Rot3Q(const Point3& r1, const Point3& r2, const Point3& r3) :
|
||||
Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
|
||||
quaternion_((Eigen::Matrix3d() <<
|
||||
r1.x(), r2.x(), r3.x(),
|
||||
r1.y(), r2.y(), r3.y(),
|
||||
r1.z(), r2.z(), r3.z()).finished()) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3Q::Rot3Q(double R11, double R12, double R13,
|
||||
Rot3::Rot3(double R11, double R12, double R13,
|
||||
double R21, double R22, double R23,
|
||||
double R31, double R32, double R33) :
|
||||
quaternion_((Eigen::Matrix3d() <<
|
||||
|
|
@ -46,69 +48,66 @@ namespace gtsam {
|
|||
R31, R32, R33).finished()) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3Q::Rot3Q(const Matrix& R) :
|
||||
Rot3::Rot3(const Matrix& R) :
|
||||
quaternion_(Eigen::Matrix3d(R)) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3Q::Rot3Q(const Quaternion& q) : quaternion_(q) {}
|
||||
Rot3::Rot3(const Quaternion& q) : quaternion_(q) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3Q::Rot3Q(const Rot3M& r) : quaternion_(Eigen::Matrix3d(r.matrix())) {}
|
||||
Rot3 Rot3::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3Q Rot3Q::Rx(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); }
|
||||
Rot3 Rot3::Ry(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3Q Rot3Q::Ry(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); }
|
||||
Rot3 Rot3::Rz(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3Q Rot3Q::Rz(double t) { return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3Q Rot3Q::RzRyRx(double x, double y, double z) { return Rot3Q(
|
||||
Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3(
|
||||
Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) *
|
||||
Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) *
|
||||
Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX())));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3Q Rot3Q::rodriguez(const Vector& w, double theta) {
|
||||
Rot3 Rot3::rodriguez(const Vector& w, double theta) {
|
||||
return Quaternion(Eigen::AngleAxisd(theta, w)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3Q Rot3Q::rodriguez(const Vector& w) {
|
||||
Rot3 Rot3::rodriguez(const Vector& w) {
|
||||
double t = w.norm();
|
||||
if (t < 1e-10) return Rot3Q();
|
||||
if (t < 1e-10) return Rot3();
|
||||
return rodriguez(w/t, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Rot3Q::equals(const Rot3Q & R, double tol) const {
|
||||
bool Rot3::equals(const Rot3 & R, double tol) const {
|
||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3Q Rot3Q::compose(const Rot3Q& R2,
|
||||
Rot3 Rot3::compose(const Rot3& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = R2.transpose();
|
||||
if (H2) *H2 = I3;
|
||||
return Rot3Q(quaternion_ * R2.quaternion_);
|
||||
return Rot3(quaternion_ * R2.quaternion_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3Q::operator*(const Point3& p) const {
|
||||
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));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3Q Rot3Q::inverse(boost::optional<Matrix&> H1) const {
|
||||
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
|
||||
if (H1) *H1 = -matrix();
|
||||
return Rot3Q(quaternion_.inverse());
|
||||
return Rot3(quaternion_.inverse());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3Q Rot3Q::between(const Rot3Q& R2,
|
||||
Rot3 Rot3::between(const Rot3& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = -(R2.transpose()*matrix());
|
||||
if (H2) *H2 = I3;
|
||||
|
|
@ -116,12 +115,12 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3Q Rot3Q::operator*(const Rot3Q& R2) const {
|
||||
return Rot3Q(quaternion_ * R2.quaternion_);
|
||||
Rot3 Rot3::operator*(const Rot3& R2) const {
|
||||
return Rot3(quaternion_ * R2.quaternion_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3Q::rotate(const Point3& p,
|
||||
Point3 Rot3::rotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
Matrix R = matrix();
|
||||
if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
|
|
@ -132,7 +131,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SO(3) section
|
||||
Point3 Rot3Q::unrotate(const Point3& p,
|
||||
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
|
||||
|
|
@ -143,7 +142,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
Vector Rot3Q::Logmap(const Rot3Q& R) {
|
||||
Vector Rot3::Logmap(const Rot3& 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
|
||||
|
|
@ -153,13 +152,13 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3Q::matrix() const { return quaternion_.toRotationMatrix(); }
|
||||
Matrix Rot3::matrix() const { return quaternion_.toRotationMatrix(); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3Q::transpose() const { return quaternion_.toRotationMatrix().transpose(); }
|
||||
Matrix Rot3::transpose() const { return quaternion_.toRotationMatrix().transpose(); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3Q::column(int index) const{
|
||||
Point3 Rot3::column(int index) const{
|
||||
if(index == 3)
|
||||
return r3();
|
||||
else if(index == 2)
|
||||
|
|
@ -171,36 +170,55 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3Q::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); }
|
||||
Point3 Rot3::r1() const { return Point3(quaternion_.toRotationMatrix().col(0)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3Q::r2() const { return Point3(quaternion_.toRotationMatrix().col(1)); }
|
||||
Point3 Rot3::r2() const { return Point3(quaternion_.toRotationMatrix().col(1)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3Q::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); }
|
||||
Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3Q::xyz() const {
|
||||
Vector Rot3::xyz() const {
|
||||
Matrix I;Vector q;
|
||||
boost::tie(I,q)=RQ(matrix());
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3Q::ypr() const {
|
||||
Vector Rot3::ypr() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(2),q(1),q(0));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3Q::rpy() const {
|
||||
Vector Rot3::rpy() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(0),q(1),q(2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Quaternion Rot3Q::toQuaternion() const { return quaternion_; }
|
||||
Quaternion Rot3::toQuaternion() const { return quaternion_; }
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix, Vector> RQ(const Matrix& A) {
|
||||
|
||||
double x = -atan2(-A(2, 1), A(2, 2));
|
||||
Rot3 Qx = Rot3::Rx(-x);
|
||||
Matrix B = A * Qx.matrix();
|
||||
|
||||
double y = -atan2(B(2, 0), B(2, 2));
|
||||
Rot3 Qy = Rot3::Ry(-y);
|
||||
Matrix C = B * Qy.matrix();
|
||||
|
||||
double z = -atan2(-C(1, 0), C(1, 1));
|
||||
Rot3 Qz = Rot3::Rz(-z);
|
||||
Matrix R = C * Qz.matrix();
|
||||
|
||||
Vector xyz = Vector_(3, x, y, z);
|
||||
return make_pair(R, xyz);
|
||||
}
|
||||
|
||||
} // namespace gtsam
|
||||
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@
|
|||
|
||||
/**
|
||||
* @file testRot3.cpp
|
||||
* @brief Unit tests for Rot3M class
|
||||
* @brief Unit tests for Rot3 class
|
||||
* @author Alireza Fathi
|
||||
*/
|
||||
|
||||
|
|
@ -23,16 +23,18 @@
|
|||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
|
||||
#ifndef GTSAM_DEFAULT_QUATERNIONS
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
Rot3M R = Rot3M::rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
|
||||
Point3 P(0.2, 0.7, -2.0);
|
||||
double error = 1e-9, epsilon = 0.001;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, constructor)
|
||||
TEST( Rot3, constructor)
|
||||
{
|
||||
Rot3M expected(eye(3, 3));
|
||||
Rot3 expected(eye(3, 3));
|
||||
Vector r1(3), r2(3), r3(3);
|
||||
r1(0) = 1;
|
||||
r1(1) = 0;
|
||||
|
|
@ -43,91 +45,91 @@ TEST( Rot3M, constructor)
|
|||
r3(0) = 0;
|
||||
r3(1) = 0;
|
||||
r3(2) = 1;
|
||||
Rot3M actual(r1, r2, r3);
|
||||
Rot3 actual(r1, r2, r3);
|
||||
CHECK(assert_equal(actual,expected));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, constructor2)
|
||||
TEST( Rot3, constructor2)
|
||||
{
|
||||
Matrix R = Matrix_(3, 3, 11., 12., 13., 21., 22., 23., 31., 32., 33.);
|
||||
Rot3M actual(R);
|
||||
Rot3M expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
|
||||
Rot3 actual(R);
|
||||
Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
|
||||
CHECK(assert_equal(actual,expected));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, constructor3)
|
||||
TEST( Rot3, constructor3)
|
||||
{
|
||||
Rot3M expected(1, 2, 3, 4, 5, 6, 7, 8, 9);
|
||||
Rot3 expected(1, 2, 3, 4, 5, 6, 7, 8, 9);
|
||||
Point3 r1(1, 4, 7), r2(2, 5, 8), r3(3, 6, 9);
|
||||
CHECK(assert_equal(Rot3M(r1,r2,r3),expected));
|
||||
CHECK(assert_equal(Rot3(r1,r2,r3),expected));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, transpose)
|
||||
TEST( Rot3, transpose)
|
||||
{
|
||||
Rot3M R(1, 2, 3, 4, 5, 6, 7, 8, 9);
|
||||
Rot3 R(1, 2, 3, 4, 5, 6, 7, 8, 9);
|
||||
Point3 r1(1, 2, 3), r2(4, 5, 6), r3(7, 8, 9);
|
||||
CHECK(assert_equal(R.inverse(),Rot3M(r1,r2,r3)));
|
||||
CHECK(assert_equal(R.inverse(),Rot3(r1,r2,r3)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, equals)
|
||||
TEST( Rot3, equals)
|
||||
{
|
||||
CHECK(R.equals(R));
|
||||
Rot3M zero;
|
||||
Rot3 zero;
|
||||
CHECK(!R.equals(zero));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
|
||||
Rot3M slow_but_correct_rodriguez(const Vector& w) {
|
||||
Rot3 slow_but_correct_rodriguez(const Vector& w) {
|
||||
double t = norm_2(w);
|
||||
Matrix J = skewSymmetric(w / t);
|
||||
if (t < 1e-5) return Rot3M();
|
||||
if (t < 1e-5) return Rot3();
|
||||
Matrix R = eye(3) + sin(t) * J + (1.0 - cos(t)) * (J * J);
|
||||
return R;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, rodriguez)
|
||||
TEST( Rot3, rodriguez)
|
||||
{
|
||||
Rot3M R1 = Rot3M::rodriguez(epsilon, 0, 0);
|
||||
Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0);
|
||||
Vector w = Vector_(3, epsilon, 0., 0.);
|
||||
Rot3M R2 = slow_but_correct_rodriguez(w);
|
||||
Rot3 R2 = slow_but_correct_rodriguez(w);
|
||||
CHECK(assert_equal(R2,R1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, rodriguez2)
|
||||
TEST( Rot3, rodriguez2)
|
||||
{
|
||||
Vector axis = Vector_(3,0.,1.,0.); // rotation around Y
|
||||
double angle = 3.14 / 4.0;
|
||||
Rot3M actual = Rot3M::rodriguez(axis, angle);
|
||||
Rot3M expected(0.707388, 0, 0.706825,
|
||||
Rot3 actual = Rot3::rodriguez(axis, angle);
|
||||
Rot3 expected(0.707388, 0, 0.706825,
|
||||
0, 1, 0,
|
||||
-0.706825, 0, 0.707388);
|
||||
CHECK(assert_equal(expected,actual,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, rodriguez3)
|
||||
TEST( Rot3, rodriguez3)
|
||||
{
|
||||
Vector w = Vector_(3, 0.1, 0.2, 0.3);
|
||||
Rot3M R1 = Rot3M::rodriguez(w / norm_2(w), norm_2(w));
|
||||
Rot3M R2 = slow_but_correct_rodriguez(w);
|
||||
Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w));
|
||||
Rot3 R2 = slow_but_correct_rodriguez(w);
|
||||
CHECK(assert_equal(R2,R1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, rodriguez4)
|
||||
TEST( Rot3, rodriguez4)
|
||||
{
|
||||
Vector axis = Vector_(3,0.,0.,1.); // rotation around Z
|
||||
double angle = M_PI_2;
|
||||
Rot3M actual = Rot3M::rodriguez(axis, angle);
|
||||
Rot3 actual = Rot3::rodriguez(axis, angle);
|
||||
double c=cos(angle),s=sin(angle);
|
||||
Rot3M expected(c,-s, 0,
|
||||
Rot3 expected(c,-s, 0,
|
||||
s, c, 0,
|
||||
0, 0, 1);
|
||||
CHECK(assert_equal(expected,actual,1e-5));
|
||||
|
|
@ -135,62 +137,62 @@ TEST( Rot3M, rodriguez4)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, expmap)
|
||||
TEST( Rot3, expmap)
|
||||
{
|
||||
Vector v = zero(3);
|
||||
CHECK(assert_equal(R.retract(v), R));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3M, log)
|
||||
TEST(Rot3, log)
|
||||
{
|
||||
Vector w1 = Vector_(3, 0.1, 0.0, 0.0);
|
||||
Rot3M R1 = Rot3M::rodriguez(w1);
|
||||
CHECK(assert_equal(w1, Rot3M::Logmap(R1)));
|
||||
Rot3 R1 = Rot3::rodriguez(w1);
|
||||
CHECK(assert_equal(w1, Rot3::Logmap(R1)));
|
||||
|
||||
Vector w2 = Vector_(3, 0.0, 0.1, 0.0);
|
||||
Rot3M R2 = Rot3M::rodriguez(w2);
|
||||
CHECK(assert_equal(w2, Rot3M::Logmap(R2)));
|
||||
Rot3 R2 = Rot3::rodriguez(w2);
|
||||
CHECK(assert_equal(w2, Rot3::Logmap(R2)));
|
||||
|
||||
Vector w3 = Vector_(3, 0.0, 0.0, 0.1);
|
||||
Rot3M R3 = Rot3M::rodriguez(w3);
|
||||
CHECK(assert_equal(w3, Rot3M::Logmap(R3)));
|
||||
Rot3 R3 = Rot3::rodriguez(w3);
|
||||
CHECK(assert_equal(w3, Rot3::Logmap(R3)));
|
||||
|
||||
Vector w = Vector_(3, 0.1, 0.4, 0.2);
|
||||
Rot3M R = Rot3M::rodriguez(w);
|
||||
CHECK(assert_equal(w, Rot3M::Logmap(R)));
|
||||
Rot3 R = Rot3::rodriguez(w);
|
||||
CHECK(assert_equal(w, Rot3::Logmap(R)));
|
||||
|
||||
Vector w5 = Vector_(3, 0.0, 0.0, 0.0);
|
||||
Rot3M R5 = Rot3M::rodriguez(w5);
|
||||
CHECK(assert_equal(w5, Rot3M::Logmap(R5)));
|
||||
Rot3 R5 = Rot3::rodriguez(w5);
|
||||
CHECK(assert_equal(w5, Rot3::Logmap(R5)));
|
||||
|
||||
Vector w6 = Vector_(3, boost::math::constants::pi<double>(), 0.0, 0.0);
|
||||
Rot3M R6 = Rot3M::rodriguez(w6);
|
||||
CHECK(assert_equal(w6, Rot3M::Logmap(R6)));
|
||||
Rot3 R6 = Rot3::rodriguez(w6);
|
||||
CHECK(assert_equal(w6, Rot3::Logmap(R6)));
|
||||
|
||||
Vector w7 = Vector_(3, 0.0, boost::math::constants::pi<double>(), 0.0);
|
||||
Rot3M R7 = Rot3M::rodriguez(w7);
|
||||
CHECK(assert_equal(w7, Rot3M::Logmap(R7)));
|
||||
Rot3 R7 = Rot3::rodriguez(w7);
|
||||
CHECK(assert_equal(w7, Rot3::Logmap(R7)));
|
||||
|
||||
Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi<double>());
|
||||
Rot3M R8 = Rot3M::rodriguez(w8);
|
||||
CHECK(assert_equal(w8, Rot3M::Logmap(R8)));
|
||||
Rot3 R8 = Rot3::rodriguez(w8);
|
||||
CHECK(assert_equal(w8, Rot3::Logmap(R8)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3M, manifold)
|
||||
TEST(Rot3, manifold)
|
||||
{
|
||||
Rot3M gR1 = Rot3M::rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3M gR2 = Rot3M::rodriguez(0.3, 0.1, 0.7);
|
||||
Rot3M origin;
|
||||
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
|
||||
Rot3 origin;
|
||||
|
||||
// log behaves correctly
|
||||
Vector d12 = gR1.localCoordinates(gR2);
|
||||
CHECK(assert_equal(gR2, gR1.retract(d12)));
|
||||
CHECK(assert_equal(gR2, gR1*Rot3M::Expmap(d12)));
|
||||
CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
|
||||
Vector d21 = gR2.localCoordinates(gR1);
|
||||
CHECK(assert_equal(gR1, gR2.retract(d21)));
|
||||
CHECK(assert_equal(gR1, gR2*Rot3M::Expmap(d21)));
|
||||
CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21)));
|
||||
|
||||
// Check that log(t1,t2)=-log(t2,t1)
|
||||
CHECK(assert_equal(d12,-d21));
|
||||
|
|
@ -198,11 +200,11 @@ TEST(Rot3M, manifold)
|
|||
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
|
||||
Vector d = Vector_(3, 0.1, 0.2, 0.3);
|
||||
// exp(-d)=inverse(exp(d))
|
||||
CHECK(assert_equal(Rot3M::Expmap(-d),Rot3M::Expmap(d).inverse()));
|
||||
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
|
||||
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
|
||||
Rot3M R2 = Rot3M::Expmap (2 * d);
|
||||
Rot3M R3 = Rot3M::Expmap (3 * d);
|
||||
Rot3M R5 = Rot3M::Expmap (5 * d);
|
||||
Rot3 R2 = Rot3::Expmap (2 * d);
|
||||
Rot3 R3 = Rot3::Expmap (3 * d);
|
||||
Rot3 R5 = Rot3::Expmap (5 * d);
|
||||
CHECK(assert_equal(R5,R2*R3));
|
||||
CHECK(assert_equal(R5,R3*R2));
|
||||
}
|
||||
|
|
@ -223,106 +225,106 @@ AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3M, BCH)
|
||||
TEST(Rot3, BCH)
|
||||
{
|
||||
// Approximate exmap by BCH formula
|
||||
AngularVelocity w1(0.2, -0.1, 0.1);
|
||||
AngularVelocity w2(0.01, 0.02, -0.03);
|
||||
Rot3M R1 = Rot3M::Expmap (w1.vector()), R2 = Rot3M::Expmap (w2.vector());
|
||||
Rot3M R3 = R1 * R2;
|
||||
Vector expected = Rot3M::Logmap(R3);
|
||||
Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector());
|
||||
Rot3 R3 = R1 * R2;
|
||||
Vector expected = Rot3::Logmap(R3);
|
||||
Vector actual = BCH(w1, w2).vector();
|
||||
CHECK(assert_equal(expected, actual,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, rotate_derivatives)
|
||||
TEST( Rot3, rotate_derivatives)
|
||||
{
|
||||
Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
|
||||
R.rotate(P, actualDrotate1a, actualDrotate2);
|
||||
R.inverse().rotate(P, actualDrotate1b, boost::none);
|
||||
Matrix numerical1 = numericalDerivative21(testing::rotate<Rot3M,Point3>, R, P);
|
||||
Matrix numerical2 = numericalDerivative21(testing::rotate<Rot3M,Point3>, R.inverse(), P);
|
||||
Matrix numerical3 = numericalDerivative22(testing::rotate<Rot3M,Point3>, R, P);
|
||||
Matrix numerical1 = numericalDerivative21(testing::rotate<Rot3,Point3>, R, P);
|
||||
Matrix numerical2 = numericalDerivative21(testing::rotate<Rot3,Point3>, R.inverse(), P);
|
||||
Matrix numerical3 = numericalDerivative22(testing::rotate<Rot3,Point3>, R, P);
|
||||
EXPECT(assert_equal(numerical1,actualDrotate1a,error));
|
||||
EXPECT(assert_equal(numerical2,actualDrotate1b,error));
|
||||
EXPECT(assert_equal(numerical3,actualDrotate2, error));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, unrotate)
|
||||
TEST( Rot3, unrotate)
|
||||
{
|
||||
Point3 w = R * P;
|
||||
Matrix H1,H2;
|
||||
Point3 actual = R.unrotate(w,H1,H2);
|
||||
CHECK(assert_equal(P,actual));
|
||||
|
||||
Matrix numerical1 = numericalDerivative21(testing::unrotate<Rot3M,Point3>, R, w);
|
||||
Matrix numerical1 = numericalDerivative21(testing::unrotate<Rot3,Point3>, R, w);
|
||||
CHECK(assert_equal(numerical1,H1,error));
|
||||
|
||||
Matrix numerical2 = numericalDerivative22(testing::unrotate<Rot3M,Point3>, R, w);
|
||||
Matrix numerical2 = numericalDerivative22(testing::unrotate<Rot3,Point3>, R, w);
|
||||
CHECK(assert_equal(numerical2,H2,error));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, compose )
|
||||
TEST( Rot3, compose )
|
||||
{
|
||||
Rot3M R1 = Rot3M::rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3M R2 = Rot3M::rodriguez(0.2, 0.3, 0.5);
|
||||
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
|
||||
|
||||
Rot3M expected = R1 * R2;
|
||||
Rot3 expected = R1 * R2;
|
||||
Matrix actualH1, actualH2;
|
||||
Rot3M actual = R1.compose(R2, actualH1, actualH2);
|
||||
Rot3 actual = R1.compose(R2, actualH1, actualH2);
|
||||
CHECK(assert_equal(expected,actual));
|
||||
|
||||
Matrix numericalH1 = numericalDerivative21(testing::compose<Rot3M>, R1,
|
||||
Matrix numericalH1 = numericalDerivative21(testing::compose<Rot3>, R1,
|
||||
R2, 1e-2);
|
||||
CHECK(assert_equal(numericalH1,actualH1));
|
||||
|
||||
Matrix numericalH2 = numericalDerivative22(testing::compose<Rot3M>, R1,
|
||||
Matrix numericalH2 = numericalDerivative22(testing::compose<Rot3>, R1,
|
||||
R2, 1e-2);
|
||||
CHECK(assert_equal(numericalH2,actualH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, inverse )
|
||||
TEST( Rot3, inverse )
|
||||
{
|
||||
Rot3M R = Rot3M::rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
|
||||
|
||||
Rot3M I;
|
||||
Rot3 I;
|
||||
Matrix actualH;
|
||||
CHECK(assert_equal(I,R*R.inverse(actualH)));
|
||||
CHECK(assert_equal(I,R.inverse()*R));
|
||||
|
||||
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3M>, R);
|
||||
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R);
|
||||
CHECK(assert_equal(numericalH,actualH));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, between )
|
||||
TEST( Rot3, between )
|
||||
{
|
||||
Rot3M R = Rot3M::rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3M origin;
|
||||
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 origin;
|
||||
CHECK(assert_equal(R, origin.between(R)));
|
||||
CHECK(assert_equal(R.inverse(), R.between(origin)));
|
||||
|
||||
Rot3M R1 = Rot3M::rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3M R2 = Rot3M::rodriguez(0.2, 0.3, 0.5);
|
||||
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
|
||||
|
||||
Rot3M expected = R1.inverse() * R2;
|
||||
Rot3 expected = R1.inverse() * R2;
|
||||
Matrix actualH1, actualH2;
|
||||
Rot3M actual = R1.between(R2, actualH1, actualH2);
|
||||
Rot3 actual = R1.between(R2, actualH1, actualH2);
|
||||
CHECK(assert_equal(expected,actual));
|
||||
|
||||
Matrix numericalH1 = numericalDerivative21(testing::between<Rot3M> , R1, R2);
|
||||
Matrix numericalH1 = numericalDerivative21(testing::between<Rot3> , R1, R2);
|
||||
CHECK(assert_equal(numericalH1,actualH1));
|
||||
|
||||
Matrix numericalH2 = numericalDerivative22(testing::between<Rot3M> , R1, R2);
|
||||
Matrix numericalH2 = numericalDerivative22(testing::between<Rot3> , R1, R2);
|
||||
CHECK(assert_equal(numericalH2,actualH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, xyz )
|
||||
TEST( Rot3, xyz )
|
||||
{
|
||||
double t = 0.1, st = sin(t), ct = cos(t);
|
||||
|
||||
|
|
@ -332,47 +334,47 @@ TEST( Rot3M, xyz )
|
|||
// z
|
||||
// | * Y=(ct,st)
|
||||
// x----y
|
||||
Rot3M expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
|
||||
CHECK(assert_equal(expected1,Rot3M::Rx(t)));
|
||||
Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
|
||||
CHECK(assert_equal(expected1,Rot3::Rx(t)));
|
||||
|
||||
// x
|
||||
// | * Z=(ct,st)
|
||||
// y----z
|
||||
Rot3M expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
|
||||
CHECK(assert_equal(expected2,Rot3M::Ry(t)));
|
||||
Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
|
||||
CHECK(assert_equal(expected2,Rot3::Ry(t)));
|
||||
|
||||
// y
|
||||
// | X=* (ct,st)
|
||||
// z----x
|
||||
Rot3M expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
|
||||
CHECK(assert_equal(expected3,Rot3M::Rz(t)));
|
||||
Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
|
||||
CHECK(assert_equal(expected3,Rot3::Rz(t)));
|
||||
|
||||
// Check compound rotation
|
||||
Rot3M expected = Rot3M::Rz(0.3) * Rot3M::Ry(0.2) * Rot3M::Rx(0.1);
|
||||
CHECK(assert_equal(expected,Rot3M::RzRyRx(0.1,0.2,0.3)));
|
||||
Rot3 expected = Rot3::Rz(0.3) * Rot3::Ry(0.2) * Rot3::Rx(0.1);
|
||||
CHECK(assert_equal(expected,Rot3::RzRyRx(0.1,0.2,0.3)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, yaw_pitch_roll )
|
||||
TEST( Rot3, yaw_pitch_roll )
|
||||
{
|
||||
double t = 0.1;
|
||||
|
||||
// yaw is around z axis
|
||||
CHECK(assert_equal(Rot3M::Rz(t),Rot3M::yaw(t)));
|
||||
CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t)));
|
||||
|
||||
// pitch is around y axis
|
||||
CHECK(assert_equal(Rot3M::Ry(t),Rot3M::pitch(t)));
|
||||
CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t)));
|
||||
|
||||
// roll is around x axis
|
||||
CHECK(assert_equal(Rot3M::Rx(t),Rot3M::roll(t)));
|
||||
CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t)));
|
||||
|
||||
// Check compound rotation
|
||||
Rot3M expected = Rot3M::yaw(0.1) * Rot3M::pitch(0.2) * Rot3M::roll(0.3);
|
||||
CHECK(assert_equal(expected,Rot3M::ypr(0.1,0.2,0.3)));
|
||||
Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3);
|
||||
CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, RQ)
|
||||
TEST( Rot3, RQ)
|
||||
{
|
||||
// Try RQ on a pure rotation
|
||||
Matrix actualK;
|
||||
|
|
@ -382,18 +384,18 @@ TEST( Rot3M, RQ)
|
|||
CHECK(assert_equal(eye(3),actualK));
|
||||
CHECK(assert_equal(expected,actual,1e-6));
|
||||
|
||||
// Try using xyz call, asserting that Rot3M::RzRyRx(x,y,z).xyz()==[x;y;z]
|
||||
// Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z]
|
||||
CHECK(assert_equal(expected,R.xyz(),1e-6));
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3M::RzRyRx(0.1,0.2,0.3).xyz()));
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz()));
|
||||
|
||||
// Try using ypr call, asserting that Rot3M::ypr(y,p,r).ypr()==[y;p;r]
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3M::ypr(0.1,0.2,0.3).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.3,0.2,0.1),Rot3M::ypr(0.1,0.2,0.3).rpy()));
|
||||
// Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r]
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy()));
|
||||
|
||||
// Try ypr for pure yaw-pitch-roll matrices
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3M::yaw (0.1).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3M::pitch(0.1).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3M::roll (0.1).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3::yaw (0.1).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3::pitch(0.1).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
|
||||
|
||||
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
|
||||
Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
|
||||
|
|
@ -404,60 +406,60 @@ TEST( Rot3M, RQ)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, expmapStability ) {
|
||||
TEST( Rot3, expmapStability ) {
|
||||
Vector w = Vector_(3, 78e-9, 5e-8, 97e-7);
|
||||
double theta = w.norm();
|
||||
double theta2 = theta*theta;
|
||||
Rot3M actualR = Rot3M::Expmap(w);
|
||||
Rot3 actualR = Rot3::Expmap(w);
|
||||
Matrix W = Matrix_(3,3, 0.0, -w(2), w(1),
|
||||
w(2), 0.0, -w(0),
|
||||
-w(1), w(0), 0.0 );
|
||||
Matrix W2 = W*W;
|
||||
Matrix Rmat = eye(3) + (1.0-theta2/6.0 + theta2*theta2/120.0
|
||||
- theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ;
|
||||
Rot3M expectedR( Rmat );
|
||||
Rot3 expectedR( Rmat );
|
||||
CHECK(assert_equal(expectedR, actualR, 1e-10));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3M, logmapStability ) {
|
||||
TEST( Rot3, logmapStability ) {
|
||||
Vector w = Vector_(3, 1e-8, 0.0, 0.0);
|
||||
Rot3M R = Rot3M::Expmap(w);
|
||||
Rot3 R = Rot3::Expmap(w);
|
||||
// double tr = R.r1().x()+R.r2().y()+R.r3().z();
|
||||
// std::cout.precision(5000);
|
||||
// std::cout << "theta: " << w.norm() << std::endl;
|
||||
// std::cout << "trace: " << tr << std::endl;
|
||||
// R.print("R = ");
|
||||
Vector actualw = Rot3M::Logmap(R);
|
||||
Vector actualw = Rot3::Logmap(R);
|
||||
CHECK(assert_equal(w, actualw, 1e-15));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3M, quaternion) {
|
||||
TEST(Rot3, quaternion) {
|
||||
// NOTE: This is also verifying the ability to convert Vector to Quaternion
|
||||
Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
|
||||
Rot3M R1 = Rot3M(Matrix_(3,3,
|
||||
Rot3 R1 = Rot3(Matrix_(3,3,
|
||||
0.271018623057411, 0.278786459830371, 0.921318086098018,
|
||||
0.578529366719085, 0.717799701969298, -0.387385285854279,
|
||||
-0.769319620053772, 0.637998195662053, 0.033250932803219));
|
||||
|
||||
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
|
||||
Rot3M R2 = Rot3M(Matrix_(3,3,
|
||||
Rot3 R2 = Rot3(Matrix_(3,3,
|
||||
-0.207341903877828, 0.250149415542075, 0.945745528564780,
|
||||
0.881304914479026, -0.371869043667957, 0.291573424846290,
|
||||
0.424630407073532, 0.893945571198514, -0.143353873763946));
|
||||
|
||||
// Check creating Rot3M from quaternion
|
||||
EXPECT(assert_equal(R1, Rot3M(q1)));
|
||||
EXPECT(assert_equal(R1, Rot3M::quaternion(q1.w(), q1.x(), q1.y(), q1.z())));
|
||||
EXPECT(assert_equal(R2, Rot3M(q2)));
|
||||
EXPECT(assert_equal(R2, Rot3M::quaternion(q2.w(), q2.x(), q2.y(), q2.z())));
|
||||
// Check creating Rot3 from quaternion
|
||||
EXPECT(assert_equal(R1, Rot3(q1)));
|
||||
EXPECT(assert_equal(R1, Rot3::quaternion(q1.w(), q1.x(), q1.y(), q1.z())));
|
||||
EXPECT(assert_equal(R2, Rot3(q2)));
|
||||
EXPECT(assert_equal(R2, Rot3::quaternion(q2.w(), q2.x(), q2.y(), q2.z())));
|
||||
|
||||
// Check converting Rot3M to quaterion
|
||||
// Check converting Rot3 to quaterion
|
||||
EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs())));
|
||||
EXPECT(assert_equal(Vector(R2.toQuaternion().coeffs()), Vector(q2.coeffs())));
|
||||
|
||||
// Check that quaternion and Rot3M represent the same rotation
|
||||
// Check that quaternion and Rot3 represent the same rotation
|
||||
Point3 p1(1.0, 2.0, 3.0);
|
||||
Point3 p2(8.0, 7.0, 9.0);
|
||||
|
||||
|
|
@ -471,6 +473,8 @@ TEST(Rot3M, quaternion) {
|
|||
EXPECT(assert_equal(expected2, actual2));
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -11,7 +11,7 @@
|
|||
|
||||
/**
|
||||
* @file testRot3.cpp
|
||||
* @brief Unit tests for Rot3Q class
|
||||
* @brief Unit tests for Rot3 class
|
||||
* @author Alireza Fathi
|
||||
*/
|
||||
|
||||
|
|
@ -23,16 +23,18 @@
|
|||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/geometry/Rot3.h>
|
||||
|
||||
#ifdef GTSAM_DEFAULT_QUATERNIONS
|
||||
|
||||
using namespace gtsam;
|
||||
|
||||
Rot3Q R = Rot3Q::rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
|
||||
Point3 P(0.2, 0.7, -2.0);
|
||||
double error = 1e-9, epsilon = 0.001;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, constructor)
|
||||
TEST( Rot3, constructor)
|
||||
{
|
||||
Rot3Q expected(eye(3, 3));
|
||||
Rot3 expected(eye(3, 3));
|
||||
Vector r1(3), r2(3), r3(3);
|
||||
r1(0) = 1;
|
||||
r1(1) = 0;
|
||||
|
|
@ -43,178 +45,146 @@ TEST( Rot3Q, constructor)
|
|||
r3(0) = 0;
|
||||
r3(1) = 0;
|
||||
r3(2) = 1;
|
||||
Rot3Q actual(r1, r2, r3);
|
||||
Rot3 actual(r1, r2, r3);
|
||||
CHECK(assert_equal(actual,expected));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, constructor2)
|
||||
TEST( Rot3, constructor2)
|
||||
{
|
||||
Matrix R = Matrix_(3, 3, 11., 12., 13., 21., 22., 23., 31., 32., 33.);
|
||||
Rot3Q actual(R);
|
||||
Rot3Q expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
|
||||
Rot3 actual(R);
|
||||
Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33);
|
||||
CHECK(assert_equal(actual,expected));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, constructor3)
|
||||
TEST( Rot3, constructor3)
|
||||
{
|
||||
Rot3Q expected(1, 2, 3, 4, 5, 6, 7, 8, 9);
|
||||
Rot3 expected(1, 2, 3, 4, 5, 6, 7, 8, 9);
|
||||
Point3 r1(1, 4, 7), r2(2, 5, 8), r3(3, 6, 9);
|
||||
CHECK(assert_equal(Rot3Q(r1,r2,r3),expected));
|
||||
CHECK(assert_equal(Rot3(r1,r2,r3),expected));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, equals)
|
||||
TEST( Rot3, equals)
|
||||
{
|
||||
CHECK(R.equals(R));
|
||||
Rot3Q zero;
|
||||
Rot3 zero;
|
||||
CHECK(!R.equals(zero));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + ....
|
||||
Rot3Q slow_but_correct_rodriguez(const Vector& w) {
|
||||
Rot3 slow_but_correct_rodriguez(const Vector& w) {
|
||||
double t = norm_2(w);
|
||||
Matrix J = skewSymmetric(w / t);
|
||||
if (t < 1e-5) return Rot3Q();
|
||||
if (t < 1e-5) return Rot3();
|
||||
Matrix R = eye(3) + sin(t) * J + (1.0 - cos(t)) * (J * J);
|
||||
return R;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, rodriguez)
|
||||
TEST( Rot3, rodriguez)
|
||||
{
|
||||
Rot3Q R1 = Rot3Q::rodriguez(epsilon, 0, 0);
|
||||
Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0);
|
||||
Vector w = Vector_(3, epsilon, 0., 0.);
|
||||
Rot3Q R2 = slow_but_correct_rodriguez(w);
|
||||
Rot3Q expected2(Rot3M::rodriguez(epsilon, 0, 0));
|
||||
CHECK(assert_equal(expected2,R1,1e-5));
|
||||
Rot3 R2 = slow_but_correct_rodriguez(w);
|
||||
CHECK(assert_equal(R2,R1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, rodriguez2)
|
||||
TEST( Rot3, rodriguez2)
|
||||
{
|
||||
Vector axis = Vector_(3,0.,1.,0.); // rotation around Y
|
||||
double angle = 3.14 / 4.0;
|
||||
Rot3Q actual = Rot3Q::rodriguez(axis, angle);
|
||||
Rot3Q expected(0.707388, 0, 0.706825,
|
||||
Rot3 actual = Rot3::rodriguez(axis, angle);
|
||||
Rot3 expected(0.707388, 0, 0.706825,
|
||||
0, 1, 0,
|
||||
-0.706825, 0, 0.707388);
|
||||
Rot3Q expected2(Rot3M::rodriguez(axis, angle));
|
||||
CHECK(assert_equal(expected,actual,1e-5));
|
||||
CHECK(assert_equal(expected2,actual,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, rodriguez3)
|
||||
TEST( Rot3, rodriguez3)
|
||||
{
|
||||
Vector w = Vector_(3, 0.1, 0.2, 0.3);
|
||||
Rot3Q R1 = Rot3Q::rodriguez(w / norm_2(w), norm_2(w));
|
||||
Rot3Q R2 = slow_but_correct_rodriguez(w);
|
||||
Rot3Q expected2(Rot3M::rodriguez(w / norm_2(w), norm_2(w)));
|
||||
CHECK(assert_equal(expected2,R1));
|
||||
Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w));
|
||||
Rot3 R2 = slow_but_correct_rodriguez(w);
|
||||
CHECK(assert_equal(R2,R1));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, rodriguez4)
|
||||
TEST( Rot3, rodriguez4)
|
||||
{
|
||||
Vector axis = Vector_(3,0.,0.,1.); // rotation around Z
|
||||
double angle = M_PI_2;
|
||||
Rot3Q actual = Rot3Q::rodriguez(axis, angle);
|
||||
Rot3 actual = Rot3::rodriguez(axis, angle);
|
||||
double c=cos(angle),s=sin(angle);
|
||||
Rot3Q expected1(c,-s, 0,
|
||||
Rot3 expected(c,-s, 0,
|
||||
s, c, 0,
|
||||
0, 0, 1);
|
||||
Rot3Q expected2(Rot3M::rodriguez(axis, angle));
|
||||
CHECK(assert_equal(expected1,actual,1e-5));
|
||||
CHECK(assert_equal(expected2,actual,1e-5));
|
||||
CHECK(assert_equal(expected,actual,1e-5));
|
||||
CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, expmap)
|
||||
TEST( Rot3, expmap)
|
||||
{
|
||||
Vector v = zero(3);
|
||||
CHECK(assert_equal(R.retract(v), R));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3Q, log)
|
||||
TEST(Rot3, log)
|
||||
{
|
||||
Vector w1 = Vector_(3, 0.1, 0.0, 0.0);
|
||||
Rot3Q R1 = Rot3Q::rodriguez(w1);
|
||||
Rot3Q R1m = Rot3M::rodriguez(w1);
|
||||
CHECK(assert_equal(w1, Rot3Q::Logmap(R1)));
|
||||
CHECK(assert_equal(R1m, R1));
|
||||
Rot3 R1 = Rot3::rodriguez(w1);
|
||||
CHECK(assert_equal(w1, Rot3::Logmap(R1)));
|
||||
|
||||
Vector w2 = Vector_(3, 0.0, 0.1, 0.0);
|
||||
Rot3Q R2 = Rot3Q::rodriguez(w2);
|
||||
Rot3Q R2m = Rot3M::rodriguez(w2);
|
||||
CHECK(assert_equal(w2, Rot3Q::Logmap(R2)));
|
||||
CHECK(assert_equal(R2m, R2));
|
||||
Rot3 R2 = Rot3::rodriguez(w2);
|
||||
CHECK(assert_equal(w2, Rot3::Logmap(R2)));
|
||||
|
||||
Vector w3 = Vector_(3, 0.0, 0.0, 0.1);
|
||||
Rot3Q R3 = Rot3Q::rodriguez(w3);
|
||||
Rot3Q R3m = Rot3M::rodriguez(w3);
|
||||
CHECK(assert_equal(w3, Rot3Q::Logmap(R3)));
|
||||
CHECK(assert_equal(R3m, R3));
|
||||
Rot3 R3 = Rot3::rodriguez(w3);
|
||||
CHECK(assert_equal(w3, Rot3::Logmap(R3)));
|
||||
|
||||
Vector w = Vector_(3, 0.1, 0.4, 0.2);
|
||||
Rot3Q R = Rot3Q::rodriguez(w);
|
||||
Rot3Q Rm = Rot3M::rodriguez(w);
|
||||
CHECK(assert_equal(w, Rot3Q::Logmap(R)));
|
||||
CHECK(assert_equal(Rm, R));
|
||||
Rot3 R = Rot3::rodriguez(w);
|
||||
CHECK(assert_equal(w, Rot3::Logmap(R)));
|
||||
|
||||
Vector w5 = Vector_(3, 0.0, 0.0, 0.0);
|
||||
Rot3Q R5 = Rot3Q::rodriguez(w5);
|
||||
Rot3Q R5m = Rot3M::rodriguez(w5);
|
||||
CHECK(assert_equal(w5, Rot3Q::Logmap(R5)));
|
||||
CHECK(assert_equal(R5m, R5));
|
||||
Rot3 R5 = Rot3::rodriguez(w5);
|
||||
CHECK(assert_equal(w5, Rot3::Logmap(R5)));
|
||||
|
||||
Vector w6 = Vector_(3, boost::math::constants::pi<double>(), 0.0, 0.0);
|
||||
Rot3Q R6 = Rot3Q::rodriguez(w6);
|
||||
Rot3Q R6m = Rot3M::rodriguez(w6);
|
||||
CHECK(assert_equal(w6, Rot3Q::Logmap(R6)));
|
||||
CHECK(assert_equal(R6m, R6));
|
||||
Rot3 R6 = Rot3::rodriguez(w6);
|
||||
CHECK(assert_equal(w6, Rot3::Logmap(R6)));
|
||||
|
||||
Vector w7 = Vector_(3, 0.0, boost::math::constants::pi<double>(), 0.0);
|
||||
Rot3Q R7 = Rot3Q::rodriguez(w7);
|
||||
Rot3Q R7m = Rot3M::rodriguez(w7);
|
||||
CHECK(assert_equal(w7, Rot3Q::Logmap(R7)));
|
||||
CHECK(assert_equal(R7m, R7));
|
||||
Rot3 R7 = Rot3::rodriguez(w7);
|
||||
CHECK(assert_equal(w7, Rot3::Logmap(R7)));
|
||||
|
||||
Vector w8 = Vector_(3, 0.0, 0.0, boost::math::constants::pi<double>());
|
||||
Rot3Q R8 = Rot3Q::rodriguez(w8);
|
||||
Rot3Q R8m = Rot3M::rodriguez(w8);
|
||||
CHECK(assert_equal(w8, Rot3Q::Logmap(R8)));
|
||||
CHECK(assert_equal(R8m, R8));
|
||||
Rot3 R8 = Rot3::rodriguez(w8);
|
||||
CHECK(assert_equal(w8, Rot3::Logmap(R8)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3Q, manifold)
|
||||
TEST(Rot3, manifold)
|
||||
{
|
||||
Rot3Q gR1 = Rot3Q::rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3Q gR2 = Rot3Q::rodriguez(0.3, 0.1, 0.7);
|
||||
Rot3Q origin;
|
||||
|
||||
Rot3M gR1m = Rot3M::rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3M gR2m = Rot3M::rodriguez(0.3, 0.1, 0.7);
|
||||
|
||||
EXPECT(assert_equal(gR1m.matrix(), gR1.matrix()));
|
||||
EXPECT(assert_equal(gR2m.matrix(), gR2.matrix()));
|
||||
Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7);
|
||||
Rot3 origin;
|
||||
|
||||
// log behaves correctly
|
||||
Vector d12 = gR1.localCoordinates(gR2);
|
||||
EXPECT(assert_equal(gR1m.localCoordinates(gR2m), d12));
|
||||
CHECK(assert_equal(gR2, gR1.retract(d12)));
|
||||
CHECK(assert_equal(gR2, gR1*Rot3Q::Expmap(d12)));
|
||||
CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12)));
|
||||
Vector d21 = gR2.localCoordinates(gR1);
|
||||
EXPECT(assert_equal(gR2m.localCoordinates(gR1m), d21));
|
||||
CHECK(assert_equal(gR1, gR2.retract(d21)));
|
||||
CHECK(assert_equal(gR1, gR2*Rot3Q::Expmap(d21)));
|
||||
CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21)));
|
||||
|
||||
// Check that log(t1,t2)=-log(t2,t1)
|
||||
CHECK(assert_equal(d12,-d21));
|
||||
|
|
@ -222,11 +192,11 @@ TEST(Rot3Q, manifold)
|
|||
// lines in canonical coordinates correspond to Abelian subgroups in SO(3)
|
||||
Vector d = Vector_(3, 0.1, 0.2, 0.3);
|
||||
// exp(-d)=inverse(exp(d))
|
||||
CHECK(assert_equal(Rot3Q::Expmap(-d),Rot3Q::Expmap(d).inverse()));
|
||||
CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse()));
|
||||
// exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d)
|
||||
Rot3Q R2 = Rot3Q::Expmap (2 * d);
|
||||
Rot3Q R3 = Rot3Q::Expmap (3 * d);
|
||||
Rot3Q R5 = Rot3Q::Expmap (5 * d);
|
||||
Rot3 R2 = Rot3::Expmap (2 * d);
|
||||
Rot3 R3 = Rot3::Expmap (3 * d);
|
||||
Rot3 R5 = Rot3::Expmap (5 * d);
|
||||
CHECK(assert_equal(R5,R2*R3));
|
||||
CHECK(assert_equal(R5,R3*R2));
|
||||
}
|
||||
|
|
@ -247,86 +217,86 @@ AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3Q, BCH)
|
||||
TEST(Rot3, BCH)
|
||||
{
|
||||
// Approximate exmap by BCH formula
|
||||
AngularVelocity w1(0.2, -0.1, 0.1);
|
||||
AngularVelocity w2(0.01, 0.02, -0.03);
|
||||
Rot3Q R1 = Rot3Q::Expmap (w1.vector()), R2 = Rot3Q::Expmap (w2.vector());
|
||||
Rot3Q R3 = R1 * R2;
|
||||
Vector expected = Rot3Q::Logmap(R3);
|
||||
Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector());
|
||||
Rot3 R3 = R1 * R2;
|
||||
Vector expected = Rot3::Logmap(R3);
|
||||
Vector actual = BCH(w1, w2).vector();
|
||||
CHECK(assert_equal(expected, actual,1e-5));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, rotate_derivatives)
|
||||
TEST( Rot3, rotate_derivatives)
|
||||
{
|
||||
Matrix actualDrotate1a, actualDrotate1b, actualDrotate2;
|
||||
R.rotate(P, actualDrotate1a, actualDrotate2);
|
||||
R.inverse().rotate(P, actualDrotate1b, boost::none);
|
||||
Matrix numerical1 = numericalDerivative21(testing::rotate<Rot3Q,Point3>, R, P);
|
||||
Matrix numerical2 = numericalDerivative21(testing::rotate<Rot3Q,Point3>, R.inverse(), P);
|
||||
Matrix numerical3 = numericalDerivative22(testing::rotate<Rot3Q,Point3>, R, P);
|
||||
Matrix numerical1 = numericalDerivative21(testing::rotate<Rot3,Point3>, R, P);
|
||||
Matrix numerical2 = numericalDerivative21(testing::rotate<Rot3,Point3>, R.inverse(), P);
|
||||
Matrix numerical3 = numericalDerivative22(testing::rotate<Rot3,Point3>, R, P);
|
||||
EXPECT(assert_equal(numerical1,actualDrotate1a,error));
|
||||
EXPECT(assert_equal(numerical2,actualDrotate1b,error));
|
||||
EXPECT(assert_equal(numerical3,actualDrotate2, error));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, unrotate)
|
||||
TEST( Rot3, unrotate)
|
||||
{
|
||||
Point3 w = R * P;
|
||||
Matrix H1,H2;
|
||||
Point3 actual = R.unrotate(w,H1,H2);
|
||||
CHECK(assert_equal(P,actual));
|
||||
|
||||
Matrix numerical1 = numericalDerivative21(testing::unrotate<Rot3Q,Point3>, R, w);
|
||||
Matrix numerical1 = numericalDerivative21(testing::unrotate<Rot3,Point3>, R, w);
|
||||
CHECK(assert_equal(numerical1,H1,error));
|
||||
|
||||
Matrix numerical2 = numericalDerivative22(testing::unrotate<Rot3Q,Point3>, R, w);
|
||||
Matrix numerical2 = numericalDerivative22(testing::unrotate<Rot3,Point3>, R, w);
|
||||
CHECK(assert_equal(numerical2,H2,error));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, compose )
|
||||
TEST( Rot3, compose )
|
||||
{
|
||||
Rot3Q R1 = Rot3Q::rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3Q R2 = Rot3Q::rodriguez(0.2, 0.3, 0.5);
|
||||
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
|
||||
|
||||
Rot3Q expected = R1 * R2;
|
||||
Rot3 expected = R1 * R2;
|
||||
Matrix actualH1, actualH2;
|
||||
Rot3Q actual = R1.compose(R2, actualH1, actualH2);
|
||||
Rot3 actual = R1.compose(R2, actualH1, actualH2);
|
||||
CHECK(assert_equal(expected,actual));
|
||||
|
||||
Matrix numericalH1 = numericalDerivative21(testing::compose<Rot3Q>, R1,
|
||||
Matrix numericalH1 = numericalDerivative21(testing::compose<Rot3>, R1,
|
||||
R2, 1e-2);
|
||||
CHECK(assert_equal(numericalH1,actualH1));
|
||||
|
||||
Matrix numericalH2 = numericalDerivative22(testing::compose<Rot3Q>, R1,
|
||||
Matrix numericalH2 = numericalDerivative22(testing::compose<Rot3>, R1,
|
||||
R2, 1e-2);
|
||||
CHECK(assert_equal(numericalH2,actualH2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, inverse )
|
||||
TEST( Rot3, inverse )
|
||||
{
|
||||
Rot3Q R = Rot3Q::rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
|
||||
|
||||
Rot3Q I;
|
||||
Rot3 I;
|
||||
Matrix actualH;
|
||||
CHECK(assert_equal(I,R*R.inverse(actualH)));
|
||||
CHECK(assert_equal(I,R.inverse()*R));
|
||||
|
||||
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3Q>, R);
|
||||
Matrix numericalH = numericalDerivative11(testing::inverse<Rot3>, R);
|
||||
CHECK(assert_equal(numericalH,actualH, 1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, between )
|
||||
TEST( Rot3, between )
|
||||
{
|
||||
Rot3Q r1 = Rot3Q::Rz(M_PI/3.0);
|
||||
Rot3Q r2 = Rot3Q::Rz(2.0*M_PI/3.0);
|
||||
Rot3 r1 = Rot3::Rz(M_PI/3.0);
|
||||
Rot3 r2 = Rot3::Rz(2.0*M_PI/3.0);
|
||||
|
||||
Matrix expectedr1 = Matrix_(3,3,
|
||||
0.5, -sqrt(3.0)/2.0, 0.0,
|
||||
|
|
@ -334,32 +304,28 @@ TEST( Rot3Q, between )
|
|||
0.0, 0.0, 1.0);
|
||||
EXPECT(assert_equal(expectedr1, r1.matrix()));
|
||||
|
||||
Rot3Q R = Rot3Q::rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3Q origin;
|
||||
Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2);
|
||||
Rot3 origin;
|
||||
CHECK(assert_equal(R, origin.between(R)));
|
||||
CHECK(assert_equal(R.inverse(), R.between(origin)));
|
||||
|
||||
Rot3Q R1 = Rot3Q::rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3Q R2 = Rot3Q::rodriguez(0.2, 0.3, 0.5);
|
||||
Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3);
|
||||
Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5);
|
||||
|
||||
Rot3Q expected = R1.inverse() * R2;
|
||||
Rot3 expected = R1.inverse() * R2;
|
||||
Matrix actualH1, actualH2;
|
||||
Rot3Q actual = R1.between(R2, actualH1, actualH2);
|
||||
Rot3 actual = R1.between(R2, actualH1, actualH2);
|
||||
CHECK(assert_equal(expected,actual));
|
||||
|
||||
Matrix numericalH1 = numericalDerivative21(testing::between<Rot3Q> , R1, R2);
|
||||
Matrix numericalH1 = numericalDerivative21(testing::between<Rot3> , R1, R2);
|
||||
CHECK(assert_equal(numericalH1,actualH1, 1e-4));
|
||||
Matrix numericalH1M = numericalDerivative21(testing::between<Rot3M> , Rot3M(R1.matrix()), Rot3M(R2.matrix()));
|
||||
CHECK(assert_equal(numericalH1M,actualH1, 1e-4));
|
||||
|
||||
Matrix numericalH2 = numericalDerivative22(testing::between<Rot3Q> , R1, R2);
|
||||
Matrix numericalH2 = numericalDerivative22(testing::between<Rot3> , R1, R2);
|
||||
CHECK(assert_equal(numericalH2,actualH2, 1e-4));
|
||||
Matrix numericalH2M = numericalDerivative22(testing::between<Rot3M> , Rot3M(R1.matrix()), Rot3M(R2.matrix()));
|
||||
CHECK(assert_equal(numericalH2M,actualH2, 1e-4));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, xyz )
|
||||
TEST( Rot3, xyz )
|
||||
{
|
||||
double t = 0.1, st = sin(t), ct = cos(t);
|
||||
|
||||
|
|
@ -369,47 +335,47 @@ TEST( Rot3Q, xyz )
|
|||
// z
|
||||
// | * Y=(ct,st)
|
||||
// x----y
|
||||
Rot3Q expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
|
||||
CHECK(assert_equal(expected1,Rot3Q::Rx(t)));
|
||||
Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct);
|
||||
CHECK(assert_equal(expected1,Rot3::Rx(t)));
|
||||
|
||||
// x
|
||||
// | * Z=(ct,st)
|
||||
// y----z
|
||||
Rot3Q expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
|
||||
CHECK(assert_equal(expected2,Rot3Q::Ry(t)));
|
||||
Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct);
|
||||
CHECK(assert_equal(expected2,Rot3::Ry(t)));
|
||||
|
||||
// y
|
||||
// | X=* (ct,st)
|
||||
// z----x
|
||||
Rot3Q expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
|
||||
CHECK(assert_equal(expected3,Rot3Q::Rz(t)));
|
||||
Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1);
|
||||
CHECK(assert_equal(expected3,Rot3::Rz(t)));
|
||||
|
||||
// Check compound rotation
|
||||
Rot3Q expected = Rot3Q::Rz(0.3) * Rot3Q::Ry(0.2) * Rot3Q::Rx(0.1);
|
||||
CHECK(assert_equal(expected,Rot3Q::RzRyRx(0.1,0.2,0.3)));
|
||||
Rot3 expected = Rot3::Rz(0.3) * Rot3::Ry(0.2) * Rot3::Rx(0.1);
|
||||
CHECK(assert_equal(expected,Rot3::RzRyRx(0.1,0.2,0.3)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, yaw_pitch_roll )
|
||||
TEST( Rot3, yaw_pitch_roll )
|
||||
{
|
||||
double t = 0.1;
|
||||
|
||||
// yaw is around z axis
|
||||
CHECK(assert_equal(Rot3Q::Rz(t),Rot3Q::yaw(t)));
|
||||
CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t)));
|
||||
|
||||
// pitch is around y axis
|
||||
CHECK(assert_equal(Rot3Q::Ry(t),Rot3Q::pitch(t)));
|
||||
CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t)));
|
||||
|
||||
// roll is around x axis
|
||||
CHECK(assert_equal(Rot3Q::Rx(t),Rot3Q::roll(t)));
|
||||
CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t)));
|
||||
|
||||
// Check compound rotation
|
||||
Rot3Q expected = Rot3Q::yaw(0.1) * Rot3Q::pitch(0.2) * Rot3Q::roll(0.3);
|
||||
CHECK(assert_equal(expected,Rot3Q::ypr(0.1,0.2,0.3)));
|
||||
Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3);
|
||||
CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3)));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, RQ)
|
||||
TEST( Rot3, RQ)
|
||||
{
|
||||
// Try RQ on a pure rotation
|
||||
Matrix actualK;
|
||||
|
|
@ -419,18 +385,18 @@ TEST( Rot3Q, RQ)
|
|||
CHECK(assert_equal(eye(3),actualK));
|
||||
CHECK(assert_equal(expected,actual,1e-6));
|
||||
|
||||
// Try using xyz call, asserting that Rot3Q::RzRyRx(x,y,z).xyz()==[x;y;z]
|
||||
// Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z]
|
||||
CHECK(assert_equal(expected,R.xyz(),1e-6));
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3Q::RzRyRx(0.1,0.2,0.3).xyz()));
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz()));
|
||||
|
||||
// Try using ypr call, asserting that Rot3Q::ypr(y,p,r).ypr()==[y;p;r]
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3Q::ypr(0.1,0.2,0.3).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.3,0.2,0.1),Rot3Q::ypr(0.1,0.2,0.3).rpy()));
|
||||
// Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r]
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy()));
|
||||
|
||||
// Try ypr for pure yaw-pitch-roll matrices
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3Q::yaw (0.1).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3Q::pitch(0.1).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3Q::roll (0.1).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.1,0.0,0.0),Rot3::yaw (0.1).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.0,0.1,0.0),Rot3::pitch(0.1).ypr()));
|
||||
CHECK(assert_equal(Vector_(3,0.0,0.0,0.1),Rot3::roll (0.1).ypr()));
|
||||
|
||||
// Try RQ to recover calibration from 3*3 sub-block of projection matrix
|
||||
Matrix K = Matrix_(3, 3, 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0);
|
||||
|
|
@ -441,61 +407,61 @@ TEST( Rot3Q, RQ)
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST( Rot3Q, expmapStability ) {
|
||||
TEST( Rot3, expmapStability ) {
|
||||
Vector w = Vector_(3, 78e-9, 5e-8, 97e-7);
|
||||
double theta = w.norm();
|
||||
double theta2 = theta*theta;
|
||||
Rot3Q actualR = Rot3Q::Expmap(w);
|
||||
Rot3 actualR = Rot3::Expmap(w);
|
||||
Matrix W = Matrix_(3,3, 0.0, -w(2), w(1),
|
||||
w(2), 0.0, -w(0),
|
||||
-w(1), w(0), 0.0 );
|
||||
Matrix W2 = W*W;
|
||||
Matrix Rmat = eye(3) + (1.0-theta2/6.0 + theta2*theta2/120.0
|
||||
- theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ;
|
||||
Rot3Q expectedR( Rmat );
|
||||
Rot3 expectedR( Rmat );
|
||||
CHECK(assert_equal(expectedR, actualR, 1e-10));
|
||||
}
|
||||
|
||||
// Does not work with Quaternions
|
||||
///* ************************************************************************* */
|
||||
//TEST( Rot3Q, logmapStability ) {
|
||||
//TEST( Rot3, logmapStability ) {
|
||||
// Vector w = Vector_(3, 1e-8, 0.0, 0.0);
|
||||
// Rot3Q R = Rot3Q::Expmap(w);
|
||||
// Rot3 R = Rot3::Expmap(w);
|
||||
//// double tr = R.r1().x()+R.r2().y()+R.r3().z();
|
||||
//// std::cout.precision(5000);
|
||||
//// std::cout << "theta: " << w.norm() << std::endl;
|
||||
//// std::cout << "trace: " << tr << std::endl;
|
||||
//// R.print("R = ");
|
||||
// Vector actualw = Rot3Q::Logmap(R);
|
||||
// Vector actualw = Rot3::Logmap(R);
|
||||
// CHECK(assert_equal(w, actualw, 1e-15));
|
||||
//}
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3Q, quaternion) {
|
||||
TEST(Rot3, quaternion) {
|
||||
// NOTE: This is also verifying the ability to convert Vector to Quaternion
|
||||
Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782);
|
||||
Rot3Q R1 = Rot3Q(Matrix_(3,3,
|
||||
Rot3 R1 = Rot3(Matrix_(3,3,
|
||||
0.271018623057411, 0.278786459830371, 0.921318086098018,
|
||||
0.578529366719085, 0.717799701969298, -0.387385285854279,
|
||||
-0.769319620053772, 0.637998195662053, 0.033250932803219));
|
||||
|
||||
Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053);
|
||||
Rot3Q R2 = Rot3Q(Matrix_(3,3,
|
||||
Rot3 R2 = Rot3(Matrix_(3,3,
|
||||
-0.207341903877828, 0.250149415542075, 0.945745528564780,
|
||||
0.881304914479026, -0.371869043667957, 0.291573424846290,
|
||||
0.424630407073532, 0.893945571198514, -0.143353873763946));
|
||||
|
||||
// Check creating Rot3Q from quaternion
|
||||
EXPECT(assert_equal(R1, Rot3Q(q1)));
|
||||
EXPECT(assert_equal(R1, Rot3Q::quaternion(q1.w(), q1.x(), q1.y(), q1.z())));
|
||||
EXPECT(assert_equal(R2, Rot3Q(q2)));
|
||||
EXPECT(assert_equal(R2, Rot3Q::quaternion(q2.w(), q2.x(), q2.y(), q2.z())));
|
||||
// Check creating Rot3 from quaternion
|
||||
EXPECT(assert_equal(R1, Rot3(q1)));
|
||||
EXPECT(assert_equal(R1, Rot3::quaternion(q1.w(), q1.x(), q1.y(), q1.z())));
|
||||
EXPECT(assert_equal(R2, Rot3(q2)));
|
||||
EXPECT(assert_equal(R2, Rot3::quaternion(q2.w(), q2.x(), q2.y(), q2.z())));
|
||||
|
||||
// Check converting Rot3Q to quaterion
|
||||
// Check converting Rot3 to quaterion
|
||||
EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs())));
|
||||
EXPECT(assert_equal(Vector(R2.toQuaternion().coeffs()), Vector(q2.coeffs())));
|
||||
|
||||
// Check that quaternion and Rot3Q represent the same rotation
|
||||
// Check that quaternion and Rot3 represent the same rotation
|
||||
Point3 p1(1.0, 2.0, 3.0);
|
||||
Point3 p2(8.0, 7.0, 9.0);
|
||||
|
||||
|
|
@ -509,6 +475,8 @@ TEST(Rot3Q, quaternion) {
|
|||
EXPECT(assert_equal(expected2, actual2));
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
/* ************************************************************************* */
|
||||
int main() {
|
||||
TestResult tr;
|
||||
|
|
|
|||
|
|
@ -30,94 +30,28 @@
|
|||
|
||||
using namespace gtsam;
|
||||
|
||||
typedef TypedSymbol<Rot3Q, 'r'> KeyQ;
|
||||
typedef Values<KeyQ> ValuesQ;
|
||||
typedef PriorFactor<ValuesQ, KeyQ> PriorQ;
|
||||
typedef BetweenFactor<ValuesQ, KeyQ> BetweenQ;
|
||||
typedef NonlinearFactorGraph<ValuesQ> GraphQ;
|
||||
|
||||
typedef TypedSymbol<Rot3M, 'r'> KeyM;
|
||||
typedef Values<KeyM> ValuesM;
|
||||
typedef PriorFactor<ValuesM, KeyM> PriorM;
|
||||
typedef BetweenFactor<ValuesM, KeyM> BetweenM;
|
||||
typedef NonlinearFactorGraph<ValuesM> GraphM;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3, optimize1) {
|
||||
// Compare Rot3Q and Rot3M optimization
|
||||
|
||||
GraphQ fgQ;
|
||||
fgQ.add(PriorQ(0, Rot3Q(), sharedSigma(3, 0.01)));
|
||||
fgQ.add(BetweenQ(0, 1, Rot3Q::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
|
||||
fgQ.add(BetweenQ(1, 0, Rot3Q::Rz(5.0*M_PI/3.0), sharedSigma(3, 0.01)));
|
||||
|
||||
GraphM fgM;
|
||||
fgM.add(PriorM(0, Rot3M(), sharedSigma(3, 0.01)));
|
||||
fgM.add(BetweenM(0, 1, Rot3M::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
|
||||
fgM.add(BetweenM(1, 0, Rot3M::Rz(5.0*M_PI/3.0), sharedSigma(3, 0.01)));
|
||||
|
||||
ValuesQ initialQ;
|
||||
initialQ.insert(0, Rot3Q::Rz(0.0));
|
||||
initialQ.insert(1, Rot3Q::Rz(M_PI/3.0 + 0.1));
|
||||
|
||||
ValuesM initialM;
|
||||
initialM.insert(0, Rot3M::Rz(0.0));
|
||||
initialM.insert(1, Rot3M::Rz(M_PI/3.0 + 0.1));
|
||||
|
||||
ValuesQ truthQ;
|
||||
truthQ.insert(0, Rot3Q::Rz(0.0));
|
||||
truthQ.insert(1, Rot3Q::Rz(M_PI/3.0));
|
||||
|
||||
ValuesM truthM;
|
||||
truthM.insert(0, Rot3M::Rz(0.0));
|
||||
truthM.insert(1, Rot3M::Rz(M_PI/3.0));
|
||||
|
||||
// Compare Matrix and Quaternion between
|
||||
Matrix H1M, H2M;
|
||||
Rot3M betwM = initialM[1].between(initialM[0], H1M, H2M);
|
||||
Matrix H1Q, H2Q;
|
||||
Rot3Q betwQ = initialM[1].between(initialM[0], H1Q, H2Q);
|
||||
EXPECT(assert_equal(betwM.matrix(), betwQ.matrix()));
|
||||
EXPECT(assert_equal(H1M, H1Q));
|
||||
EXPECT(assert_equal(H2M, H2Q));
|
||||
Point3 x1(1.0,0.0,0.0), x2(0.0,1.0,0.0);
|
||||
EXPECT(assert_equal(betwM*x1, betwQ*x1));
|
||||
EXPECT(assert_equal(betwM*x2, betwQ*x2));
|
||||
|
||||
// Compare Matrix and Quaternion logmap
|
||||
Vector logM = initialM[1].localCoordinates(initialM[0]);
|
||||
Vector logQ = initialQ[1].localCoordinates(initialQ[0]);
|
||||
EXPECT(assert_equal(logM, logQ));
|
||||
|
||||
// Compare Matrix and Quaternion linear graph
|
||||
Ordering ordering; ordering += KeyQ(0), KeyQ(1);
|
||||
GaussianFactorGraph gfgQ(*fgQ.linearize(initialQ, ordering));
|
||||
GaussianFactorGraph gfgM(*fgM.linearize(initialM, ordering));
|
||||
EXPECT(assert_equal(gfgQ, gfgM, 1e-5));
|
||||
|
||||
NonlinearOptimizationParameters params;
|
||||
//params.verbosity_ = NonlinearOptimizationParameters::TRYLAMBDA;
|
||||
ValuesQ final = optimize(fgQ, initialQ, params);
|
||||
|
||||
EXPECT(assert_equal(truthQ, final, 1e-5));
|
||||
}
|
||||
typedef TypedSymbol<Rot3, 'r'> Key;
|
||||
typedef Values<Key> Rot3Values;
|
||||
typedef PriorFactor<Rot3Values, Key> Prior;
|
||||
typedef BetweenFactor<Rot3Values, Key> Between;
|
||||
typedef NonlinearFactorGraph<Rot3Values> Graph;
|
||||
|
||||
/* ************************************************************************* */
|
||||
TEST(Rot3, optimize) {
|
||||
|
||||
// Optimize a circle
|
||||
ValuesQ truth;
|
||||
ValuesQ initial;
|
||||
GraphQ fg;
|
||||
fg.add(PriorQ(0, Rot3Q(), sharedSigma(3, 0.01)));
|
||||
Rot3Values truth;
|
||||
Rot3Values initial;
|
||||
Graph fg;
|
||||
fg.add(Prior(0, Rot3(), sharedSigma(3, 0.01)));
|
||||
for(int j=0; j<6; ++j) {
|
||||
truth.insert(j, Rot3Q::Rz(M_PI/3.0 * double(j)));
|
||||
initial.insert(j, Rot3Q::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
|
||||
fg.add(BetweenQ(j, (j+1)%6, Rot3Q::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
|
||||
truth.insert(j, Rot3::Rz(M_PI/3.0 * double(j)));
|
||||
initial.insert(j, Rot3::Rz(M_PI/3.0 * double(j) + 0.1 * double(j%2)));
|
||||
fg.add(Between(j, (j+1)%6, Rot3::Rz(M_PI/3.0), sharedSigma(3, 0.01)));
|
||||
}
|
||||
|
||||
NonlinearOptimizationParameters params;
|
||||
ValuesQ final = optimize(fg, initial, params);
|
||||
Rot3Values final = optimize(fg, initial, params);
|
||||
|
||||
EXPECT(assert_equal(truth, final, 1e-5));
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue