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

Merge pull request #1330 from ntnu-arl/feature/addn_expression_fns

release/4.3a0
Varun Agrawal 2022-12-02 20:01:18 -05:00 committed by GitHub
parent c7b5166268 8184179008
commit c29d60c410
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 18 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
gtsam/slam/expressions.h
View File

@ -48,6 +48,11 @@ inline Line3_ transformTo(const Pose3_ &wTc, const Line3_ &wL) {
return Line3_(f, wTc, wL); return Line3_(f, wTc, wL);
} }
inline Point3_ normalize(const Point3_& a){
Point3 (*f)(const Point3 &, OptionalJacobian<3, 3>) = &normalize;
return Point3_(f, a);
}
inline Point3_ cross(const Point3_& a, const Point3_& b) { inline Point3_ cross(const Point3_& a, const Point3_& b) {
Point3 (*f)(const Point3 &, const Point3 &, Point3 (*f)(const Point3 &, const Point3 &,
OptionalJacobian<3, 3>, OptionalJacobian<3, 3>) = &cross; OptionalJacobian<3, 3>, OptionalJacobian<3, 3>) = &cross;

13
tests/testExpressionFactor.cpp
View File

@ -731,6 +731,19 @@ TEST(ExpressionFactor, variadicTemplate) {
EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-8, 1e-5); EXPECT_CORRECT_FACTOR_JACOBIANS(f, values, 1e-8, 1e-5);
} }
TEST(ExpressionFactor, normalize) {
auto model = noiseModel::Isotropic::Sigma(3, 1);
// Create expression
const auto x = Vector3_(1);
Vector3_ f_expr = normalize(x);
// Check derivatives
Values values;
values.insert(1, Vector3(1, 2, 3));
ExpressionFactor<Vector3> factor(model, Vector3(1.0/sqrt(14), 2.0/sqrt(14), 3.0/sqrt(14)), f_expr);
EXPECT_CORRECT_FACTOR_JACOBIANS(factor, values, 1e-5, 1e-5);
}
TEST(ExpressionFactor, crossProduct) { TEST(ExpressionFactor, crossProduct) {
auto model = noiseModel::Isotropic::Sigma(3, 1); auto model = noiseModel::Isotropic::Sigma(3, 1);