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

Added an n-way constructor to the HessianFactor

release/4.3a0
Stephen Williams 2012-01-28 06:20:02 +00:00
parent 636176bf13
commit 918019c605
3 changed files with 178 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

60
gtsam/linear/HessianFactor.cpp
View File

@ -155,6 +155,66 @@ HessianFactor::HessianFactor(Index j1, Index j2, Index j3,
assertInvariants();
}
/* ************************************************************************* */
HessianFactor::HessianFactor(const std::vector<Index>& js, const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs, double f) : GaussianFactor(js), info_(matrix_) {
// Get the number of variables
size_t variable_count = js.size();
// Verify the provided number of entries in the vectors are consistent
if(gs.size() != variable_count || Gs.size() != (variable_count*(variable_count+1))/2)
throw invalid_argument("Inconsistent number of entries between js, Gs, and gs in HessianFactor constructor.\nThe number of keys provided \
in js must match the number of linear vector pieces in gs. The number of upper-diagonal blocks in Gs must be n*(n+1)/2");
// Verify the dimensions of each provided matrix are consistent
// Note: equations for calculating the indices derived from the "sum of an arithmetic sequence" formula
for(size_t i = 0; i < variable_count; ++i){
int block_size = gs[i].size();
// Check rows
for(size_t j = 0; j < variable_count-i; ++j){
size_t index = i*(2*variable_count - i + 1)/2 + j;
if(Gs[index].rows() != block_size){
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
}
}
// Check cols
for(size_t j = 0; j <= i; ++j){
size_t index = j*(2*variable_count - j + 1)/2 + (i-j);
if(Gs[index].cols() != block_size){
throw invalid_argument("Inconsistent matrix and/or vector dimensions in HessianFactor constructor");
}
}
}
// Create the dims vector
size_t dims[variable_count+1];
size_t total_size = 0;
for(unsigned int i = 0; i < variable_count; ++i){
dims[i] = gs[i].size();
total_size += gs[i].size();
}
dims[variable_count] = 1;
total_size += 1;
// Fill in the internal matrix with the supplied blocks
InfoMatrix fullMatrix(total_size, total_size);
BlockInfo infoMatrix(fullMatrix, dims, dims+variable_count+1);
size_t index = 0;
for(size_t i = 0; i < variable_count; ++i){
for(size_t j = i; j < variable_count; ++j){
infoMatrix(i,j) = Gs[index++];
}
infoMatrix.column(i,variable_count,0) = gs[i];
}
infoMatrix(variable_count,variable_count)(0,0) = f;
// update the BlockView variable
infoMatrix.swap(info_);
assertInvariants();
}
/* ************************************************************************* */
HessianFactor::HessianFactor(const GaussianConditional& cg) : GaussianFactor(cg), info_(matrix_) {
JacobianFactor jf(cg);

7
gtsam/linear/HessianFactor.h
View File

@ -189,6 +189,13 @@ namespace gtsam {
const Matrix& G22, const Matrix& G23, const Vector& g2,
const Matrix& G33, const Vector& g3, double f);
/** Construct an n-way factor. Gs contains the upper-triangle blocks of the
* quadratic term (the Hessian matrix) provided in row-order, gs the pieces
* of the linear vector term, and f the constant term.
*/
HessianFactor(const std::vector<Index>& js, const std::vector<Matrix>& Gs,
const std::vector<Vector>& gs, double f);
/** Construct from Conditional Gaussian */
HessianFactor(const GaussianConditional& cg);

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

@ -138,7 +138,6 @@ TEST(HessianFactor, Constructor1)
double actual = factor.error(dx);
double expected_manual = 0.5 * (f - 2.0 * dxv.dot(g) + dxv.transpose() * G.selfadjointView<Eigen::Upper>() * dxv);
EXPECT_DOUBLES_EQUAL(expected, expected_manual, 1e-10);
DOUBLES_EQUAL(expected, actual, 1e-10);
}
@ -200,6 +199,117 @@ TEST(HessianFactor, Constructor2)
EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1)));
}
/* ************************************************************************* */
TEST(HessianFactor, Constructor3)
{
Matrix G11 = Matrix_(1,1, 1.0);
Matrix G12 = Matrix_(1,2, 2.0, 4.0);
Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0);
Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, 1.0, 2.0, 4.0);
Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0);
Vector g1 = Vector_(1, -7.0);
Vector g2 = Vector_(2, -8.0, -9.0);
Vector g3 = Vector_(3, 1.0, 2.0, 3.0);
double f = 10.0;
Vector dx0 = Vector_(1, 0.5);
Vector dx1 = Vector_(2, 1.5, 2.5);
Vector dx2 = Vector_(3, 1.5, 2.5, 3.5);
vector<size_t> dims;
dims.push_back(1);
dims.push_back(2);
dims.push_back(3);
VectorValues dx(dims);
dx[0] = dx0;
dx[1] = dx1;
dx[2] = dx2;
HessianFactor factor(0, 1, 2, G11, G12, G13, g1, G22, G23, g2, G33, g3, f);
double expected = 371.3750;
double actual = factor.error(dx);
DOUBLES_EQUAL(expected, actual, 1e-10);
LONGS_EQUAL(7, factor.rows());
DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10);
Vector linearExpected(6); linearExpected << g1, g2, g3;
EXPECT(assert_equal(linearExpected, factor.linearTerm()));
EXPECT(assert_equal(G11, factor.info(factor.begin()+0, factor.begin()+0)));
EXPECT(assert_equal(G12, factor.info(factor.begin()+0, factor.begin()+1)));
EXPECT(assert_equal(G13, factor.info(factor.begin()+0, factor.begin()+2)));
EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1)));
EXPECT(assert_equal(G23, factor.info(factor.begin()+1, factor.begin()+2)));
EXPECT(assert_equal(G33, factor.info(factor.begin()+2, factor.begin()+2)));
}
/* ************************************************************************* */
TEST(HessianFactor, ConstructorNWay)
{
Matrix G11 = Matrix_(1,1, 1.0);
Matrix G12 = Matrix_(1,2, 2.0, 4.0);
Matrix G13 = Matrix_(1,3, 3.0, 6.0, 9.0);
Matrix G22 = Matrix_(2,2, 3.0, 5.0, 0.0, 6.0);
Matrix G23 = Matrix_(2,3, 4.0, 6.0, 8.0, 1.0, 2.0, 4.0);
Matrix G33 = Matrix_(3,3, 1.0, 2.0, 3.0, 0.0, 5.0, 6.0, 0.0, 0.0, 9.0);
Vector g1 = Vector_(1, -7.0);
Vector g2 = Vector_(2, -8.0, -9.0);
Vector g3 = Vector_(3, 1.0, 2.0, 3.0);
double f = 10.0;
Vector dx0 = Vector_(1, 0.5);
Vector dx1 = Vector_(2, 1.5, 2.5);
Vector dx2 = Vector_(3, 1.5, 2.5, 3.5);
vector<size_t> dims;
dims.push_back(1);
dims.push_back(2);
dims.push_back(3);
VectorValues dx(dims);
dx[0] = dx0;
dx[1] = dx1;
dx[2] = dx2;
std::vector<Index> js;
js.push_back(0); js.push_back(1); js.push_back(2);
std::vector<Matrix> Gs;
Gs.push_back(G11); Gs.push_back(G12); Gs.push_back(G13); Gs.push_back(G22); Gs.push_back(G23); Gs.push_back(G33);
std::vector<Vector> gs;
gs.push_back(g1); gs.push_back(g2); gs.push_back(g3);
HessianFactor factor(js, Gs, gs, f);
double expected = 371.3750;
double actual = factor.error(dx);
DOUBLES_EQUAL(expected, actual, 1e-10);
LONGS_EQUAL(7, factor.rows());
DOUBLES_EQUAL(10.0, factor.constantTerm(), 1e-10);
Vector linearExpected(6); linearExpected << g1, g2, g3;
EXPECT(assert_equal(linearExpected, factor.linearTerm()));
EXPECT(assert_equal(G11, factor.info(factor.begin()+0, factor.begin()+0)));
EXPECT(assert_equal(G12, factor.info(factor.begin()+0, factor.begin()+1)));
EXPECT(assert_equal(G13, factor.info(factor.begin()+0, factor.begin()+2)));
EXPECT(assert_equal(G22, factor.info(factor.begin()+1, factor.begin()+1)));
EXPECT(assert_equal(G23, factor.info(factor.begin()+1, factor.begin()+2)));
EXPECT(assert_equal(G33, factor.info(factor.begin()+2, factor.begin()+2)));
}
/* ************************************************************************* */
TEST_UNSAFE(HessianFactor, CopyConstructor_and_assignment)
{