diff --git a/.cproject b/.cproject
index bf7974660..41cddbb6b 100644
--- a/.cproject
+++ b/.cproject
@@ -663,6 +663,22 @@
true
true
+
+make
+
+timeRot3.run
+true
+true
+true
+
+
+make
+
+install
+true
+true
+true
+
make
diff --git a/cpp/Makefile.am b/cpp/Makefile.am
index eb6966d14..bbd965ef0 100644
--- a/cpp/Makefile.am
+++ b/cpp/Makefile.am
@@ -176,6 +176,11 @@ testRot3_LDADD = libgtsam.la
testPose3_LDADD = libgtsam.la
testCal3_S2_LDADD = libgtsam.la
+# not the correct way, I'm sure: Kai ?
+timeRot3: timeRot3.cpp
+timeRot3: CXXFLAGS += -I /opt/local/include
+timeRot3: LDFLAGS += -L.libs -lgtsam
+
# simulated2D example
sources += simulated2D.cpp
testSimulated2D_SOURCES = testSimulated2D.cpp
diff --git a/cpp/Rot3.cpp b/cpp/Rot3.cpp
index b89820ef5..cbfbc98c4 100644
--- a/cpp/Rot3.cpp
+++ b/cpp/Rot3.cpp
@@ -12,7 +12,7 @@ using namespace std;
namespace gtsam {
- /* ************************************************************************* */
+ /* ************************************************************************* */
bool Rot3::equals(const Rot3 & R, double tol) const {
return equal_with_abs_tol(matrix(), R.matrix(), tol);
}
@@ -23,37 +23,77 @@ namespace gtsam {
return rodriguez(v) * (*this);
}
- /* ************************************************************************* */
- /** faster than below ? */
- /* ************************************************************************* */
- Rot3 rodriguez(const Vector& w, double t) {
- double l_w = 0.0;
- for (int i = 0; i < 3; i++)
- l_w += pow(w(i), 2.0);
- if (l_w != 1.0) throw domain_error("rodriguez: length of w should be 1");
+ /* ************************************************************************* */
+ Vector Rot3::vector() const {
+ double r[] = { r1_.x(), r1_.y(), r1_.z(),
+ r2_.x(), r2_.y(), r2_.z(),
+ r3_.x(), r3_.y(), r3_.z() };
+ Vector v(9);
+ copy(r,r+9,v.begin());
+ return v;
+ }
- double ct = cos(t), st = sin(t);
+ /* ************************************************************************* */
+ Matrix Rot3::matrix() const {
+ double r[] = { r1_.x(), r2_.x(), r3_.x(),
+ r1_.y(), r2_.y(), r3_.y(),
+ r1_.z(), r2_.z(), r3_.z() };
+ return Matrix_(3,3, r);
+ }
- 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));
+ /* ************************************************************************* */
+ Matrix Rot3::transpose() const {
+ double r[] = { r1_.x(), r1_.y(), r1_.z(),
+ r2_.x(), r2_.y(), r2_.z(),
+ r3_.x(), r3_.y(), r3_.z()};
+ return Matrix_(3,3, r);
+ }
+
+ /* ************************************************************************* */
+ Point3 Rot3::column(int index) const{
+ if(index == 3)
+ return r3_;
+ else if (index == 2)
+ return r2_;
+ else
+ return r1_; // default returns r1
+ }
+
+ /* ************************************************************************* */
+ Rot3 Rot3::inverse() const {
+ return Rot3(
+ r1_.x(), r1_.y(), r1_.z(),
+ r2_.x(), r2_.y(), r2_.z(),
+ r3_.x(), r3_.y(), r3_.z());
+ }
+
+ /* ************************************************************************* */
+ Rot3 rodriguez(const Vector& n, double t) {
+ double n0 = n(0), n1=n(1), n2=n(2);
+ double n00 = n0*n0, n11 = n1*n1, n22 = n2*n2;
+#ifndef NDEBUG
+ double l_n = n00+n11+n22;
+ if (fabs(l_n-1.0)>1e-9) throw domain_error("rodriguez: length of n should be 1");
+#endif
+
+ double ct = cos(t), st = sin(t), ct_1 = 1 - ct;
+
+ double s0 = n0 * st, s1 = n1 * st, s2 = n2 * st;
+ double C01 = ct_1*n0*n1, C02 = ct_1*n0*n2, C12 = ct_1*n1*n2;
+ double C00 = ct_1*n00, C11 = ct_1*n11, C22 = ct_1*n22;
+
+ Point3 r1 = Point3( ct + C00, s2 + C01, -s1 + C02);
+ Point3 r2 = Point3(-s2 + C01, ct + C11, s0 + C12);
+ Point3 r3 = Point3( s1 + C02, -s0 + C12, ct + C22);
return Rot3(r1, r2, r3);
}
/* ************************************************************************* */
- Rot3 rodriguez(double wx, double wy, double wz) {
- Matrix J = skewSymmetric(wx, wy, wz);
- double t2 = wx * wx + wy * wy + wz * wz;
- if (t2 < 1e-10) return Rot3();
- double t = sqrt(t2);
- Matrix R = eye(3, 3) + sin(t) / t * J + (1.0 - cos(t)) / t2 * (J * J);
- return R; // matrix constructor will be tripped
- }
-
- /* ************************************************************************* */
- Rot3 rodriguez(const Vector& v) {
- return rodriguez(v(0), v(1), v(2));
+ Rot3 rodriguez(const Vector& w) {
+ double t = norm_2(w);
+ if (t < 1e-5) return Rot3();
+ return rodriguez(w/t, t);
}
/* ************************************************************************* */
diff --git a/cpp/Rot3.h b/cpp/Rot3.h
index bc7ad3247..e02d31c10 100644
--- a/cpp/Rot3.h
+++ b/cpp/Rot3.h
@@ -23,43 +23,43 @@ namespace gtsam {
public:
- /** default coonstructor, unit rotation */
- Rot3() : r1_(Point3(1.0,0.0,0.0)),
- r2_(Point3(0.0,1.0,0.0)),
- r3_(Point3(0.0,0.0,1.0)) {
- }
+ /** default constructor, unit rotation */
+ Rot3() : r1_(Point3(1.0,0.0,0.0)),
+ r2_(Point3(0.0,1.0,0.0)),
+ r3_(Point3(0.0,0.0,1.0)) {
+ }
- /** constructor from columns */
- Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
- r1_(r1), r2_(r2), r3_(r3) {
- }
-
- /** constructor from vector */
- Rot3(const Vector &v) :
- r1_(Point3(v(0),v(1),v(2))),
- r2_(Point3(v(3),v(4),v(5))),
- r3_(Point3(v(6),v(7),v(8)))
- { }
+ /** constructor from columns */
+ Rot3(const Point3& r1, const Point3& r2, const Point3& r3) :
+ r1_(r1), r2_(r2), r3_(r3) {
+ }
- /** constructor from doubles in *row* order !!! */
- Rot3(double R11, double R12, double R13,
- double R21, double R22, double R23,
- double R31, double R32, double R33) :
- r1_(Point3(R11, R21, R31)),
- r2_(Point3(R12, R22, R32)),
- r3_(Point3(R13, R23, R33)) {}
+ /** constructor from vector */
+ Rot3(const Vector &v) :
+ r1_(Point3(v(0),v(1),v(2))),
+ r2_(Point3(v(3),v(4),v(5))),
+ r3_(Point3(v(6),v(7),v(8)))
+ { }
- /** constructor from matrix */
- Rot3(const Matrix& R):
- r1_(Point3(R(0,0), R(1,0), R(2,0))),
- r2_(Point3(R(0,1), R(1,1), R(2,1))),
- r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
+ /** constructor from doubles in *row* order !!! */
+ Rot3(double R11, double R12, double R13,
+ double R21, double R22, double R23,
+ double R31, double R32, double R33) :
+ r1_(Point3(R11, R21, R31)),
+ r2_(Point3(R12, R22, R32)),
+ r3_(Point3(R13, R23, R33)) {}
- /** print */
- void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
+ /** constructor from matrix */
+ Rot3(const Matrix& R):
+ r1_(Point3(R(0,0), R(1,0), R(2,0))),
+ r2_(Point3(R(0,1), R(1,1), R(2,1))),
+ r3_(Point3(R(0,2), R(1,2), R(2,2))) {}
- /** equals with an tolerance */
- bool equals(const Rot3& p, double tol = 1e-9) const;
+ /** print */
+ void print(const std::string& s="R") const { gtsam::print(matrix(), s);}
+
+ /** equals with an tolerance */
+ bool equals(const Rot3& p, double tol = 1e-9) const;
/** return DOF, dimensionality of tangent space */
size_t dim() const { return 3;}
@@ -68,48 +68,19 @@ namespace gtsam {
Rot3 exmap(const Vector& d) const;
/** return vectorized form (column-wise)*/
- Vector vector() const {
- double r[] = { r1_.x(), r1_.y(), r1_.z(),
- r2_.x(), r2_.y(), r2_.z(),
- r3_.x(), r3_.y(), r3_.z() };
- Vector v(9);
- copy(r,r+9,v.begin());
- return v;
- }
+ Vector vector() const;
/** return 3*3 rotation matrix */
- Matrix matrix() const {
- double r[] = { r1_.x(), r2_.x(), r3_.x(),
- r1_.y(), r2_.y(), r3_.y(),
- r1_.z(), r2_.z(), r3_.z() };
- return Matrix_(3,3, r);
- }
+ Matrix matrix() const;
/** return 3*3 transpose (inverse) rotation matrix */
- Matrix transpose() const {
- double r[] = { r1_.x(), r1_.y(), r1_.z(),
- r2_.x(), r2_.y(), r2_.z(),
- r3_.x(), r3_.y(), r3_.z()};
- return Matrix_(3,3, r);
- }
+ Matrix transpose() const;
/** returns column vector specified by index */
- Point3 column(int index) const{
- if(index == 3)
- return r3_;
- else if (index == 2)
- return r2_;
- else
- return r1_; // default returns r1
- }
+ Point3 column(int index) const;
/** inverse transformation */
- Rot3 inverse() const {
- return Rot3(
- r1_.x(), r1_.y(), r1_.z(),
- r2_.x(), r2_.y(), r2_.z(),
- r3_.x(), r3_.y(), r3_.z());
- }
+ Rot3 inverse() const;
/** composition */
inline Rot3 operator*(const Rot3& B) const { return Rot3(matrix()*B.matrix());}
@@ -151,15 +122,6 @@ namespace gtsam {
*/
Rot3 rodriguez(const Vector& w, double theta);
- /**
- * Rodriguez' formula to compute an incremental rotation matrix
- * @param wx
- * @param wy
- * @param wz
- * @return incremental rotation matrix
- */
- Rot3 rodriguez(double wx, double wy, double wz);
-
/**
* Rodriguez' formula to compute an incremental rotation matrix
* @param v a vector of incremental roll,pitch,yaw
@@ -167,6 +129,15 @@ namespace gtsam {
*/
Rot3 rodriguez(const Vector& v);
+ /**
+ * Rodriguez' formula to compute an incremental rotation matrix
+ * @param wx
+ * @param wy
+ * @param wz
+ * @return incremental rotation matrix
+ */
+ inline Rot3 rodriguez(double wx, double wy, double wz) { return rodriguez(Vector_(3,wx,wy,wz));}
+
/**
* Update Rotation with incremental rotation
* @param v a vector of incremental roll,pitch,yaw
diff --git a/cpp/testRot3.cpp b/cpp/testRot3.cpp
index 84b32f553..096a2e45d 100644
--- a/cpp/testRot3.cpp
+++ b/cpp/testRot3.cpp
@@ -60,20 +60,39 @@ TEST( Rot3, equals) {
CHECK(!R.equals(zero));
}
+/* ************************************************************************* */
+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, 3) + sin(t) * J + (1.0 - cos(t)) * (J * J);
+ return R; // matrix constructor will be tripped
+}
+
/* ************************************************************************* */
TEST( Rot3, rodriguez) {
- Rot3 rd1 = rodriguez(epsilon, 0, 0);
- Vector temp(3); temp(0) = epsilon; temp(1) = 0; temp(2) = 0;
- Rot3 rd2 = rodriguez(temp);
- CHECK(assert_equal(rd1,rd2));
+ Rot3 R1 = rodriguez(epsilon, 0, 0);
+ Vector w = Vector_(3,epsilon,0.,0.);
+ Rot3 R2 = slow_but_correct_rodriguez(w);
+ CHECK(assert_equal(R1,R2));
}
/* ************************************************************************* */
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));
+ Rot3 R1 = rodriguez(v, 3.14/4.0);
+ Rot3 R2(0.707388,0,0.706825,
+ 0,1,0,
+ -0.706825,0,0.707388);
+ CHECK(assert_equal(R1,R2,1e-5));
+}
+
+/* ************************************************************************* */
+TEST( Rot3, rodriguez3) {
+ Vector w = Vector_(3,0.1,0.2,0.3);
+ Rot3 R1 = rodriguez(w/norm_2(w), norm_2(w));
+ Rot3 R2 = slow_but_correct_rodriguez(w);
+ CHECK(assert_equal(R1,R2));
}
/* ************************************************************************* */
diff --git a/cpp/timeRot3.cpp b/cpp/timeRot3.cpp
new file mode 100644
index 000000000..850a8ee5a
--- /dev/null
+++ b/cpp/timeRot3.cpp
@@ -0,0 +1,37 @@
+/**
+ * @file timeRot3.cpp
+ * @brief time Rot3 functions
+ * @author Frank Dellaert
+ */
+
+#include
+#include
+
+#include "Rot3.h"
+
+using namespace std;
+using namespace gtsam;
+
+int main()
+{
+ int n = 3000000;
+ Vector v = Vector_(3,1.,0.,0.);
+
+ long timeLog = clock();
+ for(int i = 0; i < n; i++)
+ rodriguez(v,0.001);
+ long timeLog2 = clock();
+ double seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
+ cout << seconds << " seconds" << endl;
+ cout << ((double)n/seconds) << " calls/second" << endl;
+
+ timeLog = clock();
+ for(int i = 0; i < n; i++)
+ rodriguez(v);
+ timeLog2 = clock();
+ seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC;
+ cout << seconds << " seconds" << endl;
+ cout << ((double)n/seconds) << " calls/second" << endl;
+
+ return 0;
+}