209 lines
6.1 KiB
C++
209 lines
6.1 KiB
C++
/**
|
|
* @file Rot3.h
|
|
* @brief Rotation
|
|
* @author Alireza Fathi
|
|
* @author Christian Potthast
|
|
* @author Frank Dellaert
|
|
*/
|
|
|
|
// \callgraph
|
|
|
|
#pragma once
|
|
|
|
#include "Point3.h"
|
|
#include "Testable.h"
|
|
#include "Lie.h"
|
|
|
|
namespace gtsam {
|
|
|
|
/* 3D Rotation */
|
|
class Rot3: Testable<Rot3> {
|
|
private:
|
|
/** we store columns ! */
|
|
Point3 r1_, r2_, r3_;
|
|
|
|
public:
|
|
|
|
/** default constructor, unit rotation */
|
|
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)) {}
|
|
|
|
/** constructor from columns */
|
|
Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
|
|
r1_(r1), r2_(r2), r3_(r3) {}
|
|
|
|
/** constructor from vector */
|
|
Rot3(const Vector &v) :
|
|
r1_(Point3(v(0),v(1),v(2))),
|
|
r2_(Point3(v(3),v(4),v(5))),
|
|
r3_(Point3(v(6),v(7),v(8))) {}
|
|
|
|
/** constructor from doubles in *row* order !!! */
|
|
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)) {}
|
|
|
|
/** constructor from matrix */
|
|
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))) {}
|
|
|
|
/** print */
|
|
void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
|
|
|
|
/** equals with an tolerance */
|
|
bool equals(const Rot3& p, double tol = 1e-9) const;
|
|
|
|
/** return 3*3 rotation matrix */
|
|
Matrix matrix() const;
|
|
|
|
/** return 3*3 transpose (inverse) rotation matrix */
|
|
Matrix transpose() const;
|
|
|
|
/** returns column vector specified by index */
|
|
Point3 column(int index) const;
|
|
Point3 r1() const { return r1_; }
|
|
Point3 r2() const { return r2_; }
|
|
Point3 r3() const { return r3_; }
|
|
|
|
/** use RQ to calculate yaw-pitch-roll angle representation */
|
|
Vector ypr() const;
|
|
|
|
private:
|
|
/** Serialization function */
|
|
friend class boost::serialization::access;
|
|
template<class Archive>
|
|
void serialize(Archive & ar, const unsigned int version)
|
|
{
|
|
ar & BOOST_SERIALIZATION_NVP(r1_);
|
|
ar & BOOST_SERIALIZATION_NVP(r2_);
|
|
ar & BOOST_SERIALIZATION_NVP(r3_);
|
|
}
|
|
};
|
|
|
|
|
|
/**
|
|
* Rodriguez' formula to compute an incremental rotation matrix
|
|
* @param w is the rotation axis, unit length
|
|
* @param theta rotation angle
|
|
* @return incremental rotation matrix
|
|
*/
|
|
Rot3 rodriguez(const Vector& w, double theta);
|
|
|
|
/**
|
|
* Rodriguez' formula to compute an incremental rotation matrix
|
|
* @param v a vector of incremental roll,pitch,yaw
|
|
* @return incremental rotation matrix
|
|
*/
|
|
Rot3 rodriguez(const Vector& v);
|
|
|
|
/**
|
|
* Rodriguez' formula to compute an incremental rotation matrix
|
|
* @param wx
|
|
* @param wy
|
|
* @param wz
|
|
* @return incremental rotation matrix
|
|
*/
|
|
inline Rot3 rodriguez(double wx, double wy, double wz) { return rodriguez(Vector_(3,wx,wy,wz));}
|
|
|
|
/** return DOF, dimensionality of tangent space */
|
|
inline size_t dim(const Rot3&) { return 3; }
|
|
|
|
// Exponential map at identity - create a rotation from canonical coordinates
|
|
// using Rodriguez' formula
|
|
template<> inline Rot3 expmap(const Vector& v) {
|
|
if(zero(v)) return Rot3();
|
|
else return rodriguez(v);
|
|
}
|
|
|
|
// Log map at identity - return the canonical coordinates of this rotation
|
|
inline Vector logmap(const Rot3& R) {
|
|
double tr = R.r1().x()+R.r2().y()+R.r3().z();
|
|
if (tr==3.0) return ones(3); // todo: identity?
|
|
if (tr==-1.0) throw std::domain_error("Rot3::log: trace == -1 not yet handled :-(");;
|
|
double theta = acos((tr-1.0)/2.0);
|
|
return (theta/2.0/sin(theta))*Vector_(3,
|
|
R.r2().z()-R.r3().y(),
|
|
R.r3().x()-R.r1().z(),
|
|
R.r1().y()-R.r2().x());
|
|
}
|
|
|
|
// Compose two rotations
|
|
inline Rot3 compose(const Rot3& R1, const Rot3& R2) {
|
|
return Rot3(R1.matrix() * R2.matrix()); }
|
|
|
|
// Find the inverse rotation R^T s.t. inverse(R)*R = Rot3()
|
|
inline Rot3 inverse(const Rot3& R) {
|
|
return Rot3(
|
|
R.r1().x(), R.r1().y(), R.r1().z(),
|
|
R.r2().x(), R.r2().y(), R.r2().z(),
|
|
R.r3().x(), R.r3().y(), R.r3().z());
|
|
}
|
|
|
|
/**
|
|
|
|
* Update Rotation with incremental rotation
|
|
* @param v a vector of incremental roll,pitch,yaw
|
|
* @param R a rotated frame
|
|
* @return incremental rotation matrix
|
|
*/
|
|
//Rot3 exp(const Rot3& R, const Vector& v);
|
|
|
|
/**
|
|
* @param a rotation R
|
|
* @param a rotation S
|
|
* @return log(S*R'), i.e. canonical coordinates of between(R,S)
|
|
*/
|
|
//Vector log(const Rot3& R, const Rot3& S);
|
|
|
|
/**
|
|
* rotate point from rotated coordinate frame to
|
|
* world = R*p
|
|
*/
|
|
Point3 rotate(const Rot3& R, const Point3& p);
|
|
inline Point3 operator*(const Rot3& R, const Point3& p) { return rotate(R,p); }
|
|
Matrix Drotate1(const Rot3& R, const Point3& p);
|
|
Matrix Drotate2(const Rot3& R); // does not depend on p !
|
|
|
|
/**
|
|
* rotate point from world to rotated
|
|
* frame = R'*p
|
|
*/
|
|
Point3 unrotate(const Rot3& R, const Point3& p);
|
|
Matrix Dunrotate1(const Rot3& R, const Point3& p);
|
|
Matrix Dunrotate2(const Rot3& R); // does not depend on p !
|
|
|
|
/**
|
|
* compose two rotations i.e., R=R1*R2
|
|
*/
|
|
//Rot3 compose (const Rot3& R1, const Rot3& R2);
|
|
Matrix Dcompose1(const Rot3& R1, const Rot3& R2);
|
|
Matrix Dcompose2(const Rot3& R1, const Rot3& R2);
|
|
|
|
/**
|
|
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
|
|
*/
|
|
//Rot3 between (const Rot3& R1, const Rot3& R2);
|
|
Matrix Dbetween1(const Rot3& R1, const Rot3& R2);
|
|
Matrix Dbetween2(const Rot3& R1, const Rot3& R2);
|
|
|
|
/**
|
|
* [RQ] receives a 3 by 3 matrix and returns an upper triangular matrix R
|
|
* and 3 rotation angles corresponding to the rotation matrix Q=Qz'*Qy'*Qx'
|
|
* such that A = R*Q = R*Qz'*Qy'*Qx'. When A is a rotation matrix, R will
|
|
* be the identity and Q is a yaw-pitch-roll decomposition of A.
|
|
* The implementation uses Givens rotations and is based on Hartley-Zisserman.
|
|
* @param a 3 by 3 matrix A=RQ
|
|
* @return an upper triangular matrix R
|
|
* @return a vector [thetax, thetay, thetaz] in radians.
|
|
*/
|
|
std::pair<Matrix,Vector> RQ(const Matrix& A);
|
|
|
|
}
|