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++)