diff --git a/.cproject b/.cproject index 7c1c35ba1..3283b0ffc 100644 --- a/.cproject +++ b/.cproject @@ -300,7 +300,6 @@ make - check true true @@ -308,6 +307,7 @@ make + testSimpleCamera.run true true @@ -315,6 +315,7 @@ make + testCal3_S2.run true true @@ -322,7 +323,6 @@ make - testVSLAMFactor.run true true @@ -330,6 +330,7 @@ make + testCalibratedCamera.run true true @@ -337,7 +338,6 @@ make - testConditionalGaussian.run true true @@ -345,6 +345,7 @@ make + testPose2.run true true @@ -352,15 +353,21 @@ make - testFGConfig.run true true true + +make + +testRot3.run +true +true +true + make - install true true @@ -368,7 +375,6 @@ make - clean true true @@ -376,7 +382,6 @@ make - check true true diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp index e8c9ab49a..4aa595a9e 100644 --- a/cpp/Rot3.cpp +++ b/cpp/Rot3.cpp @@ -23,11 +23,12 @@ namespace gtsam { double ct = cos(t), st = sin(t); + print(w); Point3 r1 = Point3(ct + w(0) * w(0) * (1 - ct), w(2) * st + w(0) * w(1) * (1 - ct), -w(1) * st + w(0) * w(2) * (1 - ct)); Point3 r2 = Point3(w(1) * w(0) * (1 - ct) - w(2) * st, w(1) * w(1) * (1 - ct) + ct, w(1) * w(2) * (1 - ct) + w(0) * st); Point3 r3 = Point3(w(1) * st + w(2) * w(0) * (1 - ct), -w(0) * st + w(2) * w(1) * (1 - ct), ct + w(2) * w(2) * (1 - ct)); - return Rot3(r1, r2, r2); + return Rot3(r1, r2, r3); } /* ************************************************************************* */ diff --git a/cpp/testRot3.cpp b/cpp/testRot3.cpp index 466c54d95..7c630a521 100644 --- a/cpp/testRot3.cpp +++ b/cpp/testRot3.cpp @@ -53,6 +53,14 @@ TEST( Rot3, rodriguez) { CHECK(assert_equal(rd1,rd2)); } +/* ************************************************************************* */ +TEST( Rot3, rodriguez2) { + Vector v(3); v(0) = 0; v(1) = 1; v(2) = 0; + Rot3 rd1 = rodriguez(v, 3.14/4.0); + Rot3 rd2(0.707388,0,0.706825,0,1,0,-0.706825,0,0.707388); + CHECK(rd1.equals(rd2,0.0001)); +} + /* ************************************************************************* */ TEST( Rot3, exmap) {