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

Added unit test for JacobianFactor unwhitened_error and error_vector

release/4.3a0
Richard Roberts 2011-10-15 19:56:14 +00:00
parent 2544eef735
commit 1fed4fff8a
1 changed files with 22 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
gtsam/linear/tests/testJacobianFactor.cpp
View File

@ -70,6 +70,28 @@ TEST(JacobianFactor, constructor2)
EXPECT(assert_equal(b, actualb));
}
/* ************************************************************************* */
TEST(JacobianFactor, error) {
Vector b = Vector_(3, 1., 2., 3.);
SharedDiagonal noise = noiseModel::Diagonal::Sigmas(Vector_(3,2.,2.,2.));
std::list<std::pair<Index, Matrix> > terms;
terms.push_back(make_pair(_x0_, eye(3)));
terms.push_back(make_pair(_x1_, 2.*eye(3)));
const JacobianFactor jf(terms, b, noise);
VectorValues values(2, 3);
values[0] = Vector_(3, 1.,2.,3.);
values[1] = Vector_(3, 4.,5.,6.);
Vector expected_unwhitened = Vector_(3, 8., 10., 12.);
Vector actual_unwhitened = jf.unweighted_error(values);
EXPECT(assert_equal(expected_unwhitened, actual_unwhitened));
Vector expected_whitened = Vector_(3, 4., 5., 6.);
Vector actual_whitened = jf.error_vector(values);
EXPECT(assert_equal(expected_whitened, actual_whitened));
}
/* ************************************************************************* */
#ifdef BROKEN
TEST(JacobianFactor, operators )