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

Created HessianFactor constructors from Hessian matrix, linear, and constant terms. Removed old constructors from Jacobians that were just mirroring the JacobianFactor interface.

release/4.3a0
Richard Roberts 2011-04-11 19:21:05 +00:00
parent c9a6b16bc2
commit 9f1bf3e475
3 changed files with 109 additions and 83 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
gtsam/linear/HessianFactor.cpp
View File

@ -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<std::pair<Index, Matrix> > &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<std::pair<Index, Matrix> > &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<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);
}
void HessianFactor::updateATA(const HessianFactor& update, const Scatter& scatter) {

40
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 [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<std::pair<Index, Matrix> > &terms,
const Vector &b, const SharedDiagonal& model);
HessianFactor(const std::list<std::pair<Index, Matrix> > &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?

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

@ -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<size_t> 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<size_t> 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);