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

Check derivatives of compose

release/4.3a0
dellaert 2014-09-29 12:14:59 +02:00
parent cffd226cdf
commit e789de2353
1 changed files with 40 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
gtsam_unstable/base/tests/testBAD.cpp
View File

@ -94,8 +94,48 @@ TEST(BAD, test) {
/* ************************************************************************* */
TEST(BAD, compose) {
// Create expression
Expression<Rot3> R1(1), R2(2);
Expression<Rot3> R3 = R1 * R2;
// Create factor
BADFactor<Rot3> f(Rot3(), R3);
// Create some values
Values values;
values.insert(1, Rot3());
values.insert(2, Rot3());
// Check linearization
JacobianFactor expected(1, eye(3), 2, eye(3), zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
}
/* ************************************************************************* */
// Test compose with arguments referring to the same rotation
TEST(BAD, compose2) {
// Create expression
Expression<Rot3> R1(1), R2(1);
Expression<Rot3> R3 = R1 * R2;
// Create factor
BADFactor<Rot3> f(Rot3(), R3);
// Create some values
Values values;
values.insert(1, Rot3());
// Check linearization
JacobianFactor expected(1, 2*eye(3), zero(3));
boost::shared_ptr<GaussianFactor> gf = f.linearize(values);
boost::shared_ptr<JacobianFactor> jf = //
boost::dynamic_pointer_cast<JacobianFactor>(gf);
EXPECT( assert_equal(expected, *jf,1e-9));
}
/* ************************************************************************* */