diff --git a/gtsam/geometry/Quaternion.h b/gtsam/geometry/Quaternion.h index 5096f3513..0ab4a5ee6 100644 --- a/gtsam/geometry/Quaternion.h +++ b/gtsam/geometry/Quaternion.h @@ -74,7 +74,7 @@ struct traits { return g.inverse(); } - /// Exponential map, using the inlined code from Eigen's converseion from axis/angle + /// Exponential map, using the inlined code from Eigen's conversion from axis/angle static Q Expmap(const Eigen::Ref& omega, ChartJacobian H = boost::none) { using std::cos; diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 926590ee1..6b28f5125 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -34,12 +34,12 @@ void Rot3::print(const std::string& s) const { } /* ************************************************************************* */ -Rot3 Rot3::Random(boost::mt19937 & rng) { +Rot3 Rot3::Random(boost::mt19937& rng) { // TODO allow any engine without including all of boost :-( - Unit3 w = Unit3::Random(rng); - boost::uniform_real randomAngle(-M_PI,M_PI); + Unit3 axis = Unit3::Random(rng); + boost::uniform_real randomAngle(-M_PI, M_PI); double angle = randomAngle(rng); - return Rodrigues(w,angle); + return AxisAngle(axis, angle); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index d8a5cdb07..608f41954 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -141,7 +141,7 @@ namespace gtsam { * as described in http://www.sedris.org/wg8home/Documents/WG80462.pdf. * Assumes vehicle coordinate frame X forward, Y right, Z down. */ - static Rot3 ypr (double y, double p, double r) { return RzRyRx(r,p,y);} + static Rot3 ypr(double y, double p, double r) { return RzRyRx(r,p,y);} /** Create from Quaternion coefficients */ static Rot3 quaternion(double w, double x, double y, double z) { @@ -150,54 +150,54 @@ namespace gtsam { } /** - * Rodrigues' formula to compute an incremental rotation matrix - * @param w is the rotation axis, unit length + * Convert from axis/angle representation + * @param axis is the rotation axis, unit length * @param angle rotation angle - * @return incremental rotation matrix + * @return incremental rotation */ - static Rot3 Rodrigues(const Vector3& axis, double angle) { + static Rot3 AxisAngle(const Vector3& axis, double angle) { #ifdef GTSAM_USE_QUATERNIONS return Quaternion(Eigen::AngleAxis(angle, axis)); #else - return SO3::Rodrigues(axis,angle); + return SO3::AxisAngle(axis,angle); #endif } /** - * Rodrigues' formula to compute an incremental rotation matrix - * @param w is the rotation axis, unit length + * Convert from axis/angle representation + * @param axisw is the rotation axis, unit length * @param angle rotation angle - * @return incremental rotation matrix + * @return incremental rotation */ - static Rot3 Rodrigues(const Point3& axis, double angle) { - return Rodrigues(axis.vector(),angle); + static Rot3 AxisAngle(const Point3& axis, double angle) { + return AxisAngle(axis.vector(),angle); } /** - * Rodrigues' formula to compute an incremental rotation matrix - * @param w is the rotation axis + * Convert from axis/angle representation + * @param axis is the rotation axis * @param angle rotation angle - * @return incremental rotation matrix + * @return incremental rotation */ - static Rot3 Rodrigues(const Unit3& axis, double angle) { - return Rodrigues(axis.unitVector(),angle); + static Rot3 AxisAngle(const Unit3& axis, double angle) { + return AxisAngle(axis.unitVector(),angle); } /** - * Rodrigues' formula to compute an incremental rotation matrix + * Rodrigues' formula to compute an incremental rotation * @param w a vector of incremental roll,pitch,yaw - * @return incremental rotation matrix + * @return incremental rotation */ static Rot3 Rodrigues(const Vector3& w) { return Rot3::Expmap(w); } /** - * Rodrigues' formula to compute an incremental rotation matrix + * Rodrigues' formula to compute an incremental rotation * @param wx Incremental roll (about X) * @param wy Incremental pitch (about Y) * @param wz Incremental yaw (about Z) - * @return incremental rotation matrix + * @return incremental rotation */ static Rot3 Rodrigues(double wx, double wy, double wz) { return Rodrigues(Vector3(wx, wy, wz)); @@ -442,9 +442,9 @@ namespace gtsam { /// @} /// @name Deprecated /// @{ - static Rot3 rodriguez(const Vector3& axis, double angle) { return Rodrigues(axis, angle); } - static Rot3 rodriguez(const Point3& axis, double angle) { return Rodrigues(axis, angle); } - static Rot3 rodriguez(const Unit3& axis, double angle) { return Rodrigues(axis, angle); } + static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); } + static Rot3 rodriguez(const Point3& axis, double angle) { return AxisAngle(axis, angle); } + static Rot3 rodriguez(const Unit3& axis, double angle) { return AxisAngle(axis, angle); } static Rot3 rodriguez(const Vector3& w) { return Rodrigues(w); } static Rot3 rodriguez(double wx, double wy, double wz) { return Rodrigues(wx, wy, wz); } /// @} diff --git a/gtsam/geometry/SO3.cpp b/gtsam/geometry/SO3.cpp index 354ce8de8..af5803cb7 100644 --- a/gtsam/geometry/SO3.cpp +++ b/gtsam/geometry/SO3.cpp @@ -39,7 +39,7 @@ static SO3 FirstOrder(const Vector3& omega) { return R; } -SO3 SO3::Rodrigues(const Vector3& axis, double theta) { +SO3 SO3::AxisAngle(const Vector3& axis, double theta) { if (theta*theta > std::numeric_limits::epsilon()) { using std::cos; using std::sin; @@ -79,7 +79,7 @@ SO3 SO3::Expmap(const Vector3& omega, ChartJacobian H) { double theta2 = omega.dot(omega); if (theta2 > std::numeric_limits::epsilon()) { double theta = std::sqrt(theta2); - return Rodrigues(omega / theta, theta); + return AxisAngle(omega / theta, theta); } else { return FirstOrder(omega); } diff --git a/gtsam/geometry/SO3.h b/gtsam/geometry/SO3.h index 9bcdd5f3d..580287eac 100644 --- a/gtsam/geometry/SO3.h +++ b/gtsam/geometry/SO3.h @@ -59,7 +59,7 @@ public: } /// Static, named constructor TODO think about relation with above - static SO3 Rodrigues(const Vector3& axis, double theta); + static SO3 AxisAngle(const Vector3& axis, double theta); /// @} /// @name Testable diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index e1ac55148..a61467b82 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -95,7 +95,7 @@ TEST( Rot3, equals) /* ************************************************************************* */ // Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + .... -Rot3 slow_but_correct_rodriguez(const Vector& w) { +Rot3 slow_but_correct_Rodrigues(const Vector& w) { double t = norm_2(w); Matrix J = skewSymmetric(w / t); if (t < 1e-5) return Rot3(); @@ -108,16 +108,16 @@ TEST( Rot3, Rodrigues) { Rot3 R1 = Rot3::Rodrigues(epsilon, 0, 0); Vector w = (Vector(3) << epsilon, 0., 0.).finished(); - Rot3 R2 = slow_but_correct_rodriguez(w); + Rot3 R2 = slow_but_correct_Rodrigues(w); CHECK(assert_equal(R2,R1)); } /* ************************************************************************* */ -TEST( Rot3, rodriguez2) +TEST( Rot3, Rodrigues2) { Vector axis = Vector3(0., 1., 0.); // rotation around Y double angle = 3.14 / 4.0; - Rot3 actual = Rot3::Rodrigues(axis, angle); + Rot3 actual = Rot3::AxisAngle(axis, angle); Rot3 expected(0.707388, 0, 0.706825, 0, 1, 0, -0.706825, 0, 0.707388); @@ -125,26 +125,26 @@ TEST( Rot3, rodriguez2) } /* ************************************************************************* */ -TEST( Rot3, rodriguez3) +TEST( Rot3, Rodrigues3) { Vector w = Vector3(0.1, 0.2, 0.3); - Rot3 R1 = Rot3::Rodrigues(w / norm_2(w), norm_2(w)); - Rot3 R2 = slow_but_correct_rodriguez(w); + Rot3 R1 = Rot3::AxisAngle(w / norm_2(w), norm_2(w)); + Rot3 R2 = slow_but_correct_Rodrigues(w); CHECK(assert_equal(R2,R1)); } /* ************************************************************************* */ -TEST( Rot3, rodriguez4) +TEST( Rot3, Rodrigues4) { Vector axis = Vector3(0., 0., 1.); // rotation around Z double angle = M_PI/2.0; - Rot3 actual = Rot3::Rodrigues(axis, angle); + Rot3 actual = Rot3::AxisAngle(axis, angle); double c=cos(angle),s=sin(angle); Rot3 expected(c,-s, 0, s, c, 0, 0, 0, 1); CHECK(assert_equal(expected,actual)); - CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual)); + CHECK(assert_equal(slow_but_correct_Rodrigues(axis*angle),actual)); } /* ************************************************************************* */ @@ -652,8 +652,8 @@ TEST( Rot3, slerp) } //****************************************************************************** -Rot3 T1(Rot3::Rodrigues(Vector3(0, 0, 1), 1)); -Rot3 T2(Rot3::Rodrigues(Vector3(0, 1, 0), 2)); +Rot3 T1(Rot3::AxisAngle(Vector3(0, 0, 1), 1)); +Rot3 T2(Rot3::AxisAngle(Vector3(0, 1, 0), 2)); //****************************************************************************** TEST(Rot3 , Invariants) { diff --git a/gtsam/slam/tests/testRotateFactor.cpp b/gtsam/slam/tests/testRotateFactor.cpp index afbae4f11..1283987d4 100644 --- a/gtsam/slam/tests/testRotateFactor.cpp +++ b/gtsam/slam/tests/testRotateFactor.cpp @@ -29,9 +29,9 @@ Rot3 iRc(cameraX, cameraY, cameraZ); // Now, let's create some rotations around IMU frame Unit3 p1(1, 0, 0), p2(0, 1, 0), p3(0, 0, 1); -Rot3 i1Ri2 = Rot3::Rodrigues(p1, 1), // -i2Ri3 = Rot3::Rodrigues(p2, 1), // -i3Ri4 = Rot3::Rodrigues(p3, 1); +Rot3 i1Ri2 = Rot3::AxisAngle(p1, 1), // +i2Ri3 = Rot3::AxisAngle(p2, 1), // +i3Ri4 = Rot3::AxisAngle(p3, 1); // The corresponding rotations in the camera frame Rot3 c1Zc2 = iRc.inverse() * i1Ri2 * iRc, // @@ -47,9 +47,9 @@ typedef noiseModel::Isotropic::shared_ptr Model; //************************************************************************* TEST (RotateFactor, checkMath) { - EXPECT(assert_equal(c1Zc2, Rot3::Rodrigues(z1, 1))); - EXPECT(assert_equal(c2Zc3, Rot3::Rodrigues(z2, 1))); - EXPECT(assert_equal(c3Zc4, Rot3::Rodrigues(z3, 1))); + EXPECT(assert_equal(c1Zc2, Rot3::AxisAngle(z1, 1))); + EXPECT(assert_equal(c2Zc3, Rot3::AxisAngle(z2, 1))); + EXPECT(assert_equal(c3Zc4, Rot3::AxisAngle(z3, 1))); } //************************************************************************* diff --git a/timing/timeRot3.cpp b/timing/timeRot3.cpp index e320dc925..5806149f3 100644 --- a/timing/timeRot3.cpp +++ b/timing/timeRot3.cpp @@ -42,7 +42,7 @@ int main() Vector v = (Vector(3) << x, y, z).finished(); Rot3 R = Rot3::Rodrigues(0.1, 0.4, 0.2), R2 = R.retract(v); - TEST("Rodriguez formula given axis angle", Rot3::Rodrigues(v,0.001)) + TEST("Rodriguez formula given axis angle", Rot3::AxisAngle(v,0.001)) TEST("Rodriguez formula given canonical coordinates", Rot3::Rodrigues(v)) TEST("Expmap", R*Rot3::Expmap(v)) TEST("Retract", R.retract(v))