diff --git a/.cproject b/.cproject index 4f449ae92..cf0ec25f8 100644 --- a/.cproject +++ b/.cproject @@ -315,14 +315,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -349,6 +341,7 @@ make + tests/testBayesTree.run true false @@ -356,6 +349,7 @@ make + testBinaryBayesNet.run true false @@ -403,6 +397,7 @@ make + testSymbolicBayesNet.run true false @@ -410,6 +405,7 @@ make + tests/testSymbolicFactor.run true false @@ -417,6 +413,7 @@ make + testSymbolicFactorGraph.run true false @@ -432,11 +429,20 @@ make + tests/testBayesTree true false true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j5 @@ -525,22 +531,6 @@ false true - - make - -j2 - all - true - true - true - - - make - -j2 - clean - true - true - true - make -j2 @@ -557,6 +547,22 @@ true true + + make + -j2 + all + true + true + true + + + make + -j2 + clean + true + true + true + make -j2 @@ -581,42 +587,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 @@ -669,26 +659,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 @@ -1087,6 +1093,7 @@ make + testGraph.run true false @@ -1094,6 +1101,7 @@ make + testJunctionTree.run true false @@ -1101,6 +1109,7 @@ make + testSymbolicBayesNetB.run true false @@ -1268,6 +1277,7 @@ make + testErrors.run true false @@ -1313,14 +1323,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1401,6 +1403,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1707,7 +1717,6 @@ make - testSimulated2DOriented.run true false @@ -1747,7 +1756,6 @@ make - testSimulated2D.run true false @@ -1755,7 +1763,6 @@ make - testSimulated3D.run true false @@ -1817,14 +1824,6 @@ true true - - make - -j5 - testGaussianFactorGraph.run - true - true - true - make -j5 @@ -1865,6 +1864,14 @@ true true + + make + -j5 + testGaussianFactorGraphUnordered.run + true + true + true + make -j2 @@ -2027,7 +2034,6 @@ make - tests/testGaussianISAM2 true false @@ -2049,102 +2055,6 @@ 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 @@ -2346,6 +2256,7 @@ cpack + -G DEB true false @@ -2353,6 +2264,7 @@ cpack + -G RPM true false @@ -2360,6 +2272,7 @@ cpack + -G TGZ true false @@ -2367,6 +2280,7 @@ cpack + --config CPackSourceConfig.cmake true false @@ -2532,34 +2446,98 @@ true true - + make - -j5 - testSpirit.run + -j2 + testRot3.run true true true - + make - -j5 - testWrap.run + -j2 + testRot2.run true true true - + make - -j5 - check.wrap + -j2 + testPose3.run true true true - + make - -j5 - wrap + -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 @@ -2603,6 +2581,38 @@ 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 af991231e..459c12f76 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -503,18 +503,25 @@ GaussianFactor::shared_ptr HessianFactor::negate() const void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) { - for(size_t posRow=0; posRow yi = y.tryInsert(keys_[posRow], Vector()); - if(yi.second) - yi.first->second = Vector::Zero(getDim(begin() + posRow)); + std::cout << "size() " << size() << std::endl; - for(size_t posCol=0; posCol 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)); + } + + 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) - Matrix Hij = info(begin()+posRow, begin()+posCol); - gtsam::transposeMultiplyAdd(alpha, Hij, X, yi.first->second); + if (i <= j) + yi += alpha * info(begin() + i, begin() + j) * xj; + else + yi += alpha * info(begin() + j, begin() + i).transpose() * xj; } } } diff --git a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp index 79331611e..1ba31dec1 100644 --- a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp @@ -158,15 +158,6 @@ static GaussianFactorGraph createSimpleGaussianFactorGraph() { return fg; } - -/* ************************************************************************* */ -static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() { - GaussianFactorGraph fg = createSimpleGaussianFactorGraph(); - fg += HessianFactor(1, 2, 100*ones(2,2), 200*ones(2,2), (Vec(2) << 0.0, 1.0), - 400*ones(2,2), (Vec(2) << 1.0, 1.0), 0.0); - return fg; -} - /* ************************************************************************* */ TEST( GaussianFactorGraph, gradient ) { @@ -252,24 +243,33 @@ TEST( GaussianFactorGraph, multiplyHessian ) } /* ************************************************************************* */ -//TEST( GaussianFactorGraph, multiplyHessian2 ) -//{ -// GaussianFactorGraph A = createGaussianFactorGraphWithHessianFactor(); -// -// VectorValues x = map_list_of -// (0, (Vec(2) << 1,2)) -// (1, (Vec(2) << 3,4)) -// (2, (Vec(2) << 5,6)); -// -// // expected from matlab: -450 -450 2900 2900 6750 6850 -// VectorValues expected; -// expected.insert(0, (Vec(2) << -450, -450)); -// expected.insert(1, (Vec(2) << 2900, 2900)); -// expected.insert(2, (Vec(2) << 6750, 6850)); -// -// VectorValues actual = A.multiplyHessian(x); -// EXPECT(assert_equal(expected, actual)); -//} +static GaussianFactorGraph createGaussianFactorGraphWithHessianFactor() { + GaussianFactorGraph fg = createSimpleGaussianFactorGraph(); + fg += HessianFactor(1, 2, 100*ones(2,2), 200*ones(2,2), (Vec(2) << 0.0, 1.0), + 400*ones(2,2), (Vec(2) << 1.0, 1.0), 0.0); + return fg; +} + +/* ************************************************************************* */ +TEST( GaussianFactorGraph, multiplyHessian2 ) +{ + GaussianFactorGraph A = createGaussianFactorGraphWithHessianFactor(); + + VectorValues x = map_list_of + (0, (Vec(2) << 1,2)) + (1, (Vec(2) << 3,4)) + (2, (Vec(2) << 5,6)); + + // expected from matlab: -450 -450 2900 2900 6750 6850 + VectorValues expected; + expected.insert(0, (Vec(2) << -450, -450)); + expected.insert(1, (Vec(2) << 2900, 2900)); + expected.insert(2, (Vec(2) << 6750, 6850)); + + VectorValues actual = A.multiplyHessian(x); + EXPECT(assert_equal(expected, actual)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);}