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

Added more tests to diagnose Regular-Q inconsistency

release/4.3a0
dellaert 2015-03-05 12:41:01 -08:00
parent 758aab6e80
commit 2f8d12b9ba
1 changed files with 11 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
gtsam/slam/tests/testRegularImplicitSchurFactor.cpp
View File

@ -126,13 +126,14 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
const SharedDiagonal model; const SharedDiagonal model;
JacobianFactorQ<6, 2> jf(keys, FBlocks, E, P, b, model); JacobianFactorQ<6, 2> jf(keys, FBlocks, E, P, b, model);
// TODO(frank) Neither tests pass, should get to bottom, possibly remove errorJF?
{ // error { // error
double expectedError = jf.error(xvalues); double expectedError = jf.error(xvalues);
double actualError = implicitFactor.errorJF(xvalues); EXPECT_DOUBLES_EQUAL(expectedError,implicitFactor.error(xvalues),1e-7)
DOUBLES_EQUAL(expectedError,actualError,1e-7) EXPECT_DOUBLES_EQUAL(expectedError,implicitFactor.errorJF(xvalues),1e-7)
} }
{ // JacobianFactor with same error {
VectorValues yActual = zero; VectorValues yActual = zero;
jf.multiplyHessianAdd(alpha, xvalues, yActual); jf.multiplyHessianAdd(alpha, xvalues, yActual);
EXPECT(assert_equal(yExpected, yActual, 1e-8)); EXPECT(assert_equal(yExpected, yActual, 1e-8));
@ -175,6 +176,13 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
// Create JacobianFactorQR // Create JacobianFactorQR
JacobianFactorQR<6, 2> jfq(keys, FBlocks, E, P, b, model); JacobianFactorQR<6, 2> jfq(keys, FBlocks, E, P, b, model);
{ // error
double expectedError = jfq.error(xvalues);
EXPECT_DOUBLES_EQUAL(expectedError, jf.error(xvalues),1e-7)
double actualError = implicitFactor.errorJF(xvalues);
EXPECT_DOUBLES_EQUAL(expectedError,actualError,1e-7)
}
{ {
const SharedDiagonal model; const SharedDiagonal model;
VectorValues yActual = zero; VectorValues yActual = zero;