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

borglab · Dec 3, 2022 · c29d60c · c29d60c
2 parents c7b5166 + 8184179
commit c29d60c
Show file tree

Hide file tree

Showing 2 changed files with 18 additions and 0 deletions.
diff --git 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>) = &cross;

diff --git 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<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) {
   auto model = noiseModel::Isotropic::Sigma(3, 1);