diff --git a/.cproject b/.cproject index 1b1f4989e..2250c3c7a 100644 --- a/.cproject +++ b/.cproject @@ -518,6 +518,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -544,7 +552,6 @@ make - tests/testBayesTree.run true false @@ -552,7 +559,6 @@ make - testBinaryBayesNet.run true false @@ -600,7 +606,6 @@ make - testSymbolicBayesNet.run true false @@ -608,7 +613,6 @@ make - tests/testSymbolicFactor.run true false @@ -616,7 +620,6 @@ make - testSymbolicFactorGraph.run true false @@ -632,20 +635,11 @@ make - tests/testBayesTree true false true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j5 @@ -750,22 +744,6 @@ false true - - make - -j2 - tests/testPose2.run - true - true - true - - - make - -j2 - tests/testPose3.run - true - true - true - make -j2 @@ -782,6 +760,22 @@ true true + + make + -j2 + tests/testPose2.run + true + true + true + + + make + -j2 + tests/testPose3.run + true + true + true + make -j2 @@ -806,26 +800,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 @@ -878,42 +888,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 @@ -1312,7 +1306,6 @@ make - testGraph.run true false @@ -1320,7 +1313,6 @@ make - testJunctionTree.run true false @@ -1328,7 +1320,6 @@ make - testSymbolicBayesNetB.run true false @@ -1496,7 +1487,6 @@ make - testErrors.run true false @@ -1542,6 +1532,14 @@ true true + + make + -j2 + testGaussianFactor.run + true + true + true + make -j2 @@ -1622,14 +1620,6 @@ true true - - make - -j2 - testGaussianFactor.run - true - true - true - make -j2 @@ -1936,6 +1926,7 @@ make + testSimulated2DOriented.run true false @@ -1975,6 +1966,7 @@ make + testSimulated2D.run true false @@ -1982,6 +1974,7 @@ make + testSimulated3D.run true false @@ -2035,10 +2028,10 @@ true true - + make -j5 - testHessianFactor.run + testHessianFactorUnordered.run true true true @@ -2083,10 +2076,10 @@ true true - + make -j5 - testJacobianFactor.run + testJacobianFactorUnordered.run true true true @@ -2269,6 +2262,7 @@ make + tests/testGaussianISAM2 true false @@ -2290,6 +2284,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 @@ -2491,7 +2581,6 @@ cpack - -G DEB true false @@ -2499,7 +2588,6 @@ cpack - -G RPM true false @@ -2507,7 +2595,6 @@ cpack - -G TGZ true false @@ -2515,7 +2602,6 @@ cpack - --config CPackSourceConfig.cmake true false @@ -2681,98 +2767,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 @@ -2816,38 +2838,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/GaussianFactor.h b/gtsam/linear/GaussianFactor.h index f29be5a34..c02f59b89 100644 --- a/gtsam/linear/GaussianFactor.h +++ b/gtsam/linear/GaussianFactor.h @@ -107,7 +107,10 @@ namespace gtsam { virtual GaussianFactor::shared_ptr negate() const = 0; /// y += alpha * A'*A*x - virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y)=0; + virtual void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) = 0; + + /// A'*b for Jacobian, eta for Hessian + virtual VectorValues gradientAtZero() const = 0; private: /** Serialization function */ diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index 0cc3556db..c86e3c728 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -173,15 +173,6 @@ namespace gtsam { return BaseEliminateable::eliminateMultifrontal(ordering, function)->optimize(); } - /* ************************************************************************* */ - VectorValues GaussianFactorGraph::gradient(const VectorValues& x0) const - { - VectorValues g = VectorValues::Zero(x0); - Errors e = gaussianErrors(x0); - transposeMultiplyAdd(1.0, e, g); - return g; - } - /* ************************************************************************* */ namespace { JacobianFactor::shared_ptr convertToJacobianFactorPtr(const GaussianFactor::shared_ptr &gf) { @@ -194,16 +185,25 @@ namespace gtsam { } /* ************************************************************************* */ - VectorValues GaussianFactorGraph::gradientAtZero() const + VectorValues GaussianFactorGraph::gradient(const VectorValues& x0) const { - // Zero-out the gradient - VectorValues g; - Errors e; + VectorValues g = VectorValues::Zero(x0); BOOST_FOREACH(const sharedFactor& Ai_G, *this) { JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - e.push_back(-Ai->getb()); + Vector e = Ai->error_vector(x0); + Ai->transposeMultiplyAdd(1.0, e, g); + } + return g; + } + + /* ************************************************************************* */ + VectorValues GaussianFactorGraph::gradientAtZero() const { + // Zero-out the gradient + VectorValues g; + BOOST_FOREACH(const sharedFactor& factor, *this) { + VectorValues gi = factor->gradientAtZero(); + g.addInPlace_(gi); } - transposeMultiplyAdd(1.0, e, g); return g; } diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index c2644deff..5ee9b8bb5 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -524,6 +524,15 @@ void HessianFactor::multiplyHessianAdd(double alpha, const VectorValues& x, } } +/* ************************************************************************* */ +VectorValues HessianFactor::gradientAtZero() const { + VectorValues g; + size_t n = size(); + for (size_t j = 0; j < n; ++j) + g.insert(keys_[j],-info_(j,n)); + return g; +} + /* ************************************************************************* */ std::pair, boost::shared_ptr > EliminateCholesky(const GaussianFactorGraph& factors, const Ordering& keys) diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 6cd6c817d..6b524c3d1 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -359,6 +359,9 @@ namespace gtsam { /** y += alpha * A'*A*x */ void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y); + /// eta for Hessian + VectorValues gradientAtZero() const; + /** * Densely partially eliminate with Cholesky factorization. JacobianFactors are * left-multiplied with their transpose to form the Hessian using the conversion constructor diff --git a/gtsam/linear/JacobianFactor.cpp b/gtsam/linear/JacobianFactor.cpp index 43bf2bc0d..d0512f794 100644 --- a/gtsam/linear/JacobianFactor.cpp +++ b/gtsam/linear/JacobianFactor.cpp @@ -455,6 +455,17 @@ namespace gtsam { } } + /* ************************************************************************* */ + VectorValues JacobianFactor::gradientAtZero() const { + VectorValues g; + Vector b = getb(); + // Gradient is really -A'*b / sigma^2 + // transposeMultiplyAdd will divide by sigma once, so we need one more + Vector b_sigma = model_ ? model_->whiten(b) : b; + this->transposeMultiplyAdd(-1.0, b_sigma, g); // g -= A'*b/sigma^2 + return g; + } + /* ************************************************************************* */ pair JacobianFactor::jacobian() const { pair result = jacobianUnweighted(); diff --git a/gtsam/linear/JacobianFactor.h b/gtsam/linear/JacobianFactor.h index 55bcedf1d..608560ba8 100644 --- a/gtsam/linear/JacobianFactor.h +++ b/gtsam/linear/JacobianFactor.h @@ -182,27 +182,23 @@ namespace gtsam { virtual Matrix information() const; /** - * Return (dense) matrix associated with factor - * @param ordering of variables needed for matrix column order - * @param set weight to true to bake in the weights + * @brief Returns (dense) A,b pair associated with factor, bakes in the weights */ virtual std::pair jacobian() const; /** - * Return (dense) matrix associated with factor - * @param ordering of variables needed for matrix column order - * @param set weight to true to bake in the weights + * @brief Returns (dense) A,b pair associated with factor, does not bake in weights */ std::pair jacobianUnweighted() const; /** Return (dense) matrix associated with factor. The returned system is an augmented matrix: * [A b] - * @param set weight to use whitening to bake in weights*/ + * weights are baked in */ virtual Matrix augmentedJacobian() const; /** Return (dense) matrix associated with factor. The returned system is an augmented matrix: * [A b] - * @param set weight to use whitening to bake in weights */ + * weights are not baked in */ Matrix augmentedJacobianUnweighted() const; /** Return the full augmented Jacobian matrix of this factor as a VerticalBlockMatrix object. */ @@ -242,13 +238,13 @@ namespace gtsam { /** get a copy of model (non-const version) */ SharedDiagonal& get_model() { return model_; } - /** Get a view of the r.h.s. vector b */ + /** Get a view of the r.h.s. vector b, not weighted by noise */ const constBVector getb() const { return Ab_(size()).col(0); } /** Get a view of the A matrix for the variable pointed to by the given key iterator */ constABlock getA(const_iterator variable) const { return Ab_(variable - begin()); } - /** Get a view of the A matrix */ + /** Get a view of the A matrix, not weighted by noise */ constABlock getA() const { return Ab_.range(0, size()); } /** Get a view of the r.h.s. vector b (non-const version) */ @@ -270,6 +266,9 @@ namespace gtsam { * zero vectors. */ void transposeMultiplyAdd(double alpha, const Vector& e, VectorValues& x) const; + /// A'*b for Jacobian + VectorValues gradientAtZero() const; + /** Return a whitened version of the factor, i.e. with unit diagonal noise model. */ JacobianFactor whiten() const; diff --git a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp index 23bd0137d..c0c3c5a01 100644 --- a/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp @@ -10,10 +10,11 @@ * -------------------------------------------------------------------------- */ /** - * @file testGaussianFactor.cpp + * @file testGaussianFactorGraphUnordered.cpp * @brief Unit tests for Linear Factor * @author Christian Potthast * @author Frank Dellaert + * @author Richard Roberts **/ #include // for operator += diff --git a/gtsam/linear/tests/testHessianFactorUnordered.cpp b/gtsam/linear/tests/testHessianFactorUnordered.cpp index 0ec14d80e..94f4efb36 100644 --- a/gtsam/linear/tests/testHessianFactorUnordered.cpp +++ b/gtsam/linear/tests/testHessianFactorUnordered.cpp @@ -90,9 +90,6 @@ TEST(HessianFactor, Constructor1) Vector g = (Vec(2) << -8.0, -9.0); double f = 10.0; - VectorValues dx = pair_list_of - (0, (Vec(2) << 1.5, 2.5)); - HessianFactor factor(0, G, g, f); // extract underlying parts @@ -101,6 +98,8 @@ TEST(HessianFactor, Constructor1) EXPECT(assert_equal(g, Vector(factor.linearTerm()))); EXPECT_LONGS_EQUAL(1, (long)factor.size()); + VectorValues dx = pair_list_of(0, (Vec(2) << 1.5, 2.5)); + // error 0.5*(f - 2*x'*g + x'*G*x) double expected = 80.375; double actual = factor.error(dx); @@ -109,6 +108,7 @@ TEST(HessianFactor, Constructor1) EXPECT_DOUBLES_EQUAL(expected, actual, 1e-10); } + /* ************************************************************************* */ TEST(HessianFactor, Constructor1b) { @@ -429,6 +429,27 @@ TEST(HessianFactor, combine) { } +/* ************************************************************************* */ +TEST(HessianFactor, gradientAtZero) +{ + Matrix G11 = Matrix_(1,1, 1.0); + Matrix G12 = Matrix_(1,2, 0.0, 0.0); + Matrix G22 = Matrix_(2,2, 1.0, 0.0, 0.0, 1.0); + Vector g1 = (Vec(1) << -7.0); + Vector g2 = (Vec(2) << -8.0, -9.0); + double f = 194; + + HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); + + // test gradient at zero + VectorValues expectedG = pair_list_of(0, -g1) (1, -g2); + Matrix A; Vector b; boost::tie(A,b) = factor.jacobian(); + FastVector keys; keys += 0,1; + EXPECT(assert_equal(-A.transpose()*b, expectedG.vector(keys))); + VectorValues actualG = factor.gradientAtZero(); + EXPECT(assert_equal(expectedG, actualG)); +} + /* ************************************************************************* */ int main() { TestResult tr; return TestRegistry::runAllTests(tr);} /* ************************************************************************* */ diff --git a/gtsam/linear/tests/testJacobianFactorUnordered.cpp b/gtsam/linear/tests/testJacobianFactorUnordered.cpp index 000a11544..e3fe75d61 100644 --- a/gtsam/linear/tests/testJacobianFactorUnordered.cpp +++ b/gtsam/linear/tests/testJacobianFactorUnordered.cpp @@ -24,6 +24,7 @@ #include #include +#include #include #include #include @@ -258,6 +259,16 @@ TEST(JacobianFactor, operators ) VectorValues actualX = VectorValues::Zero(expectedX); lf.transposeMultiplyAdd(1.0, actualE, actualX); EXPECT(assert_equal(expectedX, actualX)); + + // test gradient at zero + Matrix A; Vector b2; boost::tie(A,b2) = lf.jacobian(); + VectorValues expectedG; + expectedG.insert(1, (Vec(2) << 20,-10)); + expectedG.insert(2, (Vec(2) << -20, 10)); + FastVector keys; keys += 1,2; + EXPECT(assert_equal(-A.transpose()*b2, expectedG.vector(keys))); + VectorValues actualG = lf.gradientAtZero(); + EXPECT(assert_equal(expectedG, actualG)); } /* ************************************************************************* */