1/**
2 * @file testPoseToPointFactor.cpp
3 * @brief
4 * @author David Wisth
5 * @author Luca Carlone
6 * @date June 20, 2020
7 */
8
9#include <CppUnitLite/TestHarness.h>
10#include <gtsam/base/numericalDerivative.h>
11#include <gtsam_unstable/slam/PoseToPointFactor.h>
12
13using namespace gtsam;
14using namespace gtsam::noiseModel;
15
16/* ************************************************************************* */
17// Verify zero error when there is no noise
18TEST(PoseToPointFactor, errorNoiseless_2D) {
19 Pose2 pose = Pose2::Identity();
20 Point2 point(1.0, 2.0);
21 Point2 noise(0.0, 0.0);
22 Point2 measured = point + noise;
23
24 Key pose_key(1);
25 Key point_key(2);
26 PoseToPointFactor<Pose2,Point2> factor(pose_key, point_key, measured,
27 Isotropic::Sigma(dim: 2, sigma: 0.05));
28 Vector expectedError = Vector2(0.0, 0.0);
29 Vector actualError = factor.evaluateError(x: pose, x: point);
30 EXPECT(assert_equal(expectedError, actualError, 1E-5));
31}
32
33/* ************************************************************************* */
34// Verify expected error in test scenario
35TEST(PoseToPointFactor, errorNoise_2D) {
36 Pose2 pose = Pose2::Identity();
37 Point2 point(1.0, 2.0);
38 Point2 noise(-1.0, 0.5);
39 Point2 measured = point + noise;
40
41 Key pose_key(1);
42 Key point_key(2);
43 PoseToPointFactor<Pose2,Point2> factor(pose_key, point_key, measured,
44 Isotropic::Sigma(dim: 2, sigma: 0.05));
45 Vector expectedError = -noise;
46 Vector actualError = factor.evaluateError(x: pose, x: point);
47 EXPECT(assert_equal(expectedError, actualError, 1E-5));
48}
49
50/* ************************************************************************* */
51// Check Jacobians are correct
52TEST(PoseToPointFactor, jacobian_2D) {
53 // Measurement
54 gtsam::Point2 l_meas(1, 2);
55
56 // Linearisation point
57 gtsam::Point2 p_t(-5, 12);
58 gtsam::Rot2 p_R(1.5 * M_PI);
59 Pose2 p(p_R, p_t);
60
61 gtsam::Point2 l(3, 0);
62
63 // Factor
64 Key pose_key(1);
65 Key point_key(2);
66 SharedGaussian noise = noiseModel::Diagonal::Sigmas(sigmas: Vector2(0.1, 0.1));
67 PoseToPointFactor<Pose2,Point2> factor(pose_key, point_key, l_meas, noise);
68
69 // Calculate numerical derivatives
70 auto f = [&factor] (const Pose2& pose, const Point2& pt) {
71 return factor.evaluateError(x: pose, x: pt);
72 };
73 Matrix numerical_H1 = numericalDerivative21<Vector, Pose2, Point2>(h: f, x1: p, x2: l);
74 Matrix numerical_H2 = numericalDerivative22<Vector, Pose2, Point2>(h: f, x1: p, x2: l);
75
76 // Use the factor to calculate the derivative
77 Matrix actual_H1;
78 Matrix actual_H2;
79 factor.evaluateError(x: p, x: l, H&: actual_H1, H&: actual_H2);
80
81 // Verify we get the expected error
82 EXPECT(assert_equal(numerical_H1, actual_H1, 1e-8));
83 EXPECT(assert_equal(numerical_H2, actual_H2, 1e-8));
84}
85
86/* ************************************************************************* */
87// Verify zero error when there is no noise
88TEST(PoseToPointFactor, errorNoiseless_3D) {
89 Pose3 pose = Pose3::Identity();
90 Point3 point(1.0, 2.0, 3.0);
91 Point3 noise(0.0, 0.0, 0.0);
92 Point3 measured = point + noise;
93
94 Key pose_key(1);
95 Key point_key(2);
96 PoseToPointFactor<Pose3,Point3> factor(pose_key, point_key, measured,
97 Isotropic::Sigma(dim: 3, sigma: 0.05));
98 Vector expectedError = Vector3(0.0, 0.0, 0.0);
99 Vector actualError = factor.evaluateError(x: pose, x: point);
100 EXPECT(assert_equal(expectedError, actualError, 1E-5));
101}
102
103/* ************************************************************************* */
104// Verify expected error in test scenario
105TEST(PoseToPointFactor, errorNoise_3D) {
106 Pose3 pose = Pose3::Identity();
107 Point3 point(1.0, 2.0, 3.0);
108 Point3 noise(-1.0, 0.5, 0.3);
109 Point3 measured = point + noise;
110
111 Key pose_key(1);
112 Key point_key(2);
113 PoseToPointFactor<Pose3,Point3> factor(pose_key, point_key, measured,
114 Isotropic::Sigma(dim: 3, sigma: 0.05));
115 Vector expectedError = -noise;
116 Vector actualError = factor.evaluateError(x: pose, x: point);
117 EXPECT(assert_equal(expectedError, actualError, 1E-5));
118}
119
120/* ************************************************************************* */
121// Check Jacobians are correct
122TEST(PoseToPointFactor, jacobian_3D) {
123 // Measurement
124 gtsam::Point3 l_meas = gtsam::Point3(1, 2, 3);
125
126 // Linearisation point
127 gtsam::Point3 p_t = gtsam::Point3(-5, 12, 2);
128 gtsam::Rot3 p_R = gtsam::Rot3::RzRyRx(x: 1.5 * M_PI, y: -0.3 * M_PI, z: 0.4 * M_PI);
129 Pose3 p(p_R, p_t);
130
131 gtsam::Point3 l = gtsam::Point3(3, 0, 5);
132
133 // Factor
134 Key pose_key(1);
135 Key point_key(2);
136 SharedGaussian noise = noiseModel::Diagonal::Sigmas(sigmas: Vector3(0.1, 0.1, 0.1));
137 PoseToPointFactor<Pose3,Point3> factor(pose_key, point_key, l_meas, noise);
138
139 // Calculate numerical derivatives
140 auto f = [&factor] (const Pose3& pose, const Point3& pt) {
141 return factor.evaluateError(x: pose, x: pt);
142 };
143
144 Matrix numerical_H1 = numericalDerivative21<Vector, Pose3, Point3>(h: f, x1: p, x2: l);
145 Matrix numerical_H2 = numericalDerivative22<Vector, Pose3, Point3>(h: f, x1: p, x2: l);
146
147 // Use the factor to calculate the derivative
148 Matrix actual_H1;
149 Matrix actual_H2;
150 factor.evaluateError(x: p, x: l, H&: actual_H1, H&: actual_H2);
151
152 // Verify we get the expected error
153 EXPECT(assert_equal(numerical_H1, actual_H1, 1e-8));
154 EXPECT(assert_equal(numerical_H2, actual_H2, 1e-8));
155}
156
157/* ************************************************************************* */
158int main() {
159 TestResult tr;
160 return TestRegistry::runAllTests(result&: tr);
161}
162/* ************************************************************************* */
163