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

Refined testcase even more for debugging

release/4.3a0
Frank Dellaert 2015-07-09 00:40:21 -07:00
parent 4516c67389
commit 20b669bed6
1 changed files with 23 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
tests/testExpressionFactor.cpp
View File

@ -507,12 +507,16 @@ TEST(ExpressionFactor, push_back) {
/* ************************************************************************* */ /* ************************************************************************* */
// Test with multiple compositions on duplicate keys // Test with multiple compositions on duplicate keys
static double specialSum(const double& v1, const double& v2, struct Combine {
OptionalJacobian<1, 1> H1, OptionalJacobian<1, 1> H2) { double a, b;
if (H1) (*H1) << 1.0; Combine(double a, double b) : a(a), b(b) {}
if (H2) (*H2) << 2.0; double operator()(const double& x, const double& y, OptionalJacobian<1, 1> H1,
return v1 + 2.0 * v2; OptionalJacobian<1, 1> H2) {
if (H1) (*H1) << a;
if (H2) (*H2) << b;
return a * x + b * y;
} }
};
TEST(Expression, testMultipleCompositions) { TEST(Expression, testMultipleCompositions) {
const double tolerance = 1e-5; const double tolerance = 1e-5;
@ -528,33 +532,36 @@ TEST(Expression, testMultipleCompositions) {
Expression<double> v1_(Key(1)); Expression<double> v1_(Key(1));
Expression<double> v2_(Key(2)); Expression<double> v2_(Key(2));
// BinaryExpression // BinaryExpression(1,2)
// Leaf, key = 1 // Leaf, key = 1
// Leaf, key = 2 // Leaf, key = 2
Expression<double> sum1_(specialSum, v1_, v2_); Expression<double> sum1_(Combine(1, 2), v1_, v2_);
GTSAM_PRINT(sum1_); GTSAM_PRINT(sum1_);
EXPECT(sum1_.keys() == list_of(1)(2));
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance);
// BinaryExpression // BinaryExpression(3,4)
// BinaryExpression // BinaryExpression(1,2)
// Leaf, key = 1 // Leaf, key = 1
// Leaf, key = 2 // Leaf, key = 2
// Leaf, key = 1 // Leaf, key = 1
Expression<double> sum2_(specialSum, sum1_, v1_); Expression<double> sum2_(Combine(3, 4), sum1_, v1_);
GTSAM_PRINT(sum2_); GTSAM_PRINT(sum2_);
EXPECT(sum2_.keys() == list_of(1)(2));
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance);
// BinaryExpression // BinaryExpression(5,6)
// BinaryExpression // BinaryExpression(3,4)
// BinaryExpression // BinaryExpression(1,2)
// Leaf, key = 1 // Leaf, key = 1
// Leaf, key = 2 // Leaf, key = 2
// Leaf, key = 1 // Leaf, key = 1
// BinaryExpression // BinaryExpression(1,2)
// Leaf, key = 1 // Leaf, key = 1
// Leaf, key = 2 // Leaf, key = 2
Expression<double> sum3_(specialSum, sum2_, sum1_); Expression<double> sum3_(Combine(5, 6), sum1_, sum2_);
GTSAM_PRINT(sum3_); GTSAM_PRINT(sum3_);
EXPECT(sum3_.keys() == list_of(1)(2));
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance);
} }