Formatting and comments, adding Rot3 and Pose3 to matlab interface
Alex Cunningham
parent
ca121c2872
commit
a7ea0f4e04
25
gtsam.h
25
gtsam.h
|
|
@ -11,6 +11,7 @@ class Point3 {
|
|||
Point3(double x, double y, double z);
|
||||
Point3(Vector v);
|
||||
void print(string s) const;
|
||||
bool equals(const Point3& p, double tol);
|
||||
Vector vector() const;
|
||||
double x();
|
||||
double y();
|
||||
|
|
@ -21,11 +22,18 @@ class Rot2 {
|
|||
Rot2();
|
||||
Rot2(double theta);
|
||||
void print(string s) const;
|
||||
bool equals(const Rot2& pose, double tol) const;
|
||||
bool equals(const Rot2& rot, double tol) const;
|
||||
double c() const;
|
||||
double s() const;
|
||||
};
|
||||
|
||||
class Rot3 {
|
||||
Rot3();
|
||||
Rot3(Matrix R);
|
||||
void print(string s) const;
|
||||
bool equals(const Rot3& rot, double tol) const;
|
||||
};
|
||||
|
||||
class Pose2 {
|
||||
Pose2();
|
||||
Pose2(double x, double y, double theta);
|
||||
|
|
@ -44,6 +52,21 @@ class Pose2 {
|
|||
Pose2* retract_(Vector v);
|
||||
};
|
||||
|
||||
class Pose3 {
|
||||
Pose3();
|
||||
Pose3(const Rot3& r, const Point3& t);
|
||||
Pose3(Vector v);
|
||||
void print(string s) const;
|
||||
bool equals(const Pose3& pose, double tol) const;
|
||||
double x() const;
|
||||
double y() const;
|
||||
double z() const;
|
||||
int dim() const;
|
||||
Pose3* compose_(const Pose3& p2);
|
||||
Pose3* between_(const Pose3& p2);
|
||||
Vector localCoordinates(const Pose3& p);
|
||||
};
|
||||
|
||||
class SharedGaussian {
|
||||
SharedGaussian(Matrix covariance);
|
||||
void print(string s) const;
|
||||
|
|
|
|||
|
|
@ -214,22 +214,23 @@ namespace gtsam {
|
|||
|
||||
/* ************************************************************************* */
|
||||
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
|
||||
if (H1)
|
||||
if (H1)
|
||||
#ifdef CORRECT_POSE3_EXMAP
|
||||
{ *H1 = - AdjointMap(p); } // FIXME: this function doesn't exist with this interface - should this be "*H1 = -AdjointMap();" ?
|
||||
// FIXME: this function doesn't exist with this interface - should this be "*H1 = -AdjointMap();" ?
|
||||
{ *H1 = - AdjointMap(p); }
|
||||
#else
|
||||
{
|
||||
Matrix Rt = R_.transpose();
|
||||
Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3;
|
||||
Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt;
|
||||
Matrix DR = collect(2, &DR_R1, &DR_t1);
|
||||
Matrix Dt = collect(2, &Dt_R1, &Dt_t1);
|
||||
*H1 = gtsam::stack(2, &DR, &Dt);
|
||||
}
|
||||
{
|
||||
Matrix Rt = R_.transpose();
|
||||
Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3;
|
||||
Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt;
|
||||
Matrix DR = collect(2, &DR_R1, &DR_t1);
|
||||
Matrix Dt = collect(2, &Dt_R1, &Dt_t1);
|
||||
*H1 = gtsam::stack(2, &DR, &Dt);
|
||||
}
|
||||
#endif
|
||||
Rot3 Rt = R_.inverse();
|
||||
return Pose3(Rt, Rt*(-t_));
|
||||
}
|
||||
Rot3 Rt = R_.inverse();
|
||||
return Pose3(Rt, Rt*(-t_));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// between = compose(p2,inverse(p1));
|
||||
|
|
|
|||
|
|
@ -89,6 +89,11 @@ namespace gtsam {
|
|||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const;
|
||||
|
||||
/// MATLAB version returns shared pointer
|
||||
boost::shared_ptr<Pose3> compose_(const Pose3& p2) {
|
||||
return boost::shared_ptr<Pose3>(new Pose3(compose(p2)));
|
||||
}
|
||||
|
||||
/// compose syntactic sugar
|
||||
Pose3 operator*(const Pose3& T) const {
|
||||
return Pose3(R_*T.R_, t_ + R_*T.t_);
|
||||
|
|
@ -144,12 +149,17 @@ namespace gtsam {
|
|||
boost::optional<Matrix&> H1=boost::none,
|
||||
boost::optional<Matrix&> H2=boost::none) const;
|
||||
|
||||
/// MATLAB version returns shared pointer
|
||||
boost::shared_ptr<Pose3> between_(const Pose3& p2) {
|
||||
return boost::shared_ptr<Pose3>(new Pose3(between(p2)));
|
||||
}
|
||||
|
||||
/**
|
||||
* Calculate Adjoint map
|
||||
* Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
|
||||
*/
|
||||
Matrix AdjointMap() const;
|
||||
Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; }
|
||||
Matrix AdjointMap() const; /// FIXME Not tested - marked as incorrect
|
||||
Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } /// FIXME Not tested - marked as incorrect
|
||||
|
||||
/**
|
||||
* wedge for Pose3:
|
||||
|
|
|
|||
|
|
@ -25,224 +25,234 @@ using namespace std;
|
|||
|
||||
namespace gtsam {
|
||||
|
||||
/** Explicit instantiation of base class to export members */
|
||||
INSTANTIATE_LIE(Rot3M);
|
||||
/** Explicit instantiation of base class to export members */
|
||||
INSTANTIATE_LIE(Rot3M);
|
||||
|
||||
static const Matrix I3 = eye(3);
|
||||
static const Matrix I3 = eye(3);
|
||||
|
||||
/* ************************************************************************* */
|
||||
// static member functions to construct rotations
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::Rx(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3M(
|
||||
1, 0, 0,
|
||||
0, ct,-st,
|
||||
0, st, ct);
|
||||
}
|
||||
|
||||
Rot3M Rot3M::Rx(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3M(
|
||||
1, 0, 0,
|
||||
0, ct,-st,
|
||||
0, st, ct);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::Ry(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3M(
|
||||
ct, 0, st,
|
||||
0, 1, 0,
|
||||
-st, 0, ct);
|
||||
}
|
||||
|
||||
Rot3M Rot3M::Ry(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3M(
|
||||
ct, 0, st,
|
||||
0, 1, 0,
|
||||
-st, 0, ct);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::Rz(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3M(
|
||||
ct,-st, 0,
|
||||
st, ct, 0,
|
||||
0, 0, 1);
|
||||
}
|
||||
|
||||
Rot3M Rot3M::Rz(double t) {
|
||||
double st = sin(t), ct = cos(t);
|
||||
return Rot3M(
|
||||
ct,-st, 0,
|
||||
st, ct, 0,
|
||||
0, 0, 1);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
// Considerably faster than composing matrices above !
|
||||
Rot3M Rot3M::RzRyRx(double x, double y, double z) {
|
||||
double cx=cos(x),sx=sin(x);
|
||||
double cy=cos(y),sy=sin(y);
|
||||
double cz=cos(z),sz=sin(z);
|
||||
double ss_ = sx * sy;
|
||||
double cs_ = cx * sy;
|
||||
double sc_ = sx * cy;
|
||||
double cc_ = cx * cy;
|
||||
double c_s = cx * sz;
|
||||
double s_s = sx * sz;
|
||||
double _cs = cy * sz;
|
||||
double _cc = cy * cz;
|
||||
double s_c = sx * cz;
|
||||
double c_c = cx * cz;
|
||||
double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
|
||||
return Rot3M(
|
||||
_cc,- c_s + ssc, s_s + csc,
|
||||
_cs, c_c + sss, -s_c + css,
|
||||
-sy, sc_, cc_
|
||||
);
|
||||
}
|
||||
|
||||
// Considerably faster than composing matrices above !
|
||||
Rot3M Rot3M::RzRyRx(double x, double y, double z) {
|
||||
double cx=cos(x),sx=sin(x);
|
||||
double cy=cos(y),sy=sin(y);
|
||||
double cz=cos(z),sz=sin(z);
|
||||
double ss_ = sx * sy;
|
||||
double cs_ = cx * sy;
|
||||
double sc_ = sx * cy;
|
||||
double cc_ = cx * cy;
|
||||
double c_s = cx * sz;
|
||||
double s_s = sx * sz;
|
||||
double _cs = cy * sz;
|
||||
double _cc = cy * cz;
|
||||
double s_c = sx * cz;
|
||||
double c_c = cx * cz;
|
||||
double ssc = ss_ * cz, csc = cs_ * cz, sss = ss_ * sz, css = cs_ * sz;
|
||||
return Rot3M(
|
||||
_cc,- c_s + ssc, s_s + csc,
|
||||
_cs, c_c + sss, -s_c + css,
|
||||
-sy, sc_, cc_
|
||||
);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::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;
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::rodriguez(const Vector& w, double theta) {
|
||||
// get components of axis \omega
|
||||
double wx = w(0), wy=w(1), wz=w(2);
|
||||
double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
|
||||
#ifndef NDEBUG
|
||||
double l_n = wwTxx + wwTyy + wwTzz;
|
||||
if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
|
||||
double l_n = wwTxx + wwTyy + wwTzz;
|
||||
if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
|
||||
#endif
|
||||
|
||||
double c = cos(theta), s = sin(theta), c_1 = 1 - c;
|
||||
double c = cos(theta), s = sin(theta), c_1 = 1 - c;
|
||||
|
||||
double swx = wx * s, swy = wy * s, swz = wz * s;
|
||||
double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
|
||||
double C11 = c_1*wwTyy, C12 = c_1*wy*wz;
|
||||
double C22 = c_1*wwTzz;
|
||||
double swx = wx * s, swy = wy * s, swz = wz * s;
|
||||
double C00 = c_1*wwTxx, C01 = c_1*wx*wy, C02 = c_1*wx*wz;
|
||||
double C11 = c_1*wwTyy, C12 = c_1*wy*wz;
|
||||
double C22 = c_1*wwTzz;
|
||||
|
||||
return Rot3M( c + C00, -swz + C01, swy + C02,
|
||||
swz + C01, c + C11, -swx + C12,
|
||||
-swy + C02, swx + C12, c + C22);
|
||||
}
|
||||
return Rot3M(
|
||||
c + C00, -swz + C01, swy + C02,
|
||||
swz + C01, c + C11, -swx + C12,
|
||||
-swy + C02, swx + C12, c + C22);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::rodriguez(const Vector& w) {
|
||||
double t = w.norm();
|
||||
if (t < 1e-10) return Rot3M();
|
||||
return rodriguez(w/t, t);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::rodriguez(const Vector& w) {
|
||||
double t = w.norm();
|
||||
if (t < 1e-10) return Rot3M();
|
||||
return rodriguez(w/t, t);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
bool Rot3M::equals(const Rot3M & R, double tol) const {
|
||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
bool Rot3M::equals(const Rot3M & R, double tol) const {
|
||||
return equal_with_abs_tol(matrix(), R.matrix(), tol);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3M::matrix() const {
|
||||
double r[] = { r1_.x(), r2_.x(), r3_.x(),
|
||||
r1_.y(), r2_.y(), r3_.y(),
|
||||
r1_.z(), r2_.z(), r3_.z() };
|
||||
return Matrix_(3,3, r);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3M::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;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3M::transpose() const {
|
||||
double r[] = { r1_.x(), r1_.y(), r1_.z(),
|
||||
r2_.x(), r2_.y(), r2_.z(),
|
||||
r3_.x(), r3_.y(), r3_.z()};
|
||||
return Matrix_(3,3, r);
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Matrix Rot3M::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;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3M::column(int index) const{
|
||||
if(index == 3)
|
||||
return r3_;
|
||||
else if (index == 2)
|
||||
return r2_;
|
||||
else
|
||||
return r1_; // default returns r1
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3M::column(int index) const{
|
||||
if(index == 3)
|
||||
return r3_;
|
||||
else if (index == 2)
|
||||
return r2_;
|
||||
else
|
||||
return r1_; // default returns r1
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3M::xyz() const {
|
||||
Matrix I;Vector q;
|
||||
boost::tie(I,q)=RQ(matrix());
|
||||
return q;
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3M::xyz() const {
|
||||
Matrix I;Vector q;
|
||||
boost::tie(I,q)=RQ(matrix());
|
||||
return q;
|
||||
}
|
||||
|
||||
Vector Rot3M::ypr() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(2),q(1),q(0));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3M::ypr() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(2),q(1),q(0));
|
||||
}
|
||||
|
||||
Vector Rot3M::rpy() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(0),q(1),q(2));
|
||||
}
|
||||
/* ************************************************************************* */
|
||||
Vector Rot3M::rpy() const {
|
||||
Vector q = xyz();
|
||||
return Vector_(3,q(0),q(1),q(2));
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
Vector Rot3M::Logmap(const Rot3M& R) {
|
||||
double tr = R.r1().x()+R.r2().y()+R.r3().z();
|
||||
if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10)
|
||||
return zero(3);
|
||||
} else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3)
|
||||
double theta = acos((tr-1.0)/2.0);
|
||||
// Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4)
|
||||
return (0.5 + theta*theta/12)*Vector_(3,
|
||||
R.r2().z()-R.r3().y(),
|
||||
R.r3().x()-R.r1().z(),
|
||||
R.r1().y()-R.r2().x());
|
||||
} else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc.
|
||||
if(fabs(R.r3().z() - -1.0) > 1e-10)
|
||||
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) *
|
||||
Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z());
|
||||
else if(fabs(R.r2().y() - -1.0) > 1e-10)
|
||||
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r2().y())) *
|
||||
Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z());
|
||||
else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit
|
||||
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r1().x())) *
|
||||
Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z());
|
||||
} else {
|
||||
double theta = acos((tr-1.0)/2.0);
|
||||
return (theta/2.0/sin(theta))*Vector_(3,
|
||||
R.r2().z()-R.r3().y(),
|
||||
R.r3().x()-R.r1().z(),
|
||||
R.r1().y()-R.r2().x());
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3M::rotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
if (H2) *H2 = matrix();
|
||||
return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SO(3) section
|
||||
Point3 Rot3M::unrotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
const Matrix Rt(transpose());
|
||||
Point3 q(Rt*p.vector()); // q = Rt*p
|
||||
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
|
||||
if (H2) *H2 = Rt;
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::compose (const Rot3M& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = R2.transpose();
|
||||
if (H2) *H2 = I3;
|
||||
return *this * R2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::between (const Rot3M& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = -(R2.transpose()*matrix());
|
||||
if (H2) *H2 = I3;
|
||||
return between_default(*this, R2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix, Vector> RQ(const Matrix& A) {
|
||||
|
||||
double x = -atan2(-A(2, 1), A(2, 2));
|
||||
Rot3M Qx = Rot3M::Rx(-x);
|
||||
Matrix B = A * Qx.matrix();
|
||||
|
||||
double y = -atan2(B(2, 0), B(2, 2));
|
||||
Rot3M Qy = Rot3M::Ry(-y);
|
||||
Matrix C = B * Qy.matrix();
|
||||
|
||||
double z = -atan2(-C(1, 0), C(1, 1));
|
||||
Rot3M Qz = Rot3M::Rz(-z);
|
||||
Matrix R = C * Qz.matrix();
|
||||
|
||||
Vector xyz = Vector_(3, x, y, z);
|
||||
return make_pair(R, xyz);
|
||||
/* ************************************************************************* */
|
||||
// Log map at identity - return the canonical coordinates of this rotation
|
||||
Vector Rot3M::Logmap(const Rot3M& R) {
|
||||
double tr = R.r1().x()+R.r2().y()+R.r3().z();
|
||||
// FIXME should tr in statement below be absolute value?
|
||||
if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10)
|
||||
return zero(3);
|
||||
} else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3)
|
||||
double theta = acos((tr-1.0)/2.0);
|
||||
// Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4)
|
||||
return (0.5 + theta*theta/12)*Vector_(3,
|
||||
R.r2().z()-R.r3().y(),
|
||||
R.r3().x()-R.r1().z(),
|
||||
R.r1().y()-R.r2().x());
|
||||
// FIXME: in statement below, is this the right comparision?
|
||||
} else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc.
|
||||
if(fabs(R.r3().z() - -1.0) > 1e-10)
|
||||
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) *
|
||||
Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z());
|
||||
else if(fabs(R.r2().y() - -1.0) > 1e-10)
|
||||
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r2().y())) *
|
||||
Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z());
|
||||
else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit
|
||||
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r1().x())) *
|
||||
Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z());
|
||||
} else {
|
||||
double theta = acos((tr-1.0)/2.0);
|
||||
return (theta/2.0/sin(theta))*Vector_(3,
|
||||
R.r2().z()-R.r3().y(),
|
||||
R.r3().x()-R.r1().z(),
|
||||
R.r1().y()-R.r2().x());
|
||||
}
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
/* ************************************************************************* */
|
||||
Point3 Rot3M::rotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = matrix() * skewSymmetric(-p.x(), -p.y(), -p.z());
|
||||
if (H2) *H2 = matrix();
|
||||
return r1_ * p.x() + r2_ * p.y() + r3_ * p.z();
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
// see doc/math.lyx, SO(3) section
|
||||
Point3 Rot3M::unrotate(const Point3& p,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
const Matrix Rt(transpose());
|
||||
Point3 q(Rt*p.vector()); // q = Rt*p
|
||||
if (H1) *H1 = skewSymmetric(q.x(), q.y(), q.z());
|
||||
if (H2) *H2 = Rt;
|
||||
return q;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::compose (const Rot3M& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = R2.transpose();
|
||||
if (H2) *H2 = I3;
|
||||
return *this * R2;
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
Rot3M Rot3M::between (const Rot3M& R2,
|
||||
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
|
||||
if (H1) *H1 = -(R2.transpose()*matrix());
|
||||
if (H2) *H2 = I3;
|
||||
return between_default(*this, R2);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
pair<Matrix, Vector> RQ(const Matrix& A) {
|
||||
|
||||
double x = -atan2(-A(2, 1), A(2, 2));
|
||||
Rot3M Qx = Rot3M::Rx(-x);
|
||||
Matrix B = A * Qx.matrix();
|
||||
|
||||
double y = -atan2(B(2, 0), B(2, 2));
|
||||
Rot3M Qy = Rot3M::Ry(-y);
|
||||
Matrix C = B * Qy.matrix();
|
||||
|
||||
double z = -atan2(-C(1, 0), C(1, 1));
|
||||
Rot3M Qz = Rot3M::Rz(-z);
|
||||
Matrix R = C * Qz.matrix();
|
||||
|
||||
Vector xyz = Vector_(3, x, y, z);
|
||||
return make_pair(R, xyz);
|
||||
}
|
||||
|
||||
/* ************************************************************************* */
|
||||
|
||||
} // namespace gtsam
|
||||
|
|
|
|||
Loading…
Reference in New Issue