From 818417900862eb76c287b9e098d14c7118bdd51a Mon Sep 17 00:00:00 2001
From: Nikhil Khedekar <nikhilvkhedekar@gmail.com>
Date: Thu, 17 Nov 2022 17:59:59 +0100
Subject: [PATCH] Added test for normalize

---
 tests/testExpressionFactor.cpp | 13 +++++++++++++
 1 file changed, 13 insertions(+)

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<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);