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

Added test for normalize

release/4.3a0
Nikhil Khedekar 2022-11-17 17:59:59 +01:00
parent 70f490b298
commit 8184179008
1 changed files with 13 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
tests/testExpressionFactor.cpp
View File

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