12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Formatting and comments, adding Rot3 and Pose3 to matlab interface

release/4.3a0
Alex Cunningham 2011-12-01 01:59:34 +00:00
parent ca121c2872
commit a7ea0f4e04
4 changed files with 257 additions and 213 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

25
gtsam.h
View File

@ -11,6 +11,7 @@ class Point3 {
Point3(double x, double y, double z); Point3(double x, double y, double z);
Point3(Vector v); Point3(Vector v);
void print(string s) const; void print(string s) const;
bool equals(const Point3& p, double tol);
Vector vector() const; Vector vector() const;
double x(); double x();
double y(); double y();
@ -21,11 +22,18 @@ class Rot2 {
Rot2(); Rot2();
Rot2(double theta); Rot2(double theta);
void print(string s) const; 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 c() const;
double s() 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 { class Pose2 {
Pose2(); Pose2();
Pose2(double x, double y, double theta); Pose2(double x, double y, double theta);
@ -44,6 +52,21 @@ class Pose2 {
Pose2* retract_(Vector v); 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 { class SharedGaussian {
SharedGaussian(Matrix covariance); SharedGaussian(Matrix covariance);
void print(string s) const; void print(string s) const;

27
gtsam/geometry/Pose3.cpp
View File

@ -214,22 +214,23 @@ namespace gtsam {
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const { Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
if (H1) if (H1)
#ifdef CORRECT_POSE3_EXMAP #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 #else
{ {
Matrix Rt = R_.transpose(); Matrix Rt = R_.transpose();
Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3; Matrix DR_R1 = -R_.matrix(), DR_t1 = Z3;
Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt; Matrix Dt_R1 = -skewSymmetric(R_.unrotate(t_).vector()), Dt_t1 = -Rt;
Matrix DR = collect(2, &DR_R1, &DR_t1); Matrix DR = collect(2, &DR_R1, &DR_t1);
Matrix Dt = collect(2, &Dt_R1, &Dt_t1); Matrix Dt = collect(2, &Dt_R1, &Dt_t1);
*H1 = gtsam::stack(2, &DR, &Dt); *H1 = gtsam::stack(2, &DR, &Dt);
} }
#endif #endif
Rot3 Rt = R_.inverse(); Rot3 Rt = R_.inverse();
return Pose3(Rt, Rt*(-t_)); return Pose3(Rt, Rt*(-t_));
} }
/* ************************************************************************* */ /* ************************************************************************* */
// between = compose(p2,inverse(p1)); // between = compose(p2,inverse(p1));

14
gtsam/geometry/Pose3.h
View File

@ -89,6 +89,11 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const; 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 /// compose syntactic sugar
Pose3 operator*(const Pose3& T) const { Pose3 operator*(const Pose3& T) const {
return Pose3(R_*T.R_, t_ + R_*T.t_); return Pose3(R_*T.R_, t_ + R_*T.t_);
@ -144,12 +149,17 @@ namespace gtsam {
boost::optional<Matrix&> H1=boost::none, boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const; 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 * Calculate Adjoint map
* Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) * Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi)
*/ */
Matrix AdjointMap() const; Matrix AdjointMap() const; /// FIXME Not tested - marked as incorrect
Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } Vector Adjoint(const Vector& xi) const {return AdjointMap()*xi; } /// FIXME Not tested - marked as incorrect
/** /**
* wedge for Pose3: * wedge for Pose3:

404
gtsam/geometry/Rot3M.cpp
View File

@ -25,224 +25,234 @@ using namespace std;
namespace gtsam { namespace gtsam {
/** Explicit instantiation of base class to export members */ /** Explicit instantiation of base class to export members */
INSTANTIATE_LIE(Rot3M); 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); Rot3M Rot3M::Ry(double t) {
return Rot3M( double st = sin(t), ct = cos(t);
1, 0, 0, return Rot3M(
0, ct,-st, ct, 0, st,
0, st, ct); 0, 1, 0,
} -st, 0, ct);
}
Rot3M Rot3M::Ry(double t) { /* ************************************************************************* */
double st = sin(t), ct = cos(t); Rot3M Rot3M::Rz(double t) {
return Rot3M( double st = sin(t), ct = cos(t);
ct, 0, st, return Rot3M(
0, 1, 0, ct,-st, 0,
-st, 0, ct); st, ct, 0,
} 0, 0, 1);
}
Rot3M Rot3M::Rz(double t) { /* ************************************************************************* */
double st = sin(t), ct = cos(t); // Considerably faster than composing matrices above !
return Rot3M( Rot3M Rot3M::RzRyRx(double x, double y, double z) {
ct,-st, 0, double cx=cos(x),sx=sin(x);
st, ct, 0, double cy=cos(y),sy=sin(y);
0, 0, 1); 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) { Rot3M Rot3M::rodriguez(const Vector& w, double theta) {
double cx=cos(x),sx=sin(x); // get components of axis \omega
double cy=cos(y),sy=sin(y); double wx = w(0), wy=w(1), wz=w(2);
double cz=cos(z),sz=sin(z); double wwTxx = wx*wx, wwTyy = wy*wy, wwTzz = wz*wz;
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;
#ifndef NDEBUG #ifndef NDEBUG
double l_n = wwTxx + wwTyy + wwTzz; double l_n = wwTxx + wwTyy + wwTzz;
if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1"); if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
#endif #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 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 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 C11 = c_1*wwTyy, C12 = c_1*wy*wz;
double C22 = c_1*wwTzz; double C22 = c_1*wwTzz;
return Rot3M( c + C00, -swz + C01, swy + C02, return Rot3M(
swz + C01, c + C11, -swx + C12, c + C00, -swz + C01, swy + C02,
-swy + C02, swx + C12, c + C22); swz + C01, c + C11, -swx + C12,
} -swy + C02, swx + C12, c + C22);
}
/* ************************************************************************* */ /* ************************************************************************* */
Rot3M Rot3M::rodriguez(const Vector& w) { Rot3M Rot3M::rodriguez(const Vector& w) {
double t = w.norm(); double t = w.norm();
if (t < 1e-10) return Rot3M(); if (t < 1e-10) return Rot3M();
return rodriguez(w/t, t); return rodriguez(w/t, t);
} }
/* ************************************************************************* */ /* ************************************************************************* */
bool Rot3M::equals(const Rot3M & R, double tol) const { bool Rot3M::equals(const Rot3M & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol); return equal_with_abs_tol(matrix(), R.matrix(), tol);
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Rot3M::matrix() const { Matrix Rot3M::matrix() const {
double r[] = { r1_.x(), r2_.x(), r3_.x(), Matrix R(3,3);
r1_.y(), r2_.y(), r3_.y(), R <<
r1_.z(), r2_.z(), r3_.z() }; r1_.x(), r2_.x(), r3_.x(),
return Matrix_(3,3, r); r1_.y(), r2_.y(), r3_.y(),
} r1_.z(), r2_.z(), r3_.z();
return R;
}
/* ************************************************************************* */ /* ************************************************************************* */
Matrix Rot3M::transpose() const { Matrix Rot3M::transpose() const {
double r[] = { r1_.x(), r1_.y(), r1_.z(), Matrix Rt(3,3);
r2_.x(), r2_.y(), r2_.z(), Rt <<
r3_.x(), r3_.y(), r3_.z()}; r1_.x(), r1_.y(), r1_.z(),
return Matrix_(3,3, r); r2_.x(), r2_.y(), r2_.z(),
} r3_.x(), r3_.y(), r3_.z();
return Rt;
}
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Rot3M::column(int index) const{ Point3 Rot3M::column(int index) const{
if(index == 3) if(index == 3)
return r3_; return r3_;
else if (index == 2) else if (index == 2)
return r2_; return r2_;
else else
return r1_; // default returns r1 return r1_; // default returns r1
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Rot3M::xyz() const { Vector Rot3M::xyz() const {
Matrix I;Vector q; Matrix I;Vector q;
boost::tie(I,q)=RQ(matrix()); boost::tie(I,q)=RQ(matrix());
return q; return q;
} }
Vector Rot3M::ypr() const { /* ************************************************************************* */
Vector q = xyz(); Vector Rot3M::ypr() const {
return Vector_(3,q(2),q(1),q(0)); Vector q = xyz();
} return Vector_(3,q(2),q(1),q(0));
}
Vector Rot3M::rpy() const { /* ************************************************************************* */
Vector q = xyz(); Vector Rot3M::rpy() const {
return Vector_(3,q(0),q(1),q(2)); Vector q = xyz();
} return Vector_(3,q(0),q(1),q(2));
}
/* ************************************************************************* */ /* ************************************************************************* */
// Log map at identity - return the canonical coordinates of this rotation // Log map at identity - return the canonical coordinates of this rotation
Vector Rot3M::Logmap(const Rot3M& R) { Vector Rot3M::Logmap(const Rot3M& R) {
double tr = R.r1().x()+R.r2().y()+R.r3().z(); 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) // FIXME should tr in statement below be absolute value?
return zero(3); if (tr > 3.0 - 1e-17) { // when theta = 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-10)
} else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3) return zero(3);
double theta = acos((tr-1.0)/2.0); } else if (tr > 3.0 - 1e-10) { // when theta near 0, +-2pi, +-4pi, etc. (or tr > 3 + 1E-3)
// Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4) double theta = acos((tr-1.0)/2.0);
return (0.5 + theta*theta/12)*Vector_(3, // Using Taylor expansion: theta/(2*sin(theta)) \approx 1/2+theta^2/12 + O(theta^4)
R.r2().z()-R.r3().y(), return (0.5 + theta*theta/12)*Vector_(3,
R.r3().x()-R.r1().z(), R.r2().z()-R.r3().y(),
R.r1().y()-R.r2().x()); R.r3().x()-R.r1().z(),
} else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc. R.r1().y()-R.r2().x());
if(fabs(R.r3().z() - -1.0) > 1e-10) // FIXME: in statement below, is this the right comparision?
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) * } else if (fabs(tr - -1.0) < 1e-10) { // when theta = +-pi, +-3pi, +-5pi, etc.
Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z()); if(fabs(R.r3().z() - -1.0) > 1e-10)
else if(fabs(R.r2().y() - -1.0) > 1e-10) return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r3().z())) *
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r2().y())) * Vector_(3, R.r3().x(), R.r3().y(), 1.0+R.r3().z());
Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z()); else if(fabs(R.r2().y() - -1.0) > 1e-10)
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.r2().y())) *
return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r1().x())) * Vector_(3, R.r2().x(), 1.0+R.r2().y(), R.r2().z());
Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z()); else // if(fabs(R.r1().x() - -1.0) > 1e-10) This is implicit
} else { return (boost::math::constants::pi<double>() / sqrt(2.0+2.0*R.r1().x())) *
double theta = acos((tr-1.0)/2.0); Vector_(3, 1.0+R.r1().x(), R.r1().y(), R.r1().z());
return (theta/2.0/sin(theta))*Vector_(3, } else {
R.r2().z()-R.r3().y(), double theta = acos((tr-1.0)/2.0);
R.r3().x()-R.r1().z(), return (theta/2.0/sin(theta))*Vector_(3,
R.r1().y()-R.r2().x()); 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);
} }
}
/* ************************************************************************* */ /* ************************************************************************* */
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 } // namespace gtsam