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

Deal with changes in Rot3

release/4.3a0
dellaert 2015-07-21 11:23:42 -07:00
parent 4c8c669d71
commit 1a47a334de
4 changed files with 13 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
gtsam_unstable/dynamics/PoseRTV.cpp
View File

@ -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); assert(euler.size() == 3);
const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1)); const double s1 = sin(euler.x()), c1 = cos(euler.x());
const double t2 = tan(euler(2-1)), c2 = cos(euler(2-1)); const double t2 = tan(euler.y()), c2 = cos(euler.y());
Matrix Ebn(3,3); Matrix Ebn(3,3);
Ebn << 1.0, s1 * t2, c1 * t2, Ebn << 1.0, s1 * t2, c1 * t2,
0.0, c1, -s1, 0.0, c1, -s1,
@ -222,11 +222,10 @@ Matrix PoseRTV::RRTMbn(const Rot3& att) {
} }
/* ************************************************************************* */ /* ************************************************************************* */
Matrix PoseRTV::RRTMnb(const Vector& euler) { Matrix PoseRTV::RRTMnb(const Vector3& euler) {
assert(euler.size() == 3);
Matrix Enb(3,3); Matrix Enb(3,3);
const double s1 = sin(euler(1-1)), c1 = cos(euler(1-1)); const double s1 = sin(euler.x()), c1 = cos(euler.x());
const double s2 = sin(euler(2-1)), c2 = cos(euler(2-1)); const double s2 = sin(euler.y()), c2 = cos(euler.y());
Enb << 1.0, 0.0, -s2, Enb << 1.0, 0.0, -s2,
0.0, c1, s1*c2, 0.0, c1, s1*c2,
0.0, -s1, c1*c2; 0.0, -s1, c1*c2;

7
gtsam_unstable/dynamics/PoseRTV.h
View File

@ -18,6 +18,7 @@ typedef Vector3 Velocity3;
/** /**
* Robot state for use with IMU measurements * Robot state for use with IMU measurements
* - contains translation, translational velocity and rotation * - contains translation, translational velocity and rotation
* TODO(frank): Alex should deprecate/move to project
*/ */
class GTSAM_UNSTABLE_EXPORT PoseRTV : public ProductLieGroup<Pose3,Velocity3> { class GTSAM_UNSTABLE_EXPORT PoseRTV : public ProductLieGroup<Pose3,Velocity3> {
protected: protected:
@ -145,13 +146,15 @@ public:
/// RRTMbn - Function computes the rotation rate transformation matrix from /// RRTMbn - Function computes the rotation rate transformation matrix from
/// body axis rates to euler angle (global) rates /// 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); static Matrix RRTMbn(const Rot3& att);
/// RRTMnb - Function computes the rotation rate transformation matrix from /// RRTMnb - Function computes the rotation rate transformation matrix from
/// euler angle rates to body axis rates /// 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); static Matrix RRTMnb(const Rot3& att);
/// @} /// @}

2
gtsam_unstable/slam/Mechanization_bRn2.cpp
View File

@ -101,7 +101,7 @@ Mechanization_bRn2 Mechanization_bRn2::integrate(const Vector3& u,
// convert to navigation frame // convert to navigation frame
Rot3 nRb = bRn_.inverse(); 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 // integrate bRn using exponential map, assuming constant over dt
Rot3 delta_nRn = Rot3::Rodrigues(n_omega_bn*dt); Rot3 delta_nRn = Rot3::Rodrigues(n_omega_bn*dt);

2
gtsam_unstable/slam/Mechanization_bRn2.h
View File

@ -42,7 +42,7 @@ public:
/// gravity in the body frame /// gravity in the body frame
Vector3 b_g(double g_e) const { Vector3 b_g(double g_e) const {
Vector3 n_g(0, 0, g_e); Vector3 n_g(0, 0, g_e);
return (bRn_ * n_g).vector(); return bRn_ * n_g;
} }
const Rot3& bRn() const {return bRn_; } const Rot3& bRn() const {return bRn_; }