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

Add a unit test for gradientAtZero with multi-factors

release/4.3a0
Sungtae An 2014-12-22 14:51:23 -05:00
parent 1affae697c
commit 82a2d7f029
1 changed files with 46 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
gtsam/slam/tests/testRegularJacobianFactor.cpp
View File

@ -38,6 +38,17 @@ namespace {
const Vector b = (Vector(3) << 1., 2., 3.);
const SharedDiagonal noise = noiseModel::Diagonal::Sigmas((Vector(3) << 0.5,0.5,0.5));
}
namespace simple2 {
// Terms
const vector<pair<Key, Matrix> > terms2 = list_of<pair<Key,Matrix> >
(make_pair(0, 2*Matrix3::Identity()))
(make_pair(1, 4*Matrix3::Identity()))
(make_pair(2, 6*Matrix3::Identity()));
// RHS
const Vector b2 = (Vector(3) << 2., 4., 6.);
}
}
/* ************************************************************************* */
@ -99,7 +110,7 @@ TEST(RegularJacobianFactor, hessianDiagonal)
VectorValues expectedHessianDiagonal = factor.hessianDiagonal();
// we compare against the Raw memory access implementation of hessianDiagonal
double actualValue[9];
double actualValue[9]={0};
regularFactor.hessianDiagonal(actualValue);
VectorValues actualHessianDiagonalRaw = double2vv(actualValue,nrKeys,fixedDim);
EXPECT(assert_equal(expectedHessianDiagonal, actualHessianDiagonalRaw));
@ -119,12 +130,45 @@ TEST(RegularJacobian, gradientAtZero)
//EXPECT(assert_equal(expectedGradientAtZero, regularFactor.gradientAtZero()));
// we compare against the Raw memory access implementation of gradientAtZero
double actualValue[9];
double actualValue[9]={0};
regularFactor.gradientAtZero(actualValue);
VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim);
EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw));
}
/* ************************************************************************* */
TEST(RegularJacobian, gradientAtZero_multiFactors)
{
using namespace simple;
JacobianFactor factor(terms[0].first, terms[0].second,
terms[1].first, terms[1].second, terms[2].first, terms[2].second, b, noise);
RegularJacobianFactor<fixedDim> regularFactor(terms, b, noise);
// we compute gradient at zero from the standard Jacobian
VectorValues expectedGradientAtZero = factor.gradientAtZero();
// we compare against the Raw memory access implementation of gradientAtZero
double actualValue[9]={0};
regularFactor.gradientAtZero(actualValue);
VectorValues actualGradientAtZeroRaw = double2vv(actualValue,nrKeys,fixedDim);
EXPECT(assert_equal(expectedGradientAtZero, actualGradientAtZeroRaw));
// One more factor
using namespace simple2;
JacobianFactor factor2(terms2[0].first, terms2[0].second,
terms2[1].first, terms2[1].second, terms2[2].first, terms2[2].second, b2, noise);
RegularJacobianFactor<fixedDim> regularFactor2(terms2, b2, noise);
// we accumulate computed gradient at zero from the standard Jacobian
VectorValues expectedGradientAtZero2 = expectedGradientAtZero.add(factor2.gradientAtZero());
// we compare against the Raw memory access implementation of gradientAtZero
regularFactor2.gradientAtZero(actualValue);
VectorValues actualGradientAtZeroRaw2 = double2vv(actualValue,nrKeys,fixedDim);
EXPECT(assert_equal(expectedGradientAtZero2, actualGradientAtZeroRaw2));
}
/* ************************************************************************* */
TEST(RegularJacobian, multiplyHessianAdd)
{