From 6eece9cc603cf20ad9a40a777819ba5d172f3940 Mon Sep 17 00:00:00 2001 From: Frank Dellaert Date: Tue, 26 Jan 2016 23:57:34 -0800 Subject: [PATCH] Quaternion now also uppercase --- gtsam/geometry/Rot3.cpp | 2 +- gtsam/geometry/Rot3.h | 21 +++++++++------- gtsam/geometry/Rot3M.cpp | 6 ++--- gtsam/geometry/Rot3Q.cpp | 18 ++++++------- gtsam/geometry/tests/testRot3.cpp | 4 +-- gtsam/nonlinear/tests/testAdaptAutoDiff.cpp | 2 +- gtsam/slam/dataset.cpp | 4 +-- gtsam/slam/tests/testDataset.cpp | 28 ++++++++++----------- python/handwritten/geometry/Rot3.cpp | 6 ++--- 9 files changed, 47 insertions(+), 44 deletions(-) diff --git a/gtsam/geometry/Rot3.cpp b/gtsam/geometry/Rot3.cpp index 6b28f5125..ef384c3ef 100644 --- a/gtsam/geometry/Rot3.cpp +++ b/gtsam/geometry/Rot3.cpp @@ -123,7 +123,7 @@ Vector3 Rot3::rpy() const { /* ************************************************************************* */ Vector Rot3::quaternion() const { - Quaternion q = toQuaternion(); + gtsam::Quaternion q = toQuaternion(); Vector v(4); v(0) = q.w(); v(1) = q.x(); diff --git a/gtsam/geometry/Rot3.h b/gtsam/geometry/Rot3.h index 4dd582a77..5b7acb4be 100644 --- a/gtsam/geometry/Rot3.h +++ b/gtsam/geometry/Rot3.h @@ -59,7 +59,7 @@ namespace gtsam { #ifdef GTSAM_USE_QUATERNIONS /** Internal Eigen Quaternion */ - Quaternion quaternion_; + gtsam::Quaternion quaternion_; #else Matrix3 rot_; #endif @@ -166,8 +166,8 @@ namespace gtsam { 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) { - Quaternion q(w, x, y, z); + static Rot3 Quaternion(double w, double x, double y, double z) { + gtsam::Quaternion q(w, x, y, z); return Rot3(q); } @@ -179,7 +179,7 @@ namespace gtsam { */ static Rot3 AxisAngle(const Vector3& axis, double angle) { #ifdef GTSAM_USE_QUATERNIONS - return Quaternion(Eigen::AngleAxis(angle, axis)); + return gtsam::Quaternion(Eigen::AngleAxis(angle, axis)); #else return SO3::AxisAngle(axis,angle); #endif @@ -313,7 +313,7 @@ namespace gtsam { static Rot3 Expmap(const Vector3& v, OptionalJacobian<3,3> H = boost::none) { if(H) *H = Rot3::ExpmapDerivative(v); #ifdef GTSAM_USE_QUATERNIONS - return traits::Expmap(v); + return traits::Expmap(v); #else return traits::Expmap(v); #endif @@ -460,7 +460,7 @@ namespace gtsam { /** Compute the quaternion representation of this rotation. * @return The quaternion */ - Quaternion toQuaternion() const; + gtsam::Quaternion toQuaternion() const; /** * Converts to a generic matrix to allow for use with matlab @@ -479,6 +479,8 @@ namespace gtsam { GTSAM_EXPORT friend std::ostream &operator<<(std::ostream &os, const Rot3& p); /// @} + +#ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V4 /// @name Deprecated /// @{ static Rot3 rodriguez(const Vector3& axis, double angle) { return AxisAngle(axis, angle); } @@ -486,13 +488,14 @@ namespace gtsam { 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); } - /// @} - -#ifdef ALLOW_DEPRECATED_IN_GTSAM4 static Rot3 yaw (double t) { return Yaw(t); } static Rot3 pitch(double t) { return Pitch(t); } static Rot3 roll (double t) { return Roll(t); } static Rot3 ypr(double y, double p, double r) { return Ypr(r,p,y);} + static Rot3 quaternion(double w, double x, double y, double z) { + return Rot3::Quaternion q(w, x, y, z); + } + /// @} #endif private: diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index a7b654070..c3636000b 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -51,7 +51,7 @@ Rot3::Rot3(double R11, double R12, double R13, } /* ************************************************************************* */ -Rot3::Rot3(const Quaternion& q) : rot_(q.toRotationMatrix()) { +Rot3::Rot3(const gtsam::Quaternion& q) : rot_(q.toRotationMatrix()) { } /* ************************************************************************* */ @@ -191,8 +191,8 @@ Point3 Rot3::r2() const { return Point3(rot_.col(1)); } Point3 Rot3::r3() const { return Point3(rot_.col(2)); } /* ************************************************************************* */ -Quaternion Rot3::toQuaternion() const { - return Quaternion(rot_); +gtsam::Quaternion Rot3::toQuaternion() const { + return gtsam::Quaternion(rot_); } /* ************************************************************************* */ diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index 2497f6806..f8a01141b 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -47,30 +47,30 @@ namespace gtsam { R31, R32, R33).finished()) {} /* ************************************************************************* */ - Rot3::Rot3(const Quaternion& q) : + Rot3::Rot3(const gtsam::Quaternion& q) : quaternion_(q) { } /* ************************************************************************* */ Rot3 Rot3::Rx(double t) { - return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); + return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitX())); } /* ************************************************************************* */ Rot3 Rot3::Ry(double t) { - return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); + return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitY())); } /* ************************************************************************* */ Rot3 Rot3::Rz(double t) { - return Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); + return gtsam::Quaternion(Eigen::AngleAxisd(t, Eigen::Vector3d::UnitZ())); } /* ************************************************************************* */ Rot3 Rot3::RzRyRx(double x, double y, double z) { return Rot3( - Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) * - Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) * - Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); + gtsam::Quaternion(Eigen::AngleAxisd(z, Eigen::Vector3d::UnitZ())) * + gtsam::Quaternion(Eigen::AngleAxisd(y, Eigen::Vector3d::UnitY())) * + gtsam::Quaternion(Eigen::AngleAxisd(x, Eigen::Vector3d::UnitX()))); } /* ************************************************************************* */ @@ -98,7 +98,7 @@ namespace gtsam { /* ************************************************************************* */ Vector3 Rot3::Logmap(const Rot3& R, OptionalJacobian<3, 3> H) { - return traits::Logmap(R.quaternion_, H); + return traits::Logmap(R.quaternion_, H); } /* ************************************************************************* */ @@ -128,7 +128,7 @@ namespace gtsam { Point3 Rot3::r3() const { return Point3(quaternion_.toRotationMatrix().col(2)); } /* ************************************************************************* */ - Quaternion Rot3::toQuaternion() const { return quaternion_; } + gtsam::Quaternion Rot3::toQuaternion() const { return quaternion_; } /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index 296bd2b7c..3cf3f9387 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -594,9 +594,9 @@ TEST(Rot3, quaternion) { // Check creating Rot3 from quaternion EXPECT(assert_equal(R1, Rot3(q1))); - EXPECT(assert_equal(R1, Rot3::quaternion(q1.w(), q1.x(), q1.y(), q1.z()))); + EXPECT(assert_equal(R1, Rot3::Quaternion(q1.w(), q1.x(), q1.y(), q1.z()))); EXPECT(assert_equal(R2, Rot3(q2))); - EXPECT(assert_equal(R2, Rot3::quaternion(q2.w(), q2.x(), q2.y(), q2.z()))); + EXPECT(assert_equal(R2, Rot3::Quaternion(q2.w(), q2.x(), q2.y(), q2.z()))); // Check converting Rot3 to quaterion EXPECT(assert_equal(Vector(R1.toQuaternion().coeffs()), Vector(q1.coeffs()))); diff --git a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp index 377d6cd34..16117845e 100644 --- a/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp +++ b/gtsam/nonlinear/tests/testAdaptAutoDiff.cpp @@ -62,7 +62,7 @@ using namespace gtsam; // Check that ceres rotation convention is the same TEST(AdaptAutoDiff, Rotation) { Vector3 axisAngle(0.1, 0.2, 0.3); - Matrix3 expected = Rot3::rodriguez(axisAngle).matrix(); + Matrix3 expected = Rot3::Rodrigues(axisAngle).matrix(); Matrix3 actual; ceres::AngleAxisToRotationMatrix(axisAngle.data(), actual.data()); EXPECT(assert_equal(expected, actual)); diff --git a/gtsam/slam/dataset.cpp b/gtsam/slam/dataset.cpp index 9e71e3753..50516afe4 100644 --- a/gtsam/slam/dataset.cpp +++ b/gtsam/slam/dataset.cpp @@ -518,7 +518,7 @@ GraphAndValues load3D(const string& filename) { Key id; double x, y, z, qx, qy, qz, qw; ls >> id >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::quaternion(qw, qx, qy, qz); + Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); Point3 t = Point3(x, y, z); initial->insert(id, Pose3(R,t)); } @@ -553,7 +553,7 @@ GraphAndValues load3D(const string& filename) { Key id1, id2; double x, y, z, qx, qy, qz, qw; ls >> id1 >> id2 >> x >> y >> z >> qx >> qy >> qz >> qw; - Rot3 R = Rot3::quaternion(qw, qx, qy, qz); + Rot3 R = Rot3::Quaternion(qw, qx, qy, qz); Point3 t = Point3(x, y, z); for (int i = 0; i < 6; i++){ for (int j = i; j < 6; j++){ diff --git a/gtsam/slam/tests/testDataset.cpp b/gtsam/slam/tests/testDataset.cpp index 26a00360e..4e8e3dbf3 100644 --- a/gtsam/slam/tests/testDataset.cpp +++ b/gtsam/slam/tests/testDataset.cpp @@ -137,23 +137,23 @@ TEST( dataSet, readG2o3D) boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); Values expectedValues; - Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); + Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); Point3 p0 = Point3(0.000000, 0.000000, 0.000000); expectedValues.insert(0, Pose3(R0, p0)); - Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); Point3 p1 = Point3(1.001367, 0.015390, 0.004948); expectedValues.insert(1, Pose3(R1, p1)); - Rot3 R2 = Rot3::quaternion(0.421446, -0.351729, -0.597838, 0.584174 ); + Rot3 R2 = Rot3::Quaternion(0.421446, -0.351729, -0.597838, 0.584174 ); Point3 p2 = Point3(1.993500, 0.023275, 0.003793); expectedValues.insert(2, Pose3(R2, p2)); - Rot3 R3 = Rot3::quaternion(0.067024, 0.331798, -0.200659, 0.919323); + Rot3 R3 = Rot3::Quaternion(0.067024, 0.331798, -0.200659, 0.919323); Point3 p3 = Point3(2.004291, 1.024305, 0.018047); expectedValues.insert(3, Pose3(R3, p3)); - Rot3 R4 = Rot3::quaternion(0.765488, -0.035697, -0.462490, 0.445933); + Rot3 R4 = Rot3::Quaternion(0.765488, -0.035697, -0.462490, 0.445933); Point3 p4 = Point3(0.999908, 1.055073, 0.020212); expectedValues.insert(4, Pose3(R4, p4)); @@ -163,27 +163,27 @@ TEST( dataSet, readG2o3D) NonlinearFactorGraph expectedGraph; Point3 p01 = Point3(1.001367, 0.015390, 0.004948); - Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); Point3 p12 = Point3(0.523923, 0.776654, 0.326659); - Rot3 R12 = Rot3::quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); + Rot3 R12 = Rot3::Quaternion(0.105373 , 0.311512, 0.656877, -0.678505 ); expectedGraph.add(BetweenFactor(1, 2, Pose3(R12,p12), model)); Point3 p23 = Point3(0.910927, 0.055169, -0.411761); - Rot3 R23 = Rot3::quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); + Rot3 R23 = Rot3::Quaternion(0.568551 , 0.595795, -0.561677, 0.079353 ); expectedGraph.add(BetweenFactor(2, 3, Pose3(R23,p23), model)); Point3 p34 = Point3(0.775288, 0.228798, -0.596923); - Rot3 R34 = Rot3::quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); + Rot3 R34 = Rot3::Quaternion(0.542221 , -0.592077, 0.303380, -0.513226 ); expectedGraph.add(BetweenFactor(3, 4, Pose3(R34,p34), model)); Point3 p14 = Point3(-0.577841, 0.628016, -0.543592); - Rot3 R14 = Rot3::quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); + Rot3 R14 = Rot3::Quaternion(0.327419 , -0.125250, -0.534379, 0.769122 ); expectedGraph.add(BetweenFactor(1, 4, Pose3(R14,p14), model)); Point3 p30 = Point3(-0.623267, 0.086928, 0.773222); - Rot3 R30 = Rot3::quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); + Rot3 R30 = Rot3::Quaternion(0.083672 , 0.104639, 0.627755, 0.766795 ); expectedGraph.add(BetweenFactor(3, 0, Pose3(R30,p30), model)); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-5)); @@ -199,11 +199,11 @@ TEST( dataSet, readG2o3DNonDiagonalNoise) boost::tie(actualGraph, actualValues) = readG2o(g2oFile, is3D); Values expectedValues; - Rot3 R0 = Rot3::quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); + Rot3 R0 = Rot3::Quaternion(1.000000, 0.000000, 0.000000, 0.000000 ); Point3 p0 = Point3(0.000000, 0.000000, 0.000000); expectedValues.insert(0, Pose3(R0, p0)); - Rot3 R1 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Rot3 R1 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); Point3 p1 = Point3(1.001367, 0.015390, 0.004948); expectedValues.insert(1, Pose3(R1, p1)); @@ -223,7 +223,7 @@ TEST( dataSet, readG2o3DNonDiagonalNoise) noiseModel::Gaussian::shared_ptr model = noiseModel::Gaussian::Covariance(Info.inverse()); NonlinearFactorGraph expectedGraph; Point3 p01 = Point3(1.001367, 0.015390, 0.004948); - Rot3 R01 = Rot3::quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); + Rot3 R01 = Rot3::Quaternion(0.854230, 0.190253, 0.283162, -0.392318 ); expectedGraph.add(BetweenFactor(0, 1, Pose3(R01,p01), model)); EXPECT(assert_equal(expectedGraph,*actualGraph,1e-2)); diff --git a/python/handwritten/geometry/Rot3.cpp b/python/handwritten/geometry/Rot3.cpp index b4551ca5c..68643fe2c 100644 --- a/python/handwritten/geometry/Rot3.cpp +++ b/python/handwritten/geometry/Rot3.cpp @@ -27,12 +27,12 @@ using namespace gtsam; static Rot3 Quaternion_0(const Vector4& q) { - return Rot3::quaternion(q[0],q[1],q[2],q[3]); + return Rot3::Quaternion(q[0],q[1],q[2],q[3]); } static Rot3 Quaternion_1(double w, double x, double y, double z) { - return Rot3::quaternion(w,x,y,z); + return Rot3::Quaternion(w,x,y,z); } // Prototypes used to perform overloading @@ -108,4 +108,4 @@ void exportRot3(){ .def(repr(self)) // __repr__ ; -} \ No newline at end of file +}