From 1b2d86929a37bb141b238a20ae497b766a687477 Mon Sep 17 00:00:00 2001 From: nsrinivasan7 Date: Mon, 1 Dec 2014 15:52:30 -0500 Subject: [PATCH] partially fixed Pose3(). Also adressed some of frank's comments from previous commits --- gtsam/geometry/Pose2.cpp | 2 +- gtsam/geometry/Pose2.h | 10 ++--- gtsam/geometry/Pose3.cpp | 85 +++++++++++++++++++++------------------- gtsam/geometry/Pose3.h | 28 ++++++------- 4 files changed, 64 insertions(+), 61 deletions(-) diff --git a/gtsam/geometry/Pose2.cpp b/gtsam/geometry/Pose2.cpp index cb77bfc7d..af0d330b8 100644 --- a/gtsam/geometry/Pose2.cpp +++ b/gtsam/geometry/Pose2.cpp @@ -45,7 +45,7 @@ Matrix3 Pose2::matrix() const { Matrix31 T; T << t_.x(), t_.y(), 1.0; Matrix3 RT_; - RT_.block<3,2<(0,0) = R0; + RT_.block<3,2>(0,0) = R0; RT_.block<3,1>(0,2) = T; return RT_; } diff --git a/gtsam/geometry/Pose2.h b/gtsam/geometry/Pose2.h index 7e3c29826..49eb444b2 100644 --- a/gtsam/geometry/Pose2.h +++ b/gtsam/geometry/Pose2.h @@ -75,7 +75,7 @@ public: Pose2(const Rot2& r, const Point2& t) : r_(r), t_(t) {} /** 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)) { assert(T.rows() == 3 && T.cols() == 3); } @@ -169,12 +169,10 @@ public: * @return xihat, 3*3 element of Lie algebra that can be exponentiated */ static inline Matrix3 wedge(double vx, double vy, double w) { - Matrix3 wedge_; - wedge_ << + return (Matrix(3,3) << 0.,-w, vx, w, 0., vy, - 0., 0., 0.; - return wedge_; + 0., 0., 0.).finished(); } /// @} @@ -289,7 +287,7 @@ private: /** specialization for pose2 wedge function (generic template in Lie.h) */ template <> -inline Matrix wedge(const Vector& xi) { // TODO : Convert to Optional Jacobian ? +inline Matrix wedge(const Vector& xi) { return Pose2::wedge(xi(0),xi(1),xi(2)); } diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index 93a27ed58..2e3437573 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -32,8 +32,7 @@ INSTANTIATE_LIE(Pose3); /** instantiate concept checks */ GTSAM_CONCEPT_POSE_INST(Pose3); -static const Matrix3 I3 = eye(3), Z3 = zeros(3, 3), _I3 = -I3; -static const Matrix6 I6 = eye(6); +static const Matrix3 I3 = Eigen::Matrix3d::Identity(), Z3 = Eigen::Matrix3d::Zero(), _I3 = -I3; /* ************************************************************************* */ 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 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); Matrix6 adj; @@ -65,14 +64,15 @@ Matrix6 Pose3::adjointMap(const Vector& xi) { } /* ************************************************************************* */ -Vector Pose3::adjoint(const Vector& xi, const Vector& y, - boost::optional H) { +Vector6 Pose3::adjoint(const Vector6& xi, const Vector6& y, + OptionalJacobian<6,6> H) { if (H) { - *H = zeros(6, 6); + (*H).setZero(); for (int i = 0; i < 6; ++i) { - Vector dxi = zero(6); + Vector6 dxi; + dxi.setZero(); dxi(i) = 1.0; - Matrix Gi = adjointMap(dxi); + Matrix6 Gi = adjointMap(dxi); (*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, - boost::optional H) { +Vector6 Pose3::adjointTranspose(const Vector6& xi, const Vector6& y, + OptionalJacobian<6,6> H) { if (H) { - *H = zeros(6, 6); + (*H).setZero(); for (int i = 0; i < 6; ++i) { - Vector dxi = zero(6); + Vector6 dxi; + dxi.setZero(); dxi(i) = 1.0; Matrix GTi = adjointMap(dxi).transpose(); (*H).col(i) = GTi * y; } } - Matrix adjT = adjointMap(xi).transpose(); return adjointMap(xi).transpose() * y; } /* ************************************************************************* */ -Matrix6 Pose3::dExpInv_exp(const Vector& xi) { +Matrix6 Pose3::dExpInv_exp(const Vector6& xi) { // Bernoulli numbers, from Wikipedia 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(); static const int N = 5; // order of approximation - Matrix res = I6; - Matrix6 ad_i = I6; + Matrix6 res; + res.setIdentity(); + Matrix6 ad_i; + ad_i.setIdentity(); Matrix6 ad_xi = adjointMap(xi); double fac = 1.0; for (int i = 1; i < N; ++i) { @@ -225,7 +227,7 @@ Vector6 Pose3::localCoordinates(const Pose3& T, Matrix4 Pose3::matrix() const { const Matrix3 R = R_.matrix(); const Vector3 T = t_.vector(); - Eigen::Matrix A14; + Matrix14 A14; A14 << 0.0, 0.0, 0.0, 1.0; Matrix4 mat; 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 Dpose, - boost::optional Dpoint) const { +Point3 Pose3::transform_from(const Point3& p, OptionalJacobian<3,6> Dpose, + OptionalJacobian<3,3> Dpoint) const { if (Dpose) { const Matrix3 R = R_.matrix(); Matrix3 DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); - Dpose->resize(3, 6); (*Dpose) << DR, R; } 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 H1, - boost::optional H2) const { +Pose3 Pose3::compose(const Pose3& p2, OptionalJacobian<6,6> H1, + OptionalJacobian<6,6> H2) const { if (H1) *H1 = p2.inverse().AdjointMap(); if (H2) - *H2 = I6; + (*H2).setIdentity(); return (*this) * p2; } /* ************************************************************************* */ -Pose3 Pose3::inverse(boost::optional H1) const { +Pose3 Pose3::inverse(OptionalJacobian<6,6> H1) const { if (H1) *H1 = -AdjointMap(); Rot3 Rt = R_.inverse(); @@ -292,40 +293,44 @@ Pose3 Pose3::inverse(boost::optional H1) const { /* ************************************************************************* */ // between = compose(p2,inverse(p1)); -Pose3 Pose3::between(const Pose3& p2, boost::optional H1, - boost::optional H2) const { +Pose3 Pose3::between(const Pose3& p2, OptionalJacobian<6,6> H1, + OptionalJacobian<6,6> H2) const { Pose3 result = inverse() * p2; if (H1) *H1 = -result.inverse().AdjointMap(); if (H2) - *H2 = I6; + (*H2).setIdentity(); return result; } /* ************************************************************************* */ -double Pose3::range(const Point3& point, boost::optional H1, - boost::optional H2) const { +double Pose3::range(const Point3& point, OptionalJacobian<1,6> H1, + OptionalJacobian<1,3> H2) const { if (!H1 && !H2) 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( 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) - *H1 = D_result_d * (*H1); + *H1 = D_result_d * (D1); if (H2) - *H2 = D_result_d * (*H2); + *H2 = D_result_d * (D2); return n; } /* ************************************************************************* */ -double Pose3::range(const Pose3& point, boost::optional H1, - boost::optional H2) const { - double r = range(point.translation(), H1, H2); +double Pose3::range(const Pose3& pose, OptionalJacobian<1,6> H1, + OptionalJacobian<1,6> H2) const { + Matrix13 D2; + double r = range(pose.translation(), H1, H2? &D2 : 0); if (H2) { - Matrix H2_ = *H2 * point.rotation().matrix(); - *H2 = zeros(1, 6); - insertSub(*H2, H2_, 0, 3); + Matrix13 H2_ = D2 * pose.rotation().matrix(); + (*H2).setZero(); + (*H2).block<1,3>(0,3) = H2_; } return r; } @@ -347,7 +352,7 @@ boost::optional align(const vector& pairs) { cq *= f; // Add to form H matrix - Matrix H = zeros(3, 3); + Matrix3 H = Eigen::Matrix3d::Zero(); BOOST_FOREACH(const Point3Pair& pair, pairs){ Vector dp = pair.first.vector() - cp; Vector dq = pair.second.vector() - cq; diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index c31cc48cc..d396fe61f 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -99,11 +99,11 @@ public: } /// inverse transformation with derivatives - Pose3 inverse(boost::optional H1 = boost::none) const; + Pose3 inverse(OptionalJacobian<6, 6> H1 = boost::none) const; ///compose this transformation onto another (first *this and then p2) - Pose3 compose(const Pose3& p2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Pose3 compose(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none, + OptionalJacobian<6, 6> H2 = boost::none) const; /// compose syntactic sugar Pose3 operator*(const Pose3& T) const { @@ -114,8 +114,8 @@ public: * Return relative pose between p1 and p2, in p1 coordinate frame * as well as optionally the derivatives */ - Pose3 between(const Pose3& p2, boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const; + Pose3 between(const Pose3& p2, OptionalJacobian<6, 6> H1 = boost::none, + OptionalJacobian<6, 6> H2 = boost::none) const; /// @} /// @name Manifold @@ -186,17 +186,17 @@ public: * 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 */ - static Vector adjoint(const Vector& xi, const Vector& y, boost::optional 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. */ - static Vector adjointTranspose(const Vector& xi, const Vector& y, boost::optional 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, @@ -208,7 +208,7 @@ public: * Ernst Hairer, et al., Geometric Numerical Integration, * 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: @@ -237,7 +237,7 @@ public: * @return point in world coordinates */ Point3 transform_from(const Point3& p, - boost::optional Dpose=boost::none, boost::optional Dpoint=boost::none) const; + OptionalJacobian<3,6> Dpose=boost::none, OptionalJacobian<3,3> Dpoint=boost::none) const; /** syntactic sugar for transform_from */ inline Point3 operator*(const Point3& p) const { return transform_from(p); } @@ -284,8 +284,8 @@ public: * @return range (double) */ double range(const Point3& point, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1,6> H1=boost::none, + OptionalJacobian<1,3> H2=boost::none) const; /** * Calculate range to another pose @@ -293,8 +293,8 @@ public: * @return range (double) */ double range(const Pose3& pose, - boost::optional H1=boost::none, - boost::optional H2=boost::none) const; + OptionalJacobian<1,6> H1=boost::none, + OptionalJacobian<1,6> H2=boost::none) const; /// @} /// @name Advanced Interface