12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Fixed HessianFactor error to be consistent with JacobianFactor error and added unit test.

release/4.3a0
Richard Roberts 2011-04-12 22:06:33 +00:00
parent 5057138ef1
commit 7a0daaa7d9
3 changed files with 13 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
gtsam/linear/HessianFactor.cpp
View File

@ -210,12 +210,12 @@ namespace gtsam {
/* ************************************************************************* */
double HessianFactor::error(const VectorValues& c) const {
return ublas::inner_prod(c.vector(),
return 0.5 * (ublas::inner_prod(c.vector(),
ublas::prod(
ublas::symmetric_adaptor<const constBlock,ublas::upper>(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);
info_(this->size(), this->size())(0,0));
}
void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) {

8
gtsam/linear/HessianFactor.h
View File

@ -47,7 +47,7 @@ namespace gtsam {
typedef MatrixColMajor InfoMatrix;
typedef SymmetricBlockView<InfoMatrix> BlockInfo;
InfoMatrix matrix_; // The full information matrix, s.t. the quadratic error is [x -1]'*H*[x -1]
InfoMatrix matrix_; // The full information matrix, s.t. the quadratic error is 0.5*[x -1]'*H*[x -1]
BlockInfo info_; // The block view of the full information matrix.
void updateATA(const JacobianFactor& update, const Scatter& scatter);
@ -68,14 +68,14 @@ namespace gtsam {
/** 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
* 0.5*(f - 2*x'*g + x'*G*x)
*/
HessianFactor(Index j, const Matrix& G, const Vector& g, double f);
/** 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
* 0.5*(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,
@ -98,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; /** [x -1]'*H*[x -1] (also see constructor documentation) */
virtual double error(const VectorValues& c) const; /** 0.5*[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?

11
gtsam/linear/tests/testHessianFactor.cpp
View File

@ -31,7 +31,6 @@ using namespace gtsam;
using namespace std;
/* ************************************************************************* */
#ifdef BROKEN // because accesses keys_, now private
TEST(HessianFactor, ConversionConstructor) {
HessianFactor expected;
@ -84,9 +83,13 @@ TEST(HessianFactor, ConversionConstructor) {
HessianFactor actual(combined);
VectorValues values(std::vector<size_t>(dims, dims+2));
values[0] = Vector_(2, 1.0, 2.0);
values[1] = Vector_(4, 3.0, 4.0, 5.0, 6.0);
DOUBLES_EQUAL(combined.error(values), actual.error(values), 1e-9);
EXPECT(assert_equal(expected, actual, 1e-9));
}
#endif
/* ************************************************************************* */
TEST(HessianFactor, Constructor1)
@ -105,7 +108,7 @@ TEST(HessianFactor, Constructor1)
HessianFactor factor(0, G, g, f);
double expected = 160.75;
double expected = 80.375;
double actual = factor.error(dx);
DOUBLES_EQUAL(expected, actual, 1e-10);
@ -134,7 +137,7 @@ TEST(HessianFactor, Constructor2)
HessianFactor factor(0, 1, G11, G12, g1, G22, g2, f);
double expected = 181.0;
double expected = 90.5;
double actual = factor.error(dx);
DOUBLES_EQUAL(expected, actual, 1e-10);