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

added test on hessian diagonal

release/4.3a0
Luca 2014-03-13 22:44:03 -04:00
parent a6d73e8884
commit dfeb9e2459
1 changed files with 17 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
gtsam/linear/tests/testGaussianFactorGraphUnordered.cpp
View File

@ -11,9 +11,10 @@
/** /**
* @file testGaussianFactorGraphUnordered.cpp * @file testGaussianFactorGraphUnordered.cpp
* @brief Unit tests for Linear Factor * @brief Unit tests for Linear Factor Graph
* @author Christian Potthast * @author Christian Potthast
* @author Frank Dellaert * @author Frank Dellaert
* @author Luca Carlone
* @author Richard Roberts * @author Richard Roberts
**/ **/
@ -325,10 +326,8 @@ TEST( GaussianFactorGraph, multiplyHessianAdd3 )
EXPECT(assert_equal(Y,AtA*X)); EXPECT(assert_equal(Y,AtA*X));
double* x = &X[0]; double* x = &X[0];
double* y = &Y[0];
Vector fast_y = gtsam::zero(6); Vector fast_y = gtsam::zero(6);
double* actual = &fast_y[0];
gfg.multiplyHessianAdd(1.0, x, fast_y.data()); gfg.multiplyHessianAdd(1.0, x, fast_y.data());
EXPECT(assert_equal(Y, fast_y)); EXPECT(assert_equal(Y, fast_y));
@ -409,6 +408,21 @@ TEST( GaussianFactorGraph, negate ) {
EXPECT(assert_equal(expNegation, actNegation)); EXPECT(assert_equal(expNegation, actNegation));
} }
/* ************************************************************************* */
TEST( GaussianFactorGraph, hessianDiagonal )
{
GaussianFactorGraph gfg = createGaussianFactorGraphWithHessianFactor();
VectorValues expected;
Matrix infoMatrix = gfg.hessian().first;
Vector d = infoMatrix.diagonal();
VectorValues actual = gfg.hessianDiagonal();
expected.insert(0, d.segment<2>(0));
expected.insert(1, d.segment<2>(2));
expected.insert(2, d.segment<2>(4));
EXPECT(assert_equal(expected, actual));
}
/* ************************************************************************* */ /* ************************************************************************* */
int main() { TestResult tr; return TestRegistry::runAllTests(tr);} int main() { TestResult tr; return TestRegistry::runAllTests(tr);}
/* ************************************************************************* */ /* ************************************************************************* */