From 752a9877c5ee9c2cce206b8fab01fa08b4ea98a7 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Sat, 12 Oct 2013 05:13:36 +0000 Subject: [PATCH] Reformatted with new style file, renamed some derivatives to Dcal, Dpose, Dpoint etc. --- gtsam/geometry/Cal3_S2.cpp | 101 +++--- gtsam/geometry/Cal3_S2.h | 263 ++++++++-------- gtsam/geometry/Pose3.cpp | 625 +++++++++++++++++++------------------ gtsam/geometry/Pose3.h | 245 ++++++++------- 4 files changed, 648 insertions(+), 586 deletions(-) diff --git a/gtsam/geometry/Cal3_S2.cpp b/gtsam/geometry/Cal3_S2.cpp index 35e18057a..79e563d3f 100644 --- a/gtsam/geometry/Cal3_S2.cpp +++ b/gtsam/geometry/Cal3_S2.cpp @@ -22,61 +22,72 @@ #include namespace gtsam { - using namespace std; +using namespace std; - /* ************************************************************************* */ - Cal3_S2::Cal3_S2(double fov, int w, int h) : +/* ************************************************************************* */ +Cal3_S2::Cal3_S2(double fov, int w, int h) : s_(0), u0_((double) w / 2.0), v0_((double) h / 2.0) { - double a = fov * M_PI / 360.0; // fov/2 in radians - fx_ = (double)w / (2.0*tan(a)); // old formula: fx_ = (double) w * tan(a); - fy_ = fx_; - } + double a = fov * M_PI / 360.0; // fov/2 in radians + fx_ = (double) w / (2.0 * tan(a)); // old formula: fx_ = (double) w * tan(a); + fy_ = fx_; +} - /* ************************************************************************* */ - Cal3_S2::Cal3_S2(const std::string &path) : +/* ************************************************************************* */ +Cal3_S2::Cal3_S2(const std::string &path) : fx_(320), fy_(320), s_(0), u0_(320), v0_(140) { - char buffer[200]; - buffer[0] = 0; - sprintf(buffer, "%s/calibration_info.txt", path.c_str()); - std::ifstream infile(buffer, std::ios::in); + char buffer[200]; + buffer[0] = 0; + sprintf(buffer, "%s/calibration_info.txt", path.c_str()); + std::ifstream infile(buffer, std::ios::in); - if (infile) - infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_; - else { - printf("Unable to load the calibration\n"); - exit(0); - } - - infile.close(); + if (infile) + infile >> fx_ >> fy_ >> s_ >> u0_ >> v0_; + else { + printf("Unable to load the calibration\n"); + exit(0); } - /* ************************************************************************* */ - void Cal3_S2::print(const std::string& s) const { - gtsam::print(matrix(), s); - } + infile.close(); +} - /* ************************************************************************* */ - bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { - if (fabs(fx_ - K.fx_) > tol) return false; - if (fabs(fy_ - K.fy_) > tol) return false; - if (fabs(s_ - K.s_) > tol) return false; - if (fabs(u0_ - K.u0_) > tol) return false; - if (fabs(v0_ - K.v0_) > tol) return false; - return true; - } +/* ************************************************************************* */ +void Cal3_S2::print(const std::string& s) const { + gtsam::print(matrix(), s); +} - /* ************************************************************************* */ - Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional H1, - boost::optional H2) const { - const double x = p.x(), y = p.y(); - if (H1) - *H1 = Matrix_(2, 5, - x, 0.0, y, 1.0, 0.0, - 0.0, y, 0.0, 0.0, 1.0); - if (H2) *H2 = Matrix_(2, 2, fx_, s_, 0.000, fy_); - return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); - } +/* ************************************************************************* */ +bool Cal3_S2::equals(const Cal3_S2& K, double tol) const { + if (fabs(fx_ - K.fx_) > tol) + return false; + if (fabs(fy_ - K.fy_) > tol) + return false; + if (fabs(s_ - K.s_) > tol) + return false; + if (fabs(u0_ - K.u0_) > tol) + return false; + if (fabs(v0_ - K.v0_) > tol) + return false; + return true; +} + +/* ************************************************************************* */ +Point2 Cal3_S2::uncalibrate(const Point2& p, boost::optional Dcal, + boost::optional Dp) const { + const double x = p.x(), y = p.y(); + if (Dcal) + *Dcal = Matrix_(2, 5, x, 0.0, y, 1.0, 0.0, 0.0, y, 0.0, 0.0, 1.0); + if (Dp) + *Dp = Matrix_(2, 2, fx_, s_, 0.000, fy_); + return Point2(fx_ * x + s_ * y + u0_, fy_ * y + v0_); +} + +/* ************************************************************************* */ +Point2 Cal3_S2::calibrate(const Point2& p) const { + const double u = p.x(), v = p.y(); + return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)), + (1 / fy_) * (v - v0_)); +} /* ************************************************************************* */ diff --git a/gtsam/geometry/Cal3_S2.h b/gtsam/geometry/Cal3_S2.h index e3ae8859d..237fe0ab6 100644 --- a/gtsam/geometry/Cal3_S2.h +++ b/gtsam/geometry/Cal3_S2.h @@ -26,166 +26,179 @@ namespace gtsam { - /** - * @brief The most common 5DOF 3D->2D calibration - * @addtogroup geometry - * \nosubgrouping - */ - class GTSAM_EXPORT Cal3_S2 : public DerivedValue { - private: - double fx_, fy_, s_, u0_, v0_; +/** + * @brief The most common 5DOF 3D->2D calibration + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Cal3_S2: public DerivedValue { +private: + double fx_, fy_, s_, u0_, v0_; - public: +public: - typedef boost::shared_ptr shared_ptr; ///< shared pointer to calibration object + typedef boost::shared_ptr shared_ptr; ///< shared pointer to calibration object - /// @name Standard Constructors - /// @{ + /// @name Standard Constructors + /// @{ - /// Create a default calibration that leaves coordinates unchanged - Cal3_S2() : + /// Create a default calibration that leaves coordinates unchanged + Cal3_S2() : fx_(1), fy_(1), s_(0), u0_(0), v0_(0) { - } + } - /// constructor from doubles - Cal3_S2(double fx, double fy, double s, double u0, double v0) : + /// constructor from doubles + Cal3_S2(double fx, double fy, double s, double u0, double v0) : fx_(fx), fy_(fy), s_(s), u0_(u0), v0_(v0) { - } + } - /// constructor from vector - Cal3_S2(const Vector &d): fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)){} + /// constructor from vector + Cal3_S2(const Vector &d) : + fx_(d(0)), fy_(d(1)), s_(d(2)), u0_(d(3)), v0_(d(4)) { + } + /** + * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect + * @param fov field of view in degrees + * @param w image width + * @param h image height + */ + Cal3_S2(double fov, int w, int h); - /** - * Easy constructor, takes fov in degrees, asssumes zero skew, unit aspect - * @param fov field of view in degrees - * @param w image width - * @param h image height - */ - Cal3_S2(double fov, int w, int h); + /// @} + /// @name Advanced Constructors + /// @{ - /// @} - /// @name Advanced Constructors - /// @{ + /// load calibration from location (default name is calibration_info.txt) + Cal3_S2(const std::string &path); - /// load calibration from location (default name is calibration_info.txt) - Cal3_S2(const std::string &path); + /// @} + /// @name Testable + /// @{ - /// @} - /// @name Testable - /// @{ + /// print with optional string + void print(const std::string& s = "Cal3_S2") const; - /// print with optional string - void print(const std::string& s = "Cal3_S2") const; + /// Check if equal up to specified tolerance + bool equals(const Cal3_S2& K, double tol = 10e-9) const; - /// Check if equal up to specified tolerance - bool equals(const Cal3_S2& K, double tol = 10e-9) const; + /// @} + /// @name Standard Interface + /// @{ - /// @} - /// @name Standard Interface - /// @{ + /// focal length x + inline double fx() const { + return fx_; + } - /// focal length x - inline double fx() const { return fx_;} + /// focal length y + inline double fy() const { + return fy_; + } - /// focal length y - inline double fy() const { return fy_;} + /// skew + inline double skew() const { + return s_; + } - /// skew - inline double skew() const { return s_;} + /// image center in x + inline double px() const { + return u0_; + } - /// image center in x - inline double px() const { return u0_;} + /// image center in y + inline double py() const { + return v0_; + } - /// image center in y - inline double py() const { return v0_;} + /// return the principal point + Point2 principalPoint() const { + return Point2(u0_, v0_); + } - /// return the principal point - Point2 principalPoint() const { - return Point2(u0_, v0_); - } + /// vectorized form (column-wise) + Vector vector() const { + double r[] = { fx_, fy_, s_, u0_, v0_ }; + Vector v(5); + std::copy(r, r + 5, v.data()); + return v; + } - /// vectorized form (column-wise) - Vector vector() const { - double r[] = { fx_, fy_, s_, u0_, v0_ }; - Vector v(5); - std::copy(r, r + 5, v.data()); - return v; - } + /// return calibration matrix K + Matrix matrix() const { + return Matrix_(3, 3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); + } - /// return calibration matrix K - Matrix matrix() const { - return Matrix_(3, 3, fx_, s_, u0_, 0.0, fy_, v0_, 0.0, 0.0, 1.0); - } + /// return inverted calibration matrix inv(K) + Matrix matrix_inverse() const { + const double fxy = fx_ * fy_, sv0 = s_ * v0_, fyu0 = fy_ * u0_; + return Matrix_(3, 3, 1.0 / fx_, -s_ / fxy, (sv0 - fyu0) / fxy, 0.0, + 1.0 / fy_, -v0_ / fy_, 0.0, 0.0, 1.0); + } - /// return inverted calibration matrix inv(K) - Matrix matrix_inverse() const { - const double fxy = fx_*fy_, sv0 = s_*v0_, fyu0 = fy_*u0_; - return Matrix_(3, 3, - 1.0/fx_, -s_/fxy, (sv0-fyu0)/fxy, - 0.0, 1.0/fy_, -v0_/fy_, - 0.0, 0.0, 1.0); - } + /** + * convert intrinsic coordinates xy to image coordinates uv + * @param p point in intrinsic coordinates + * @param Dcal optional 2*5 Jacobian wrpt Cal3_S2 parameters + * @param Dp optional 2*2 Jacobian wrpt intrinsic coordinates + * @return point in image coordinates + */ + Point2 uncalibrate(const Point2& p, boost::optional Dcal = + boost::none, boost::optional Dp = boost::none) const; - /** - * convert intrinsic coordinates xy to image coordinates uv - * with optional derivatives - */ - Point2 uncalibrate(const Point2& p, boost::optional H1 = - boost::none, boost::optional H2 = boost::none) const; + /** + * convert image coordinates uv to intrinsic coordinates xy + * @param p point in image coordinates + * @return point in intrinsic coordinates + */ + Point2 calibrate(const Point2& p) const; - /// convert image coordinates uv to intrinsic coordinates xy - Point2 calibrate(const Point2& p) const { - const double u = p.x(), v = p.y(); - return Point2((1 / fx_) * (u - u0_ - (s_ / fy_) * (v - v0_)), - (1 / fy_) * (v - v0_)); - } + /// @} + /// @name Manifold + /// @{ - /// @} - /// @name Manifold - /// @{ + /// return DOF, dimensionality of tangent space + inline size_t dim() const { + return 5; + } - /// return DOF, dimensionality of tangent space - inline size_t dim() const { - return 5; - } + /// return DOF, dimensionality of tangent space + static size_t Dim() { + return 5; + } - /// return DOF, dimensionality of tangent space - static size_t Dim() { - return 5; - } + /// Given 5-dim tangent vector, create new calibration + inline Cal3_S2 retract(const Vector& d) const { + return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4)); + } - /// Given 5-dim tangent vector, create new calibration - inline Cal3_S2 retract(const Vector& d) const { - return Cal3_S2(fx_ + d(0), fy_ + d(1), s_ + d(2), u0_ + d(3), v0_ + d(4)); - } + /// Unretraction for the calibration + Vector localCoordinates(const Cal3_S2& T2) const { + return vector() - T2.vector(); + } - /// Unretraction for the calibration - Vector localCoordinates(const Cal3_S2& T2) const { - return vector() - T2.vector(); - } + /// @} + /// @name Advanced Interface + /// @{ - /// @} - /// @name Advanced Interface - /// @{ +private: - private: + /// Serialization function + friend class boost::serialization::access; + template + void serialize(Archive & ar, const unsigned int version) { + ar + & boost::serialization::make_nvp("Cal3_S2", + boost::serialization::base_object(*this)); + ar & BOOST_SERIALIZATION_NVP(fx_); + ar & BOOST_SERIALIZATION_NVP(fy_); + ar & BOOST_SERIALIZATION_NVP(s_); + ar & BOOST_SERIALIZATION_NVP(u0_); + ar & BOOST_SERIALIZATION_NVP(v0_); + } - /// Serialization function - friend class boost::serialization::access; - template - void serialize(Archive & ar, const unsigned int version) { - ar & boost::serialization::make_nvp("Cal3_S2", - boost::serialization::base_object(*this)); - ar & BOOST_SERIALIZATION_NVP(fx_); - ar & BOOST_SERIALIZATION_NVP(fy_); - ar & BOOST_SERIALIZATION_NVP(s_); - ar & BOOST_SERIALIZATION_NVP(u0_); - ar & BOOST_SERIALIZATION_NVP(v0_); - } + /// @} - /// @} - - }; +}; } // \ namespace gtsam diff --git a/gtsam/geometry/Pose3.cpp b/gtsam/geometry/Pose3.cpp index faec92a6b..0916cd989 100644 --- a/gtsam/geometry/Pose3.cpp +++ b/gtsam/geometry/Pose3.cpp @@ -26,333 +26,348 @@ using namespace std; namespace gtsam { - /** Explicit instantiation of base class to export members */ - INSTANTIATE_LIE(Pose3); +/** Explicit instantiation of base class to export members */ +INSTANTIATE_LIE(Pose3); - /** instantiate concept checks */ - GTSAM_CONCEPT_POSE_INST(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 = eye(3), Z3 = zeros(3, 3), _I3 = -I3; +static const Matrix6 I6 = eye(6); - /* ************************************************************************* */ - Pose3::Pose3(const Pose2& pose2) : - R_(Rot3::rodriguez(0, 0, pose2.theta())), - t_(Point3(pose2.x(), pose2.y(), 0)) { - } +/* ************************************************************************* */ +Pose3::Pose3(const Pose2& pose2) : + R_(Rot3::rodriguez(0, 0, pose2.theta())), t_( + Point3(pose2.x(), pose2.y(), 0)) { +} - /* ************************************************************************* */ - // Calculate Adjoint map - // Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) - // Experimental - unit tests of derivatives based on it do not check out yet - Matrix6 Pose3::AdjointMap() const { - const Matrix3 R = R_.matrix(); - const Vector3 t = t_.vector(); - Matrix3 A = skewSymmetric(t)*R; - Matrix6 adj; - adj << R, Z3, A, R; - return adj; - } +/* ************************************************************************* */ +// Calculate Adjoint map +// Ad_pose is 6*6 matrix that when applied to twist xi, returns Ad_pose(xi) +// Experimental - unit tests of derivatives based on it do not check out yet +Matrix6 Pose3::AdjointMap() const { + const Matrix3 R = R_.matrix(); + const Vector3 t = t_.vector(); + Matrix3 A = skewSymmetric(t) * R; + Matrix6 adj; + adj << R, Z3, A, R; + return adj; +} - /* ************************************************************************* */ - Matrix6 Pose3::adjointMap(const Vector& xi) { - Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); - Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); - Matrix6 adj; - adj << w_hat, Z3, v_hat, w_hat; +/* ************************************************************************* */ +Matrix6 Pose3::adjointMap(const Vector& xi) { + Matrix3 w_hat = skewSymmetric(xi(0), xi(1), xi(2)); + Matrix3 v_hat = skewSymmetric(xi(3), xi(4), xi(5)); + Matrix6 adj; + adj << w_hat, Z3, v_hat, w_hat; - return adj; - } + return adj; +} - /* ************************************************************************* */ - Vector Pose3::adjoint(const Vector& xi, const Vector& y, boost::optional H) { - if (H) { - *H = zeros(6,6); - for (int i = 0; i<6; ++i) { - Vector dxi = zero(6); dxi(i) = 1.0; - Matrix Gi = adjointMap(dxi); - (*H).col(i) = Gi*y; - } - } - return adjointMap(xi)*y; - } - - /* ************************************************************************* */ - Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, boost::optional H) { - if (H) { - *H = zeros(6,6); - for (int i = 0; i<6; ++i) { - Vector dxi = zero(6); 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) { - // 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); - static const int N = 5; // order of approximation - Matrix res = I6; - Matrix6 ad_i = I6; - Matrix6 ad_xi = adjointMap(xi); - double fac = 1.0; - for (int i = 1 ; i H) { + if (H) { + *H = zeros(6, 6); + for (int i = 0; i < 6; ++i) { + Vector dxi = zero(6); + dxi(i) = 1.0; + Matrix Gi = adjointMap(dxi); + (*H).col(i) = Gi * y; } } + return adjointMap(xi) * y; +} - /* ************************************************************************* */ - Vector6 Pose3::Logmap(const Pose3& p) { - Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); - double t = w.norm(); - if (t < 1e-10) { - Vector6 log; - log << w, T; - return log; - } - else { - Matrix3 W = skewSymmetric(w/t); - // Formula from Agrawal06iros, equation (14) - // simplified with Mathematica, and multiplying in T to avoid matrix math - double Tan = tan(0.5*t); - Vector3 WT = W*T; - Vector3 u = T - (0.5*t)*WT + (1 - t/(2.*Tan)) * (W * WT); - Vector6 log; - log << w, u; - return log; +/* ************************************************************************* */ +Vector Pose3::adjointTranspose(const Vector& xi, const Vector& y, + boost::optional H) { + if (H) { + *H = zeros(6, 6); + for (int i = 0; i < 6; ++i) { + Vector dxi = zero(6); + dxi(i) = 1.0; + Matrix GTi = adjointMap(dxi).transpose(); + (*H).col(i) = GTi * y; } } + Matrix adjT = adjointMap(xi).transpose(); + return adjointMap(xi).transpose() * y; +} - /* ************************************************************************* */ - Pose3 Pose3::retractFirstOrder(const Vector& xi) const { - Vector3 omega(sub(xi, 0, 3)); - Point3 v(sub(xi, 3, 6)); - Rot3 R = R_.retract(omega); // R is done exactly - Point3 t = t_ + R_ * v; // First order t approximation - return Pose3(R, t); +/* ************************************************************************* */ +Matrix6 Pose3::dExpInv_exp(const Vector& 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); + static const int N = 5; // order of approximation + Matrix res = I6; + Matrix6 ad_i = I6; + Matrix6 ad_xi = adjointMap(xi); + double fac = 1.0; + for (int i = 1; i < N; ++i) { + ad_i = ad_xi * ad_i; + fac = fac * i; + res = res + B(i) / fac * ad_i; } + return res; +} - /* ************************************************************************* */ - // Different versions of retract - Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const { - if(mode == Pose3::EXPMAP) { - // Lie group exponential map, traces out geodesic - return compose(Expmap(xi)); - } else if(mode == Pose3::FIRST_ORDER) { - // First order - return retractFirstOrder(xi); - } else { - // Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently - // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation - assert(false); - exit(1); - } - } +/* ************************************************************************* */ +void Pose3::print(const string& s) const { + cout << s; + R_.print("R:\n"); + t_.print("t: "); +} - /* ************************************************************************* */ - // different versions of localCoordinates - Vector6 Pose3::localCoordinates(const Pose3& T, Pose3::CoordinatesMode mode) const { - if(mode == Pose3::EXPMAP) { - // Lie group logarithm map, exact inverse of exponential map - return Logmap(between(T)); - } else if(mode == Pose3::FIRST_ORDER) { - // R is always done exactly in all three retract versions below - Vector3 omega = R_.localCoordinates(T.rotation()); +/* ************************************************************************* */ +bool Pose3::equals(const Pose3& pose, double tol) const { + return R_.equals(pose.R_, tol) && t_.equals(pose.t_, tol); +} - // Incorrect version - // Independently computes the logmap of the translation and rotation - // Vector v = t_.localCoordinates(T.translation()); +/* ************************************************************************* */ +/** Modified from Murray94book version (which assumes w and v normalized?) */ +Pose3 Pose3::Expmap(const Vector& xi) { - // Correct first order t inverse - Point3 d = R_.unrotate(T.translation() - t_); + // get angular velocity omega and translational velocity v from twist xi + Point3 w(xi(0), xi(1), xi(2)), v(xi(3), xi(4), xi(5)); - // TODO: correct second order t inverse - Vector6 local; - local << omega(0),omega(1),omega(2),d.x(),d.y(),d.z(); - return local; - } else { - assert(false); - exit(1); - } - } - - /* ************************************************************************* */ - Matrix4 Pose3::matrix() const { - const Matrix3 R = R_.matrix(); - const Vector3 T = t_.vector(); - Eigen::Matrix A14; - A14 << 0.0, 0.0, 0.0, 1.0; - Matrix4 mat; - mat << R, T, A14; - return mat; - } - - /* ************************************************************************* */ - Pose3 Pose3::transform_to(const Pose3& pose) const { - Rot3 cRv = R_ * Rot3(pose.R_.inverse()); - Point3 t = pose.transform_to(t_); - return Pose3(cRv, t); - } - - /* ************************************************************************* */ - Point3 Pose3::transform_from(const Point3& p, - boost::optional H1, boost::optional H2) const { - if (H1) { - const Matrix R = R_.matrix(); - Matrix DR = R*skewSymmetric(-p.x(), -p.y(), -p.z()); - H1->resize(3,6); - (*H1) << DR, R; - } - if (H2) *H2 = R_.matrix(); - return R_ * p + t_; - } - - /* ************************************************************************* */ - Point3 Pose3::transform_to(const Point3& p, - boost::optional H1, boost::optional H2) const { - const Point3 result = R_.unrotate(p - t_); - if (H1) { - const Point3& q = result; - Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); - H1->resize(3,6); - (*H1) << DR, _I3; - } - if (H2) *H2 = R_.transpose(); - return result; - } - - /* ************************************************************************* */ - Pose3 Pose3::compose(const Pose3& p2, - boost::optional H1, boost::optional H2) const { - if (H1) *H1 = p2.inverse().AdjointMap(); - if (H2) *H2 = I6; - return (*this) * p2; - } - - /* ************************************************************************* */ - Pose3 Pose3::inverse(boost::optional H1) const { - if (H1) *H1 = -AdjointMap(); - Rot3 Rt = R_.inverse(); - return Pose3(Rt, Rt*(-t_)); - } - - /* ************************************************************************* */ - // between = compose(p2,inverse(p1)); - Pose3 Pose3::between(const Pose3& p2, boost::optional H1, - boost::optional H2) const { - Pose3 result = inverse()*p2; - if (H1) *H1 = -result.inverse().AdjointMap(); - if (H2) *H2 = I6; - return result; - } - - /* ************************************************************************* */ - double Pose3::range(const Point3& point, - boost::optional H1, - boost::optional H2) const { - if (!H1 && !H2) return transform_to(point).norm(); - Point3 d = transform_to(point, H1, H2); - 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); - if (H1) *H1 = D_result_d * (*H1); - if (H2) *H2 = D_result_d * (*H2); - return n; - } - - /* ************************************************************************* */ - double Pose3::range(const Pose3& point, - boost::optional H1, boost::optional H2) const { - double r = range(point.translation(), H1, H2); - if (H2) { - Matrix H2_ = *H2 * point.rotation().matrix(); - *H2 = zeros(1, 6); - insertSub(*H2, H2_, 0, 3); - } - return r; - } - - /* ************************************************************************* */ - boost::optional align(const vector& pairs) { - const size_t n = pairs.size(); - if (n<3) return boost::none; // we need at least three pairs - - // calculate centroids - Vector cp = zero(3),cq = zero(3); - BOOST_FOREACH(const Point3Pair& pair, pairs) { - cp += pair.first.vector(); - cq += pair.second.vector(); - } - double f = 1.0/n; - cp *= f; cq *= f; - - // Add to form H matrix - Matrix H = zeros(3,3); - BOOST_FOREACH(const Point3Pair& pair, pairs) { - Vector dp = pair.first.vector() - cp; - Vector dq = pair.second.vector() - cq; - H += dp * dq.transpose(); - } - - // Compute SVD - 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); - detWeighting(2,2) = UVtranspose.determinant(); - Rot3 R(Matrix(V * detWeighting * U.transpose())); - Point3 t = Point3(cq) - R * Point3(cp); + double theta = w.norm(); + if (theta < 1e-10) { + static const Rot3 I; + return Pose3(I, v); + } else { + Point3 n(w / theta); // axis unit vector + Rot3 R = Rot3::rodriguez(n.vector(), theta); + double vn = n.dot(v); // translation parallel to n + Point3 n_cross_v = n.cross(v); // points towards axis + Point3 t = (n_cross_v - R * n_cross_v) / theta + vn * n; return Pose3(R, t); } +} - /* ************************************************************************* */ - std::ostream &operator<<(std::ostream &os, const Pose3& pose) { - os << pose.rotation() << "\n" << pose.translation() << endl; - return os; +/* ************************************************************************* */ +Vector6 Pose3::Logmap(const Pose3& p) { + Vector3 w = Rot3::Logmap(p.rotation()), T = p.translation().vector(); + double t = w.norm(); + if (t < 1e-10) { + Vector6 log; + log << w, T; + return log; + } else { + Matrix3 W = skewSymmetric(w / t); + // Formula from Agrawal06iros, equation (14) + // simplified with Mathematica, and multiplying in T to avoid matrix math + double Tan = tan(0.5 * t); + Vector3 WT = W * T; + Vector3 u = T - (0.5 * t) * WT + (1 - t / (2. * Tan)) * (W * WT); + Vector6 log; + log << w, u; + return log; } +} + +/* ************************************************************************* */ +Pose3 Pose3::retractFirstOrder(const Vector& xi) const { + Vector3 omega(sub(xi, 0, 3)); + Point3 v(sub(xi, 3, 6)); + Rot3 R = R_.retract(omega); // R is done exactly + Point3 t = t_ + R_ * v; // First order t approximation + return Pose3(R, t); +} + +/* ************************************************************************* */ +// Different versions of retract +Pose3 Pose3::retract(const Vector& xi, Pose3::CoordinatesMode mode) const { + if (mode == Pose3::EXPMAP) { + // Lie group exponential map, traces out geodesic + return compose(Expmap(xi)); + } else if (mode == Pose3::FIRST_ORDER) { + // First order + return retractFirstOrder(xi); + } else { + // Point3 t = t_.retract(v.vector()); // Incorrect version retracts t independently + // Point3 t = t_ + R_ * (v+Point3(omega).cross(v)/2); // Second order t approximation + assert(false); + exit(1); + } +} + +/* ************************************************************************* */ +// different versions of localCoordinates +Vector6 Pose3::localCoordinates(const Pose3& T, + Pose3::CoordinatesMode mode) const { + if (mode == Pose3::EXPMAP) { + // Lie group logarithm map, exact inverse of exponential map + return Logmap(between(T)); + } else if (mode == Pose3::FIRST_ORDER) { + // R is always done exactly in all three retract versions below + Vector3 omega = R_.localCoordinates(T.rotation()); + + // Incorrect version + // Independently computes the logmap of the translation and rotation + // Vector v = t_.localCoordinates(T.translation()); + + // Correct first order t inverse + Point3 d = R_.unrotate(T.translation() - t_); + + // TODO: correct second order t inverse + Vector6 local; + local << omega(0), omega(1), omega(2), d.x(), d.y(), d.z(); + return local; + } else { + assert(false); + exit(1); + } +} + +/* ************************************************************************* */ +Matrix4 Pose3::matrix() const { + const Matrix3 R = R_.matrix(); + const Vector3 T = t_.vector(); + Eigen::Matrix A14; + A14 << 0.0, 0.0, 0.0, 1.0; + Matrix4 mat; + mat << R, T, A14; + return mat; +} + +/* ************************************************************************* */ +Pose3 Pose3::transform_to(const Pose3& pose) const { + Rot3 cRv = R_ * Rot3(pose.R_.inverse()); + Point3 t = pose.transform_to(t_); + return Pose3(cRv, t); +} + +/* ************************************************************************* */ +Point3 Pose3::transform_from(const Point3& p, boost::optional Dpose, + boost::optional Dpoint) const { + if (Dpose) { + const Matrix R = R_.matrix(); + Matrix DR = R * skewSymmetric(-p.x(), -p.y(), -p.z()); + Dpose->resize(3, 6); + (*Dpose) << DR, R; + } + if (Dpoint) + *Dpoint = R_.matrix(); + return R_ * p + t_; +} + +/* ************************************************************************* */ +Point3 Pose3::transform_to(const Point3& p, boost::optional Dpose, + boost::optional Dpoint) const { + const Point3 result = R_.unrotate(p - t_); + if (Dpose) { + const Point3& q = result; + Matrix DR = skewSymmetric(q.x(), q.y(), q.z()); + Dpose->resize(3, 6); + (*Dpose) << DR, _I3; + } + if (Dpoint) + *Dpoint = R_.transpose(); + return result; +} + +/* ************************************************************************* */ +Pose3 Pose3::compose(const Pose3& p2, boost::optional H1, + boost::optional H2) const { + if (H1) + *H1 = p2.inverse().AdjointMap(); + if (H2) + *H2 = I6; + return (*this) * p2; +} + +/* ************************************************************************* */ +Pose3 Pose3::inverse(boost::optional H1) const { + if (H1) + *H1 = -AdjointMap(); + Rot3 Rt = R_.inverse(); + return Pose3(Rt, Rt * (-t_)); +} + +/* ************************************************************************* */ +// between = compose(p2,inverse(p1)); +Pose3 Pose3::between(const Pose3& p2, boost::optional H1, + boost::optional H2) const { + Pose3 result = inverse() * p2; + if (H1) + *H1 = -result.inverse().AdjointMap(); + if (H2) + *H2 = I6; + return result; +} + +/* ************************************************************************* */ +double Pose3::range(const Point3& point, boost::optional H1, + boost::optional H2) const { + if (!H1 && !H2) + return transform_to(point).norm(); + Point3 d = transform_to(point, H1, H2); + 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); + if (H1) + *H1 = D_result_d * (*H1); + if (H2) + *H2 = D_result_d * (*H2); + return n; +} + +/* ************************************************************************* */ +double Pose3::range(const Pose3& point, boost::optional H1, + boost::optional H2) const { + double r = range(point.translation(), H1, H2); + if (H2) { + Matrix H2_ = *H2 * point.rotation().matrix(); + *H2 = zeros(1, 6); + insertSub(*H2, H2_, 0, 3); + } + return r; +} + +/* ************************************************************************* */ +boost::optional align(const vector& pairs) { + const size_t n = pairs.size(); + if (n < 3) + return boost::none; // we need at least three pairs + + // calculate centroids + Vector cp = zero(3), cq = zero(3); + BOOST_FOREACH(const Point3Pair& pair, pairs){ + cp += pair.first.vector(); + cq += pair.second.vector(); +} + double f = 1.0 / n; + cp *= f; + cq *= f; + + // Add to form H matrix + Matrix H = zeros(3, 3); + BOOST_FOREACH(const Point3Pair& pair, pairs){ + Vector dp = pair.first.vector() - cp; + Vector dq = pair.second.vector() - cq; + H += dp * dq.transpose(); +} + +// Compute SVD + 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); + detWeighting(2, 2) = UVtranspose.determinant(); + Rot3 R(Matrix(V * detWeighting * U.transpose())); + Point3 t = Point3(cq) - R * Point3(cp); + return Pose3(R, t); +} + +/* ************************************************************************* */ +std::ostream &operator<<(std::ostream &os, const Pose3& pose) { + os << pose.rotation() << "\n" << pose.translation() << endl; + return os; +} } // namespace gtsam diff --git a/gtsam/geometry/Pose3.h b/gtsam/geometry/Pose3.h index 4556f2200..631cea4d5 100644 --- a/gtsam/geometry/Pose3.h +++ b/gtsam/geometry/Pose3.h @@ -15,7 +15,6 @@ */ // \callgraph - #pragma once #include @@ -32,111 +31,123 @@ namespace gtsam { - class Pose2; // forward declare +class Pose2; +// forward declare + +/** + * A 3D pose (R,t) : (Rot3,Point3) + * @addtogroup geometry + * \nosubgrouping + */ +class GTSAM_EXPORT Pose3: public DerivedValue { +public: + static const size_t dimension = 6; + + /** Pose Concept requirements */ + typedef Rot3 Rotation; + typedef Point3 Translation; + +private: + Rot3 R_; + Point3 t_; + +public: + + /// @name Standard Constructors + /// @{ + + /** Default constructor is origin */ + Pose3() { + } + + /** Copy constructor */ + Pose3(const Pose3& pose) : + R_(pose.R_), t_(pose.t_) { + } + + /** Construct from R,t */ + Pose3(const Rot3& R, const Point3& t) : + R_(R), t_(t) { + } + + /** Construct from Pose2 */ + explicit Pose3(const Pose2& pose2); + + /** Constructor from 4*4 matrix */ + Pose3(const Matrix &T) : + R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), T(2, 1), + T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) { + } + + /// @} + /// @name Testable + /// @{ + + /// print with optional string + void print(const std::string& s = "") const; + + /// assert equality up to a tolerance + bool equals(const Pose3& pose, double tol = 1e-9) const; + + /// @} + /// @name Group + /// @{ + + /// identity for group operation + static Pose3 identity() { + return Pose3(); + } + + /// inverse transformation with derivatives + Pose3 inverse(boost::optional 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; + + /// compose syntactic sugar + Pose3 operator*(const Pose3& T) const { + return Pose3(R_ * T.R_, t_ + R_ * T.t_); + } /** - * A 3D pose (R,t) : (Rot3,Point3) - * @addtogroup geometry - * \nosubgrouping + * Return relative pose between p1 and p2, in p1 coordinate frame + * as well as optionally the derivatives */ - class GTSAM_EXPORT Pose3 : public DerivedValue { - public: - static const size_t dimension = 6; + Pose3 between(const Pose3& p2, boost::optional H1 = boost::none, + boost::optional H2 = boost::none) const; - /** Pose Concept requirements */ - typedef Rot3 Rotation; - typedef Point3 Translation; + /// @} + /// @name Manifold + /// @{ - private: - Rot3 R_; - Point3 t_; + /** Enum to indicate which method should be used in Pose3::retract() and + * Pose3::localCoordinates() + */ + enum CoordinatesMode { + EXPMAP, ///< The correct exponential map, computationally expensive. + FIRST_ORDER ///< A fast first-order approximation to the exponential map. + }; - public: + /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes + static size_t Dim() { + return dimension; + } - /// @name Standard Constructors - /// @{ + /// Dimensionality of the tangent space = 6 DOF + size_t dim() const { + return dimension; + } - /** Default constructor is origin */ - Pose3() {} + /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map + Pose3 retractFirstOrder(const Vector& d) const; - /** Copy constructor */ - Pose3(const Pose3& pose) : R_(pose.R_), t_(pose.t_) {} + /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ to Pose3 manifold neighborhood around current pose + Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = + POSE3_DEFAULT_COORDINATES_MODE) const; - /** Construct from R,t */ - Pose3(const Rot3& R, const Point3& t) : R_(R), t_(t) {} - - /** Construct from Pose2 */ - explicit Pose3(const Pose2& pose2); - - /** Constructor from 4*4 matrix */ - Pose3(const Matrix &T) : - R_(T(0, 0), T(0, 1), T(0, 2), T(1, 0), T(1, 1), T(1, 2), T(2, 0), - T(2, 1), T(2, 2)), t_(T(0, 3), T(1, 3), T(2, 3)) {} - - /// @} - /// @name Testable - /// @{ - - /// print with optional string - void print(const std::string& s = "") const; - - /// assert equality up to a tolerance - bool equals(const Pose3& pose, double tol = 1e-9) const; - - /// @} - /// @name Group - /// @{ - - /// identity for group operation - static Pose3 identity() { return Pose3(); } - - /// inverse transformation with derivatives - Pose3 inverse(boost::optional 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; - - /// compose syntactic sugar - Pose3 operator*(const Pose3& T) const { - return Pose3(R_*T.R_, t_ + R_*T.t_); - } - - /** - * 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; - - /// @} - /// @name Manifold - /// @{ - - /** Enum to indicate which method should be used in Pose3::retract() and - * Pose3::localCoordinates() - */ - enum CoordinatesMode { - EXPMAP, ///< The correct exponential map, computationally expensive. - FIRST_ORDER ///< A fast first-order approximation to the exponential map. - }; - - /// Dimensionality of tangent space = 6 DOF - used to autodetect sizes - static size_t Dim() { return dimension; } - - /// Dimensionality of the tangent space = 6 DOF - size_t dim() const { return dimension; } - - /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ from R^ with fast first-order approximation to the exponential map - Pose3 retractFirstOrder(const Vector& d) const; - - /// Retraction from R^6 \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ to Pose3 manifold neighborhood around current pose - Pose3 retract(const Vector& d, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const; - - /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose - Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode = POSE3_DEFAULT_COORDINATES_MODE) const; + /// Local 6D coordinates \f$ [R_x,R_y,R_z,T_x,T_y,T_z] \f$ of Pose3 manifold neighborhood around current pose + Vector6 localCoordinates(const Pose3& T2, Pose3::CoordinatesMode mode =POSE3_DEFAULT_COORDINATES_MODE) const; /// @} /// @name Lie Group @@ -218,16 +229,28 @@ namespace gtsam { /// @name Group Action on Point3 /// @{ - /** receives the point in Pose coordinates and transforms it to world coordinates */ + /** + * @brief takes point in Pose coordinates and transforms it to world coordinates + * @param p point in Pose coordinates + * @param Dpose optional 3*6 Jacobian wrpt this pose + * @param Dpoint optional 3*3 Jacobian wrpt point + * @return point in world coordinates + */ Point3 transform_from(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + boost::optional Dpose=boost::none, boost::optional Dpoint=boost::none) const; /** syntactic sugar for transform_from */ inline Point3 operator*(const Point3& p) const { return transform_from(p); } - /** receives the point in world coordinates and transforms it to Pose coordinates */ + /** + * @brief takes point in world coordinates and transforms it to Pose coordinates + * @param p point in world coordinates + * @param Dpose optional 3*6 Jacobian wrpt this pose + * @param Dpoint optional 3*3 Jacobian wrpt point + * @return point in Pose coordinates + */ Point3 transform_to(const Point3& p, - boost::optional H1=boost::none, boost::optional H2=boost::none) const; + boost::optional Dpose=boost::none, boost::optional Dpoint=boost::none) const; /// @} /// @name Standard Interface @@ -305,7 +328,7 @@ namespace gtsam { } /// @} - }; // Pose3 class + };// Pose3 class /** * wedge for Pose3: @@ -314,16 +337,16 @@ namespace gtsam { * v = 3D velocity * @return xihat, 4*4 element of Lie algebra that can be exponentiated */ - template <> - inline Matrix wedge(const Vector& xi) { - return Pose3::wedge(xi(0),xi(1),xi(2),xi(3),xi(4),xi(5)); - } +template<> +inline Matrix wedge(const Vector& xi) { + return Pose3::wedge(xi(0), xi(1), xi(2), xi(3), xi(4), xi(5)); +} - /** - * Calculate pose between a vector of 3D point correspondences (p,q) - * where q = Pose3::transform_from(p) = t + R*p - */ - typedef std::pair Point3Pair; - GTSAM_EXPORT boost::optional align(const std::vector& pairs); +/** + * Calculate pose between a vector of 3D point correspondences (p,q) + * where q = Pose3::transform_from(p) = t + R*p + */ +typedef std::pair Point3Pair; +GTSAM_EXPORT boost::optional align(const std::vector& pairs); } // namespace gtsam