diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 3481ac146..01f62b8cb 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -37,7 +37,6 @@ void Rot3::print(const std::string& s) const { /* ************************************************************************* */ Rot3 Rot3::Random(std::mt19937& rng) { - // TODO allow any engine without including all of boost :-( Unit3 axis = Unit3::Random(rng); uniform_real_distribution randomAngle(-M_PI, M_PI); double angle = randomAngle(rng); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index e5e65b77d..fc3a8b3f2 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -196,21 +196,23 @@ namespace gtsam { /** * Convert from axis/angle representation * @param axis is the rotation axis, unit length - * @param angle rotation angle + * @param angle rotation angle * @return incremental rotation */ static Rot3 AxisAngle(const Point3& axis, double angle) { + // Convert to unit vector. + Vector3 unitAxis = Unit3(axis).unitVector(); #ifdef GTSAM_USE_QUATERNIONS - return gtsam::Quaternion(Eigen::AngleAxis(angle, axis)); + return gtsam::Quaternion(Eigen::AngleAxis(angle, unitAxis)); #else - return Rot3(SO3::AxisAngle(axis,angle)); + return Rot3(SO3::AxisAngle(unitAxis,angle)); #endif } /** * Convert from axis/angle representation - * @param axis is the rotation axis - * @param angle rotation angle + * @param axis is the rotation axis + * @param angle rotation angle * @return incremental rotation */ static Rot3 AxisAngle(const Unit3& axis, double angle) { @@ -269,9 +271,13 @@ namespace gtsam { /// Syntatic sugar for composing two rotations Rot3 operator*(const Rot3& R2) const; - /// inverse of a rotation, TODO should be different for M/Q + /// inverse of a rotation Rot3 inverse() const { - return Rot3(Matrix3(transpose())); +#ifdef GTSAM_USE_QUATERNIONS + return Rot3(quaternion_.inverse()); +#else + return Rot3(rot_.matrix().transpose()); +#endif } /** @@ -407,7 +413,6 @@ namespace gtsam { * Return 3*3 transpose (inverse) rotation matrix */ Matrix3 transpose() const; - // TODO: const Eigen::Transpose transpose() const; /// @deprecated, this is base 1, and was just confusing Point3 column(int index) const; @@ -440,7 +445,7 @@ namespace gtsam { * you should instead use xyz() or ypr() * TODO: make this more efficient */ - inline double roll() const { return ypr()(2); } + inline double roll() const { return xyz()(0); } /** * Accessor to get to component of angle representations @@ -448,7 +453,7 @@ namespace gtsam { * you should instead use xyz() or ypr() * TODO: make this more efficient */ - inline double pitch() const { return ypr()(1); } + inline double pitch() const { return xyz()(1); } /** * Accessor to get to component of angle representations @@ -456,7 +461,7 @@ namespace gtsam { * you should instead use xyz() or ypr() * TODO: make this more efficient */ - inline double yaw() const { return ypr()(0); } + inline double yaw() const { return xyz()(2); } /// @} /// @name Advanced Interface diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index d38d33bf1..46a07e50a 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -110,7 +110,6 @@ Rot3 Rot3::operator*(const Rot3& R2) const { } /* ************************************************************************* */ -// TODO const Eigen::Transpose Rot3::transpose() const { Matrix3 Rot3::transpose() const { return rot_.matrix().transpose(); } diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 8af9a7144..d609c289c 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -19,8 +19,8 @@ #ifdef GTSAM_USE_QUATERNIONS -#include #include +#include #include using namespace std; @@ -79,9 +79,13 @@ namespace gtsam { } /* ************************************************************************* */ - // TODO: Could we do this? It works in Rot3M but not here, probably because - // here we create an intermediate value by calling matrix() - // const Eigen::Transpose Rot3::transpose() const { + // TODO: Maybe use return type `const Eigen::Transpose`? + // It works in Rot3M but not here, because of some weird behavior + // due to Eigen's expression templates which needs more investigation. + // For example, if we use matrix(), the value returned has a 1e-10, + // and if we use quaternion_.toRotationMatrix(), the matrix is arbitrary. + // Using eval() here doesn't help, it only helps if we use it in + // the downstream code. Matrix3 Rot3::transpose() const { return matrix().transpose(); } diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index efb4bf70b..182db59b0 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -116,6 +116,10 @@ TEST( Rot3, AxisAngle) CHECK(assert_equal(expected,actual,1e-5)); Rot3 actual2 = Rot3::AxisAngle(axis, angle-2*M_PI); CHECK(assert_equal(expected,actual2,1e-5)); + + axis = Vector3(0, 50, 0); + Rot3 actual3 = Rot3::AxisAngle(axis, angle); + CHECK(assert_equal(expected,actual3,1e-5)); } /* ************************************************************************* */ @@ -377,7 +381,7 @@ TEST( Rot3, inverse ) Rot3 actual = R.inverse(actualH); CHECK(assert_equal(I,R*actual)); CHECK(assert_equal(I,actual*R)); - CHECK(assert_equal((Matrix)actual.matrix(), R.transpose())); + CHECK(assert_equal(actual.matrix(), R.transpose())); Matrix numericalH = numericalDerivative11(testing::inverse, R); CHECK(assert_equal(numericalH,actualH)); diff --git a/gtsam/navigation/tests/testAHRSFactor.cpp b/gtsam/navigation/tests/testAHRSFactor.cpp index d32553b94..dd216ce34 100644 --- a/gtsam/navigation/tests/testAHRSFactor.cpp +++ b/gtsam/navigation/tests/testAHRSFactor.cpp @@ -15,6 +15,7 @@ * @author Krunal Chande * @author Luca Carlone * @author Frank Dellaert + * @author Varun Agrawal */ #include @@ -200,7 +201,7 @@ TEST(AHRSFactor, Error) { // 1e-5 needs to be added only when using quaternions for rotations EXPECT(assert_equal(H3e, H3a, 1e-5)); - // FIXME!! DOes not work. Different matrix dimensions. + // 1e-5 needs to be added only when using quaternions for rotations } /* ************************************************************************* */