fixed Rot3(). @dellaert, I will do the '->', Identity(), setZero() etc . once I am fully done with geometry.
nsrinivasan7
parent
1b2d86929a
commit
595afb51fe
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
@ -365,8 +365,8 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
|
|||
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);
|
||||
|
|
|
|||
|
|
@ -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 :-)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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());
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -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_;
|
||||
|
|
|
|||
|
|
@ -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));
|
||||
|
|
|
|||
Loading…
Reference in New Issue