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

fixed Rot3(). @dellaert, I will do the '->', Identity(), setZero() etc . once I am fully done with geometry.

release/4.3a0
nsrinivasan7 2014-12-01 18:20:03 -05:00
parent 1b2d86929a
commit 595afb51fe
7 changed files with 43 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
gtsam/geometry/Pose3.cpp
View File

@ -83,12 +83,12 @@ Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
OptionalJacobian<6,6> H) {
if (H) {
(*H).setZero();
H->setZero();
for (int i = 0; i < 6; ++i) {
Vector6 dxi;
dxi.setZero();
dxi(i) = 1.0;
Matrix GTi = adjointMap(dxi).transpose();
Matrix6 GTi = adjointMap(dxi).transpose();
(*H).col(i) = GTi * y;
}
}
@ -360,13 +360,13 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
}
// Compute SVD
Matrix U, V;
Matrix U,V;
Vector S;
svd(H, U, S, V);
// Recover transform with correction from Eggert97machinevisionandapplications
Matrix UVtranspose = U * V.transpose();
Matrix detWeighting = eye(3, 3);
Matrix3 UVtranspose = U * V.transpose();
Matrix3 detWeighting = Eigen::Matrix3d::Identity();
detWeighting(2, 2) = UVtranspose.determinant();
Rot3 R(Matrix(V * detWeighting * U.transpose()));
Point3 t = Point3(cq) - R * Point3(cp);

54
gtsam/geometry/Rot3.cpp
View File

@ -72,10 +72,11 @@ Point3 Rot3::operator*(const Point3& p) const {
/* ************************************************************************* */
Unit3 Rot3::rotate(const Unit3& p,
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
Unit3 q = Unit3(rotate(p.point3(Hp)));
OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
Matrix32 Dp;
Unit3 q = Unit3(rotate(p.point3(Hp ? &Dp : 0)));
if (Hp)
(*Hp) = q.basis().transpose() * matrix() * (*Hp);
(*Hp) = q.basis().transpose() * matrix() * Dp;
if (HR)
(*HR) = -q.basis().transpose() * matrix() * p.skew();
return q;
@ -83,10 +84,11 @@ Unit3 Rot3::rotate(const Unit3& p,
/* ************************************************************************* */
Unit3 Rot3::unrotate(const Unit3& p,
boost::optional<Matrix&> HR, boost::optional<Matrix&> Hp) const {
Unit3 q = Unit3(unrotate(p.point3(Hp)));
OptionalJacobian<2,3> HR, OptionalJacobian<2,2> Hp) const {
Matrix32 Dp;
Unit3 q = Unit3(unrotate(p.point3(Dp)));
if (Hp)
(*Hp) = q.basis().transpose() * matrix().transpose () * (*Hp);
(*Hp) = q.basis().transpose() * matrix().transpose () * Dp;
if (HR)
(*HR) = q.basis().transpose() * q.skew();
return q;
@ -99,8 +101,8 @@ Unit3 Rot3::operator*(const Unit3& p) const {
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const {
Point3 Rot3::unrotate(const Point3& p, OptionalJacobian<3,3> H1,
OptionalJacobian<3,3> H2) const {
const Matrix3& Rt = transpose();
Point3 q(Rt * p.vector()); // q = Rt*p
const double wx = q.x(), wy = q.y(), wz = q.z();
@ -111,32 +113,16 @@ Point3 Rot3::unrotate(const Point3& p, boost::optional<Matrix3&> H1,
return q;
}
/* ************************************************************************* */
// see doc/math.lyx, SO(3) section
Point3 Rot3::unrotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
const Matrix3& Rt = transpose();
Point3 q(Rt * p.vector()); // q = Rt*p
const double wx = q.x(), wy = q.y(), wz = q.z();
if (H1) {
H1->resize(3,3);
*H1 << 0.0, -wz, +wy, +wz, 0.0, -wx, -wy, +wx, 0.0;
}
if (H2)
*H2 = Rt;
return q;
}
/* ************************************************************************* */
/// Follow Iserles05an, B10, pg 147, with a sign change in the second term (left version)
Matrix3 Rot3::dexpL(const Vector3& v) {
if(zero(v)) return eye(3);
Matrix x = skewSymmetric(v);
Matrix x2 = x*x;
Matrix3 x = skewSymmetric(v);
Matrix3 x2 = x*x;
double theta = v.norm(), vi = theta/2.0;
double s1 = sin(vi)/vi;
double s2 = (theta - sin(theta))/(theta*theta*theta);
Matrix res = eye(3) - 0.5*s1*s1*x + s2*x2;
Matrix3 res = I3 - 0.5*s1*s1*x + s2*x2;
return res;
}
@ -144,11 +130,11 @@ Matrix3 Rot3::dexpL(const Vector3& v) {
/// Follow Iserles05an, B11, pg 147, with a sign change in the second term (left version)
Matrix3 Rot3::dexpInvL(const Vector3& v) {
if(zero(v)) return eye(3);
Matrix x = skewSymmetric(v);
Matrix x2 = x*x;
Matrix3 x = skewSymmetric(v);
Matrix3 x2 = x*x;
double theta = v.norm(), vi = theta/2.0;
double s2 = (theta*tan(M_PI_2-vi) - 2)/(2*theta*theta);
Matrix res = eye(3) + 0.5*x - s2*x2;
Matrix3 res = I3 + 0.5*x - s2*x2;
return res;
}
@ -167,7 +153,7 @@ Point3 Rot3::column(int index) const{
/* ************************************************************************* */
Vector3 Rot3::xyz() const {
Matrix I;Vector3 q;
Matrix3 I;Vector3 q;
boost::tie(I,q)=RQ(matrix());
return q;
}
@ -255,12 +241,6 @@ ostream &operator<<(ostream &os, const Rot3& R) {
return os;
}
/* ************************************************************************* */
Point3 Rot3::unrotate(const Point3& p) const {
// Eigen expression
return Point3(transpose()*p.vector()); // q = Rt*p
}
/* ************************************************************************* */
Rot3 Rot3::slerp(double t, const Rot3& other) const {
// amazingly simple in GTSAM :-)

29
gtsam/geometry/Rot3.h
View File

@ -215,7 +215,7 @@ namespace gtsam {
}
/// derivative of inverse rotation R^T s.t. inverse(R)*R = identity
Rot3 inverse(boost::optional<Matrix&> H1=boost::none) const;
Rot3 inverse(boost::optional<Matrix3&> H1=boost::none) const;
/// Compose two rotations i.e., R= (*this) * R2
Rot3 compose(const Rot3& R2, OptionalJacobian<3, 3> H1 = boost::none,
@ -238,8 +238,8 @@ namespace gtsam {
* Return relative rotation D s.t. R2=D*R1, i.e. D=R2*R1'
*/
Rot3 between(const Rot3& R2,
boost::optional<Matrix&> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const;
OptionalJacobian<3,3> H1=boost::none,
OptionalJacobian<3,3> H2=boost::none) const;
/// @}
/// @name Manifold
@ -321,34 +321,27 @@ namespace gtsam {
/**
* rotate point from rotated coordinate frame to world \f$ p^w = R_c^w p^c \f$
*/
Point3 rotate(const Point3& p, boost::optional<Matrix&> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const;
Point3 rotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2 = boost::none) const;
/// rotate point from rotated coordinate frame to world = R*p
Point3 operator*(const Point3& p) const;
/// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
Point3 unrotate(const Point3& p) const;
/// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
Point3 unrotate(const Point3& p, boost::optional<Matrix3&> H1,
boost::optional<Matrix3&> H2) const;
/// rotate point from world to rotated frame \f$ p^c = (R_c^w)^T p^w \f$
Point3 unrotate(const Point3& p, boost::optional<Matrix&> H1,
boost::optional<Matrix&> H2) const;
Point3 unrotate(const Point3& p, OptionalJacobian<3,3> H1 = boost::none,
OptionalJacobian<3,3> H2=boost::none) const;
/// @}
/// @name Group Action on Unit3
/// @{
/// rotate 3D direction from rotated coordinate frame to world frame
Unit3 rotate(const Unit3& p, boost::optional<Matrix&> HR = boost::none,
boost::optional<Matrix&> Hp = boost::none) const;
Unit3 rotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
OptionalJacobian<2,2> Hp = boost::none) const;
/// unrotate 3D direction from world frame to rotated coordinate frame
Unit3 unrotate(const Unit3& p, boost::optional<Matrix&> HR = boost::none,
boost::optional<Matrix&> Hp = boost::none) const;
Unit3 unrotate(const Unit3& p, OptionalJacobian<2,3> HR = boost::none,
OptionalJacobian<2,2> Hp = boost::none) const;
/// rotate 3D direction from rotated coordinate frame to world frame
Unit3 operator*(const Unit3& p) const;

8
gtsam/geometry/Rot3M.cpp
View File

@ -162,14 +162,14 @@ Matrix3 Rot3::transpose() const {
}
/* ************************************************************************* */
Rot3 Rot3::inverse(boost::optional<Matrix&> H1) const {
Rot3 Rot3::inverse(boost::optional<Matrix3 &> H1) const {
if (H1) *H1 = -rot_;
return Rot3(Matrix3(transpose()));
}
/* ************************************************************************* */
Rot3 Rot3::between (const Rot3& R2,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1) *H1 = -(R2.transpose()*rot_);
if (H2) *H2 = I3;
Matrix3 R12 = transpose()*R2.rot_;
@ -178,7 +178,7 @@ Rot3 Rot3::between (const Rot3& R2,
/* ************************************************************************* */
Point3 Rot3::rotate(const Point3& p,
boost::optional<Matrix&> H1, boost::optional<Matrix&> H2) const {
OptionalJacobian<3,3> H1, OptionalJacobian<3,3> H2) const {
if (H1 || H2) {
if (H1) *H1 = rot_ * skewSymmetric(-p.x(), -p.y(), -p.z());
if (H2) *H2 = rot_;
@ -245,7 +245,7 @@ Rot3 Rot3::retract(const Vector& omega, Rot3::CoordinatesMode mode) const {
} else if(mode == Rot3::CAYLEY) {
return retractCayley(omega);
} else if(mode == Rot3::SLOW_CAYLEY) {
Matrix Omega = skewSymmetric(omega);
Matrix3 Omega = skewSymmetric(omega);
return (*this)*CayleyFixed<3>(-Omega/2);
} else {
assert(false);

6
gtsam/geometry/Unit3.cpp
View File

@ -67,7 +67,7 @@ Unit3 Unit3::Random(boost::mt19937 & rng) {
}
/* ************************************************************************* */
const Unit3::Matrix32& Unit3::basis() const {
const Matrix32& Unit3::basis() const {
// Return cached version if exists
if (B_)
@ -92,7 +92,7 @@ const Unit3::Matrix32& Unit3::basis() const {
b2 = b2 / b2.norm();
// Create the basis matrix
B_.reset(Unit3::Matrix32());
B_.reset(Matrix32());
(*B_) << b1.x(), b2.x(), b1.y(), b2.y(), b1.z(), b2.z();
return *B_;
}
@ -104,7 +104,7 @@ void Unit3::print(const std::string& s) const {
}
/* ************************************************************************* */
Matrix Unit3::skew() const {
Matrix3 Unit3::skew() const {
return skewSymmetric(p_.x(), p_.y(), p_.z());
}

6
gtsam/geometry/Unit3.h
View File

@ -32,8 +32,6 @@ class GTSAM_EXPORT Unit3{
private:
typedef Eigen::Matrix<double,3,2> Matrix32;
Point3 p_; ///< The location of the point on the unit sphere
mutable boost::optional<Matrix32> B_; ///< Cached basis
@ -90,10 +88,10 @@ public:
const Matrix32& basis() const;
/// Return skew-symmetric associated with 3D point on unit sphere
Matrix skew() const;
Matrix3 skew() const;
/// Return unit-norm Point3
const Point3& point3(boost::optional<Matrix&> H = boost::none) const {
const Point3& point3(OptionalJacobian<3,2> H = boost::none) const {
if (H)
*H = basis();
return p_;

2
gtsam/geometry/tests/testRot3.cpp
View File

@ -359,7 +359,7 @@ TEST( Rot3, inverse )
Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3);
Rot3 I;
Matrix actualH;
Matrix3 actualH;
Rot3 actual = R.inverse(actualH);
CHECK(assert_equal(I,R*actual));
CHECK(assert_equal(I,actual*R));