diff --git a/gtsam/linear/HessianFactor.cpp b/gtsam/linear/HessianFactor.cpp index a5e876b6e..8402e4f91 100644 --- a/gtsam/linear/HessianFactor.cpp +++ b/gtsam/linear/HessianFactor.cpp @@ -155,6 +155,66 @@ HessianFactor::HessianFactor(Index j1, Index j2, Index j3, assertInvariants(); } +/* ************************************************************************* */ +HessianFactor::HessianFactor(const std::vector& js, const std::vector& Gs, + const std::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); diff --git a/gtsam/linear/HessianFactor.h b/gtsam/linear/HessianFactor.h index b51f0c143..2a16c0e38 100644 --- a/gtsam/linear/HessianFactor.h +++ b/gtsam/linear/HessianFactor.h @@ -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& js, const std::vector& Gs, + const std::vector& gs, double f); + /** Construct from Conditional Gaussian */ HessianFactor(const GaussianConditional& cg); diff --git a/gtsam/linear/tests/testHessianFactor.cpp b/gtsam/linear/tests/testHessianFactor.cpp index a085a1e82..c2ccdb4a7 100644 --- a/gtsam/linear/tests/testHessianFactor.cpp +++ b/gtsam/linear/tests/testHessianFactor.cpp @@ -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() * 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 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 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 js; + js.push_back(0); js.push_back(1); js.push_back(2); + std::vector 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 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) {