From 660127489daf87c630be8ce06016dc70d8a60768 Mon Sep 17 00:00:00 2001 From: Richard Roberts Date: Wed, 7 Sep 2011 15:42:25 +0000 Subject: [PATCH] HessianFactor documentation --- gtsam/linear/HessianFactor.cpp | 8 +-- gtsam/linear/HessianFactor.h | 63 +++++++++++++++++------- gtsam/linear/tests/testHessianFactor.cpp | 16 +++--- 3 files changed, 58 insertions(+), 29 deletions(-) diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index cf6b779af..bed5ff1d6 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -217,20 +217,20 @@ bool HessianFactor::equals(const GaussianFactor& lf, double tol) const { } /* ************************************************************************* */ -double HessianFactor::constant_term() const { +double HessianFactor::constantTerm() const { return info_(this->size(), this->size())(0,0); } /* ************************************************************************* */ -HessianFactor::constColumn HessianFactor::linear_term() const { +HessianFactor::constColumn HessianFactor::linearTerm() const { return info_.rangeColumn(0, this->size(), this->size(), 0); } /* ************************************************************************* */ double HessianFactor::error(const VectorValues& c) const { // error 0.5*(f - 2*x'*g + x'*G*x) - const double f = constant_term(); - const double xtg = c.vector().dot(linear_term()); + const double f = constantTerm(); + const double xtg = c.vector().dot(linearTerm()); const double xGx = c.vector().transpose() * info_.range(0, this->size(), 0, this->size()).selfadjointView() * c.vector(); return 0.5 * (f - 2.0 * xtg + xGx); diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index d5f35c9b2..ab380a7d4 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -51,14 +51,14 @@ namespace gtsam { /** * A general quadratic factor of the form - * f(x) = x'Hx + hx + c - * and stores the matrix H, the vector h, and the constant term c. + * \f[ e(x) = x^T G x + gx + f \f] + * that stores the matrix \f$ G \f$, the vector \f$ g \f$, and the constant term \f$ f \f$. * - * When H is positive semidefinite, this factor represents a Gaussian, - * in which case H is the information - * matrix \Lambda, which is the inverse of the covariance matrix \Sigma, - * h is the information vector \eta = \Lambda \mu, and c is the error - * at the mean, when x = \mu. + * When \f$ G \f$ is positive semidefinite, this factor represents a Gaussian, + * in which case \f$ G \f$ is the information + * matrix \f$ \Lambda \f$, which is the inverse of the covariance matrix \f$ \Sigma \f$, + * \f$ g \f$ is the information vector \f$ \eta = \Lambda \mu \f$, and \f$ f \f$ is the error + * at the mean, when \f$ x = \mu \f$ . * * This factor is one of the factors that can be in a GaussianFactorGraph. * It may be returned from NonlinearFactor::linearize(), but is also @@ -66,9 +66,10 @@ namespace gtsam { * * This can represent a quadratic factor with characteristics that cannot be * represented using a JacobianFactor (which has the form - * f(x) = || Ax - b ||^2 and stores the Jacobian A and error vector b, i.e. - * is a sum-of-squares factor). For example, a HessianFactor need not be - * positive semidefinite, it can be indefinite or even negative semidefinite. + * \f$ e(x) = \Vert Ax - b \Vert^2 \f$ and stores the Jacobian \f$ A \f$ + * and error vector \f$ b \f$, i.e. is a sum-of-squares factor). For example, + * a HessianFactor need not be positive semidefinite, it can be indefinite or + * even negative semidefinite. * * If a HessianFactor is indefinite or negative semi-definite, then in order * for solving the linear system to be possible, @@ -76,7 +77,26 @@ namespace gtsam { * small Hessians are combined, the result must be positive definite). If * this is not the case, an error will occur during elimination. * - * This class stores H, h, and c as an augmented matrix HessianFactor::matrix_. + * This class stores G, g, and f as an augmented matrix HessianFactor::matrix_. + * The upper-left n x n blocks of HessianFactor::matrix_ store the upper-right + * triangle of G, the upper-right-most column of length n of HessianFactor::matrix_ + * stores g, and the lower-right entry of HessianFactor::matrix_ stores f, i.e. + * \code + HessianFactor::matrix_ = [ G11 G12 G13 ... g1 + 0 G22 G23 ... g2 + 0 0 G33 ... g3 + : : : : + 0 0 0 ... f ] + \endcode + Blocks can be accessed as follows: + \code + G11 = info(begin(), begin()); + G12 = info(begin(), begin()+1); + G23 = info(begin()+1, begin()+2); + g2 = linearTerm(begin()+1); + f = constantTerm(); + ....... + \endcode */ class HessianFactor : public GaussianFactor { protected: @@ -167,20 +187,29 @@ namespace gtsam { /** Return the number of columns and rows of the Hessian matrix */ size_t rows() const { return info_.rows(); } - /** Return a view of the block at (j1,j2) of the information matrix + /** Return a view of the block at (j1,j2) of the information matrix \f$ H \f$, no data is copied. * @param j1 Which block row to get, as an iterator pointing to the slot in this factor. You can * use, for example, begin() + 2 to get the 3rd variable in this factor. * @param j2 Which block column to get, as an iterator pointing to the slot in this factor. You can * use, for example, begin() + 2 to get the 3rd variable in this factor. - * @return + * @return A view of the requested block, not a copy. */ constBlock info(const_iterator j1, const_iterator j2) const { return info_(j1-begin(), j2-begin()); } - /** returns the constant term f as described above */ - double constant_term() const; + /** Return the constant term \f$ f \f$ as described above + * @return The constant term \f$ f \f$ + */ + double constantTerm() const; - /** returns the linear term g as described above */ - constColumn linear_term() const; + /** Return the part of linear term \f$ g \f$ as described above corresponding to the requested variable. + * @param j Which block row to get, as an iterator pointing to the slot in this factor. You can + * use, for example, begin() + 2 to get the 3rd variable in this factor. + * @return The linear term \f$ g \f$ */ + constColumn linearTerm(const_iterator j) const { return info_.column(j-begin(), size(), 0); } + + /** Return the complete linear term \f$ g \f$ as described above. + * @return The linear term \f$ g \f$ */ + constColumn linearTerm() const; /** * Permutes the GaussianFactor, but for efficiency requires the permutation diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index a4cf88487..2e35515f3 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -121,8 +121,8 @@ TEST(HessianFactor, Constructor1) // extract underlying parts Matrix info_matrix = factor.info_.range(0, 1, 0, 1); EXPECT(assert_equal(Matrix(G), info_matrix)); - EXPECT_DOUBLES_EQUAL(f, factor.constant_term(), 1e-10); - EXPECT(assert_equal(g, Vector(factor.linear_term()), 1e-10)); + EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10); + EXPECT(assert_equal(g, Vector(factor.linearTerm()), 1e-10)); EXPECT_LONGS_EQUAL(1, factor.size()); // error 0.5*(f - 2*x'*g + x'*G*x) @@ -149,8 +149,8 @@ TEST(HessianFactor, Constructor1b) // Check Matrix info_matrix = factor.info_.range(0, 1, 0, 1); EXPECT(assert_equal(Matrix(G), info_matrix)); - EXPECT_DOUBLES_EQUAL(f, factor.constant_term(), 1e-10); - EXPECT(assert_equal(g, Vector(factor.linear_term()), 1e-10)); + EXPECT_DOUBLES_EQUAL(f, factor.constantTerm(), 1e-10); + EXPECT(assert_equal(g, Vector(factor.linearTerm()), 1e-10)); EXPECT_LONGS_EQUAL(1, factor.size()); } @@ -182,10 +182,10 @@ TEST(HessianFactor, Constructor2) DOUBLES_EQUAL(expected, actual, 1e-10); LONGS_EQUAL(4, factor.rows()); - DOUBLES_EQUAL(10.0, factor.constant_term(), 1e-10); + DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10); Vector linearExpected(3); linearExpected << g1, g2; - EXPECT(assert_equal(linearExpected, factor.linear_term())); + EXPECT(assert_equal(linearExpected, factor.linearTerm())); EXPECT(assert_equal(G11, factor.info(factor.begin(), factor.begin()))); EXPECT(assert_equal(G12, factor.info(factor.begin(), factor.begin()+1))); @@ -222,10 +222,10 @@ TEST_UNSAFE(HessianFactor, CopyConstructor) DOUBLES_EQUAL(expected, actual, 1e-10); LONGS_EQUAL(4, factor.rows()); - DOUBLES_EQUAL(10.0, factor.constant_term(), 1e-10); + DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10); Vector linearExpected(3); linearExpected << g1, g2; - EXPECT(assert_equal(linearExpected, factor.linear_term())); + EXPECT(assert_equal(linearExpected, factor.linearTerm())); EXPECT(assert_equal(G11, factor.info(factor.begin(), factor.begin()))); EXPECT(assert_equal(G12, factor.info(factor.begin(), factor.begin()+1)));