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

Successful representation of E as product manifold

release/4.3a0
dellaert 2015-05-24 15:34:48 -07:00
parent 387dfe5229
commit 994196d37d
2 changed files with 79 additions and 72 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
gtsam/geometry/EssentialMatrix.cpp
View File

@ -13,64 +13,46 @@ using namespace std;
namespace gtsam {
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::FromPose3(const Pose3& _1P2_,
EssentialMatrix EssentialMatrix::FromPose3(const Pose3& aPb,
OptionalJacobian<5, 6> H) {
const Rot3& _1R2_ = _1P2_.rotation();
const Point3& _1T2_ = _1P2_.translation();
const Rot3& aRb = aPb.rotation();
const Point3& aTb = aPb.translation();
if (!H) {
// just make a direction out of translation and create E
Unit3 direction(_1T2_);
return EssentialMatrix(_1R2_, direction);
Unit3 direction(aTb);
return EssentialMatrix(aRb, direction);
} else {
// Calculate the 5*6 Jacobian H = D_E_1P2
// D_E_1P2 = [D_E_1R2 D_E_1T2], 5*3 wrpt rotation, 5*3 wrpt translation
// First get 2*3 derivative from Unit3::FromPoint3
Matrix23 D_direction_1T2;
Unit3 direction = Unit3::FromPoint3(_1T2_, D_direction_1T2);
Unit3 direction = Unit3::FromPoint3(aTb, D_direction_1T2);
*H << I_3x3, Z_3x3, //
Matrix23::Zero(), D_direction_1T2 * _1R2_.matrix();
return EssentialMatrix(_1R2_, direction);
Matrix23::Zero(), D_direction_1T2 * aRb.matrix();
return EssentialMatrix(aRb, direction);
}
}
/* ************************************************************************* */
void EssentialMatrix::print(const string& s) const {
cout << s;
aRb_.print("R:\n");
aTb_.print("d: ");
}
/* ************************************************************************* */
EssentialMatrix EssentialMatrix::retract(const Vector& xi) const {
assert(xi.size() == 5);
Vector3 omega(sub(xi, 0, 3));
Vector2 z(sub(xi, 3, 5));
Rot3 R = aRb_.retract(omega);
Unit3 t = aTb_.retract(z);
return EssentialMatrix(R, t);
}
/* ************************************************************************* */
Vector5 EssentialMatrix::localCoordinates(const EssentialMatrix& other) const {
Vector5 v;
v << aRb_.localCoordinates(other.aRb_),
aTb_.localCoordinates(other.aTb_);
return v;
rotation().print("R:\n");
direction().print("d: ");
}
/* ************************************************************************* */
Point3 EssentialMatrix::transform_to(const Point3& p, OptionalJacobian<3, 5> DE,
OptionalJacobian<3, 3> Dpoint) const {
Pose3 pose(aRb_, aTb_.point3());
Pose3 pose(rotation(), direction().point3());
Matrix36 DE_;
Point3 q = pose.transform_to(p, DE ? &DE_ : 0, Dpoint);
if (DE) {
// DE returned by pose.transform_to is 3*6, but we need it to be 3*5
// The last 3 columns are derivative with respect to change in translation
// The derivative of translation with respect to a 2D sphere delta is 3*2 aTb_.basis()
// The derivative of translation with respect to a 2D sphere delta is 3*2 direction().basis()
// Duy made an educated guess that this needs to be rotated to the local frame
Matrix35 H;
H << DE_.block < 3, 3 > (0, 0), -aRb_.transpose() * aTb_.basis();
H << DE_.block < 3, 3 > (0, 0), -rotation().transpose() * direction().basis();
*DE = H;
}
return q;
@ -81,17 +63,17 @@ EssentialMatrix EssentialMatrix::rotate(const Rot3& cRb,
OptionalJacobian<5, 5> HE, OptionalJacobian<5, 3> HR) const {
// The rotation must be conjugated to act in the camera frame
Rot3 c1Rc2 = aRb_.conjugate(cRb);
Rot3 c1Rc2 = rotation().conjugate(cRb);
if (!HE && !HR) {
// Rotate translation direction and return
Unit3 c1Tc2 = cRb * aTb_;
Unit3 c1Tc2 = cRb * direction();
return EssentialMatrix(c1Rc2, c1Tc2);
} else {
// Calculate derivatives
Matrix23 D_c1Tc2_cRb; // 2*3
Matrix2 D_c1Tc2_aTb; // 2*2
Unit3 c1Tc2 = cRb.rotate(aTb_, D_c1Tc2_cRb, D_c1Tc2_aTb);
Unit3 c1Tc2 = cRb.rotate(direction(), D_c1Tc2_cRb, D_c1Tc2_aTb);
if (HE)
*HE << cRb.matrix(), Matrix32::Zero(), //
Matrix23::Zero(), D_c1Tc2_aTb;
@ -113,8 +95,8 @@ double EssentialMatrix::error(const Vector3& vA, const Vector3& vB, //
if (H) {
// See math.lyx
Matrix13 HR = vA.transpose() * E_ * skewSymmetric(-vB);
Matrix12 HD = vA.transpose() * skewSymmetric(-aRb_.matrix() * vB)
* aTb_.basis();
Matrix12 HD = vA.transpose() * skewSymmetric(-rotation().matrix() * vB)
* direction().basis();
*H << HR, HD;
}
return dot(vA, E_ * vB);

97
gtsam/geometry/EssentialMatrix.h
View File

@ -7,6 +7,55 @@
#pragma once
#include <gtsam/base/Manifold.h>
namespace gtsam {
/// CRTP to construct the product manifold of two other manifolds, M1 and M2
/// Assumes manifold structure from M1 and M2, and binary constructor
template<class Derived, typename M1, typename M2>
class ProductManifold: public std::pair<M1, M2> {
BOOST_CONCEPT_ASSERT((IsManifold<M1>));
BOOST_CONCEPT_ASSERT((IsManifold<M2>));
private:
const M1& g() const {return this->first;}
const M2& h() const {return this->second;}
public:
// Dimension of the manifold
enum { dimension = M1::dimension + M2::dimension };
typedef Eigen::Matrix<double, dimension, 1> TangentVector;
/// Default constructor yields identity
ProductManifold():std::pair<M1,M2>(traits<M1>::Identity(),traits<M2>::Identity()) {}
// Construct from two subgroup elements
ProductManifold(const M1& g, const M2& h):std::pair<M1,M2>(g,h) {}
/// Retract delta to manifold
Derived retract(const TangentVector& xi) const {
return Derived(traits<M1>::Retract(g(),xi.head(M1::dimension)),
traits<M2>::Retract(h(),xi.tail(M2::dimension)));
}
/// Compute the coordinates in the tangent space
TangentVector localCoordinates(const Derived& other) const {
TangentVector xi;
xi << traits<M1>::Local(g(),other.g()), traits<M2>::Local(h(),other.h());
return xi;
}
};
// Define any direct product group to be a model of the multiplicative Group concept
template<class Derived, typename M1, typename M2>
struct traits<ProductManifold<Derived, M1, M2> > : internal::Manifold<
ProductManifold<Derived, M1, M2> > {
};
} // namespace gtsam
#include <gtsam/geometry/Pose3.h>
#include <gtsam/geometry/Unit3.h>
#include <gtsam/geometry/Point2.h>
@ -20,18 +69,15 @@ namespace gtsam {
* but here we choose instead to parameterize it as a (Rot3,Unit3) pair.
* We can then non-linearly optimize immediately on this 5-dimensional manifold.
*/
class GTSAM_EXPORT EssentialMatrix {
class GTSAM_EXPORT EssentialMatrix : public ProductManifold<EssentialMatrix, Rot3, Unit3> {
private:
Rot3 aRb_; ///< Rotation between a and b
Unit3 aTb_; ///< translation direction from a to b
typedef ProductManifold<EssentialMatrix, Rot3, Unit3> Base;
Matrix3 E_; ///< Essential matrix
public:
enum { dimension = 5 };
/// Static function to convert Point2 to homogeneous coordinates
static Vector3 Homogeneous(const Point2& p) {
return Vector3(p.x(), p.y(), 1);
@ -42,12 +88,12 @@ public:
/// Default constructor
EssentialMatrix() :
aTb_(1, 0, 0), E_(aTb_.skew()) {
Base(Rot3(), Unit3(1, 0, 0)), E_(direction().skew()) {
}
/// Construct from rotation and translation
EssentialMatrix(const Rot3& aRb, const Unit3& aTb) :
aRb_(aRb), aTb_(aTb), E_(aTb_.skew() * aRb_.matrix()) {
Base(aRb, aTb), E_(direction().skew() * rotation().matrix()) {
}
/// Named constructor converting a Pose3 with scale to EssentialMatrix (no scale)
@ -72,43 +118,23 @@ public:
/// assert equality up to a tolerance
bool equals(const EssentialMatrix& other, double tol = 1e-8) const {
return aRb_.equals(other.aRb_, tol) && aTb_.equals(other.aTb_, tol);
return rotation().equals(other.rotation(), tol)
&& direction().equals(other.direction(), tol);
}
/// @}
/// @name Manifold
/// @{
/// Dimensionality of tangent space = 5 DOF
inline static size_t Dim() {
return 5;
}
/// Return the dimensionality of the tangent space
size_t dim() const {
return 5;
}
/// Retract delta to manifold
EssentialMatrix retract(const Vector& xi) const;
/// Compute the coordinates in the tangent space
Vector5 localCoordinates(const EssentialMatrix& other) const;
/// @}
/// @name Essential matrix methods
/// @{
/// Rotation
inline const Rot3& rotation() const {
return aRb_;
return this->first;
}
/// Direction
inline const Unit3& direction() const {
return aTb_;
return this->second;
}
/// Return 3*3 matrix representation
@ -118,12 +144,12 @@ public:
/// Return epipole in image_a , as Unit3 to allow for infinity
inline const Unit3& epipole_a() const {
return aTb_; // == direction()
return direction();
}
/// Return epipole in image_b, as Unit3 to allow for infinity
inline Unit3 epipole_b() const {
return aRb_.unrotate(aTb_); // == rotation.unrotate(direction())
return rotation().unrotate(direction());
}
/**
@ -180,8 +206,8 @@ private:
friend class boost::serialization::access;
template<class ARCHIVE>
void serialize(ARCHIVE & ar, const unsigned int /*version*/) {
ar & BOOST_SERIALIZATION_NVP(aRb_);
ar & BOOST_SERIALIZATION_NVP(aTb_);
ar & BOOST_SERIALIZATION_NVP(rotation());
ar & BOOST_SERIALIZATION_NVP(direction());
ar & boost::serialization::make_nvp("E11", E_(0,0));
ar & boost::serialization::make_nvp("E12", E_(0,1));
@ -195,7 +221,6 @@ private:
}
/// @}
};
template<>