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

minor improvements to RQ and cleanup

release/4.3a0
Chris Beall 2012-04-11 15:12:39 +00:00
parent 91e7dc5882
commit 5839d1bfaa
3 changed files with 6 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/geometry/Rot3.h
View File

@ -377,9 +377,6 @@ namespace gtsam {
ar & boost::serialization::make_nvp("rot31", rot_(2,0)); ar & boost::serialization::make_nvp("rot31", rot_(2,0));
ar & boost::serialization::make_nvp("rot32", rot_(2,1)); ar & boost::serialization::make_nvp("rot32", rot_(2,1));
ar & boost::serialization::make_nvp("rot33", rot_(2,2)); ar & boost::serialization::make_nvp("rot33", rot_(2,2));
// ar & BOOST_SERIALIZATION_NVP(r1_);
// ar & BOOST_SERIALIZATION_NVP(r2_);
// ar & BOOST_SERIALIZATION_NVP(r3_);
#else #else
ar & boost::serialization::make_nvp("w", quaternion_.w()); ar & boost::serialization::make_nvp("w", quaternion_.w());
ar & boost::serialization::make_nvp("x", quaternion_.x()); ar & boost::serialization::make_nvp("x", quaternion_.x());
@ -401,5 +398,5 @@ namespace gtsam {
* @return an upper triangular matrix R * @return an upper triangular matrix R
* @return a vector [thetax, thetay, thetaz] in radians. * @return a vector [thetax, thetay, thetaz] in radians.
*/ */
std::pair<Matrix3,Vector3> RQ(const Matrix& A); std::pair<Matrix3,Vector3> RQ(const Matrix3& A);
} }

9
gtsam/geometry/Rot3M.cpp
View File

@ -322,8 +322,8 @@ Point3 Rot3::r3() const { return Point3(rot_.col(2)); }
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 Rot3::xyz() const { Vector3 Rot3::xyz() const {
Matrix I;Vector3 q; Matrix3 I;Vector3 q;
boost::tie(I,q)=RQ(matrix()); boost::tie(I,q)=RQ(rot_);
return q; return q;
} }
@ -335,8 +335,7 @@ Vector3 Rot3::ypr() const {
/* ************************************************************************* */ /* ************************************************************************* */
Vector3 Rot3::rpy() const { Vector3 Rot3::rpy() const {
Vector3 q = xyz(); return xyz();
return Vector3(q(0),q(1),q(2));
} }
/* ************************************************************************* */ /* ************************************************************************* */
@ -345,7 +344,7 @@ Quaternion Rot3::toQuaternion() const {
} }
/* ************************************************************************* */ /* ************************************************************************* */
pair<Matrix3, Vector3> RQ(const Matrix& A) { pair<Matrix3, Vector3> RQ(const Matrix3& A) {
double x = -atan2(-A(2, 1), A(2, 2)); double x = -atan2(-A(2, 1), A(2, 2));
Rot3 Qx = Rot3::Rx(-x); Rot3 Qx = Rot3::Rx(-x);

2
gtsam/geometry/Rot3Q.cpp
View File

@ -213,7 +213,7 @@ namespace gtsam {
Quaternion Rot3::toQuaternion() const { return quaternion_; } Quaternion Rot3::toQuaternion() const { return quaternion_; }
/* ************************************************************************* */ /* ************************************************************************* */
pair<Matrix3, Vector3> RQ(const Matrix& A) { pair<Matrix3, Vector3> RQ(const Matrix3& A) {
double x = -atan2(-A(2, 1), A(2, 2)); double x = -atan2(-A(2, 1), A(2, 2));
Rot3 Qx = Rot3::Rx(-x); Rot3 Qx = Rot3::Rx(-x);