1/**
2 * @file testBiasedGPSFactor.cpp
3 * @brief
4 * @author Luca Carlone
5 * @date July 30, 2014
6 */
7
8#include <gtsam/base/numericalDerivative.h>
9#include <gtsam/geometry/Pose3.h>
10#include <gtsam/inference/Symbol.h>
11#include <gtsam_unstable/slam/BiasedGPSFactor.h>
12
13#include <CppUnitLite/TestHarness.h>
14
15using namespace std::placeholders;
16using namespace gtsam;
17using namespace gtsam::symbol_shorthand;
18using namespace gtsam::noiseModel;
19// Convenience for named keys
20
21using symbol_shorthand::X;
22using symbol_shorthand::B;
23
24TEST(BiasedGPSFactor, errorNoiseless) {
25
26 Rot3 R = Rot3::Rodrigues(wx: 0.1, wy: 0.2, wz: 0.3);
27 Point3 t(1.0, 0.5, 0.2);
28 Pose3 pose(R,t);
29 Point3 bias(0.0,0.0,0.0);
30 Point3 noise(0.0,0.0,0.0);
31 Point3 measured = t + noise;
32
33 BiasedGPSFactor factor(X(j: 1), B(j: 1), measured, Isotropic::Sigma(dim: 3, sigma: 0.05));
34 Vector expectedError = Vector3(0.0, 0.0, 0.0 );
35 Vector actualError = factor.evaluateError(x: pose,x: bias);
36 EXPECT(assert_equal(expectedError,actualError, 1E-5));
37}
38
39TEST(BiasedGPSFactor, errorNoisy) {
40
41 Rot3 R = Rot3::Rodrigues(wx: 0.1, wy: 0.2, wz: 0.3);
42 Point3 t(1.0, 0.5, 0.2);
43 Pose3 pose(R,t);
44 Point3 bias(0.0,0.0,0.0);
45 Point3 noise(1.0,2.0,3.0);
46 Point3 measured = t - noise;
47
48 BiasedGPSFactor factor(X(j: 1), B(j: 1), measured, Isotropic::Sigma(dim: 3, sigma: 0.05));
49 Vector expectedError = Vector3(1.0, 2.0, 3.0 );
50 Vector actualError = factor.evaluateError(x: pose,x: bias);
51 EXPECT(assert_equal(expectedError,actualError, 1E-5));
52}
53
54TEST(BiasedGPSFactor, jacobian) {
55
56 Rot3 R = Rot3::Rodrigues(wx: 0.1, wy: 0.2, wz: 0.3);
57 Point3 t(1.0, 0.5, 0.2);
58 Pose3 pose(R,t);
59 Point3 bias(0.0,0.0,0.0);
60
61 Point3 noise(0.0,0.0,0.0);
62 Point3 measured = t + noise;
63
64 BiasedGPSFactor factor(X(j: 1), B(j: 1), measured, Isotropic::Sigma(dim: 3, sigma: 0.05));
65
66 Matrix actualH1, actualH2;
67 factor.evaluateError(x: pose,x: bias, H&: actualH1, H&: actualH2);
68
69 std::function<Vector(const Pose3&, const Point3&)> f = [&factor](const Pose3& pose, const Point3& bias) {
70 return factor.evaluateError(x: pose, x: bias);
71 };
72 Matrix numericalH1 = numericalDerivative21(h: f, x1: pose, x2: bias, delta: 1e-5);
73 EXPECT(assert_equal(numericalH1,actualH1, 1E-5));
74
75 Matrix numericalH2 = numericalDerivative22(h: f, x1: pose, x2: bias, delta: 1e-5);
76 EXPECT(assert_equal(numericalH2,actualH2, 1E-5));
77}
78
79/* ************************************************************************* */
80int main() {
81 TestResult tr;
82 return TestRegistry::runAllTests(result&: tr);
83}
84/* ************************************************************************* */
85