converted Rot3M to fixed-size Matrix, and changed some methods elsewhere to return fixed-size Vector3 to avoid heap allocations for speedup.
Chris Beall
parent
2365ade34c
commit
1623b8ce12
|
|
@ -37,6 +37,8 @@ namespace gtsam {
|
|||
typedef Eigen::MatrixXd Matrix;
|
||||
typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic, Eigen::RowMajor> MatrixRowMajor;
|
||||
|
||||
typedef Eigen::Matrix3d Matrix3;
|
||||
|
||||
// Matrix expressions for accessing parts of matrices
|
||||
typedef Eigen::Block<Matrix> SubMatrix;
|
||||
typedef Eigen::Block<const Matrix> ConstSubMatrix;
|
||||
|
|
|
|||
|
|
@ -33,6 +33,10 @@ namespace gtsam {
|
|||
// Typedef arbitary length vector
|
||||
typedef Eigen::VectorXd Vector;
|
||||
|
||||
// Commonly used fixed size vectors
|
||||
typedef Eigen::Vector3d Vector3;
|
||||
typedef Eigen::Matrix<double, 6, 1> Vector6;
|
||||
|
||||
typedef Eigen::VectorBlock<Vector> SubVector;
|
||||
typedef Eigen::VectorBlock<const Vector> ConstSubVector;
|
||||
|
||||
|
|
|
|||
|
|
@ -126,7 +126,7 @@ namespace gtsam {
|
|||
inline Point3 retract(const Vector& v) const { return Point3(*this + v); }
|
||||
|
||||
/// Returns inverse retraction
|
||||
inline Vector localCoordinates(const Point3& q) const { return (q -*this).vector(); }
|
||||
inline Vector3 localCoordinates(const Point3& q) const { return (q -*this).vector(); }
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
|
|
@ -136,7 +136,7 @@ namespace gtsam {
|
|||
static inline Point3 Expmap(const Vector& v) { return Point3(v); }
|
||||
|
||||
/** Log map at identity - return the x,y,z of this point */
|
||||
static inline Vector Logmap(const Point3& dp) { return Vector_(3, dp.x(), dp.y(), dp.z()); }
|
||||
static inline Vector3 Logmap(const Point3& dp) { return Vector3(dp.x(), dp.y(), dp.z()); }
|
||||
|
||||
/// @}
|
||||
/// @name Vector Space
|
||||
|
|
@ -170,9 +170,8 @@ namespace gtsam {
|
|||
bool operator ==(const Point3& q) const;
|
||||
|
||||
/** return vectorized form (column-wise)*/
|
||||
Vector vector() const {
|
||||
Vector v(3); v(0)=x_; v(1)=y_; v(2)=z_;
|
||||
return v;
|
||||
Vector3 vector() const {
|
||||
return Vector3(x_,y_,z_);
|
||||
}
|
||||
|
||||
/// get x
|
||||
|
|
|
|||
|
|
@ -136,7 +136,7 @@ namespace gtsam {
|
|||
return Logmap(between(T));
|
||||
} else if(mode == Pose3::FIRST_ORDER) {
|
||||
// R is always done exactly in all three retract versions below
|
||||
Vector omega = R_.localCoordinates(T.rotation());
|
||||
Vector3 omega = R_.localCoordinates(T.rotation());
|
||||
|
||||
// Incorrect version
|
||||
// Independently computes the logmap of the translation and rotation
|
||||
|
|
|
|||
|
|
@ -33,6 +33,7 @@
|
|||
#endif
|
||||
|
||||
#include <gtsam/base/DerivedValue.h>
|
||||
#include <gtsam/base/Matrix.h>
|
||||
#include <gtsam/geometry/Point3.h>
|
||||
#include <gtsam/3rdparty/Eigen/Eigen/Geometry>
|
||||
|
||||
|
|
@ -59,8 +60,7 @@ namespace gtsam {
|
|||
/** Internal Eigen Quaternion */
|
||||
Quaternion quaternion_;
|
||||
#else
|
||||
/** We store columns ! */
|
||||
Point3 r1_, r2_, r3_;
|
||||
Matrix3 rot_;
|
||||
#endif
|
||||
|
||||
public:
|
||||
|
|
@ -87,6 +87,9 @@ namespace gtsam {
|
|||
/** constructor from a rotation matrix */
|
||||
Rot3(const Matrix& R);
|
||||
|
||||
// /** constructor from a fixed size rotation matrix */
|
||||
// Rot3(const Matrix3& R);
|
||||
|
||||
/** Constructor from a quaternion. This can also be called using a plain
|
||||
* Vector, due to implicit conversion from Vector to Quaternion
|
||||
* @param q The quaternion
|
||||
|
|
@ -183,7 +186,7 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/** print */
|
||||
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
|
||||
void print(const std::string& s="R") const { gtsam::print((Matrix)matrix(), s);}
|
||||
|
||||
/** equals with an tolerance */
|
||||
bool equals(const Rot3& p, double tol = 1e-9) const;
|
||||
|
|
@ -250,7 +253,7 @@ namespace gtsam {
|
|||
Rot3 retract(const Vector& omega, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
|
||||
|
||||
/// Returns local retract coordinates in neighborhood around current rotation
|
||||
Vector localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
|
||||
Vector3 localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode = ROT3_DEFAULT_COORDINATES_MODE) const;
|
||||
|
||||
/// @}
|
||||
/// @name Lie Group
|
||||
|
|
@ -268,7 +271,7 @@ namespace gtsam {
|
|||
/**
|
||||
* Log map at identity - return the canonical coordinates of this rotation
|
||||
*/
|
||||
static Vector Logmap(const Rot3& R);
|
||||
static Vector3 Logmap(const Rot3& R);
|
||||
|
||||
/// @}
|
||||
/// @name Group Action on Point3
|
||||
|
|
@ -294,10 +297,10 @@ namespace gtsam {
|
|||
/// @{
|
||||
|
||||
/** return 3*3 rotation matrix */
|
||||
Matrix matrix() const;
|
||||
Matrix3 matrix() const;
|
||||
|
||||
/** return 3*3 transpose (inverse) rotation matrix */
|
||||
Matrix transpose() const;
|
||||
Matrix3 transpose() const;
|
||||
|
||||
/** returns column vector specified by index */
|
||||
Point3 column(int index) const;
|
||||
|
|
@ -309,19 +312,19 @@ namespace gtsam {
|
|||
* Use RQ to calculate xyz angle representation
|
||||
* @return a vector containing x,y,z s.t. R = Rot3::RzRyRx(x,y,z)
|
||||
*/
|
||||
Vector xyz() const;
|
||||
Vector3 xyz() const;
|
||||
|
||||
/**
|
||||
* Use RQ to calculate yaw-pitch-roll angle representation
|
||||
* @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
|
||||
*/
|
||||
Vector ypr() const;
|
||||
Vector3 ypr() const;
|
||||
|
||||
/**
|
||||
* Use RQ to calculate roll-pitch-yaw angle representation
|
||||
* @return a vector containing ypr s.t. R = Rot3::ypr(y,p,r)
|
||||
*/
|
||||
Vector rpy() const;
|
||||
Vector3 rpy() const;
|
||||
|
||||
/**
|
||||
* Accessor to get to component of angle representations
|
||||
|
|
@ -365,9 +368,18 @@ namespace gtsam {
|
|||
ar & boost::serialization::make_nvp("Rot3",
|
||||
boost::serialization::base_object<Value>(*this));
|
||||
#ifndef GTSAM_DEFAULT_QUATERNIONS
|
||||
ar & BOOST_SERIALIZATION_NVP(r1_);
|
||||
ar & BOOST_SERIALIZATION_NVP(r2_);
|
||||
ar & BOOST_SERIALIZATION_NVP(r3_);
|
||||
ar & boost::serialization::make_nvp("rot11", rot_(0,0));
|
||||
ar & boost::serialization::make_nvp("rot12", rot_(0,1));
|
||||
ar & boost::serialization::make_nvp("rot13", rot_(0,2));
|
||||
ar & boost::serialization::make_nvp("rot21", rot_(1,0));
|
||||
ar & boost::serialization::make_nvp("rot22", rot_(1,1));
|
||||
ar & boost::serialization::make_nvp("rot23", rot_(1,2));
|
||||
ar & boost::serialization::make_nvp("rot31", rot_(2,0));
|
||||
ar & boost::serialization::make_nvp("rot32", rot_(2,1));
|
||||
ar & boost::serialization::make_nvp("rot33", rot_(2,2));
|
||||
// ar & BOOST_SERIALIZATION_NVP(r1_);
|
||||
// ar & BOOST_SERIALIZATION_NVP(r2_);
|
||||
// ar & BOOST_SERIALIZATION_NVP(r3_);
|
||||
#else
|
||||
ar & boost::serialization::make_nvp("w", quaternion_.w());
|
||||
ar & boost::serialization::make_nvp("x", quaternion_.x());
|
||||
|
|
@ -389,5 +401,5 @@ namespace gtsam {
|
|||
* @return an upper triangular matrix R
|
||||
* @return a vector [thetax, thetay, thetaz] in radians.
|
||||
*/
|
||||
std::pair<Matrix,Vector> RQ(const Matrix& A);
|
||||
std::pair<Matrix3,Vector3> RQ(const Matrix& A);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -28,42 +28,39 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
static const Matrix I3 = eye(3);
|
||||
static const Matrix3 I3 = Matrix3::Identity();
|
||||
|
||||
/* ************************************************************************* */
|
||||
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)) {}
|
||||
Rot3::Rot3() : rot_(Matrix3::Identity()) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
|
||||
r1_(r1), r2_(r2), r3_(r3) {}
|
||||
Rot3::Rot3(const Point3& r1, const Point3& r2, const Point3& r3) {
|
||||
rot_.col(0) = r1.vector();
|
||||
rot_.col(1) = r2.vector();
|
||||
rot_.col(2) = r3.vector();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
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)),
|
||||
r2_(Point3(R12, R22, R32)),
|
||||
r3_(Point3(R13, R23, R33)) {}
|
||||
double R31, double R32, double R33) {
|
||||
rot_ << R11, R12, R13,
|
||||
R21, R22, R23,
|
||||
R31, R32, R33;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3::Rot3(const Matrix& R) {
|
||||
if (R.rows()!=3 || R.cols()!=3)
|
||||
throw invalid_argument("Rot3 constructor expects 3*3 matrix");
|
||||
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));
|
||||
rot_ = R;
|
||||
}
|
||||
|
||||
///* ************************************************************************* */
|
||||
//Rot3::Rot3(const Matrix3& R) : rot_(R) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3::Rot3(const Quaternion& q) {
|
||||
Eigen::Matrix3d R = q.toRotationMatrix();
|
||||
r1_ = Point3(R.col(0));
|
||||
r2_ = Point3(R.col(1));
|
||||
r3_ = Point3(R.col(2));
|
||||
}
|
||||
Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::Rx(double t) {
|
||||
|
|
@ -164,46 +161,39 @@ Point3 Rot3::operator*(const Point3& p) const { return rotate(p); }
|
|||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
|
||||
if (H1) *H1 = -matrix();
|
||||
return Rot3(
|
||||
r1_.x(), r1_.y(), r1_.z(),
|
||||
r2_.x(), r2_.y(), r2_.z(),
|
||||
r3_.x(), r3_.y(), r3_.z());
|
||||
if (H1) *H1 = -rot_;
|
||||
return Rot3(rot_.transpose());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::between (const Rot3& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = -(R2.transpose()*matrix());
|
||||
if (H1) *H1 = -(R2.transpose()*rot_);
|
||||
if (H2) *H2 = I3;
|
||||
return between_default(*this, R2);
|
||||
return Rot3(rot_.transpose()*R2.rot_);
|
||||
//return between_default(*this, R2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3 Rot3::operator*(const Rot3& R2) const {
|
||||
return Rot3(rotate(R2.r1_), rotate(R2.r2_), rotate(R2.r3_));
|
||||
return Rot3(rot_*R2.rot_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::rotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1 || H2) {
|
||||
const Matrix R(matrix());
|
||||
if (H1) *H1 = R * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
if (H2) *H2 = R;
|
||||
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
if (H2) *H2 = rot_;
|
||||
}
|
||||
return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();
|
||||
return Point3(rot_ * p.vector());
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SO(3) section
|
||||
Point3 Rot3::unrotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
Point3 q(
|
||||
r1_.x()*p.x()+r1_.y()*p.y()+r1_.z()*p.z(),
|
||||
r2_.x()*p.x()+r2_.y()*p.y()+r2_.z()*p.z(),
|
||||
r3_.x()*p.x()+r3_.y()*p.y()+r3_.z()*p.z()
|
||||
); // q = Rt*p
|
||||
Point3 q(rot_.transpose()*p.vector()); // q = Rt*p
|
||||
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
|
||||
if (H2) *H2 = transpose();
|
||||
return q;
|
||||
|
|
@ -211,25 +201,26 @@ Point3 Rot3::unrotate(const Point3& p,
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
Vector Rot3::Logmap(const Rot3& R) {
|
||||
Vector3 Rot3::Logmap(const Rot3& R) {
|
||||
|
||||
static const double PI = boost::math::constants::pi<double>();
|
||||
|
||||
const Matrix3& rot = R.rot_;
|
||||
// Get trace(R)
|
||||
double tr = R.r1_.x()+R.r2_.y()+R.r3_.z();
|
||||
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(R.r3_.z()+1.0) > 1e-10)
|
||||
return (PI / sqrt(2.0+2.0*R.r3_.z())) *
|
||||
Vector_(3, R.r3_.x(), R.r3_.y(), 1.0+R.r3_.z());
|
||||
else if(std::abs(R.r2_.y()+1.0) > 1e-10)
|
||||
return (PI / sqrt(2.0+2.0*R.r2_.y())) *
|
||||
Vector_(3, R.r2_.x(), 1.0+R.r2_.y(), R.r2_.z());
|
||||
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*R.r1_.x())) *
|
||||
Vector_(3, 1.0+R.r1_.x(), R.r1_.y(), R.r1_.z());
|
||||
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
|
||||
|
|
@ -241,10 +232,10 @@ Vector Rot3::Logmap(const Rot3& R) {
|
|||
// 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*Vector_(3,
|
||||
R.r2_.z()-R.r3_.y(),
|
||||
R.r3_.x()-R.r1_.z(),
|
||||
R.r1_.y()-R.r2_.x());
|
||||
return magnitude*Vector3(
|
||||
rot(2,1)-rot(1,2),
|
||||
rot(0,2)-rot(2,0),
|
||||
rot(1,0)-rot(0,1));
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -276,7 +267,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
|
||||
Vector3 Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
|
||||
if(mode == Rot3::EXPMAP) {
|
||||
return Logmap(between(T));
|
||||
} else if(mode == Rot3::CAYLEY) {
|
||||
|
|
@ -292,13 +283,13 @@ Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
|
|||
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 * Vector_(3, x, y, z);
|
||||
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
|
||||
Matrix Omega = Cayley<3>(A);
|
||||
return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0));
|
||||
return -2*Vector3(Omega(2,1),Omega(0,2),Omega(1,0));
|
||||
} else {
|
||||
assert(false);
|
||||
exit(1);
|
||||
|
|
@ -306,89 +297,69 @@ Vector Rot3::localCoordinates(const Rot3& T, Rot3::CoordinatesMode mode) const {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3::matrix() const {
|
||||
Matrix R(3,3);
|
||||
R <<
|
||||
r1_.x(), r2_.x(), r3_.x(),
|
||||
r1_.y(), r2_.y(), r3_.y(),
|
||||
r1_.z(), r2_.z(), r3_.z();
|
||||
return R;
|
||||
Matrix3 Rot3::matrix() const {
|
||||
return rot_;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3::transpose() const {
|
||||
Matrix Rt(3,3);
|
||||
Rt <<
|
||||
r1_.x(), r1_.y(), r1_.z(),
|
||||
r2_.x(), r2_.y(), r2_.z(),
|
||||
r3_.x(), r3_.y(), r3_.z();
|
||||
return Rt;
|
||||
Matrix3 Rot3::transpose() const {
|
||||
return rot_.transpose();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::column(int index) const{
|
||||
if(index == 3)
|
||||
return r3_;
|
||||
else if(index == 2)
|
||||
return r2_;
|
||||
else if(index == 1)
|
||||
return r1_; // default returns r1
|
||||
else
|
||||
throw invalid_argument("Argument to Rot3::column must be 1, 2, or 3");
|
||||
return Point3(rot_.col(index));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::r1() const { return r1_; }
|
||||
Point3 Rot3::r1() const { return Point3(rot_.col(0)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::r2() const { return r2_; }
|
||||
Point3 Rot3::r2() const { return Point3(rot_.col(1)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::r3() const { return r3_; }
|
||||
Point3 Rot3::r3() const { return Point3(rot_.col(2)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3::xyz() const {
|
||||
Matrix I;Vector q;
|
||||
Vector3 Rot3::xyz() const {
|
||||
Matrix I;Vector3 q;
|
||||
boost::tie(I,q)=RQ(matrix());
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3::ypr() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(2),q(1),q(0));
|
||||
Vector3 Rot3::ypr() const {
|
||||
Vector3 q = xyz();
|
||||
return Vector3(q(2),q(1),q(0));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3::rpy() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(0),q(1),q(2));
|
||||
Vector3 Rot3::rpy() const {
|
||||
Vector3 q = xyz();
|
||||
return Vector3(q(0),q(1),q(2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Quaternion Rot3::toQuaternion() const {
|
||||
return Quaternion((Eigen::Matrix3d() <<
|
||||
r1_.x(), r2_.x(), r3_.x(),
|
||||
r1_.y(), r2_.y(), r3_.y(),
|
||||
r1_.z(), r2_.z(), r3_.z()).finished());
|
||||
return Quaternion(rot_);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix, Vector> RQ(const Matrix& A) {
|
||||
pair<Matrix3, Vector3> RQ(const Matrix& A) {
|
||||
|
||||
double x = -atan2(-A(2, 1), A(2, 2));
|
||||
Rot3 Qx = Rot3::Rx(-x);
|
||||
Matrix B = A * Qx.matrix();
|
||||
Matrix3 B = A * Qx.matrix();
|
||||
|
||||
double y = -atan2(B(2, 0), B(2, 2));
|
||||
Rot3 Qy = Rot3::Ry(-y);
|
||||
Matrix C = B * Qy.matrix();
|
||||
Matrix3 C = B * Qy.matrix();
|
||||
|
||||
double z = -atan2(-C(1, 0), C(1, 1));
|
||||
Rot3 Qz = Rot3::Rz(-z);
|
||||
Matrix R = C * Qz.matrix();
|
||||
Matrix3 R = C * Qz.matrix();
|
||||
|
||||
Vector xyz = Vector_(3, x, y, z);
|
||||
Vector xyz = Vector3(x, y, z);
|
||||
return make_pair(R, xyz);
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -49,6 +49,10 @@ namespace gtsam {
|
|||
Rot3::Rot3(const Matrix& R) :
|
||||
quaternion_(Eigen::Matrix3d(R)) {}
|
||||
|
||||
// /* ************************************************************************* */
|
||||
// Rot3::Rot3(const Matrix3& R) :
|
||||
// quaternion_(R) {}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3::Rot3(const Quaternion& q) : quaternion_(q) {}
|
||||
|
||||
|
|
@ -140,7 +144,7 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
Vector Rot3::Logmap(const Rot3& R) {
|
||||
Vector3 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
|
||||
|
|
@ -155,15 +159,15 @@ namespace gtsam {
|
|||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const {
|
||||
Vector3 Rot3::localCoordinates(const Rot3& t2, Rot3::CoordinatesMode mode) const {
|
||||
return Logmap(between(t2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3::matrix() const { return quaternion_.toRotationMatrix(); }
|
||||
Matrix3 Rot3::matrix() const { return quaternion_.toRotationMatrix(); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3::transpose() const { return quaternion_.toRotationMatrix().transpose(); }
|
||||
Matrix3 Rot3::transpose() const { return quaternion_.toRotationMatrix().transpose(); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3::column(int index) const{
|
||||
|
|
@ -187,29 +191,29 @@ namespace gtsam {
|
|||
Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); }
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3::xyz() const {
|
||||
Matrix I;Vector q;
|
||||
Vector3 Rot3::xyz() const {
|
||||
Matrix I;Vector3 q;
|
||||
boost::tie(I,q)=RQ(matrix());
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3::ypr() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(2),q(1),q(0));
|
||||
Vector3 Rot3::ypr() const {
|
||||
Vector3 q = xyz();
|
||||
return Vector3(q(2),q(1),q(0));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3::rpy() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(0),q(1),q(2));
|
||||
Vector3 Rot3::rpy() const {
|
||||
Vector3 q = xyz();
|
||||
return Vector3(q(0),q(1),q(2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Quaternion Rot3::toQuaternion() const { return quaternion_; }
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix, Vector> RQ(const Matrix& A) {
|
||||
pair<Matrix, Vector3> RQ(const Matrix& A) {
|
||||
|
||||
double x = -atan2(-A(2, 1), A(2, 2));
|
||||
Rot3 Qx = Rot3::Rx(-x);
|
||||
|
|
@ -223,7 +227,7 @@ namespace gtsam {
|
|||
Rot3 Qz = Rot3::Rz(-z);
|
||||
Matrix R = C * Qz.matrix();
|
||||
|
||||
Vector xyz = Vector_(3, x, y, z);
|
||||
Vector xyz = Vector3(x, y, z);
|
||||
return make_pair(R, xyz);
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue