diff --git a/gtsam/slam/expressions.h b/gtsam/slam/expressions.h index 3b8ea86d3..24476cb5e 100644 --- a/gtsam/slam/expressions.h +++ b/gtsam/slam/expressions.h @@ -48,6 +48,11 @@ inline Line3_ transformTo(const Pose3_ &wTc, const Line3_ &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) { Point3 (*f)(const Point3 &, const Point3 &, OptionalJacobian<3, 3>, OptionalJacobian<3, 3>) = ✗ diff --git a/tests/testExpressionFactor.cpp b/tests/testExpressionFactor.cpp index 5b27eea4d..9a4e01c46 100644 --- a/tests/testExpressionFactor.cpp +++ b/tests/testExpressionFactor.cpp @@ -731,6 +731,19 @@ TEST(ExpressionFactor, variadicTemplate) { 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 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) { auto model = noiseModel::Isotropic::Sigma(3, 1);