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