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

Slightly changed example, debugging output

release/4.3a0
Frank Dellaert 2015-07-08 23:29:21 -07:00
parent 6df0d49769
commit 2bc0d580e7
1 changed files with 30 additions and 31 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

61
tests/testExpressionFactor.cpp
View File

@ -507,23 +507,19 @@ TEST(ExpressionFactor, push_back) {
/* ************************************************************************* */ /* ************************************************************************* */
// Test with multiple compositions on duplicate keys // Test with multiple compositions on duplicate keys
static double doubleSum(const double& v1, const double& v2, static double specialSum(const double& v1, const double& v2,
OptionalJacobian<1, 1> H1, OptionalJacobian<1, 1> H2) { OptionalJacobian<1, 1> H1, OptionalJacobian<1, 1> H2) {
if (H1) { if (H1) (*H1) << 1.0;
H1->setIdentity(); if (H2) (*H2) << 2.0;
} return v1 + 2.0 * v2;
if (H2) {
H2->setIdentity();
}
return v1 + v2;
} }
TEST(Expression, testMultipleCompositions) { TEST(Expression, testMultipleCompositions) {
const double tolerance = 1e-5; const double tolerance = 1e-5;
const double fd_step = 1e-9; const double fd_step = 1e-9;
double v1 = 0; double v1 = 2;
double v2 = 1; double v2 = 3;
Values values; Values values;
values.insert(1, v1); values.insert(1, v1);
@ -532,30 +528,33 @@ TEST(Expression, testMultipleCompositions) {
Expression<double> v1_(Key(1)); Expression<double> v1_(Key(1));
Expression<double> v2_(Key(2)); Expression<double> v2_(Key(2));
// binary(doubleSum) // BinaryExpression
// - leaf 1 // Leaf, key = 1
// - leaf 2 // Leaf, key = 2
Expression<double> sum_(doubleSum, v1_, v2_); Expression<double> sum1_(specialSum, v1_, v2_);
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum_, values, fd_step, tolerance); GTSAM_PRINT(sum1_);
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum1_, values, fd_step, tolerance);
// binary(doubleSum) // BinaryExpression
// - sum_ // BinaryExpression
// - leaf 1 // Leaf, key = 1
// - leaf 2 // Leaf, key = 2
// - leaf 1 // Leaf, key = 1
Expression<double> sum2_(doubleSum, sum_, v1_); Expression<double> sum2_(specialSum, sum1_, v1_);
GTSAM_PRINT(sum2_);
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum2_, values, fd_step, tolerance);
// binary(doubleSum) // BinaryExpression
// sum2_ // BinaryExpression
// - sum_ // BinaryExpression
// - leaf 1 // Leaf, key = 1
// - leaf 2 // Leaf, key = 2
// - leaf 1 // Leaf, key = 1
// - sum_ // BinaryExpression
// - leaf 1 // Leaf, key = 1
// - leaf 2 // Leaf, key = 2
Expression<double> sum3_(doubleSum, sum2_, sum_); Expression<double> sum3_(specialSum, sum2_, sum1_);
GTSAM_PRINT(sum3_);
EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance); EXPECT_CORRECT_EXPRESSION_JACOBIANS(sum3_, values, fd_step, tolerance);
} }