diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index 452a7a87b..734c3fb72 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -77,60 +77,38 @@ namespace gtsam { } /* ************************************************************************* */ - HessianFactor::HessianFactor(const Vector& b_in) : info_(matrix_) { - JacobianFactor jf(b_in); - info_.copyStructureFrom(jf.Ab_); - ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); + HessianFactor::HessianFactor(Index j1, const Matrix& G, const Vector& g, double f) : + GaussianFactor(j1), info_(matrix_) { + if(G.size1() != G.size2() || G.size1() != g.size()) + throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + size_t dims[] = { G.size1(), 1 }; + InfoMatrix fullMatrix(G.size1() + 1, G.size1() + 1); + BlockInfo infoMatrix(fullMatrix, dims, dims+2); + infoMatrix(0,0) = G; + infoMatrix.column(0,1,0) = g; + infoMatrix(1,1)(0,0) = f; + infoMatrix.swap(info_); assertInvariants(); } /* ************************************************************************* */ - HessianFactor::HessianFactor(Index i1, const Matrix& A1, - const Vector& b, const SharedDiagonal& model) : - GaussianFactor(i1), info_(matrix_) { - JacobianFactor jf(i1, A1, b, model); - info_.copyStructureFrom(jf.Ab_); - ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); - assertInvariants(); - } - - /* ************************************************************************* */ - HessianFactor::HessianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2, - const Vector& b, const SharedDiagonal& model) : - GaussianFactor(i1,i2), info_(matrix_) { - JacobianFactor jf(i1, A1, i2, A2, b, model); - info_.copyStructureFrom(jf.Ab_); - ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); - assertInvariants(); - } - - /* ************************************************************************* */ - HessianFactor::HessianFactor(Index i1, const Matrix& A1, Index i2, const Matrix& A2, - Index i3, const Matrix& A3, const Vector& b, const SharedDiagonal& model) : - GaussianFactor(i1,i2,i3), info_(matrix_) { - JacobianFactor jf(i1, A1, i2, A2, i3, A3, b, model); - info_.copyStructureFrom(jf.Ab_); - ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); - assertInvariants(); - } - - /* ************************************************************************* */ - HessianFactor::HessianFactor(const std::vector > &terms, - const Vector &b, const SharedDiagonal& model) : info_(matrix_) { - JacobianFactor jf(terms, b, model); - keys_ = jf.keys_; - info_.copyStructureFrom(jf.Ab_); - ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); - assertInvariants(); - } - - /* ************************************************************************* */ - HessianFactor::HessianFactor(const std::list > &terms, - const Vector &b, const SharedDiagonal& model) : info_(matrix_) { - JacobianFactor jf(terms, b, model); - keys_ = jf.keys_; - info_.copyStructureFrom(jf.Ab_); - ublas::noalias(matrix_) = ublas::prod(ublas::trans(jf.matrix_), jf.matrix_); + HessianFactor::HessianFactor(Index j1, Index j2, + const Matrix& G11, const Matrix& G12, const Vector& g1, + const Matrix& G22, const Vector& g2, double f) : + GaussianFactor(j1, j2), info_(matrix_) { + if(G11.size1() != G11.size2() || G11.size1() != G12.size1() || G11.size1() != g1.size() || + G22.size2() != G12.size2() || G22.size2() != g2.size()) + throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor"); + size_t dims[] = { G11.size1(), G22.size1(), 1 }; + InfoMatrix fullMatrix(G11.size1() + G22.size1() + 1, G11.size1() + G22.size1() + 1); + BlockInfo infoMatrix(fullMatrix, dims, dims+3); + infoMatrix(0,0) = G11; + infoMatrix(0,1) = G12; + infoMatrix.column(0,2,0) = g1; + infoMatrix(1,1) = G22; + infoMatrix.column(1,2,0) = g2; + infoMatrix(2,2)(0,0) = f; + infoMatrix.swap(info_); assertInvariants(); } @@ -232,8 +210,12 @@ namespace gtsam { /* ************************************************************************* */ double HessianFactor::error(const VectorValues& c) const { - return ublas::inner_prod(c.vector(), ublas::prod(info_.range(0, this->size(), 0, this->size()), c.vector())) - - 2.0*ublas::inner_prod(c.vector(), info_.rangeColumn(0, this->size(), this->size(), 0)); + return ublas::inner_prod(c.vector(), + ublas::prod( + ublas::symmetric_adaptor(info_.range(0, this->size(), 0, this->size())), + c.vector())) - + 2.0*ublas::inner_prod(c.vector(), info_.rangeColumn(0, this->size(), this->size(), 0)) + + info_(this->size(), this->size())(0,0); } void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) { diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index 2fd1e8b5f..2906a231a 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -47,7 +47,7 @@ namespace gtsam { typedef MatrixColMajor InfoMatrix; typedef SymmetricBlockView BlockInfo; - InfoMatrix matrix_; // The full information matrix [A b]^T * [A b] + InfoMatrix matrix_; // The full information matrix, s.t. the quadratic error is [x -1]'*H*[x -1] BlockInfo info_; // The block view of the full information matrix. void updateATA(const JacobianFactor& update, const Scatter& scatter); @@ -65,29 +65,21 @@ namespace gtsam { /** default constructor for I/O */ HessianFactor(); - /** Construct Null factor */ - HessianFactor(const Vector& b_in); + /** Construct a unary factor. G is the quadratic term (Hessian matrix), g + * the linear term (a vector), and f the constant term. The quadratic + * error is: + * f - 2*x'*g + x'*G*x + */ + HessianFactor(Index j, const Matrix& G, const Vector& g, double f); - /** Construct unary factor */ - HessianFactor(Index i1, const Matrix& A1, - const Vector& b, const SharedDiagonal& model); - - /** Construct binary factor */ - HessianFactor(Index i1, const Matrix& A1, - Index i2, const Matrix& A2, - const Vector& b, const SharedDiagonal& model); - - /** Construct ternary factor */ - HessianFactor(Index i1, const Matrix& A1, Index i2, - const Matrix& A2, Index i3, const Matrix& A3, - const Vector& b, const SharedDiagonal& model); - - /** Construct an n-ary factor */ - HessianFactor(const std::vector > &terms, - const Vector &b, const SharedDiagonal& model); - - HessianFactor(const std::list > &terms, - const Vector &b, const SharedDiagonal& model); + /** Construct a binary factor. Gxx are the upper-triangle blocks of the + * quadratic term (the Hessian matrix), gx the pieces of the linear vector + * term, and f the constant term. The quadratic error is: + * f - 2*x1'*g1 - 2*x2'*g2 + x1'*G11*x1 + x1'*G12*x2 + */ + HessianFactor(Index j1, Index j2, + const Matrix& G11, const Matrix& G12, const Vector& g1, + const Matrix& G22, const Vector& g2, double f); /** Construct from Conditional Gaussian */ HessianFactor(const GaussianConditional& cg); @@ -106,7 +98,7 @@ namespace gtsam { virtual void print(const std::string& s = "") const; virtual bool equals(const GaussianFactor& lf, double tol = 1e-9) const; - virtual double error(const VectorValues& c) const; /** 0.5*(A*x-b)'*D*(A*x-b) */ + virtual double error(const VectorValues& c) const; /** [x -1]'*H*[x -1] (also see constructor documentation) */ /** Return the dimension of the variable pointed to by the given key iterator * todo: Remove this in favor of keeping track of dimensions with variables? diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index 0911f06bd..64ae7b845 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -89,7 +89,59 @@ TEST(HessianFactor, ConversionConstructor) { #endif /* ************************************************************************* */ -TEST(GaussianFactor, CombineAndEliminate) +TEST(HessianFactor, Constructor1) +{ + Matrix G = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); + Vector g = Vector_(2, -8.0, -9.0); + double f = 10.0; + + Vector dxv = Vector_(2, 1.5, 2.5); + + vector dims; + dims.push_back(2); + VectorValues dx(dims); + + dx[0] = dxv; + + HessianFactor factor(0, G, g, f); + + double expected = 160.75; + double actual = factor.error(dx); + + DOUBLES_EQUAL(expected, actual, 1e-10); +} + +/* ************************************************************************* */ +TEST(HessianFactor, Constructor2) +{ + Matrix G11 = Matrix_(1,1, 1.0); + Matrix G12 = Matrix_(1,2, 2.0, 4.0); + Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0); + Vector g1 = Vector_(1, -7.0); + Vector g2 = Vector_(2, -8.0, -9.0); + double f = 10.0; + + Vector dx0 = Vector_(1, 0.5); + Vector dx1 = Vector_(2, 1.5, 2.5); + + vector dims; + dims.push_back(1); + dims.push_back(2); + VectorValues dx(dims); + + dx[0] = dx0; + dx[1] = dx1; + + HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f); + + double expected = 181.0; + double actual = factor.error(dx); + + DOUBLES_EQUAL(expected, actual, 1e-10); +} + +/* ************************************************************************* */ +TEST(HessianFactor, CombineAndEliminate) { Matrix A01 = Matrix_(3,3, 1.0, 0.0, 0.0, @@ -140,7 +192,7 @@ TEST(GaussianFactor, CombineAndEliminate) } /* ************************************************************************* */ -TEST(GaussianFactor, eliminate2 ) +TEST(HessianFactor, eliminate2 ) { // sigmas double sigma1 = 0.2; @@ -213,7 +265,7 @@ TEST(GaussianFactor, eliminate2 ) } /* ************************************************************************* */ -TEST(GaussianFactor, eliminateUnsorted) { +TEST(HessianFactor, eliminateUnsorted) { JacobianFactor::shared_ptr factor1( new JacobianFactor(0, @@ -229,7 +281,7 @@ TEST(GaussianFactor, eliminateUnsorted) { Vector_(3, 1.98916e-17, -4.96503e-15, -7.75792e-17), noiseModel::Unit::Create(3))); HessianFactor::shared_ptr unsorted_factor2( - new HessianFactor(0, + new HessianFactor(JacobianFactor(0, Matrix_(6,3, 25.8367, 0.1211, 0.0593, 0.0, 23.4099, 30.8733, @@ -246,14 +298,14 @@ TEST(GaussianFactor, eliminateUnsorted) { 0.0, 0.9973, 0.9517, 0.0, 0.0, 0.9973), Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), - noiseModel::Unit::Create(6))); + noiseModel::Unit::Create(6)))); Permutation permutation(2); permutation[0] = 1; permutation[1] = 0; unsorted_factor2->permuteWithInverse(permutation); HessianFactor::shared_ptr sorted_factor2( - new HessianFactor(0, + new HessianFactor(JacobianFactor(0, Matrix_(6,3, 25.7429, -1.6897, 0.4587, 1.6400, 23.3095, -8.4816, @@ -270,7 +322,7 @@ TEST(GaussianFactor, eliminateUnsorted) { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), Vector_(6, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0), - noiseModel::Unit::Create(6))); + noiseModel::Unit::Create(6)))); GaussianFactorGraph sortedGraph; // sortedGraph.push_back(factor1);