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

Fixed up conversion from Hessian -> Jacobian to accommodate QPSolver (same solution as Ivan, now tested that it does not interfere with normal workings)

release/4.3a0
Frank Dellaert 2019-04-25 00:46:20 -04:00
parent 31556ff981
commit 372ae8d092
2 changed files with 36 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
gtsam/linear/JacobianFactor.cpp
View File

@ -102,10 +102,9 @@ JacobianFactor::JacobianFactor(const Key i1, const Matrix& A1, Key i2,
}
/* ************************************************************************* */
JacobianFactor::JacobianFactor(const HessianFactor& factor) :
Base(factor), Ab_(
VerticalBlockMatrix::LikeActiveViewOf(factor.info(),
factor.rows())) {
JacobianFactor::JacobianFactor(const HessianFactor& factor)
: Base(factor),
Ab_(VerticalBlockMatrix::LikeActiveViewOf(factor.info(), factor.rows())) {
// Copy Hessian into our matrix and then do in-place Cholesky
Ab_.full() = factor.info().selfadjointView();
@ -114,16 +113,19 @@ JacobianFactor::JacobianFactor(const HessianFactor& factor) :
bool success;
boost::tie(maxrank, success) = choleskyCareful(Ab_.matrix());
// Check for indefinite system
if (!success and maxrank < factor.rows() - 1)
// Check that Cholesky succeeded OR it managed to factor the full Hessian.
// THe latter case occurs with non-positive definite matrices arising from QP.
if (success || maxrank == factor.rows() - 1) {
// Zero out lower triangle
Ab_.matrix().topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
Matrix::Zero(maxrank, Ab_.matrix().cols());
// FIXME: replace with triangular system
Ab_.rowEnd() = maxrank;
model_ = SharedDiagonal(); // is equivalent to Unit::Create(maxrank)
} else {
// indefinite system
throw IndeterminantLinearSystemException(factor.keys().front());
// Zero out lower triangle
Ab_.matrix().topRows(maxrank).triangularView<Eigen::StrictlyLower>() =
Matrix::Zero(maxrank, Ab_.matrix().cols());
// FIXME: replace with triangular system
Ab_.rowEnd() = maxrank;
model_ = SharedDiagonal(); // should be same as Unit::Create(maxrank);
}
}
/* ************************************************************************* */

21
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -162,6 +162,27 @@ TEST(JabobianFactor, Hessian_conversion) {
EXPECT(assert_equal(expected, JacobianFactor(hessian), 1e-3));
}
/* ************************************************************************* */
TEST(JabobianFactor, Hessian_conversion2) {
JacobianFactor jf(0, (Matrix(3,3) <<
1, 2, 3,
0, 0, 3,
2, 1, 1).finished(),
Vector3(1, 2, 2));
HessianFactor hessian(jf);
EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9));
}
/* ************************************************************************* */
TEST(JabobianFactor, Hessian_conversion3) {
JacobianFactor jf(0, (Matrix(2,4) <<
1, 2, 3, 0,
0, 3, 2, 1).finished(),
Vector2(1, 2));
HessianFactor hessian(jf);
EXPECT(assert_equal(jf, JacobianFactor(hessian), 1e-9));
}
/* ************************************************************************* */
namespace simple_graph {