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

partially fixed Pose3(). Also adressed some of frank's comments from previous commits

release/4.3a0
nsrinivasan7 2014-12-01 15:52:30 -05:00
parent ccda2e1b3b
commit 1b2d86929a
4 changed files with 64 additions and 61 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
gtsam/geometry/Pose2.cpp
View File

@ -45,7 +45,7 @@ Matrix3 Pose2::matrix() const {
Matrix31 T; Matrix31 T;
T << t_.x(), t_.y(), 1.0; T << t_.x(), t_.y(), 1.0;
Matrix3 RT_; Matrix3 RT_;
RT_.block<3,2<(0,0) = R0; RT_.block<3,2>(0,0) = R0;
RT_.block<3,1>(0,2) = T; RT_.block<3,1>(0,2) = T;
return RT_; return RT_;
} }

10
gtsam/geometry/Pose2.h
View File

@ -75,7 +75,7 @@ public:
Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {}
/** Constructor from 3*3 matrix */ /** Constructor from 3*3 matrix */
Pose2(const Matrix &T) : // TODO : Change this to Optional Jacobian ?? Pose2(const Matrix &T) :
r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) { r_(Rot2::atan2(T(1, 0), T(0, 0))), t_(T(0, 2), T(1, 2)) {
assert(T.rows() == 3 && T.cols() == 3); assert(T.rows() == 3 && T.cols() == 3);
} }
@ -169,12 +169,10 @@ public:
* @return xihat, 3*3 element of Lie algebra that can be exponentiated * @return xihat, 3*3 element of Lie algebra that can be exponentiated
*/ */
static inline Matrix3 wedge(double vx, double vy, double w) { static inline Matrix3 wedge(double vx, double vy, double w) {
Matrix3 wedge_; return (Matrix(3,3) <<
wedge_ <<
0.,-w, vx, 0.,-w, vx,
w, 0., vy, w, 0., vy,
0., 0., 0.; 0., 0., 0.).finished();
return wedge_;
} }
/// @} /// @}
@ -289,7 +287,7 @@ private:
/** specialization for pose2 wedge function (generic template in Lie.h) */ /** specialization for pose2 wedge function (generic template in Lie.h) */
template <> template <>
inline Matrix wedge<Pose2>(const Vector& xi) { // TODO : Convert to Optional Jacobian ? inline Matrix wedge<Pose2>(const Vector& xi) {
return Pose2::wedge(xi(0),xi(1),xi(2)); return Pose2::wedge(xi(0),xi(1),xi(2));
} }

85
gtsam/geometry/Pose3.cpp
View File

@ -32,8 +32,7 @@ INSTANTIATE_LIE(Pose3);
/** instantiate concept checks */ /** instantiate concept checks */
GTSAM_CONCEPT_POSE_INST(Pose3); GTSAM_CONCEPT_POSE_INST(Pose3);
static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3 = -I3; static const Matrix3 I3 = Eigen::Matrix3d::Identity(), Z3 = Eigen::Matrix3d::Zero(), _I3 = -I3;
static const Matrix6 I6 = eye(6);
/* ************************************************************************* */ /* ************************************************************************* */
Pose3::Pose3(const Pose2& pose2) : Pose3::Pose3(const Pose2& pose2) :
@ -55,7 +54,7 @@ Matrix6 Pose3::AdjointMap() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix6 Pose3::adjointMap(const Vector& xi) { Matrix6 Pose3::adjointMap(const Vector6& xi) {
Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2));
Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5));
Matrix6 adj; Matrix6 adj;
@ -65,14 +64,15 @@ Matrix6 Pose3::adjointMap(const Vector& xi) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Pose3::adjoint(const Vector& xi, const Vector& y, Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y,
boost::optional<Matrix&> H) { OptionalJacobian<6,6> H) {
if (H) { if (H) {
*H = zeros(6, 6); (*H).setZero();
for (int i = 0; i < 6; ++i) { for (int i = 0; i < 6; ++i) {
Vector dxi = zero(6); Vector6 dxi;
dxi.setZero();
dxi(i) = 1.0; dxi(i) = 1.0;
Matrix Gi = adjointMap(dxi); Matrix6 Gi = adjointMap(dxi);
(*H).col(i) = Gi * y; (*H).col(i) = Gi * y;
} }
} }
@ -80,29 +80,31 @@ Vector Pose3::adjoint(const Vector& xi, const Vector& y,
} }
/* ************************************************************************* */ /* ************************************************************************* */
Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y,
boost::optional<Matrix&> H) { OptionalJacobian<6,6> H) {
if (H) { if (H) {
*H = zeros(6, 6); (*H).setZero();
for (int i = 0; i < 6; ++i) { for (int i = 0; i < 6; ++i) {
Vector dxi = zero(6); Vector6 dxi;
dxi.setZero();
dxi(i) = 1.0; dxi(i) = 1.0;
Matrix GTi = adjointMap(dxi).transpose(); Matrix GTi = adjointMap(dxi).transpose();
(*H).col(i) = GTi * y; (*H).col(i) = GTi * y;
} }
} }
Matrix adjT = adjointMap(xi).transpose();
return adjointMap(xi).transpose() * y; return adjointMap(xi).transpose() * y;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix6 Pose3::dExpInv_exp(const Vector& xi) { Matrix6 Pose3::dExpInv_exp(const Vector6& xi) {
// Bernoulli numbers, from Wikipedia // Bernoulli numbers, from Wikipedia
static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0, static const Vector B = (Vector(9) << 1.0, -1.0 / 2.0, 1. / 6., 0.0, -1.0 / 30.0,
0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished(); 0.0, 1.0 / 42.0, 0.0, -1.0 / 30).finished();
static const int N = 5; // order of approximation static const int N = 5; // order of approximation
Matrix res = I6; Matrix6 res;
Matrix6 ad_i = I6; res.setIdentity();
Matrix6 ad_i;
ad_i.setIdentity();
Matrix6 ad_xi = adjointMap(xi); Matrix6 ad_xi = adjointMap(xi);
double fac = 1.0; double fac = 1.0;
for (int i = 1; i < N; ++i) { for (int i = 1; i < N; ++i) {
@ -225,7 +227,7 @@ Vector6 Pose3::localCoordinates(const Pose3& T,
Matrix4 Pose3::matrix() const { Matrix4 Pose3::matrix() const {
const Matrix3 R = R_.matrix(); const Matrix3 R = R_.matrix();
const Vector3 T = t_.vector(); const Vector3 T = t_.vector();
Eigen::Matrix<double, 1, 4> A14; Matrix14 A14;
A14 << 0.0, 0.0, 0.0, 1.0; A14 << 0.0, 0.0, 0.0, 1.0;
Matrix4 mat; Matrix4 mat;
mat << R, T, A14; mat << R, T, A14;
@ -240,12 +242,11 @@ Pose3 Pose3::transform_to(const Pose3& pose) const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Point3 Pose3::transform_from(const Point3& p, boost::optional<Matrix&> Dpose, Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose,
boost::optional<Matrix&> Dpoint) const { OptionalJacobian<3,3> Dpoint) const {
if (Dpose) { if (Dpose) {
const Matrix3 R = R_.matrix(); const Matrix3 R = R_.matrix();
Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z());
Dpose->resize(3, 6);
(*Dpose) << DR, R; (*Dpose) << DR, R;
} }
if (Dpoint) if (Dpoint)
@ -273,17 +274,17 @@ Point3 Pose3::transform_to(const Point3& p, OptionalJacobian<3,6> Dpose,
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::compose(const Pose3& p2, boost::optional<Matrix&> H1, Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1,
boost::optional<Matrix&> H2) const { OptionalJacobian<6,6> H2) const {
if (H1) if (H1)
*H1 = p2.inverse().AdjointMap(); *H1 = p2.inverse().AdjointMap();
if (H2) if (H2)
*H2 = I6; (*H2).setIdentity();
return (*this) * p2; return (*this) * p2;
} }
/* ************************************************************************* */ /* ************************************************************************* */
Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const { Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const {
if (H1) if (H1)
*H1 = -AdjointMap(); *H1 = -AdjointMap();
Rot3 Rt = R_.inverse(); Rot3 Rt = R_.inverse();
@ -292,40 +293,44 @@ Pose3 Pose3::inverse(boost::optional<Matrix&> H1) const {
/* ************************************************************************* */ /* ************************************************************************* */
// between = compose(p2,inverse(p1)); // between = compose(p2,inverse(p1));
Pose3 Pose3::between(const Pose3& p2, boost::optional<Matrix&> H1, Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1,
boost::optional<Matrix&> H2) const { OptionalJacobian<6,6> H2) const {
Pose3 result = inverse() * p2; Pose3 result = inverse() * p2;
if (H1) if (H1)
*H1 = -result.inverse().AdjointMap(); *H1 = -result.inverse().AdjointMap();
if (H2) if (H2)
*H2 = I6; (*H2).setIdentity();
return result; return result;
} }
/* ************************************************************************* */ /* ************************************************************************* */
double Pose3::range(const Point3& point, boost::optional<Matrix&> H1, double Pose3::range(const Point3& point, OptionalJacobian<1,6> H1,
boost::optional<Matrix&> H2) const { OptionalJacobian<1,3> H2) const {
if (!H1 && !H2) if (!H1 && !H2)
return transform_to(point).norm(); return transform_to(point).norm();
Point3 d = transform_to(point, H1, H2); Matrix36 D1;
Matrix3 D2;
Point3 d = transform_to(point, H1 ? &D1 : 0, H2 ? &D2 : 0);
double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt( double x = d.x(), y = d.y(), z = d.z(), d2 = x * x + y * y + z * z, n = sqrt(
d2); d2);
Matrix D_result_d = (Matrix(1, 3) << x / n, y / n, z / n).finished(); Matrix13 D_result_d ;
D_result_d << x / n, y / n, z / n;
if (H1) if (H1)
*H1 = D_result_d * (*H1); *H1 = D_result_d * (D1);
if (H2) if (H2)
*H2 = D_result_d * (*H2); *H2 = D_result_d * (D2);
return n; return n;
} }
/* ************************************************************************* */ /* ************************************************************************* */
double Pose3::range(const Pose3& point, boost::optional<Matrix&> H1, double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1,
boost::optional<Matrix&> H2) const { OptionalJacobian<1,6> H2) const {
double r = range(point.translation(), H1, H2); Matrix13 D2;
double r = range(pose.translation(), H1, H2? &D2 : 0);
if (H2) { if (H2) {
Matrix H2_ = *H2 * point.rotation().matrix(); Matrix13 H2_ = D2 * pose.rotation().matrix();
*H2 = zeros(1, 6); (*H2).setZero();
insertSub(*H2, H2_, 0, 3); (*H2).block<1,3>(0,3) = H2_;
} }
return r; return r;
} }
@ -347,7 +352,7 @@ boost::optional<Pose3> align(const vector<Point3Pair>& pairs) {
cq *= f; cq *= f;
// Add to form H matrix // Add to form H matrix
Matrix H = zeros(3, 3); Matrix3 H = Eigen::Matrix3d::Zero();
BOOST_FOREACH(const Point3Pair& pair, pairs){ BOOST_FOREACH(const Point3Pair& pair, pairs){
Vector dp = pair.first.vector() - cp; Vector dp = pair.first.vector() - cp;
Vector dq = pair.second.vector() - cq; Vector dq = pair.second.vector() - cq;

28
gtsam/geometry/Pose3.h
View File

@ -99,11 +99,11 @@ public:
} }
/// inverse transformation with derivatives /// inverse transformation with derivatives
Pose3 inverse(boost::optional<Matrix&> H1 = boost::none) const; Pose3 inverse(OptionalJacobian<6, 6> H1 = boost::none) const;
///compose this transformation onto another (first *this and then p2) ///compose this transformation onto another (first *this and then p2)
Pose3 compose(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none, Pose3 compose(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const; OptionalJacobian<6, 6> H2 = boost::none) const;
/// compose syntactic sugar /// compose syntactic sugar
Pose3 operator*(const Pose3& T) const { Pose3 operator*(const Pose3& T) const {
@ -114,8 +114,8 @@ public:
* Return relative pose between p1 and p2, in p1 coordinate frame * Return relative pose between p1 and p2, in p1 coordinate frame
* as well as optionally the derivatives * as well as optionally the derivatives
*/ */
Pose3 between(const Pose3& p2, boost::optional<Matrix&> H1 = boost::none, Pose3 between(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none,
boost::optional<Matrix&> H2 = boost::none) const; OptionalJacobian<6, 6> H2 = boost::none) const;
/// @} /// @}
/// @name Manifold /// @name Manifold
@ -186,17 +186,17 @@ public:
* and its inverse transpose in the discrete Euler Poincare' (DEP) operator. * and its inverse transpose in the discrete Euler Poincare' (DEP) operator.
* *
*/ */
static Matrix6 adjointMap(const Vector& xi); static Matrix6 adjointMap(const Vector6 &xi);
/** /**
* Action of the adjointMap on a Lie-algebra vector y, with optional derivatives * Action of the adjointMap on a Lie-algebra vector y, with optional derivatives
*/ */
static Vector adjoint(const Vector& xi, const Vector& y, boost::optional<Matrix&> H = boost::none); static Vector6 adjoint(const Vector6 &xi, const Vector6 &y, OptionalJacobian<6,6> = boost::none);
/** /**
* The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space. * The dual version of adjoint action, acting on the dual space of the Lie-algebra vector space.
*/ */
static Vector adjointTranspose(const Vector& xi, const Vector& y, boost::optional<Matrix&> H = boost::none); static Vector6 adjointTranspose(const Vector6& xi, const Vector6& y, OptionalJacobian<6, 6> H = boost::none);
/** /**
* Compute the inverse right-trivialized tangent (derivative) map of the exponential map, * Compute the inverse right-trivialized tangent (derivative) map of the exponential map,
@ -208,7 +208,7 @@ public:
* Ernst Hairer, et al., Geometric Numerical Integration, * Ernst Hairer, et al., Geometric Numerical Integration,
* Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006. * Structure-Preserving Algorithms for Ordinary Differential Equations, 2nd edition, Springer-Verlag, 2006.
*/ */
static Matrix6 dExpInv_exp(const Vector& xi); static Matrix6 dExpInv_exp(const Vector6 &xi);
/** /**
* wedge for Pose3: * wedge for Pose3:
@ -237,7 +237,7 @@ public:
* @return point in world coordinates * @return point in world coordinates
*/ */
Point3 transform_from(const Point3& p, Point3 transform_from(const Point3& p,
boost::optional<Matrix&> Dpose=boost::none, boost::optional<Matrix&> Dpoint=boost::none) const; OptionalJacobian<3,6> Dpose=boost::none, OptionalJacobian<3,3> Dpoint=boost::none) const;
/** syntactic sugar for transform_from */ /** syntactic sugar for transform_from */
inline Point3 operator*(const Point3& p) const { return transform_from(p); } inline Point3 operator*(const Point3& p) const { return transform_from(p); }
@ -284,8 +284,8 @@ public:
* @return range (double) * @return range (double)
*/ */
double range(const Point3& point, double range(const Point3& point,
boost::optional<Matrix&> H1=boost::none, OptionalJacobian<1,6> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const; OptionalJacobian<1,3> H2=boost::none) const;
/** /**
* Calculate range to another pose * Calculate range to another pose
@ -293,8 +293,8 @@ public:
* @return range (double) * @return range (double)
*/ */
double range(const Pose3& pose, double range(const Pose3& pose,
boost::optional<Matrix&> H1=boost::none, OptionalJacobian<1,6> H1=boost::none,
boost::optional<Matrix&> H2=boost::none) const; OptionalJacobian<1,6> H2=boost::none) const;
/// @} /// @}
/// @name Advanced Interface /// @name Advanced Interface