diff --git a/gtsam/linear/GaussianFactorGraph.cpp b/gtsam/linear/GaussianFactorGraph.cpp index ad43b675b..4dd1af4e7 100644 --- a/gtsam/linear/GaussianFactorGraph.cpp +++ b/gtsam/linear/GaussianFactorGraph.cpp @@ -355,14 +355,6 @@ namespace gtsam { f->multiplyHessianAdd(alpha, x, y); } - ///* ************************************************************************* */ - //void GaussianFactorGraph::multiplyHessianAdd(double alpha, - // const double* x, double* y) const { - //vector FactorKeys = getkeydim(); - //BOOST_FOREACH(const GaussianFactor::shared_ptr& f, *this) - // f->multiplyHessianAdd(alpha, x, y, FactorKeys); - //} - /* ************************************************************************* */ void GaussianFactorGraph::multiplyInPlace(const VectorValues& x, Errors& e) const { multiplyInPlace(x, e.begin()); @@ -392,42 +384,15 @@ namespace gtsam { /* ************************************************************************* */ // x += alpha*A'*e -void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, - VectorValues& x) const { - // For each factor add the gradient contribution - Errors::const_iterator ei = e.begin(); - BOOST_FOREACH(const sharedFactor& Ai_G, *this) { - JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - Ai->transposeMultiplyAdd(alpha, *(ei++), x); + void GaussianFactorGraph::transposeMultiplyAdd(double alpha, const Errors& e, + VectorValues& x) const { + // For each factor add the gradient contribution + Errors::const_iterator ei = e.begin(); + BOOST_FOREACH(const sharedFactor& Ai_G, *this) { + JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); + Ai->transposeMultiplyAdd(alpha, *(ei++), x); + } } -} - - ///* ************************************************************************* */ - //void residual(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { - // Key i = 0 ; - // BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - // r[i] = Ai->getb(); - // i++; - // } - // VectorValues Ax = VectorValues::SameStructure(r); - // multiply(fg,x,Ax); - // axpy(-1.0,Ax,r); - //} - - ///* ************************************************************************* */ - //void multiply(const GaussianFactorGraph& fg, const VectorValues &x, VectorValues &r) { - // r.setZero(); - // Key i = 0; - // BOOST_FOREACH(const GaussianFactor::shared_ptr& Ai_G, fg) { - // JacobianFactor::shared_ptr Ai = convertToJacobianFactorPtr(Ai_G); - // Vector &y = r[i]; - // for(JacobianFactor::const_iterator j = Ai->begin(); j != Ai->end(); ++j) { - // y += Ai->getA(j) * x[*j]; - // } - // ++i; - // } - //} /* ************************************************************************* */ VectorValues GaussianFactorGraph::transposeMultiply(const Errors& e) const diff --git a/gtsam/linear/GaussianFactorGraph.h b/gtsam/linear/GaussianFactorGraph.h index 1861e2607..9b8a91a6e 100644 --- a/gtsam/linear/GaussianFactorGraph.h +++ b/gtsam/linear/GaussianFactorGraph.h @@ -310,10 +310,6 @@ namespace gtsam { void multiplyHessianAdd(double alpha, const VectorValues& x, VectorValues& y) const; - ///** y += alpha*A'A*x */ - //void multiplyHessianAdd(double alpha, const double* x, - // double* y) const; - ///** In-place version e <- A*x that overwrites e. */ void multiplyInPlace(const VectorValues& x, Errors& e) const; diff --git a/gtsam/linear/tests/testGaussianFactorGraph.cpp b/gtsam/linear/tests/testGaussianFactorGraph.cpp index 2fc1e359b..49a6e30e4 100644 --- a/gtsam/linear/tests/testGaussianFactorGraph.cpp +++ b/gtsam/linear/tests/testGaussianFactorGraph.cpp @@ -315,31 +315,6 @@ TEST( GaussianFactorGraph, multiplyHessianAdd2 ) EXPECT(assert_equal(2*expected, actual)); } -/* ************************************************************************* */ -// Raw memory access -//TEST( GaussianFactorGraph, multiplyHessianAdd3 ) -//{ -// 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)); - -// double* x = &X[0]; - -// Vector fast_y = gtsam::zero(6); -// gfg.multiplyHessianAdd(1.0, x, fast_y.data()); -// EXPECT(assert_equal(Y, fast_y)); - -// // now, do it with non-zero y -// gfg.multiplyHessianAdd(1.0, x, fast_y.data()); -// EXPECT(assert_equal(2*Y, fast_y)); - -//} - - /* ************************************************************************* */ TEST( GaussianFactorGraph, matricesMixed ) { @@ -352,7 +327,6 @@ TEST( GaussianFactorGraph, matricesMixed ) EXPECT(assert_equal(A.transpose()*b, eta)); } - /* ************************************************************************* */ TEST( GaussianFactorGraph, gradientAtZero ) {