From 1a47a334de20429ec6f21cb19e0aafac960cc6ac Mon Sep 17 00:00:00 2001 From: dellaert Date: Tue, 21 Jul 2015 11:23:42 -0700 Subject: [PATCH] Deal with changes in Rot3 --- gtsam_unstable/dynamics/PoseRTV.cpp | 13 ++++++------- gtsam_unstable/dynamics/PoseRTV.h | 7 +++++-- gtsam_unstable/slam/Mechanization_bRn2.cpp | 2 +- gtsam_unstable/slam/Mechanization_bRn2.h | 2 +- 4 files changed, 13 insertions(+), 11 deletions(-) diff --git a/gtsam_unstable/dynamics/PoseRTV.cpp b/gtsam_unstable/dynamics/PoseRTV.cpp index 92f3b9b0b..51737285a 100644 --- a/gtsam_unstable/dynamics/PoseRTV.cpp +++ b/gtsam_unstable/dynamics/PoseRTV.cpp @@ -205,10 +205,10 @@ PoseRTV PoseRTV::transformed_from(const Pose3& trans, ChartJacobian Dglobal, } /* ************************************************************************* */ -Matrix PoseRTV::RRTMbn(const Vector& euler) { +Matrix PoseRTV::RRTMbn(const Vector3& euler) { assert(euler.size() == 3); - const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1)); - const double t2 = tan(euler(2-1)), c2 = cos(euler(2-1)); + const double s1 = sin(euler.x()), c1 = cos(euler.x()); + const double t2 = tan(euler.y()), c2 = cos(euler.y()); Matrix Ebn(3,3); Ebn << 1.0, s1 * t2, c1 * t2, 0.0, c1, -s1, @@ -222,11 +222,10 @@ Matrix PoseRTV::RRTMbn(const Rot3& att) { } /* ************************************************************************* */ -Matrix PoseRTV::RRTMnb(const Vector& euler) { - assert(euler.size() == 3); +Matrix PoseRTV::RRTMnb(const Vector3& euler) { Matrix Enb(3,3); - const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1)); - const double s2 = sin(euler(2-1)), c2 = cos(euler(2-1)); + const double s1 = sin(euler.x()), c1 = cos(euler.x()); + const double s2 = sin(euler.y()), c2 = cos(euler.y()); Enb << 1.0, 0.0, -s2, 0.0, c1, s1*c2, 0.0, -s1, c1*c2; diff --git a/gtsam_unstable/dynamics/PoseRTV.h b/gtsam_unstable/dynamics/PoseRTV.h index b1603efe2..ed5fa9be0 100644 --- a/gtsam_unstable/dynamics/PoseRTV.h +++ b/gtsam_unstable/dynamics/PoseRTV.h @@ -18,6 +18,7 @@ typedef Vector3 Velocity3; /** * Robot state for use with IMU measurements * - contains translation, translational velocity and rotation + * TODO(frank): Alex should deprecate/move to project */ class GTSAM_UNSTABLE_EXPORT PoseRTV : public ProductLieGroup { protected: @@ -145,13 +146,15 @@ public: /// RRTMbn - Function computes the rotation rate transformation matrix from /// body axis rates to euler angle (global) rates - static Matrix RRTMbn(const Vector& euler); + /// TODO(frank): seems to ignore euler.z() + static Matrix RRTMbn(const Vector3& euler); static Matrix RRTMbn(const Rot3& att); /// RRTMnb - Function computes the rotation rate transformation matrix from /// euler angle rates to body axis rates - static Matrix RRTMnb(const Vector& euler); + /// TODO(frank): seems to ignore euler.z() + static Matrix RRTMnb(const Vector3& euler); static Matrix RRTMnb(const Rot3& att); /// @} diff --git a/gtsam_unstable/slam/Mechanization_bRn2.cpp b/gtsam_unstable/slam/Mechanization_bRn2.cpp index d2dd02dd3..f6f1c4a5c 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.cpp +++ b/gtsam_unstable/slam/Mechanization_bRn2.cpp @@ -101,7 +101,7 @@ Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector3& u, // convert to navigation frame Rot3 nRb = bRn_.inverse(); - Vector3 n_omega_bn = (nRb*b_omega_bn).vector(); + Vector3 n_omega_bn = nRb * b_omega_bn; // integrate bRn using exponential map, assuming constant over dt Rot3 delta_nRn = Rot3::Rodrigues(n_omega_bn*dt); diff --git a/gtsam_unstable/slam/Mechanization_bRn2.h b/gtsam_unstable/slam/Mechanization_bRn2.h index fa33ce5b9..4c85614d4 100644 --- a/gtsam_unstable/slam/Mechanization_bRn2.h +++ b/gtsam_unstable/slam/Mechanization_bRn2.h @@ -42,7 +42,7 @@ public: /// gravity in the body frame Vector3 b_g(double g_e) const { Vector3 n_g(0, 0, g_e); - return (bRn_ * n_g).vector(); + return bRn_ * n_g; } const Rot3& bRn() const {return bRn_; }