From 610faa84396a3dfd2961d065e51739ef50c99d2d Mon Sep 17 00:00:00 2001 From: Alex Cunningham Date: Fri, 6 Jan 2012 16:45:07 +0000 Subject: [PATCH] Experimented with using Eigen in Cayley implementations - ~4x speedup in Rot3M localCoordinates --- .cproject | 281 +++++++++++++++--------------- gtsam/base/Matrix.cpp | 11 +- gtsam/base/Matrix.h | 8 + gtsam/geometry/Rot3M.cpp | 13 +- gtsam/geometry/tests/timeRot3.cpp | 11 +- 5 files changed, 177 insertions(+), 147 deletions(-) diff --git a/.cproject b/.cproject index 90afc0af0..aa3836131 100644 --- a/.cproject +++ b/.cproject @@ -378,14 +378,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -412,6 +404,7 @@ make + tests/testBayesTree.run true false @@ -419,6 +412,7 @@ make + testBinaryBayesNet.run true false @@ -466,6 +460,7 @@ make + testSymbolicBayesNet.run true false @@ -473,6 +468,7 @@ make + tests/testSymbolicFactor.run true false @@ -480,6 +476,7 @@ make + testSymbolicFactorGraph.run true false @@ -495,11 +492,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -526,7 +532,6 @@ make - testGraph.run true false @@ -598,7 +603,6 @@ make - testInference.run true false @@ -606,7 +610,6 @@ make - testGaussianFactor.run true false @@ -614,7 +617,6 @@ make - testJunctionTree.run true false @@ -622,7 +624,6 @@ make - testSymbolicBayesNet.run true false @@ -630,7 +631,6 @@ make - testSymbolicFactorGraph.run true false @@ -700,22 +700,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -732,6 +716,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -756,7 +756,15 @@ true true - + + make + -j2 + all + true + true + true + + make -j2 check @@ -764,6 +772,14 @@ true true + + make + -j2 + clean + true + true + true + make -j2 @@ -804,15 +820,7 @@ true true - - make - -j2 - all - true - true - true - - + make -j2 check @@ -820,14 +828,6 @@ true true - - make - -j2 - clean - true - true - true - make -j2 @@ -1150,6 +1150,7 @@ make + testErrors.run true false @@ -1443,10 +1444,10 @@ true true - + make -j2 - tests/testRot3M.run + tests/testRot3.run true true true @@ -1581,7 +1582,6 @@ make - testSimulated2DOriented.run true false @@ -1621,7 +1621,6 @@ make - testSimulated2D.run true false @@ -1629,7 +1628,6 @@ make - testSimulated3D.run true false @@ -1972,7 +1970,6 @@ make - tests/testGaussianISAM2 true false @@ -1994,6 +1991,46 @@ true true + + make + -j2 + install + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + check + true + true + true + + + make + -j2 + all + true + true + true + + + make + -j2 + dist + true + true + true + make -j2 @@ -2090,94 +2127,6 @@ true true - - make - -j2 - install - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - all - true - true - true - - - make - -j2 - dist - true - true - true - - - make - -j2 - check - true - true - true - - - make - -j2 - install - true - true - true - - - make - -j2 - tests/testSpirit.run - true - true - true - - - make - -j2 - tests/testWrap.run - true - true - true - - - make - -j2 - clean - true - true - true - - - make - -j2 - all - true - true - true - make -j2 @@ -2234,6 +2183,54 @@ false true + + make + -j2 + check + true + true + true + + + make + -j2 + install + true + true + true + + + make + -j2 + tests/testSpirit.run + true + true + true + + + make + -j2 + tests/testWrap.run + true + true + true + + + make + -j2 + clean + true + true + true + + + make + -j2 + all + true + true + true + diff --git a/gtsam/base/Matrix.cpp b/gtsam/base/Matrix.cpp index b1940e335..b3c51325d 100644 --- a/gtsam/base/Matrix.cpp +++ b/gtsam/base/Matrix.cpp @@ -686,10 +686,15 @@ Matrix expm(const Matrix& A, size_t K) { /* ************************************************************************* */ Matrix Cayley(const Matrix& A) { - size_t n = A.cols(); + int n = A.cols(); assert(A.rows() == n); - const Matrix I = eye(n); - return (I-A)*inverse(I+A); + + // original +// const Matrix I = eye(n); +// return (I-A)*inverse(I+A); + + // inlined to let Eigen do more optimization + return (Matrix::Identity(n, n) - A)*(Matrix::Identity(n, n) + A).inverse(); } /* ************************************************************************* */ diff --git a/gtsam/base/Matrix.h b/gtsam/base/Matrix.h index 51e39ad75..fee61f62b 100644 --- a/gtsam/base/Matrix.h +++ b/gtsam/base/Matrix.h @@ -442,6 +442,14 @@ Matrix expm(const Matrix& A, size_t K=7); /// Cayley transform Matrix Cayley(const Matrix& A); +/// Implementation of Cayley transform using fixed size matrices to let +/// Eigen do more optimization +template +Matrix Cayley(const Eigen::Matrix& A) { + typedef Eigen::Matrix FMat; + return Matrix((FMat::Identity() - A)*(FMat::Identity() + A).inverse()); +} + } // namespace gtsam #include diff --git a/gtsam/geometry/Rot3M.cpp b/gtsam/geometry/Rot3M.cpp index 4468df666..837501343 100644 --- a/gtsam/geometry/Rot3M.cpp +++ b/gtsam/geometry/Rot3M.cpp @@ -263,7 +263,18 @@ Vector Rot3::localCoordinates(const Rot3& T) const { #ifdef CORRECT_ROT3_EXMAP return Logmap(between(T)); #else - Matrix Omega = Cayley(between(T).matrix()); + // original +// Matrix Omega = Cayley(between(T).matrix()); + + // Create a fixed-size matrix + Eigen::Matrix3d A(between(T).matrix()); + + // using templated version of Cayley + Matrix Omega = Cayley<3>(A); + + // completely inlined Cayley from template +// Eigen::Matrix3d Omega = (Eigen::Matrix3d::Identity() - A)*(Eigen::Matrix3d::Identity() + A).inverse(); + return -2*Vector_(3,Omega(2,1),Omega(0,2),Omega(1,0)); #endif } diff --git a/gtsam/geometry/tests/timeRot3.cpp b/gtsam/geometry/tests/timeRot3.cpp index c4a04f3e8..c2908549a 100644 --- a/gtsam/geometry/tests/timeRot3.cpp +++ b/gtsam/geometry/tests/timeRot3.cpp @@ -47,7 +47,7 @@ int main() cout << seconds << " seconds" << endl; cout << ((double)n/seconds) << " calls/second" << endl; - cout << "Exmpap" << endl; + cout << "Expmap" << endl; timeLog = clock(); for(int i = 0; i < n; i++) R*Rot3::Expmap(v); @@ -65,6 +65,15 @@ int main() cout << seconds << " seconds" << endl; cout << ((double)n/seconds) << " calls/second" << endl; + cout << "localCoordinates" << endl; + timeLog = clock(); + for(int i = 0; i < n; i++) + R.localCoordinates(R); + timeLog2 = clock(); + seconds = (double)(timeLog2-timeLog)/CLOCKS_PER_SEC; + cout << seconds << " seconds" << endl; + cout << ((double)n/seconds) << " calls/second" << endl; + cout << "Slow rotation matrix" << endl; timeLog = clock(); for(int i = 0; i < n; i++)