diff --git a/.cproject b/.cproject
index ac227e005..4056d4b86 100644
--- a/.cproject
+++ b/.cproject
@@ -542,6 +542,14 @@
true
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -568,7 +576,6 @@
make
-
tests/testBayesTree.run
true
false
@@ -576,7 +583,6 @@
make
-
testBinaryBayesNet.run
true
false
@@ -624,7 +630,6 @@
make
-
testSymbolicBayesNet.run
true
false
@@ -632,7 +637,6 @@
make
-
tests/testSymbolicFactor.run
true
false
@@ -640,7 +644,6 @@
make
-
testSymbolicFactorGraph.run
true
false
@@ -656,20 +659,11 @@
make
-
tests/testBayesTree
true
false
true
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j5
@@ -766,22 +760,6 @@
false
true
-
- make
- -j2
- tests/testPose2.run
- true
- true
- true
-
-
- make
- -j2
- tests/testPose3.run
- true
- true
- true
-
make
-j2
@@ -798,6 +776,22 @@
true
true
+
+ make
+ -j2
+ tests/testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ tests/testPose3.run
+ true
+ true
+ true
+
make
-j2
@@ -822,26 +816,42 @@
true
true
-
+
make
- -j2
- all
+ -j5
+ testValues.run
true
true
true
-
+
make
- -j2
- check
+ -j5
+ testOrdering.run
true
true
true
-
+
make
- -j2
- clean
+ -j5
+ testKey.run
+ true
+ true
+ true
+
+
+ make
+ -j5
+ testLinearContainerFactor.run
+ true
+ true
+ true
+
+
+ make
+ -j6 -j8
+ testWhiteNoiseFactor.run
true
true
true
@@ -894,42 +904,26 @@
true
true
-
+
make
- -j5
- testValues.run
+ -j2
+ all
true
true
true
-
+
make
- -j5
- testOrdering.run
+ -j2
+ check
true
true
true
-
+
make
- -j5
- testKey.run
- true
- true
- true
-
-
- make
- -j5
- testLinearContainerFactor.run
- true
- true
- true
-
-
- make
- -j6 -j8
- testWhiteNoiseFactor.run
+ -j2
+ clean
true
true
true
@@ -1320,7 +1314,6 @@
make
-
testGraph.run
true
false
@@ -1328,7 +1321,6 @@
make
-
testJunctionTree.run
true
false
@@ -1336,7 +1328,6 @@
make
-
testSymbolicBayesNetB.run
true
false
@@ -1504,7 +1495,6 @@
make
-
testErrors.run
true
false
@@ -1550,6 +1540,14 @@
true
true
+
+ make
+ -j2
+ testGaussianFactor.run
+ true
+ true
+ true
+
make
-j2
@@ -1630,14 +1628,6 @@
true
true
-
- make
- -j2
- testGaussianFactor.run
- true
- true
- true
-
make
-j2
@@ -1984,6 +1974,7 @@
make
+
testSimulated2DOriented.run
true
false
@@ -2023,6 +2014,7 @@
make
+
testSimulated2D.run
true
false
@@ -2030,6 +2022,7 @@
make
+
testSimulated3D.run
true
false
@@ -2091,10 +2084,10 @@
true
true
-
+
make
-j5
- testGaussianFactorGraph.run
+ testGaussianFactorGraphUnordered.run
true
true
true
@@ -2309,6 +2302,7 @@
make
+
tests/testGaussianISAM2
true
false
@@ -2330,6 +2324,102 @@
true
true
+
+ make
+ -j2
+ testRot3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testRot2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ timeRot3.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPose2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testCal3_S2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testSimpleCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testHomography2.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testCalibratedCamera.run
+ true
+ true
+ true
+
+
+ make
+ -j2
+ check
+ true
+ true
+ true
+
+
+ make
+ -j2
+ clean
+ true
+ true
+ true
+
+
+ make
+ -j2
+ testPoint2.run
+ true
+ true
+ true
+
make
-j3
@@ -2531,7 +2621,6 @@
cpack
-
-G DEB
true
false
@@ -2539,7 +2628,6 @@
cpack
-
-G RPM
true
false
@@ -2547,7 +2635,6 @@
cpack
-
-G TGZ
true
false
@@ -2555,7 +2642,6 @@
cpack
-
--config CPackSourceConfig.cmake
true
false
@@ -2721,98 +2807,34 @@
true
true
-
+
make
- -j2
- testRot3.run
+ -j5
+ testSpirit.run
true
true
true
-
+
make
- -j2
- testRot2.run
+ -j5
+ testWrap.run
true
true
true
-
+
make
- -j2
- testPose3.run
+ -j5
+ check.wrap
true
true
true
-
+
make
- -j2
- timeRot3.run
- true
- true
- true
-
-
- make
- -j2
- testPose2.run
- true
- true
- true
-
-
- make
- -j2
- testCal3_S2.run
- true
- true
- true
-
-
- make
- -j2
- testSimpleCamera.run
- true
- true
- true
-
-
- make
- -j2
- testHomography2.run
- true
- true
- true
-
-
- make
- -j2
- testCalibratedCamera.run
- true
- true
- true
-
-
- make
- -j2
- check
- true
- true
- true
-
-
- make
- -j2
- clean
- true
- true
- true
-
-
- make
- -j2
- testPoint2.run
+ -j5
+ wrap
true
true
true
@@ -2856,38 +2878,6 @@
false
true
-
- make
- -j5
- testSpirit.run
- true
- true
- true
-
-
- make
- -j5
- testWrap.run
- true
- true
- true
-
-
- make
- -j5
- check.wrap
- true
- true
- true
-
-
- make
- -j5
- wrap
- true
- true
- true
-
diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp
index c631d8d14..400f2251d 100644
--- a/gtsam/linear/HessianFactor.cpp
+++ b/gtsam/linear/HessianFactor.cpp
@@ -505,26 +505,39 @@ GaussianFactor::shared_ptr HessianFactor::negate() const
/* ************************************************************************* */
void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x,
- VectorValues& y) const {
+ VectorValues& yvalues) const {
- for (size_t i = 0; i < size(); ++i) {
- pair it = y.tryInsert(keys_[i], Vector());
- Vector& yi = it.first->second;
- if (it.second) {
- // if the value does not exist we initialize it
- yi = Vector::Zero(getDim(begin() + i));
- }
+ // Create a vector of temporary y values, corresponding to rows i
+ vector y;
+ y.reserve(size());
+ for (const_iterator it = begin(); it != end(); it++)
+ y.push_back(zero(getDim(it)));
- for (size_t j = 0; j < size(); ++j) { // loops over the columns
- Vector xj = x.at(keys_[j]);
- // yi.at(keys_[i])// this is what we have to update
- // xj is the input vector
- // we should select the blocks we need in (A'A)
- if (i <= j)
- yi += alpha * info(begin() + i, begin() + j) * xj;
- else
- yi += alpha * info(begin() + j, begin() + i).transpose() * xj;
- }
+ // Accessing the VectorValues one by one is expensive
+ // So we will loop over columns to access x only once per column
+ // And fill the above temporary y values, to be added into yvalues after
+ for (DenseIndex j = 0; j < size(); ++j) {
+ // xj is the input vector
+ Vector xj = x.at(keys_[j]);
+ DenseIndex i = 0;
+ for (; i < j; ++i)
+ y[i] += info_(i, j) * xj;
+ // blocks on the diagonal are only half
+ y[i] += info_(j, j).selfadjointView() * xj;
+ // for below diagonal, we take transpose block from upper triangular part
+ for (i=j+1; i < size(); ++i)
+ y[i] += info_(j, i).transpose() * xj;
+ }
+
+ // copy to yvalues
+ for (DenseIndex i = 0; i < size(); ++i) {
+ bool didNotExist;
+ VectorValues::iterator it;
+ boost::tie(it, didNotExist) = yvalues.tryInsert(keys_[i], Vector());
+ if (didNotExist)
+ it->second = alpha * y[i]; // init
+ else
+ it->second += alpha * y[i]; // add
}
}
diff --git a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp
index 3283670bb..0d54103a3 100644
--- a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp
+++ b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp
@@ -271,6 +271,12 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 )
{
GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
+ // brute force
+ Matrix AtA; Vector eta; boost::tie(AtA,eta) = gfg.hessian();
+ Vector X(6); X<<1,2,3,4,5,6;
+ Vector Y(6); Y<<-450, -450, 300, 400, 2950, 3450;
+ EXPECT(assert_equal(Y,AtA*X));
+
VectorValues x = map_list_of
(0, (Vec(2) << 1,2))
(1, (Vec(2) << 3,4))