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

Refactor

release/4.3a0
dellaert 2015-03-05 12:53:10 -08:00
parent 2f8d12b9ba
commit d095933527
1 changed files with 21 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -124,33 +124,34 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
// Create JacobianFactor with same error // Create JacobianFactor with same error
const SharedDiagonal model; const SharedDiagonal model;
JacobianFactorQ<6, 2> jf(keys, FBlocks, E, P, b, model); JacobianFactorQ<6, 2> jfQ(keys, FBlocks, E, P, b, model);
// TODO(frank) Neither tests pass, should get to bottom, possibly remove errorJF? // Calculate expected error
double expectedError = 1000;
{ // error { // error
double expectedError = jf.error(xvalues); EXPECT_DOUBLES_EQUAL(expectedError,jfQ.error(xvalues),1e-7)
EXPECT_DOUBLES_EQUAL(expectedError,implicitFactor.error(xvalues),1e-7) EXPECT_DOUBLES_EQUAL(expectedError,implicitFactor.error(xvalues),1e-7)
EXPECT_DOUBLES_EQUAL(expectedError,implicitFactor.errorJF(xvalues),1e-7) EXPECT_DOUBLES_EQUAL(expectedError,implicitFactor.errorJF(xvalues),1e-7)
} }
{ {
VectorValues yActual = zero; VectorValues yActual = zero;
jf.multiplyHessianAdd(alpha, xvalues, yActual); jfQ.multiplyHessianAdd(alpha, xvalues, yActual);
EXPECT(assert_equal(yExpected, yActual, 1e-8)); EXPECT(assert_equal(yExpected, yActual, 1e-8));
jf.multiplyHessianAdd(alpha, xvalues, yActual); jfQ.multiplyHessianAdd(alpha, xvalues, yActual);
EXPECT(assert_equal(2 * yExpected, yActual, 1e-8)); EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
jf.multiplyHessianAdd(-1, xvalues, yActual); jfQ.multiplyHessianAdd(-1, xvalues, yActual);
EXPECT(assert_equal(zero, yActual, 1e-8)); EXPECT(assert_equal(zero, yActual, 1e-8));
} }
{ // check hessian Diagonal { // check hessian Diagonal
VectorValues diagExpected = jf.hessianDiagonal(); VectorValues diagExpected = jfQ.hessianDiagonal();
VectorValues diagActual = implicitFactor.hessianDiagonal(); VectorValues diagActual = implicitFactor.hessianDiagonal();
EXPECT(assert_equal(diagExpected, diagActual, 1e-8)); EXPECT(assert_equal(diagExpected, diagActual, 1e-8));
} }
{ // check hessian Block Diagonal { // check hessian Block Diagonal
map<Key,Matrix> BD = jf.hessianBlockDiagonal(); map<Key,Matrix> BD = jfQ.hessianBlockDiagonal();
map<Key,Matrix> actualBD = implicitFactor.hessianBlockDiagonal(); map<Key,Matrix> actualBD = implicitFactor.hessianBlockDiagonal();
LONGS_EQUAL(3,actualBD.size()); LONGS_EQUAL(3,actualBD.size());
EXPECT(assert_equal(BD[0],actualBD[0])); EXPECT(assert_equal(BD[0],actualBD[0]));
@ -160,47 +161,42 @@ TEST( regularImplicitSchurFactor, addHessianMultiply ) {
{ // Raw memory Version { // Raw memory Version
std::fill(y, y + 24, 0);// zero y ! std::fill(y, y + 24, 0);// zero y !
jf.multiplyHessianAdd(alpha, xdata, y); jfQ.multiplyHessianAdd(alpha, xdata, y);
EXPECT(assert_equal(expected, XMap(y), 1e-8)); EXPECT(assert_equal(expected, XMap(y), 1e-8));
jf.multiplyHessianAdd(alpha, xdata, y); jfQ.multiplyHessianAdd(alpha, xdata, y);
EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8)); EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
jf.multiplyHessianAdd(-1, xdata, y); jfQ.multiplyHessianAdd(-1, xdata, y);
EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8)); EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
} }
{ // Check gradientAtZero { // Check gradientAtZero
VectorValues expected = jf.gradientAtZero(); VectorValues expected = jfQ.gradientAtZero();
VectorValues actual = implicitFactor.gradientAtZero(); VectorValues actual = implicitFactor.gradientAtZero();
EXPECT(assert_equal(expected, actual, 1e-8)); EXPECT(assert_equal(expected, actual, 1e-8));
} }
// Create JacobianFactorQR // Create JacobianFactorQR
JacobianFactorQR<6, 2> jfq(keys, FBlocks, E, P, b, model); JacobianFactorQR<6, 2> jfQR(keys, FBlocks, E, P, b, model);
{ // error EXPECT_DOUBLES_EQUAL(expectedError, jfQR.error(xvalues),1e-7)
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;
jfq.multiplyHessianAdd(alpha, xvalues, yActual); jfQR.multiplyHessianAdd(alpha, xvalues, yActual);
EXPECT(assert_equal(yExpected, yActual, 1e-8)); EXPECT(assert_equal(yExpected, yActual, 1e-8));
jfq.multiplyHessianAdd(alpha, xvalues, yActual); jfQR.multiplyHessianAdd(alpha, xvalues, yActual);
EXPECT(assert_equal(2 * yExpected, yActual, 1e-8)); EXPECT(assert_equal(2 * yExpected, yActual, 1e-8));
jfq.multiplyHessianAdd(-1, xvalues, yActual); jfQR.multiplyHessianAdd(-1, xvalues, yActual);
EXPECT(assert_equal(zero, yActual, 1e-8)); EXPECT(assert_equal(zero, yActual, 1e-8));
} }
{ // Raw memory Version { // Raw memory Version
std::fill(y, y + 24, 0);// zero y ! std::fill(y, y + 24, 0);// zero y !
jfq.multiplyHessianAdd(alpha, xdata, y); jfQR.multiplyHessianAdd(alpha, xdata, y);
EXPECT(assert_equal(expected, XMap(y), 1e-8)); EXPECT(assert_equal(expected, XMap(y), 1e-8));
jfq.multiplyHessianAdd(alpha, xdata, y); jfQR.multiplyHessianAdd(alpha, xdata, y);
EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8)); EXPECT(assert_equal(Vector(2 * expected), XMap(y), 1e-8));
jfq.multiplyHessianAdd(-1, xdata, y); jfQR.multiplyHessianAdd(-1, xdata, y);
EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8)); EXPECT(assert_equal(Vector(0 * expected), XMap(y), 1e-8));
} }
delete [] y; delete [] y;