From 19e34489cfab60981b3b4b700a8c88140963de15 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 23 Oct 2014 01:54:14 +0200 Subject: [PATCH 1/5] Made a common test --- gtsam/geometry/tests/testRot3.cpp | 583 +++++++++++++++++++++++++++++ gtsam/geometry/tests/testRot3M.cpp | 543 +-------------------------- gtsam/geometry/tests/testRot3Q.cpp | 474 +---------------------- 3 files changed, 599 insertions(+), 1001 deletions(-) create mode 100644 gtsam/geometry/tests/testRot3.cpp diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp new file mode 100644 index 000000000..b495a4fb2 --- /dev/null +++ b/gtsam/geometry/tests/testRot3.cpp @@ -0,0 +1,583 @@ +/* ---------------------------------------------------------------------------- + + * GTSAM Copyright 2010, Georgia Tech Research Corporation, + * Atlanta, Georgia 30332-0415 + * All Rights Reserved + * Authors: Frank Dellaert, et al. (see THANKS for the full author list) + + * See LICENSE for the license information + + * -------------------------------------------------------------------------- */ + +/** + * @file testRot3.cpp + * @brief Unit tests for Rot3 class - common between Matrix and Quaternion + * @author Alireza Fathi + * @author Frank Dellaert + */ + +#include +#include + +#include +#include +#include + +#include + +#include + +using namespace std; +using namespace gtsam; + +GTSAM_CONCEPT_TESTABLE_INST(Rot3) +GTSAM_CONCEPT_LIE_INST(Rot3) + +static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); +static Point3 P(0.2, 0.7, -2.0); +static double error = 1e-9, epsilon = 0.001; +static const Matrix I3 = eye(3); + +/* ************************************************************************* */ +TEST( Rot3, constructor) +{ + Rot3 expected(I3); + Point3 r1(1,0,0), r2(0,1,0), r3(0,0,1); + Rot3 actual(r1, r2, r3); + CHECK(assert_equal(actual,expected)); +} + +/* ************************************************************************* */ +TEST( Rot3, constructor2) +{ + Matrix R = (Matrix(3, 3) << 0, 1, 0, 1, 0, 0, 0, 0, -1); + Rot3 actual(R); + Rot3 expected(0, 1, 0, 1, 0, 0, 0, 0, -1); + CHECK(assert_equal(actual,expected)); +} + +/* ************************************************************************* */ +TEST( Rot3, constructor3) +{ + Rot3 expected(0, 1, 0, 1, 0, 0, 0, 0, -1); + Point3 r1(0,1,0), r2(1,0,0), r3(0,0,-1); + CHECK(assert_equal(expected,Rot3(r1,r2,r3))); +} + +/* ************************************************************************* */ +TEST( Rot3, transpose) +{ + Point3 r1(0,1,0), r2(1,0,0), r3(0,0,-1); + Rot3 R(0, 1, 0, 1, 0, 0, 0, 0, -1); + CHECK(assert_equal(R.inverse(),Rot3(r1,r2,r3))); +} + +/* ************************************************************************* */ +TEST( Rot3, equals) +{ + CHECK(R.equals(R)); + Rot3 zero; + CHECK(!R.equals(zero)); +} + +/* ************************************************************************* */ +// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + .... +Rot3 slow_but_correct_rodriguez(const Vector& w) { + double t = norm_2(w); + Matrix J = skewSymmetric(w / t); + if (t < 1e-5) return Rot3(); + Matrix R = I3 + sin(t) * J + (1.0 - cos(t)) * (J * J); + return R; +} + +/* ************************************************************************* */ +TEST( Rot3, rodriguez) +{ + Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); + Vector w = (Vector(3) << epsilon, 0., 0.); + Rot3 R2 = slow_but_correct_rodriguez(w); + CHECK(assert_equal(R2,R1)); +} + +/* ************************************************************************* */ +TEST( Rot3, rodriguez2) +{ + Vector axis = (Vector(3) << 0., 1., 0.); // rotation around Y + double angle = 3.14 / 4.0; + Rot3 actual = Rot3::rodriguez(axis, angle); + Rot3 expected(0.707388, 0, 0.706825, + 0, 1, 0, + -0.706825, 0, 0.707388); + CHECK(assert_equal(expected,actual,1e-5)); +} + +/* ************************************************************************* */ +TEST( Rot3, rodriguez3) +{ + Vector w = (Vector(3) << 0.1, 0.2, 0.3); + Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w)); + Rot3 R2 = slow_but_correct_rodriguez(w); + CHECK(assert_equal(R2,R1)); +} + +/* ************************************************************************* */ +TEST( Rot3, rodriguez4) +{ + Vector axis = (Vector(3) << 0., 0., 1.); // rotation around Z + double angle = M_PI/2.0; + Rot3 actual = Rot3::rodriguez(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,1e-5)); + CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual,1e-5)); +} + +/* ************************************************************************* */ +TEST( Rot3, retract) +{ + Vector v = zero(3); + CHECK(assert_equal(R, R.retract(v))); + + // test Canonical coordinates + Canonical chart; + Vector v2 = chart.apply(R); + CHECK(assert_equal(R, chart.retract(v2))); +} + +/* ************************************************************************* */ +TEST(Rot3, log) +{ + static const double PI = boost::math::constants::pi(); + Vector w; + Rot3 R; + +#define CHECK_OMEGA(X,Y,Z) \ + w = (Vector(3) << (double)X, (double)Y, double(Z)); \ + R = Rot3::rodriguez(w); \ + EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12)); + + // Check zero + CHECK_OMEGA( 0, 0, 0) + + // create a random direction: + double norm=sqrt(1.0+16.0+4.0); + double x=1.0/norm, y=4.0/norm, z=2.0/norm; + + // Check very small rotation for Taylor expansion + // Note that tolerance above is 1e-12, so Taylor is pretty good ! + double d = 0.0001; + CHECK_OMEGA( d, 0, 0) + CHECK_OMEGA( 0, d, 0) + CHECK_OMEGA( 0, 0, d) + CHECK_OMEGA(x*d, y*d, z*d) + + // check normal rotation + d = 0.1; + CHECK_OMEGA( d, 0, 0) + CHECK_OMEGA( 0, d, 0) + CHECK_OMEGA( 0, 0, d) + CHECK_OMEGA(x*d, y*d, z*d) + + // Check 180 degree rotations + CHECK_OMEGA( PI, 0, 0) + CHECK_OMEGA( 0, PI, 0) + CHECK_OMEGA( 0, 0, PI) + CHECK_OMEGA(x*PI,y*PI,z*PI) + + // Check 360 degree rotations +#define CHECK_OMEGA_ZERO(X,Y,Z) \ + w = (Vector(3) << (double)X, (double)Y, double(Z)); \ + R = Rot3::rodriguez(w); \ + EXPECT(assert_equal(zero(3), Rot3::Logmap(R))); + + CHECK_OMEGA_ZERO( 2.0*PI, 0, 0) + CHECK_OMEGA_ZERO( 0, 2.0*PI, 0) + CHECK_OMEGA_ZERO( 0, 0, 2.0*PI) + CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI) +} + +Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ + return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); +} + +/* ************************************************************************* */ +TEST( Rot3, rightJacobianExpMapSO3 ) +{ + // Linearization point + Vector3 thetahat; thetahat << 0.1, 0, 0; + + Matrix expectedJacobian = numericalDerivative11( + boost::bind(&Rot3::Expmap, _1), thetahat); + Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat); + CHECK(assert_equal(expectedJacobian, actualJacobian)); +} + +/* ************************************************************************* */ +TEST( Rot3, rightJacobianExpMapSO3inverse ) +{ + // Linearization point + Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias + Vector3 deltatheta; deltatheta << 0, 0, 0; + + Matrix expectedJacobian = numericalDerivative11( + boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta); + Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat); + EXPECT(assert_equal(expectedJacobian, actualJacobian)); +} + +/* ************************************************************************* */ +TEST(Rot3, manifold_expmap) +{ + Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); + Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); + Rot3 origin; + + // log behaves correctly + Vector d12 = gR1.localCoordinates(gR2, Rot3::EXPMAP); + CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::EXPMAP))); + Vector d21 = gR2.localCoordinates(gR1, Rot3::EXPMAP); + CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::EXPMAP))); + + // Check that it is expmap + CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12))); + CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21))); + + // Check that log(t1,t2)=-log(t2,t1) + CHECK(assert_equal(d12,-d21)); + + // lines in canonical coordinates correspond to Abelian subgroups in SO(3) + Vector d = (Vector(3) << 0.1, 0.2, 0.3); + // exp(-d)=inverse(exp(d)) + CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); + // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) + Rot3 R2 = Rot3::Expmap (2 * d); + Rot3 R3 = Rot3::Expmap (3 * d); + Rot3 R5 = Rot3::Expmap (5 * d); + CHECK(assert_equal(R5,R2*R3)); + CHECK(assert_equal(R5,R3*R2)); +} + +/* ************************************************************************* */ +class AngularVelocity: public Point3 { +public: + AngularVelocity(const Point3& p) : + Point3(p) { + } + AngularVelocity(double wx, double wy, double wz) : + Point3(wx, wy, wz) { + } +}; + +AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) { + return X.cross(Y); +} + +/* ************************************************************************* */ +TEST(Rot3, BCH) +{ + // Approximate exmap by BCH formula + AngularVelocity w1(0.2, -0.1, 0.1); + AngularVelocity w2(0.01, 0.02, -0.03); + Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector()); + Rot3 R3 = R1 * R2; + Vector expected = Rot3::Logmap(R3); + Vector actual = BCH(w1, w2).vector(); + CHECK(assert_equal(expected, actual,1e-5)); +} + +/* ************************************************************************* */ +TEST( Rot3, rotate_derivatives) +{ + Matrix actualDrotate1a, actualDrotate1b, actualDrotate2; + R.rotate(P, actualDrotate1a, actualDrotate2); + R.inverse().rotate(P, actualDrotate1b, boost::none); + Matrix numerical1 = numericalDerivative21(testing::rotate, R, P); + Matrix numerical2 = numericalDerivative21(testing::rotate, R.inverse(), P); + Matrix numerical3 = numericalDerivative22(testing::rotate, R, P); + EXPECT(assert_equal(numerical1,actualDrotate1a,error)); + EXPECT(assert_equal(numerical2,actualDrotate1b,error)); + EXPECT(assert_equal(numerical3,actualDrotate2, error)); +} + +/* ************************************************************************* */ +TEST( Rot3, unrotate) +{ + Point3 w = R * P; + Matrix H1,H2; + Point3 actual = R.unrotate(w,H1,H2); + CHECK(assert_equal(P,actual)); + + Matrix numerical1 = numericalDerivative21(testing::unrotate, R, w); + CHECK(assert_equal(numerical1,H1,error)); + + Matrix numerical2 = numericalDerivative22(testing::unrotate, R, w); + CHECK(assert_equal(numerical2,H2,error)); +} + +/* ************************************************************************* */ +TEST( Rot3, compose ) +{ + Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); + Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); + + Rot3 expected = R1 * R2; + Matrix actualH1, actualH2; + Rot3 actual = R1.compose(R2, actualH1, actualH2); + CHECK(assert_equal(expected,actual)); + + Matrix numericalH1 = numericalDerivative21(testing::compose, R1, + R2, 1e-2); + CHECK(assert_equal(numericalH1,actualH1)); + + Matrix numericalH2 = numericalDerivative22(testing::compose, R1, + R2, 1e-2); + CHECK(assert_equal(numericalH2,actualH2)); +} + +/* ************************************************************************* */ +TEST( Rot3, inverse ) +{ + Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); + + Rot3 I; + Matrix actualH; + 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())); + + Matrix numericalH = numericalDerivative11(testing::inverse, R); + CHECK(assert_equal(numericalH,actualH)); +} + +/* ************************************************************************* */ +TEST( Rot3, between ) +{ + Rot3 r1 = Rot3::Rz(M_PI/3.0); + Rot3 r2 = Rot3::Rz(2.0*M_PI/3.0); + + Matrix expectedr1 = (Matrix(3, 3) << + 0.5, -sqrt(3.0)/2.0, 0.0, + sqrt(3.0)/2.0, 0.5, 0.0, + 0.0, 0.0, 1.0); + EXPECT(assert_equal(expectedr1, r1.matrix())); + + Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); + Rot3 origin; + EXPECT(assert_equal(R, origin.between(R))); + EXPECT(assert_equal(R.inverse(), R.between(origin))); + + Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); + Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); + + Rot3 expected = R1.inverse() * R2; + Matrix actualH1, actualH2; + Rot3 actual = R1.between(R2, actualH1, actualH2); + EXPECT(assert_equal(expected,actual)); + + Matrix numericalH1 = numericalDerivative21(testing::between , R1, R2); + CHECK(assert_equal(numericalH1,actualH1, 1e-4)); + + Matrix numericalH2 = numericalDerivative22(testing::between , R1, R2); + CHECK(assert_equal(numericalH2,actualH2, 1e-4)); +} + +/* ************************************************************************* */ +Vector w = (Vector(3) << 0.1, 0.27, -0.2); + +// Left trivialization Derivative of exp(w) wrpt w: +// How does exp(w) change when w changes? +// We find a y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 +// => y = log (exp(-w) * exp(w+dw)) +Vector3 testDexpL(const Vector3& dw) { + return Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); +} + +TEST( Rot3, dexpL) { + Matrix actualDexpL = Rot3::dexpL(w); + Matrix expectedDexpL = numericalDerivative11(testDexpL, + Vector3::Zero(), 1e-2); + EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); + + Matrix actualDexpInvL = Rot3::dexpInvL(w); + EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); +} + +/* ************************************************************************* */ +TEST( Rot3, xyz ) +{ + double t = 0.1, st = sin(t), ct = cos(t); + + // Make sure all counterclockwise + // Diagrams below are all from from unchanging axis + + // z + // | * Y=(ct,st) + // x----y + Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct); + CHECK(assert_equal(expected1,Rot3::Rx(t))); + + // x + // | * Z=(ct,st) + // y----z + Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct); + CHECK(assert_equal(expected2,Rot3::Ry(t))); + + // y + // | X=* (ct,st) + // z----x + Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1); + CHECK(assert_equal(expected3,Rot3::Rz(t))); + + // Check compound rotation + Rot3 expected = Rot3::Rz(0.3) * Rot3::Ry(0.2) * Rot3::Rx(0.1); + CHECK(assert_equal(expected,Rot3::RzRyRx(0.1,0.2,0.3))); +} + +/* ************************************************************************* */ +TEST( Rot3, yaw_pitch_roll ) +{ + double t = 0.1; + + // yaw is around z axis + CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t))); + + // pitch is around y axis + CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t))); + + // roll is around x axis + CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t))); + + // Check compound rotation + Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3); + CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3))); + + CHECK(assert_equal((Vector)(Vector(3) << 0.1, 0.2, 0.3),expected.ypr())); +} + +/* ************************************************************************* */ +TEST( Rot3, RQ) +{ + // Try RQ on a pure rotation + Matrix actualK; + Vector actual; + boost::tie(actualK, actual) = RQ(R.matrix()); + Vector expected = (Vector(3) << 0.14715, 0.385821, 0.231671); + CHECK(assert_equal(I3,actualK)); + CHECK(assert_equal(expected,actual,1e-6)); + + // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] + CHECK(assert_equal(expected,R.xyz(),1e-6)); + CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); + + // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] + CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); + CHECK(assert_equal((Vector)(Vector(3) << 0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); + + // Try ypr for pure yaw-pitch-roll matrices + CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); + CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); + CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.0,0.1),Rot3::roll (0.1).ypr())); + + // Try RQ to recover calibration from 3*3 sub-block of projection matrix + Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0); + Matrix A = K * R.matrix(); + boost::tie(actualK, actual) = RQ(A); + CHECK(assert_equal(K,actualK)); + CHECK(assert_equal(expected,actual,1e-6)); +} + +/* ************************************************************************* */ +TEST( Rot3, expmapStability ) { + Vector w = (Vector(3) << 78e-9, 5e-8, 97e-7); + double theta = w.norm(); + double theta2 = theta*theta; + Rot3 actualR = Rot3::Expmap(w); + Matrix W = (Matrix(3, 3) << 0.0, -w(2), w(1), + w(2), 0.0, -w(0), + -w(1), w(0), 0.0 ); + Matrix W2 = W*W; + Matrix Rmat = I3 + (1.0-theta2/6.0 + theta2*theta2/120.0 + - theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ; + Rot3 expectedR( Rmat ); + CHECK(assert_equal(expectedR, actualR, 1e-10)); +} + +/* ************************************************************************* */ +TEST( Rot3, logmapStability ) { + Vector w = (Vector(3) << 1e-8, 0.0, 0.0); + Rot3 R = Rot3::Expmap(w); +// double tr = R.r1().x()+R.r2().y()+R.r3().z(); +// std::cout.precision(5000); +// std::cout << "theta: " << w.norm() << std::endl; +// std::cout << "trace: " << tr << std::endl; +// R.print("R = "); + Vector actualw = Rot3::Logmap(R); + CHECK(assert_equal(w, actualw, 1e-15)); // this should be fixed for Quaternions!!! +} + +/* ************************************************************************* */ +TEST(Rot3, quaternion) { + // NOTE: This is also verifying the ability to convert Vector to Quaternion + Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782); + Rot3 R1 = Rot3((Matrix)(Matrix(3, 3) << + 0.271018623057411, 0.278786459830371, 0.921318086098018, + 0.578529366719085, 0.717799701969298, -0.387385285854279, + -0.769319620053772, 0.637998195662053, 0.033250932803219)); + + Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053); + Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) << + -0.207341903877828, 0.250149415542075, 0.945745528564780, + 0.881304914479026, -0.371869043667957, 0.291573424846290, + 0.424630407073532, 0.893945571198514, -0.143353873763946)); + + // 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(R2, Rot3(q2))); + 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()))); + EXPECT(assert_equal(Vector(R2.toQuaternion().coeffs()), Vector(q2.coeffs()))); + + // Check that quaternion and Rot3 represent the same rotation + Point3 p1(1.0, 2.0, 3.0); + Point3 p2(8.0, 7.0, 9.0); + + Point3 expected1 = R1*p1; + Point3 expected2 = R2*p2; + + Point3 actual1 = Point3(q1*p1.vector()); + Point3 actual2 = Point3(q2*p2.vector()); + + EXPECT(assert_equal(expected1, actual1)); + EXPECT(assert_equal(expected2, actual2)); +} + +/* ************************************************************************* */ +TEST( Rot3, Cayley ) { + Matrix A = skewSymmetric(1,2,-3); + Matrix Q = Cayley(A); + EXPECT(assert_equal(I3, trans(Q)*Q)); + EXPECT(assert_equal(A, Cayley(Q))); +} + +/* ************************************************************************* */ +TEST( Rot3, stream) +{ + Rot3 R; + std::ostringstream os; + os << R; + EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n"); +} + +/* ************************************************************************* */ +int main() { + TestResult tr; + return TestRegistry::runAllTests(tr); +} +/* ************************************************************************* */ + diff --git a/gtsam/geometry/tests/testRot3M.cpp b/gtsam/geometry/tests/testRot3M.cpp index f0e60ba03..12fb9bdad 100644 --- a/gtsam/geometry/tests/testRot3M.cpp +++ b/gtsam/geometry/tests/testRot3M.cpp @@ -13,15 +13,16 @@ * @file testRot3.cpp * @brief Unit tests for Rot3 class * @author Alireza Fathi + * @author Frank Dellaert */ +#include +#include + #include #include #include -#include -#include - #include #include @@ -37,202 +38,7 @@ static double error = 1e-9, epsilon = 0.001; static const Matrix I3 = eye(3); /* ************************************************************************* */ -TEST( Rot3, constructor) -{ - Rot3 expected(I3); - Vector r1(3), r2(3), r3(3); - r1(0) = 1; - r1(1) = 0; - r1(2) = 0; - r2(0) = 0; - r2(1) = 1; - r2(2) = 0; - r3(0) = 0; - r3(1) = 0; - r3(2) = 1; - Rot3 actual(r1, r2, r3); - CHECK(assert_equal(actual,expected)); -} - -/* ************************************************************************* */ -TEST( Rot3, constructor2) -{ - Matrix R = (Matrix(3, 3) << 11., 12., 13., 21., 22., 23., 31., 32., 33.); - Rot3 actual(R); - Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33); - CHECK(assert_equal(actual,expected)); -} - -/* ************************************************************************* */ -TEST( Rot3, constructor3) -{ - Rot3 expected(1, 2, 3, 4, 5, 6, 7, 8, 9); - Point3 r1(1, 4, 7), r2(2, 5, 8), r3(3, 6, 9); - CHECK(assert_equal(Rot3(r1,r2,r3),expected)); -} - -/* ************************************************************************* */ -TEST( Rot3, transpose) -{ - Rot3 R(1, 2, 3, 4, 5, 6, 7, 8, 9); - Point3 r1(1, 2, 3), r2(4, 5, 6), r3(7, 8, 9); - CHECK(assert_equal(R.inverse(),Rot3(r1,r2,r3))); -} - -/* ************************************************************************* */ -TEST( Rot3, equals) -{ - CHECK(R.equals(R)); - Rot3 zero; - CHECK(!R.equals(zero)); -} - -/* ************************************************************************* */ -// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + .... -Rot3 slow_but_correct_rodriguez(const Vector& w) { - double t = norm_2(w); - Matrix J = skewSymmetric(w / t); - if (t < 1e-5) return Rot3(); - Matrix R = I3 + sin(t) * J + (1.0 - cos(t)) * (J * J); - return R; -} - -/* ************************************************************************* */ -TEST( Rot3, rodriguez) -{ - Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); - Vector w = (Vector(3) << epsilon, 0., 0.); - Rot3 R2 = slow_but_correct_rodriguez(w); - CHECK(assert_equal(R2,R1)); -} - -/* ************************************************************************* */ -TEST( Rot3, rodriguez2) -{ - Vector axis = (Vector(3) << 0., 1., 0.); // rotation around Y - double angle = 3.14 / 4.0; - Rot3 actual = Rot3::rodriguez(axis, angle); - Rot3 expected(0.707388, 0, 0.706825, - 0, 1, 0, - -0.706825, 0, 0.707388); - CHECK(assert_equal(expected,actual,1e-5)); -} - -/* ************************************************************************* */ -TEST( Rot3, rodriguez3) -{ - Vector w = (Vector(3) << 0.1, 0.2, 0.3); - Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w)); - Rot3 R2 = slow_but_correct_rodriguez(w); - CHECK(assert_equal(R2,R1)); -} - -/* ************************************************************************* */ -TEST( Rot3, rodriguez4) -{ - Vector axis = (Vector(3) << 0., 0., 1.); // rotation around Z - double angle = M_PI/2.0; - Rot3 actual = Rot3::rodriguez(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,1e-5)); - CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual,1e-5)); -} - -/* ************************************************************************* */ -TEST( Rot3, expmap) -{ - Vector v = zero(3); - CHECK(assert_equal(R.retract(v), R)); -} - -/* ************************************************************************* */ -TEST(Rot3, log) -{ - static const double PI = boost::math::constants::pi(); - Vector w; - Rot3 R; - -#define CHECK_OMEGA(X,Y,Z) \ - w = (Vector(3) << (double)X, (double)Y, double(Z)); \ - R = Rot3::rodriguez(w); \ - EXPECT(assert_equal(w, Rot3::Logmap(R),1e-12)); - - // Check zero - CHECK_OMEGA( 0, 0, 0) - - // create a random direction: - double norm=sqrt(1.0+16.0+4.0); - double x=1.0/norm, y=4.0/norm, z=2.0/norm; - - // Check very small rotation for Taylor expansion - // Note that tolerance above is 1e-12, so Taylor is pretty good ! - double d = 0.0001; - CHECK_OMEGA( d, 0, 0) - CHECK_OMEGA( 0, d, 0) - CHECK_OMEGA( 0, 0, d) - CHECK_OMEGA(x*d, y*d, z*d) - - // check normal rotation - d = 0.1; - CHECK_OMEGA( d, 0, 0) - CHECK_OMEGA( 0, d, 0) - CHECK_OMEGA( 0, 0, d) - CHECK_OMEGA(x*d, y*d, z*d) - - // Check 180 degree rotations - CHECK_OMEGA( PI, 0, 0) - CHECK_OMEGA( 0, PI, 0) - CHECK_OMEGA( 0, 0, PI) - CHECK_OMEGA(x*PI,y*PI,z*PI) - - // Check 360 degree rotations -#define CHECK_OMEGA_ZERO(X,Y,Z) \ - w = (Vector(3) << (double)X, (double)Y, double(Z)); \ - R = Rot3::rodriguez(w); \ - EXPECT(assert_equal(zero(3), Rot3::Logmap(R))); - - CHECK_OMEGA_ZERO( 2.0*PI, 0, 0) - CHECK_OMEGA_ZERO( 0, 2.0*PI, 0) - CHECK_OMEGA_ZERO( 0, 0, 2.0*PI) - CHECK_OMEGA_ZERO(x*2.*PI,y*2.*PI,z*2.*PI) -} - -Rot3 evaluateRotation(const Vector3 thetahat){ - return Rot3::Expmap(thetahat); -} - -Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ - return Rot3::Logmap( Rot3::Expmap(thetahat).compose( Rot3::Expmap(deltatheta) ) ); -} - -/* ************************************************************************* */ -TEST( Rot3, rightJacobianExpMapSO3 ) -{ - // Linearization point - Vector3 thetahat; thetahat << 0.1, 0, 0; - - Matrix expectedJacobian = numericalDerivative11(boost::bind(&evaluateRotation, _1), LieVector(thetahat)); - Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat); - EXPECT(assert_equal(expectedJacobian, actualJacobian)); -} - -/* ************************************************************************* */ -TEST( Rot3, rightJacobianExpMapSO3inverse ) -{ - // Linearization point - Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias - Vector3 deltatheta; deltatheta << 0, 0, 0; - - Matrix expectedJacobian = numericalDerivative11(boost::bind(&evaluateLogRotation, thetahat, _1), LieVector(deltatheta)); - Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat); - EXPECT(assert_equal(expectedJacobian, actualJacobian)); -} - -/* ************************************************************************* */ -TEST(Rot3, manifold_caley) +TEST(Rot3, manifold_cayley) { Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); @@ -260,7 +66,7 @@ TEST(Rot3, manifold_caley) } /* ************************************************************************* */ -TEST(Rot3, manifold_slow_caley) +TEST(Rot3, manifold_slow_cayley) { Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); @@ -287,343 +93,6 @@ TEST(Rot3, manifold_slow_caley) CHECK(assert_equal(R5,R3*R2)); } -/* ************************************************************************* */ -TEST(Rot3, manifold_expmap) -{ - Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); - Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); - Rot3 origin; - - // log behaves correctly - Vector d12 = gR1.localCoordinates(gR2, Rot3::EXPMAP); - CHECK(assert_equal(gR2, gR1.retract(d12, Rot3::EXPMAP))); - Vector d21 = gR2.localCoordinates(gR1, Rot3::EXPMAP); - CHECK(assert_equal(gR1, gR2.retract(d21, Rot3::EXPMAP))); - - // Check that it is expmap - CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12))); - CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21))); - - // Check that log(t1,t2)=-log(t2,t1) - CHECK(assert_equal(d12,-d21)); - - // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = (Vector(3) << 0.1, 0.2, 0.3); - // exp(-d)=inverse(exp(d)) - CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); - // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) - Rot3 R2 = Rot3::Expmap (2 * d); - Rot3 R3 = Rot3::Expmap (3 * d); - Rot3 R5 = Rot3::Expmap (5 * d); - CHECK(assert_equal(R5,R2*R3)); - CHECK(assert_equal(R5,R3*R2)); -} - -/* ************************************************************************* */ -class AngularVelocity: public Point3 { -public: - AngularVelocity(const Point3& p) : - Point3(p) { - } - AngularVelocity(double wx, double wy, double wz) : - Point3(wx, wy, wz) { - } -}; - -AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) { - return X.cross(Y); -} - -/* ************************************************************************* */ -TEST(Rot3, BCH) -{ - // Approximate exmap by BCH formula - AngularVelocity w1(0.2, -0.1, 0.1); - AngularVelocity w2(0.01, 0.02, -0.03); - Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector()); - Rot3 R3 = R1 * R2; - Vector expected = Rot3::Logmap(R3); - Vector actual = BCH(w1, w2).vector(); - CHECK(assert_equal(expected, actual,1e-5)); -} - -/* ************************************************************************* */ -TEST( Rot3, rotate_derivatives) -{ - Matrix actualDrotate1a, actualDrotate1b, actualDrotate2; - R.rotate(P, actualDrotate1a, actualDrotate2); - R.inverse().rotate(P, actualDrotate1b, boost::none); - Matrix numerical1 = numericalDerivative21(testing::rotate, R, P); - Matrix numerical2 = numericalDerivative21(testing::rotate, R.inverse(), P); - Matrix numerical3 = numericalDerivative22(testing::rotate, R, P); - EXPECT(assert_equal(numerical1,actualDrotate1a,error)); - EXPECT(assert_equal(numerical2,actualDrotate1b,error)); - EXPECT(assert_equal(numerical3,actualDrotate2, error)); -} - -/* ************************************************************************* */ -TEST( Rot3, unrotate) -{ - Point3 w = R * P; - Matrix H1,H2; - Point3 actual = R.unrotate(w,H1,H2); - CHECK(assert_equal(P,actual)); - - Matrix numerical1 = numericalDerivative21(testing::unrotate, R, w); - CHECK(assert_equal(numerical1,H1,error)); - - Matrix numerical2 = numericalDerivative22(testing::unrotate, R, w); - CHECK(assert_equal(numerical2,H2,error)); -} - -/* ************************************************************************* */ -TEST( Rot3, compose ) -{ - Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); - Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); - - Rot3 expected = R1 * R2; - Matrix actualH1, actualH2; - Rot3 actual = R1.compose(R2, actualH1, actualH2); - CHECK(assert_equal(expected,actual)); - - Matrix numericalH1 = numericalDerivative21(testing::compose, R1, - R2, 1e-2); - CHECK(assert_equal(numericalH1,actualH1)); - - Matrix numericalH2 = numericalDerivative22(testing::compose, R1, - R2, 1e-2); - CHECK(assert_equal(numericalH2,actualH2)); -} - -/* ************************************************************************* */ -TEST( Rot3, inverse ) -{ - Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); - - Rot3 I; - Matrix actualH; - CHECK(assert_equal(I,R*R.inverse(actualH))); - CHECK(assert_equal(I,R.inverse()*R)); - - Matrix numericalH = numericalDerivative11(testing::inverse, R); - CHECK(assert_equal(numericalH,actualH)); -} - -/* ************************************************************************* */ -TEST( Rot3, between ) -{ - Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); - Rot3 origin; - CHECK(assert_equal(R, origin.between(R))); - CHECK(assert_equal(R.inverse(), R.between(origin))); - - Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); - Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); - - Rot3 expected = R1.inverse() * R2; - Matrix actualH1, actualH2; - Rot3 actual = R1.between(R2, actualH1, actualH2); - CHECK(assert_equal(expected,actual)); - - Matrix numericalH1 = numericalDerivative21(testing::between , R1, R2); - CHECK(assert_equal(numericalH1,actualH1)); - - Matrix numericalH2 = numericalDerivative22(testing::between , R1, R2); - CHECK(assert_equal(numericalH2,actualH2)); -} - -/* ************************************************************************* */ -Vector w = (Vector(3) << 0.1, 0.27, -0.2); - -// Left trivialization Derivative of exp(w) over w: How exp(w) changes when w changes? -// We find y such that: exp(w) exp(y) = exp(w + dw) for dw --> 0 -// => y = log (exp(-w) * exp(w+dw)) -Vector testDexpL(const LieVector& dw) { - Vector y = Rot3::Logmap(Rot3::Expmap(-w) * Rot3::Expmap(w + dw)); - return y; -} - -TEST( Rot3, dexpL) { - Matrix actualDexpL = Rot3::dexpL(w); - Matrix expectedDexpL = numericalDerivative11( - boost::function( - boost::bind(testDexpL, _1)), LieVector(zero(3)), 1e-2); - EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); - - Matrix actualDexpInvL = Rot3::dexpInvL(w); - EXPECT(assert_equal(expectedDexpL.inverse(), actualDexpInvL, 1e-5)); -} - -/* ************************************************************************* */ -TEST( Rot3, xyz ) -{ - double t = 0.1, st = sin(t), ct = cos(t); - - // Make sure all counterclockwise - // Diagrams below are all from from unchanging axis - - // z - // | * Y=(ct,st) - // x----y - Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct); - CHECK(assert_equal(expected1,Rot3::Rx(t))); - - // x - // | * Z=(ct,st) - // y----z - Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct); - CHECK(assert_equal(expected2,Rot3::Ry(t))); - - // y - // | X=* (ct,st) - // z----x - Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1); - CHECK(assert_equal(expected3,Rot3::Rz(t))); - - // Check compound rotation - Rot3 expected = Rot3::Rz(0.3) * Rot3::Ry(0.2) * Rot3::Rx(0.1); - CHECK(assert_equal(expected,Rot3::RzRyRx(0.1,0.2,0.3))); -} - -/* ************************************************************************* */ -TEST( Rot3, yaw_pitch_roll ) -{ - double t = 0.1; - - // yaw is around z axis - CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t))); - - // pitch is around y axis - CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t))); - - // roll is around x axis - CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t))); - - // Check compound rotation - Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3); - CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3))); - - CHECK(assert_equal((Vector)(Vector(3) << 0.1, 0.2, 0.3),expected.ypr())); -} - -/* ************************************************************************* */ -TEST( Rot3, RQ) -{ - // Try RQ on a pure rotation - Matrix actualK; - Vector actual; - boost::tie(actualK, actual) = RQ(R.matrix()); - Vector expected = (Vector(3) << 0.14715, 0.385821, 0.231671); - CHECK(assert_equal(I3,actualK)); - CHECK(assert_equal(expected,actual,1e-6)); - - // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] - CHECK(assert_equal(expected,R.xyz(),1e-6)); - CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); - - // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] - CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); - CHECK(assert_equal((Vector)(Vector(3) << 0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); - - // Try ypr for pure yaw-pitch-roll matrices - CHECK(assert_equal((Vector)(Vector(3) << 0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); - CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); - CHECK(assert_equal((Vector)(Vector(3) << 0.0,0.0,0.1),Rot3::roll (0.1).ypr())); - - // Try RQ to recover calibration from 3*3 sub-block of projection matrix - Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0); - Matrix A = K * R.matrix(); - boost::tie(actualK, actual) = RQ(A); - CHECK(assert_equal(K,actualK)); - CHECK(assert_equal(expected,actual,1e-6)); -} - -/* ************************************************************************* */ -TEST( Rot3, expmapStability ) { - Vector w = (Vector(3) << 78e-9, 5e-8, 97e-7); - double theta = w.norm(); - double theta2 = theta*theta; - Rot3 actualR = Rot3::Expmap(w); - Matrix W = (Matrix(3, 3) << 0.0, -w(2), w(1), - w(2), 0.0, -w(0), - -w(1), w(0), 0.0 ); - Matrix W2 = W*W; - Matrix Rmat = I3 + (1.0-theta2/6.0 + theta2*theta2/120.0 - - theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ; - Rot3 expectedR( Rmat ); - CHECK(assert_equal(expectedR, actualR, 1e-10)); -} - -/* ************************************************************************* */ -TEST( Rot3, logmapStability ) { - Vector w = (Vector(3) << 1e-8, 0.0, 0.0); - Rot3 R = Rot3::Expmap(w); -// double tr = R.r1().x()+R.r2().y()+R.r3().z(); -// std::cout.precision(5000); -// std::cout << "theta: " << w.norm() << std::endl; -// std::cout << "trace: " << tr << std::endl; -// R.print("R = "); - Vector actualw = Rot3::Logmap(R); - CHECK(assert_equal(w, actualw, 1e-15)); -} - -/* ************************************************************************* */ -TEST(Rot3, quaternion) { - // NOTE: This is also verifying the ability to convert Vector to Quaternion - Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782); - Rot3 R1 = Rot3((Matrix)(Matrix(3, 3) << - 0.271018623057411, 0.278786459830371, 0.921318086098018, - 0.578529366719085, 0.717799701969298, -0.387385285854279, - -0.769319620053772, 0.637998195662053, 0.033250932803219)); - - Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053); - Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) << - -0.207341903877828, 0.250149415542075, 0.945745528564780, - 0.881304914479026, -0.371869043667957, 0.291573424846290, - 0.424630407073532, 0.893945571198514, -0.143353873763946)); - - // 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(R2, Rot3(q2))); - 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()))); - EXPECT(assert_equal(Vector(R2.toQuaternion().coeffs()), Vector(q2.coeffs()))); - - // Check that quaternion and Rot3 represent the same rotation - Point3 p1(1.0, 2.0, 3.0); - Point3 p2(8.0, 7.0, 9.0); - - Point3 expected1 = R1*p1; - Point3 expected2 = R2*p2; - - Point3 actual1 = Point3(q1*p1.vector()); - Point3 actual2 = Point3(q2*p2.vector()); - - EXPECT(assert_equal(expected1, actual1)); - EXPECT(assert_equal(expected2, actual2)); -} - -/* ************************************************************************* */ -TEST( Rot3, Cayley ) { - Matrix A = skewSymmetric(1,2,-3); - Matrix Q = Cayley(A); - EXPECT(assert_equal(I3, trans(Q)*Q)); - EXPECT(assert_equal(A, Cayley(Q))); -} - -/* ************************************************************************* */ -TEST( Rot3, stream) -{ - Rot3 R; - std::ostringstream os; - os << R; - EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n"); -} - #endif /* ************************************************************************* */ diff --git a/gtsam/geometry/tests/testRot3Q.cpp b/gtsam/geometry/tests/testRot3Q.cpp index 5db99e4e3..65ca9e067 100644 --- a/gtsam/geometry/tests/testRot3Q.cpp +++ b/gtsam/geometry/tests/testRot3Q.cpp @@ -11,478 +11,24 @@ /** * @file testRot3.cpp - * @brief Unit tests for Rot3 class + * @brief Unit tests for Rot3 class, Quaternion specific * @author Alireza Fathi */ -#include -#include -#include -#include -#include #include #include +#include +#include +#include + +#include + +#include + #ifdef GTSAM_USE_QUATERNIONS -using namespace gtsam; - -static Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); -static Point3 P(0.2, 0.7, -2.0); -static double error = 1e-9, epsilon = 0.001; - -/* ************************************************************************* */ -TEST( Rot3, constructor) -{ - Rot3 expected(eye(3, 3)); - Vector r1(3), r2(3), r3(3); - r1(0) = 1; - r1(1) = 0; - r1(2) = 0; - r2(0) = 0; - r2(1) = 1; - r2(2) = 0; - r3(0) = 0; - r3(1) = 0; - r3(2) = 1; - Rot3 actual(r1, r2, r3); - CHECK(assert_equal(actual,expected)); -} - -/* ************************************************************************* */ -TEST( Rot3, constructor2) -{ - Matrix R = (Matrix(3, 3) << 11., 12., 13., 21., 22., 23., 31., 32., 33.); - Rot3 actual(R); - Rot3 expected(11, 12, 13, 21, 22, 23, 31, 32, 33); - CHECK(assert_equal(actual,expected)); -} - -/* ************************************************************************* */ -TEST( Rot3, constructor3) -{ - Rot3 expected(1, 2, 3, 4, 5, 6, 7, 8, 9); - Point3 r1(1, 4, 7), r2(2, 5, 8), r3(3, 6, 9); - CHECK(assert_equal(Rot3(r1,r2,r3),expected)); -} - -/* ************************************************************************* */ -TEST( Rot3, equals) -{ - CHECK(R.equals(R)); - Rot3 zero; - CHECK(!R.equals(zero)); -} - -/* ************************************************************************* */ -// Notice this uses J^2 whereas fast uses w*w', and has cos(t)*I + .... -Rot3 slow_but_correct_rodriguez(const Vector& w) { - double t = norm_2(w); - Matrix J = skewSymmetric(w / t); - if (t < 1e-5) return Rot3(); - Matrix R = eye(3) + sin(t) * J + (1.0 - cos(t)) * (J * J); - return R; -} - -/* ************************************************************************* */ -TEST( Rot3, rodriguez) -{ - Rot3 R1 = Rot3::rodriguez(epsilon, 0, 0); - Vector w = (Vector(3) << epsilon, 0., 0.); - Rot3 R2 = slow_but_correct_rodriguez(w); - CHECK(assert_equal(R2,R1)); -} - -/* ************************************************************************* */ -TEST( Rot3, rodriguez2) -{ - Vector axis = (Vector(3) << 0.,1.,0.); // rotation around Y - double angle = 3.14 / 4.0; - Rot3 actual = Rot3::rodriguez(axis, angle); - Rot3 expected(0.707388, 0, 0.706825, - 0, 1, 0, - -0.706825, 0, 0.707388); - CHECK(assert_equal(expected,actual,1e-5)); -} - -/* ************************************************************************* */ -TEST( Rot3, rodriguez3) -{ - Vector w = (Vector(3) << 0.1, 0.2, 0.3); - Rot3 R1 = Rot3::rodriguez(w / norm_2(w), norm_2(w)); - Rot3 R2 = slow_but_correct_rodriguez(w); - CHECK(assert_equal(R2,R1)); -} - -/* ************************************************************************* */ -TEST( Rot3, rodriguez4) -{ - Vector axis = (Vector(3) << 0., 0., 1.); // rotation around Z - double angle = M_PI_2; - Rot3 actual = Rot3::rodriguez(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,1e-5)); - CHECK(assert_equal(slow_but_correct_rodriguez(axis*angle),actual,1e-5)); -} - -/* ************************************************************************* */ -TEST( Rot3, expmap) -{ - Vector v = zero(3); - CHECK(assert_equal(R.retract(v), R)); -} - -/* ************************************************************************* */ -TEST(Rot3, log) -{ - Vector w1 = (Vector(3) << 0.1, 0.0, 0.0); - Rot3 R1 = Rot3::rodriguez(w1); - CHECK(assert_equal(w1, Rot3::Logmap(R1))); - - Vector w2 = (Vector(3) << 0.0, 0.1, 0.0); - Rot3 R2 = Rot3::rodriguez(w2); - CHECK(assert_equal(w2, Rot3::Logmap(R2))); - - Vector w3 = (Vector(3) << 0.0, 0.0, 0.1); - Rot3 R3 = Rot3::rodriguez(w3); - CHECK(assert_equal(w3, Rot3::Logmap(R3))); - - Vector w = (Vector(3) << 0.1, 0.4, 0.2); - Rot3 R = Rot3::rodriguez(w); - CHECK(assert_equal(w, Rot3::Logmap(R))); - - Vector w5 = (Vector(3) << 0.0, 0.0, 0.0); - Rot3 R5 = Rot3::rodriguez(w5); - CHECK(assert_equal(w5, Rot3::Logmap(R5))); - - Vector w6 = (Vector(3) << boost::math::constants::pi(), 0.0, 0.0); - Rot3 R6 = Rot3::rodriguez(w6); - CHECK(assert_equal(w6, Rot3::Logmap(R6))); - - Vector w7 = (Vector(3) << 0.0, boost::math::constants::pi(), 0.0); - Rot3 R7 = Rot3::rodriguez(w7); - CHECK(assert_equal(w7, Rot3::Logmap(R7))); - - Vector w8 = (Vector(3) << 0.0, 0.0, boost::math::constants::pi()); - Rot3 R8 = Rot3::rodriguez(w8); - CHECK(assert_equal(w8, Rot3::Logmap(R8))); -} - -/* ************************************************************************* */ -TEST(Rot3, manifold) -{ - Rot3 gR1 = Rot3::rodriguez(0.1, 0.4, 0.2); - Rot3 gR2 = Rot3::rodriguez(0.3, 0.1, 0.7); - Rot3 origin; - - // log behaves correctly - Vector d12 = gR1.localCoordinates(gR2); - CHECK(assert_equal(gR2, gR1.retract(d12))); - CHECK(assert_equal(gR2, gR1*Rot3::Expmap(d12))); - Vector d21 = gR2.localCoordinates(gR1); - CHECK(assert_equal(gR1, gR2.retract(d21))); - CHECK(assert_equal(gR1, gR2*Rot3::Expmap(d21))); - - // Check that log(t1,t2)=-log(t2,t1) - CHECK(assert_equal(d12,-d21)); - - // lines in canonical coordinates correspond to Abelian subgroups in SO(3) - Vector d = (Vector(3) << 0.1, 0.2, 0.3); - // exp(-d)=inverse(exp(d)) - CHECK(assert_equal(Rot3::Expmap(-d),Rot3::Expmap(d).inverse())); - // exp(5d)=exp(2*d+3*d)=exp(2*d)exp(3*d)=exp(3*d)exp(2*d) - Rot3 R2 = Rot3::Expmap (2 * d); - Rot3 R3 = Rot3::Expmap (3 * d); - Rot3 R5 = Rot3::Expmap (5 * d); - CHECK(assert_equal(R5,R2*R3)); - CHECK(assert_equal(R5,R3*R2)); -} - -/* ************************************************************************* */ -class AngularVelocity: public Point3 { -public: - AngularVelocity(const Point3& p) : - Point3(p) { - } - AngularVelocity(double wx, double wy, double wz) : - Point3(wx, wy, wz) { - } -}; - -AngularVelocity bracket(const AngularVelocity& X, const AngularVelocity& Y) { - return X.cross(Y); -} - -/* ************************************************************************* */ -TEST(Rot3, BCH) -{ - // Approximate exmap by BCH formula - AngularVelocity w1(0.2, -0.1, 0.1); - AngularVelocity w2(0.01, 0.02, -0.03); - Rot3 R1 = Rot3::Expmap (w1.vector()), R2 = Rot3::Expmap (w2.vector()); - Rot3 R3 = R1 * R2; - Vector expected = Rot3::Logmap(R3); - Vector actual = BCH(w1, w2).vector(); - CHECK(assert_equal(expected, actual,1e-5)); -} - -/* ************************************************************************* */ -TEST( Rot3, rotate_derivatives) -{ - Matrix actualDrotate1a, actualDrotate1b, actualDrotate2; - R.rotate(P, actualDrotate1a, actualDrotate2); - R.inverse().rotate(P, actualDrotate1b, boost::none); - Matrix numerical1 = numericalDerivative21(testing::rotate, R, P); - Matrix numerical2 = numericalDerivative21(testing::rotate, R.inverse(), P); - Matrix numerical3 = numericalDerivative22(testing::rotate, R, P); - EXPECT(assert_equal(numerical1,actualDrotate1a,error)); - EXPECT(assert_equal(numerical2,actualDrotate1b,error)); - EXPECT(assert_equal(numerical3,actualDrotate2, error)); -} - -/* ************************************************************************* */ -TEST( Rot3, unrotate) -{ - Point3 w = R * P; - Matrix H1,H2; - Point3 actual = R.unrotate(w,H1,H2); - CHECK(assert_equal(P,actual)); - - Matrix numerical1 = numericalDerivative21(testing::unrotate, R, w); - CHECK(assert_equal(numerical1,H1,error)); - - Matrix numerical2 = numericalDerivative22(testing::unrotate, R, w); - CHECK(assert_equal(numerical2,H2,error)); -} - -/* ************************************************************************* */ -TEST( Rot3, compose ) -{ - Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); - Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); - - Rot3 expected = R1 * R2; - Matrix actualH1, actualH2; - Rot3 actual = R1.compose(R2, actualH1, actualH2); - CHECK(assert_equal(expected,actual)); - - Matrix numericalH1 = numericalDerivative21(testing::compose, R1, - R2, 1e-2); - CHECK(assert_equal(numericalH1,actualH1)); - - Matrix numericalH2 = numericalDerivative22(testing::compose, R1, - R2, 1e-2); - CHECK(assert_equal(numericalH2,actualH2)); -} - -/* ************************************************************************* */ -TEST( Rot3, inverse ) -{ - Rot3 R = Rot3::rodriguez(0.1, 0.2, 0.3); - - Rot3 I; - Matrix actualH; - CHECK(assert_equal(I,R*R.inverse(actualH))); - CHECK(assert_equal(I,R.inverse()*R)); - - Matrix numericalH = numericalDerivative11(testing::inverse, R); - CHECK(assert_equal(numericalH,actualH, 1e-4)); -} - -/* ************************************************************************* */ -TEST( Rot3, between ) -{ - Rot3 r1 = Rot3::Rz(M_PI/3.0); - Rot3 r2 = Rot3::Rz(2.0*M_PI/3.0); - - Matrix expectedr1 = (Matrix(3, 3) << - 0.5, -sqrt(3.0)/2.0, 0.0, - sqrt(3.0)/2.0, 0.5, 0.0, - 0.0, 0.0, 1.0); - EXPECT(assert_equal(expectedr1, r1.matrix())); - - Rot3 R = Rot3::rodriguez(0.1, 0.4, 0.2); - Rot3 origin; - CHECK(assert_equal(R, origin.between(R))); - CHECK(assert_equal(R.inverse(), R.between(origin))); - - Rot3 R1 = Rot3::rodriguez(0.1, 0.2, 0.3); - Rot3 R2 = Rot3::rodriguez(0.2, 0.3, 0.5); - - Rot3 expected = R1.inverse() * R2; - Matrix actualH1, actualH2; - Rot3 actual = R1.between(R2, actualH1, actualH2); - CHECK(assert_equal(expected,actual)); - - Matrix numericalH1 = numericalDerivative21(testing::between , R1, R2); - CHECK(assert_equal(numericalH1,actualH1, 1e-4)); - - Matrix numericalH2 = numericalDerivative22(testing::between , R1, R2); - CHECK(assert_equal(numericalH2,actualH2, 1e-4)); -} - -/* ************************************************************************* */ -TEST( Rot3, xyz ) -{ - double t = 0.1, st = sin(t), ct = cos(t); - - // Make sure all counterclockwise - // Diagrams below are all from from unchanging axis - - // z - // | * Y=(ct,st) - // x----y - Rot3 expected1(1, 0, 0, 0, ct, -st, 0, st, ct); - CHECK(assert_equal(expected1,Rot3::Rx(t))); - - // x - // | * Z=(ct,st) - // y----z - Rot3 expected2(ct, 0, st, 0, 1, 0, -st, 0, ct); - CHECK(assert_equal(expected2,Rot3::Ry(t))); - - // y - // | X=* (ct,st) - // z----x - Rot3 expected3(ct, -st, 0, st, ct, 0, 0, 0, 1); - CHECK(assert_equal(expected3,Rot3::Rz(t))); - - // Check compound rotation - Rot3 expected = Rot3::Rz(0.3) * Rot3::Ry(0.2) * Rot3::Rx(0.1); - CHECK(assert_equal(expected,Rot3::RzRyRx(0.1,0.2,0.3))); -} - -/* ************************************************************************* */ -TEST( Rot3, yaw_pitch_roll ) -{ - double t = 0.1; - - // yaw is around z axis - CHECK(assert_equal(Rot3::Rz(t),Rot3::yaw(t))); - - // pitch is around y axis - CHECK(assert_equal(Rot3::Ry(t),Rot3::pitch(t))); - - // roll is around x axis - CHECK(assert_equal(Rot3::Rx(t),Rot3::roll(t))); - - // Check compound rotation - Rot3 expected = Rot3::yaw(0.1) * Rot3::pitch(0.2) * Rot3::roll(0.3); - CHECK(assert_equal(expected,Rot3::ypr(0.1,0.2,0.3))); -} - -/* ************************************************************************* */ -TEST( Rot3, RQ) -{ - // Try RQ on a pure rotation - Matrix actualK; - Vector actual; - boost::tie(actualK, actual) = RQ(R.matrix()); - Vector expected = (Vector(3) << 0.14715, 0.385821, 0.231671); - CHECK(assert_equal(eye(3),actualK)); - CHECK(assert_equal(expected,actual,1e-6)); - - // Try using xyz call, asserting that Rot3::RzRyRx(x,y,z).xyz()==[x;y;z] - CHECK(assert_equal(expected,R.xyz(),1e-6)); - CHECK(assert_equal((Vector)(Vector(3) <<0.1,0.2,0.3),Rot3::RzRyRx(0.1,0.2,0.3).xyz())); - - // Try using ypr call, asserting that Rot3::ypr(y,p,r).ypr()==[y;p;r] - CHECK(assert_equal((Vector)(Vector(3) <<0.1,0.2,0.3),Rot3::ypr(0.1,0.2,0.3).ypr())); - CHECK(assert_equal((Vector)(Vector(3) <<0.3,0.2,0.1),Rot3::ypr(0.1,0.2,0.3).rpy())); - - // Try ypr for pure yaw-pitch-roll matrices - CHECK(assert_equal((Vector)(Vector(3) <<0.1,0.0,0.0),Rot3::yaw (0.1).ypr())); - CHECK(assert_equal((Vector)(Vector(3) <<0.0,0.1,0.0),Rot3::pitch(0.1).ypr())); - CHECK(assert_equal((Vector)(Vector(3) <<0.0,0.0,0.1),Rot3::roll (0.1).ypr())); - - // Try RQ to recover calibration from 3*3 sub-block of projection matrix - Matrix K = (Matrix(3, 3) << 500.0, 0.0, 320.0, 0.0, 500.0, 240.0, 0.0, 0.0, 1.0); - Matrix A = K * R.matrix(); - boost::tie(actualK, actual) = RQ(A); - CHECK(assert_equal(K,actualK)); - CHECK(assert_equal(expected,actual,1e-6)); -} - -/* ************************************************************************* */ -TEST( Rot3, expmapStability ) { - Vector w = (Vector(3) << 78e-9, 5e-8, 97e-7); - double theta = w.norm(); - double theta2 = theta*theta; - Rot3 actualR = Rot3::Expmap(w); - Matrix W = (Matrix(3, 3) << 0.0, -w(2), w(1), - w(2), 0.0, -w(0), - -w(1), w(0), 0.0 ); - Matrix W2 = W*W; - Matrix Rmat = eye(3) + (1.0-theta2/6.0 + theta2*theta2/120.0 - - theta2*theta2*theta2/5040.0)*W + (0.5 - theta2/24.0 + theta2*theta2/720.0)*W2 ; - Rot3 expectedR( Rmat ); - CHECK(assert_equal(expectedR, actualR, 1e-10)); -} - -// Does not work with Quaternions -///* ************************************************************************* */ -//TEST( Rot3, logmapStability ) { -// Vector w = (Vector(3) << 1e-8, 0.0, 0.0); -// Rot3 R = Rot3::Expmap(w); -//// double tr = R.r1().x()+R.r2().y()+R.r3().z(); -//// std::cout.precision(5000); -//// std::cout << "theta: " << w.norm() << std::endl; -//// std::cout << "trace: " << tr << std::endl; -//// R.print("R = "); -// Vector actualw = Rot3::Logmap(R); -// CHECK(assert_equal(w, actualw, 1e-15)); -//} - -/* ************************************************************************* */ -TEST(Rot3, quaternion) { - // NOTE: This is also verifying the ability to convert Vector to Quaternion - Quaternion q1(0.710997408193224, 0.360544029310185, 0.594459869568306, 0.105395217842782); - Rot3 R1 = Rot3((Matrix)(Matrix(3, 3) << - 0.271018623057411, 0.278786459830371, 0.921318086098018, - 0.578529366719085, 0.717799701969298, -0.387385285854279, - -0.769319620053772, 0.637998195662053, 0.033250932803219)); - - Quaternion q2(0.263360579192421, 0.571813128030932, 0.494678363680335, 0.599136268678053); - Rot3 R2 = Rot3((Matrix)(Matrix(3, 3) << - -0.207341903877828, 0.250149415542075, 0.945745528564780, - 0.881304914479026, -0.371869043667957, 0.291573424846290, - 0.424630407073532, 0.893945571198514, -0.143353873763946)); - - // 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(R2, Rot3(q2))); - 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()))); - EXPECT(assert_equal(Vector(R2.toQuaternion().coeffs()), Vector(q2.coeffs()))); - - // Check that quaternion and Rot3 represent the same rotation - Point3 p1(1.0, 2.0, 3.0); - Point3 p2(8.0, 7.0, 9.0); - - Point3 expected1 = R1*p1; - Point3 expected2 = R2*p2; - - Point3 actual1 = Point3(q1*p1.vector()); - Point3 actual2 = Point3(q2*p2.vector()); - - EXPECT(assert_equal(expected1, actual1)); - EXPECT(assert_equal(expected2, actual2)); -} - -/* ************************************************************************* */ -TEST( Rot3, stream) -{ - Rot3 R; - std::ostringstream os; - os << R; - EXPECT(os.str() == "\n|1, 0, 0|\n|0, 1, 0|\n|0, 0, 1|\n"); -} +// No quaternion only tests #endif From 941d6dfe07ac4df964ed185c87d0dd0e351d2eca Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 23 Oct 2014 01:54:46 +0200 Subject: [PATCH 2/5] Fixed LogMap - something fishy in Eigen? Or at least low-accuracy. --- gtsam/geometry/Rot3Q.cpp | 27 ++++++++++++++++++++------- 1 file changed, 20 insertions(+), 7 deletions(-) diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index c5990153a..d4545b886 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -21,6 +21,7 @@ #include #include +#include using namespace std; @@ -120,14 +121,26 @@ namespace gtsam { } /* ************************************************************************* */ - // Log map at identity - return the canonical coordinates of this rotation +// Log map at identity - return the canonical coordinates of this rotation Vector3 Rot3::Logmap(const Rot3& R) { - Eigen::AngleAxisd angleAxis(R.quaternion_); - if(angleAxis.angle() > M_PI) // Important: use the smallest possible - angleAxis.angle() -= 2.0*M_PI; // angle, e.g. no more than PI, to keep - if(angleAxis.angle() < -M_PI) // error continuous. - angleAxis.angle() += 2.0*M_PI; - return angleAxis.axis() * angleAxis.angle(); + const Quaternion& q = R.quaternion_; + double qw = q.w(); + if (std::abs(qw - 1.0) < 1e-10) { + // Taylor expansion of (angle / s) at 1 + return (2 - 2 * (qw - 1) / 3) * q.vec(); + } else if (std::abs(qw + 1.0) < 1e-10) { + // Angle is zero, return zero vector + return Vector3::Zero(); + } else { + // Normal, away from zero case + double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); + // TODO C++ says: acos range is [0..pi], so below cannot occur !? + if (angle > M_PI) // Important: use the smallest possible + angle -= 2.0 * M_PI; // angle, e.g. no more than PI, to keep + if (angle < -M_PI) // error continuous. + angle += 2.0 * M_PI; + return (angle / s) * q.vec(); + } } /* ************************************************************************* */ From a0be48ef752bec0194cd81d5cdcdf48b0c7fe81a Mon Sep 17 00:00:00 2001 From: cbeall3 Date: Wed, 22 Oct 2014 22:00:25 -0400 Subject: [PATCH 3/5] revert from fixed size vectors to make it compile with existing numericalDerivatives --- gtsam/geometry/tests/testRot3.cpp | 21 ++++++++------------- 1 file changed, 8 insertions(+), 13 deletions(-) diff --git a/gtsam/geometry/tests/testRot3.cpp b/gtsam/geometry/tests/testRot3.cpp index b495a4fb2..2bc4c58f0 100644 --- a/gtsam/geometry/tests/testRot3.cpp +++ b/gtsam/geometry/tests/testRot3.cpp @@ -138,12 +138,7 @@ TEST( Rot3, rodriguez4) TEST( Rot3, retract) { Vector v = zero(3); - CHECK(assert_equal(R, R.retract(v))); - - // test Canonical coordinates - Canonical chart; - Vector v2 = chart.apply(R); - CHECK(assert_equal(R, chart.retract(v2))); + CHECK(assert_equal(R.retract(v), R)); } /* ************************************************************************* */ @@ -206,9 +201,9 @@ Vector3 evaluateLogRotation(const Vector3 thetahat, const Vector3 deltatheta){ TEST( Rot3, rightJacobianExpMapSO3 ) { // Linearization point - Vector3 thetahat; thetahat << 0.1, 0, 0; + Vector thetahat = (Vector(3) << 0.1, 0, 0); - Matrix expectedJacobian = numericalDerivative11( + Matrix expectedJacobian = numericalDerivative11( boost::bind(&Rot3::Expmap, _1), thetahat); Matrix actualJacobian = Rot3::rightJacobianExpMapSO3(thetahat); CHECK(assert_equal(expectedJacobian, actualJacobian)); @@ -218,10 +213,10 @@ TEST( Rot3, rightJacobianExpMapSO3 ) TEST( Rot3, rightJacobianExpMapSO3inverse ) { // Linearization point - Vector3 thetahat; thetahat << 0.1,0.1,0; ///< Current estimate of rotation rate bias - Vector3 deltatheta; deltatheta << 0, 0, 0; + Vector thetahat = (Vector(3) << 0.1,0.1,0); ///< Current estimate of rotation rate bias + Vector deltatheta = (Vector(3) << 0, 0, 0); - Matrix expectedJacobian = numericalDerivative11( + Matrix expectedJacobian = numericalDerivative11( boost::bind(&evaluateLogRotation, thetahat, _1), deltatheta); Matrix actualJacobian = Rot3::rightJacobianExpMapSO3inverse(thetahat); EXPECT(assert_equal(expectedJacobian, actualJacobian)); @@ -397,8 +392,8 @@ Vector3 testDexpL(const Vector3& dw) { TEST( Rot3, dexpL) { Matrix actualDexpL = Rot3::dexpL(w); - Matrix expectedDexpL = numericalDerivative11(testDexpL, - Vector3::Zero(), 1e-2); + Matrix expectedDexpL = numericalDerivative11(testDexpL, + LieVector(zero(3)), 1e-2); EXPECT(assert_equal(expectedDexpL, actualDexpL, 1e-5)); Matrix actualDexpInvL = Rot3::dexpInvL(w); From 056254bf93b54b25c4e235618b1060be0f8f7388 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 23 Oct 2014 09:51:43 +0200 Subject: [PATCH 4/5] Optimized a bit more --- gtsam/geometry/Rot3Q.cpp | 23 ++++++++++++++--------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/gtsam/geometry/Rot3Q.cpp b/gtsam/geometry/Rot3Q.cpp index d4545b886..6b7a4e0ce 100644 --- a/gtsam/geometry/Rot3Q.cpp +++ b/gtsam/geometry/Rot3Q.cpp @@ -121,24 +121,29 @@ namespace gtsam { } /* ************************************************************************* */ -// Log map at identity - return the canonical coordinates of this rotation Vector3 Rot3::Logmap(const Rot3& R) { + using std::acos; + using std::sqrt; + static const double twoPi = 2.0 * M_PI, + // define these compile time constants to avoid std::abs: + NearlyOne = 1.0 - 1e-10, NearlyNegativeOne = -1.0 + 1e-10; + const Quaternion& q = R.quaternion_; - double qw = q.w(); - if (std::abs(qw - 1.0) < 1e-10) { + const double qw = q.w(); + if (qw > NearlyOne) { // Taylor expansion of (angle / s) at 1 return (2 - 2 * (qw - 1) / 3) * q.vec(); - } else if (std::abs(qw + 1.0) < 1e-10) { + } else if (qw < NearlyNegativeOne) { // Angle is zero, return zero vector return Vector3::Zero(); } else { // Normal, away from zero case double angle = 2 * acos(qw), s = sqrt(1 - qw * qw); - // TODO C++ says: acos range is [0..pi], so below cannot occur !? - if (angle > M_PI) // Important: use the smallest possible - angle -= 2.0 * M_PI; // angle, e.g. no more than PI, to keep - if (angle < -M_PI) // error continuous. - angle += 2.0 * M_PI; + // Important: convert to [-pi,pi] to keep error continuous + if (angle > M_PI) + angle -= twoPi; + else if (angle < -M_PI) + angle += twoPi; return (angle / s) * q.vec(); } } From 845697555a99016eb64ea0028023e86d55392dc0 Mon Sep 17 00:00:00 2001 From: dellaert Date: Thu, 23 Oct 2014 09:52:35 +0200 Subject: [PATCH 5/5] Fixed tests for Quaternion mode but Pose3 not in EXPMAP mode --- .../slam/tests/testPoseBetweenFactor.cpp | 17 +++++++++++-- .../slam/tests/testPosePriorFactor.cpp | 24 ++++++++++++++----- 2 files changed, 33 insertions(+), 8 deletions(-) diff --git a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp index bf9dc0e8e..900356213 100644 --- a/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp +++ b/gtsam_unstable/slam/tests/testPoseBetweenFactor.cpp @@ -95,13 +95,20 @@ TEST( PoseBetweenFactor, Error ) { // The expected error Vector expectedError(6); + // The solution depends on choice of Pose3 and Rot3 Expmap mode! #if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) expectedError << -0.0298135267953815, 0.0131341515747393, 0.0968868439682154, +#if defined(GTSAM_POSE3_EXPMAP) -0.145701634472172, -0.134898525569125, -0.0421026389164264; +#else + -0.13918755, + -0.142346243, + -0.0390885321; +#endif #else expectedError << -0.029839512616488, 0.013145599455949, @@ -132,14 +139,20 @@ TEST( PoseBetweenFactor, ErrorWithTransform ) { // The expected error Vector expectedError(6); - // TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! -#if defined(GTSAM_ROT3_EXPMAP) + // The solution depends on choice of Pose3 and Rot3 Expmap mode! +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) expectedError << 0.0173358202010741, 0.0222210698409755, -0.0125032003886145, +#if defined(GTSAM_POSE3_EXPMAP) 0.0263800787416566, 0.00540285006310398, 0.000175859555693563; +#else + 0.0264132886, + 0.0052376953, + -7.16127036e-05; +#endif #else expectedError << 0.017337193670445, 0.022222830355243, diff --git a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp index cbfa45431..16b2bccc1 100644 --- a/gtsam_unstable/slam/tests/testPosePriorFactor.cpp +++ b/gtsam_unstable/slam/tests/testPosePriorFactor.cpp @@ -90,14 +90,20 @@ TEST( PosePriorFactor, Error ) { // The expected error Vector expectedError(6); - // TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! -#if defined(GTSAM_ROT3_EXPMAP) + // The solution depends on choice of Pose3 and Rot3 Expmap mode! +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) expectedError << -0.182948257976108, 0.13851858011118, -0.157375974517456, +#if defined(GTSAM_POSE3_EXPMAP) 0.766913166076379, -1.22976117053126, 0.949345561430261; +#else + 0.740211734, + -1.19821028, + 1.00815609; +#endif #else expectedError << -0.184137861505414, 0.139419283914526, @@ -115,7 +121,7 @@ TEST( PosePriorFactor, Error ) { Vector actualError(factor.evaluateError(pose)); // Verify we get the expected error - CHECK(assert_equal(expectedError, actualError, 1e-9)); + CHECK(assert_equal(expectedError, actualError, 1e-8)); } /* ************************************************************************* */ @@ -127,14 +133,20 @@ TEST( PosePriorFactor, ErrorWithTransform ) { // The expected error Vector expectedError(6); - // TODO: The solution depends on choice of Pose3 and Rot3 Expmap mode! -#if defined(GTSAM_ROT3_EXPMAP) + // The solution depends on choice of Pose3 and Rot3 Expmap mode! +#if defined(GTSAM_ROT3_EXPMAP) || defined(GTSAM_USE_QUATERNIONS) expectedError << -0.0224998729281528, 0.191947887288328, 0.273826035236257, +#if defined(GTSAM_POSE3_EXPMAP) 1.36483391560855, -0.754590051075035, 0.585710674473659; +#else + 1.49751986, + -0.549375791, + 0.452761203; +#endif #else expectedError << -0.022712885347328, 0.193765110165872, @@ -151,7 +163,7 @@ TEST( PosePriorFactor, ErrorWithTransform ) { Vector actualError(factor.evaluateError(pose)); // Verify we get the expected error - CHECK(assert_equal(expectedError, actualError, 1e-9)); + CHECK(assert_equal(expectedError, actualError, 1e-8)); } /* ************************************************************************* */