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

Merge pull request #913 from borglab/feature/expression_double_multiply 

enable expression double multiply
release/4.3a0
yetongumich 2021-11-02 19:38:48 -04:00 committed by GitHub
parent 7b22090352 9b393c3fc9
commit b76e9ef52b
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 25 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
gtsam/nonlinear/Expression-inl.h
View File

@ -246,6 +246,18 @@ struct apply_compose {
return x.compose(y, H1, H2);
}
};
template <>
struct apply_compose<double> {
double operator()(const double& x, const double& y,
OptionalJacobian<1, 1> H1 = boost::none,
OptionalJacobian<1, 1> H2 = boost::none) const {
if (H1) H1->setConstant(y);
if (H2) H2->setConstant(x);
return x * y;
}
};
}
// Global methods:

13
gtsam/nonlinear/tests/testExpression.cpp
View File

@ -293,6 +293,19 @@ TEST(Expression, compose3) {
EXPECT(expected == R3.keys());
}
/* ************************************************************************* */
// Test compose with double type (should be multiplication).
TEST(Expression, compose4) {
// Create expression
gtsam::Key key = 1;
Double_ R1(key), R2(key);
Double_ R3 = R1 * R2;
// Check keys
set<Key> expected = list_of(1);
EXPECT(expected == R3.keys());
}
/* ************************************************************************* */
// Test with ternary function.
Rot3 composeThree(const Rot3& R1, const Rot3& R2, const Rot3& R3, OptionalJacobian<3, 3> H1,