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

Starting to diagnose issue with lower-left entry of Hessian

release/4.3a0
dellaert 2015-06-14 16:01:54 -07:00
parent 7698c52ce9
commit 5771664622
3 changed files with 16 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
gtsam/linear/HessianFactor.cpp
View File

@ -268,18 +268,11 @@ void HessianFactor::print(const std::string& s,
/* ************************************************************************* */
bool HessianFactor::equals(const GaussianFactor& lf, double tol) const {
if (!dynamic_cast<const HessianFactor*>(&lf))
const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&lf);
if (!rhs || !Factor::equals(lf, tol))
return false;
else {
if (!Factor::equals(lf, tol))
return false;
Matrix thisMatrix = info_.full().selfadjointView();
thisMatrix(thisMatrix.rows() - 1, thisMatrix.cols() - 1) = 0.0;
Matrix rhsMatrix =
static_cast<const HessianFactor&>(lf).info_.full().selfadjointView();
rhsMatrix(rhsMatrix.rows() - 1, rhsMatrix.cols() - 1) = 0.0;
return equal_with_abs_tol(thisMatrix, rhsMatrix, tol);
}
return equal_with_abs_tol(augmentedInformation(), rhs->augmentedInformation(),
tol);
}
/* ************************************************************************* */

8
gtsam/linear/JacobianFactor.cpp
View File

@ -66,10 +66,10 @@ JacobianFactor::JacobianFactor() :
/* ************************************************************************* */
JacobianFactor::JacobianFactor(const GaussianFactor& gf) {
// Copy the matrix data depending on what type of factor we're copying from
if (const JacobianFactor* rhs = dynamic_cast<const JacobianFactor*>(&gf))
*this = JacobianFactor(*rhs);
else if (const HessianFactor* rhs = dynamic_cast<const HessianFactor*>(&gf))
*this = JacobianFactor(*rhs);
if (const JacobianFactor* asJacobian = dynamic_cast<const JacobianFactor*>(&gf))
*this = JacobianFactor(*asJacobian);
else if (const HessianFactor* asHessian = dynamic_cast<const HessianFactor*>(&gf))
*this = JacobianFactor(*asHessian);
else
throw std::invalid_argument(
"In JacobianFactor(const GaussianFactor& rhs), rhs is neither a JacobianFactor nor a HessianFactor");

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

@ -286,8 +286,8 @@ TEST(HessianFactor, CombineAndEliminate)
1.0, 0.0, 0.0,
0.0, 1.0, 0.0,
0.0, 0.0, 1.0).finished();
Vector3 b0(1.5, 1.5, 1.5);
Vector3 s0(1.6, 1.6, 1.6);
Vector3 b0(1,0,0);//(1.5, 1.5, 1.5);
Vector3 s0=Vector3::Ones();//(1.6, 1.6, 1.6);
Matrix A10 = (Matrix(3,3) <<
2.0, 0.0, 0.0,
@ -297,15 +297,15 @@ TEST(HessianFactor, CombineAndEliminate)
-2.0, 0.0, 0.0,
0.0, -2.0, 0.0,
0.0, 0.0, -2.0).finished();
Vector3 b1(2.5, 2.5, 2.5);
Vector3 s1(2.6, 2.6, 2.6);
Vector3 b1 = Vector3::Zero();//(2.5, 2.5, 2.5);
Vector3 s1=Vector3::Ones();//(2.6, 2.6, 2.6);
Matrix A21 = (Matrix(3,3) <<
3.0, 0.0, 0.0,
0.0, 3.0, 0.0,
0.0, 0.0, 3.0).finished();
Vector3 b2(3.5, 3.5, 3.5);
Vector3 s2(3.6, 3.6, 3.6);
Vector3 b2 = Vector3::Zero();//(3.5, 3.5, 3.5);
Vector3 s2=Vector3::Ones();//(3.6, 3.6, 3.6);
GaussianFactorGraph gfg;
gfg.add(1, A01, b0, noiseModel::Diagonal::Sigmas(s0, true));
@ -334,6 +334,8 @@ TEST(HessianFactor, CombineAndEliminate)
boost::tie(actualConditional, actualCholeskyFactor) = EliminateCholesky(gfg, Ordering(list_of(0)));
EXPECT(assert_equal(*expectedConditional, *actualConditional, 1e-6));
VectorValues vv; vv.insert(1, Vector3(1,2,3));
EXPECT_DOUBLES_EQUAL(expectedRemainingFactor->error(vv), actualCholeskyFactor->error(vv), 1e-9);
EXPECT(assert_equal(HessianFactor(*expectedRemainingFactor), *actualCholeskyFactor, 1e-6));
}