1/* ----------------------------------------------------------------------------
2
3 * GTSAM Copyright 2010, Georgia Tech Research Corporation,
4 * Atlanta, Georgia 30332-0415
5 * All Rights Reserved
6 * Authors: Frank Dellaert, et al. (see THANKS for the full author list)
7
8 * See LICENSE for the license information
9
10 * -------------------------------------------------------------------------- */
11
12/*
13 * @file testLocalOrientedPlane3Factor.cpp
14 * @date Feb 12, 2021
15 * @author David Wisth
16 * @brief Tests the LocalOrientedPlane3Factor class
17 */
18
19#include <gtsam_unstable/slam/LocalOrientedPlane3Factor.h>
20
21#include <gtsam/base/numericalDerivative.h>
22#include <gtsam/inference/Symbol.h>
23#include <gtsam/nonlinear/ISAM2.h>
24
25#include <CppUnitLite/TestHarness.h>
26#include "gtsam/base/Vector.h"
27#include "gtsam/geometry/OrientedPlane3.h"
28#include "gtsam/geometry/Pose3.h"
29
30using namespace std::placeholders;
31using namespace gtsam;
32using namespace std;
33
34GTSAM_CONCEPT_TESTABLE_INST(OrientedPlane3)
35GTSAM_CONCEPT_MANIFOLD_INST(OrientedPlane3)
36
37using symbol_shorthand::P; //< Planes
38using symbol_shorthand::X; //< Pose3
39
40// *************************************************************************
41TEST(LocalOrientedPlane3Factor, lm_translation_error) {
42 // Tests one pose, two measurements of the landmark that differ in range only.
43 // Normal along -x, 3m away
44 OrientedPlane3 test_lm0(-1.0, 0.0, 0.0, 3.0);
45
46 NonlinearFactorGraph graph;
47
48 // Init pose and prior. Pose Prior is needed since a single plane measurement
49 // does not fully constrain the pose
50 Pose3 init_pose = Pose3::Identity();
51 Pose3 anchor_pose = Pose3::Identity();
52 graph.addPrior(key: X(j: 0), prior: init_pose, model: noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.001));
53 graph.addPrior(key: X(j: 1), prior: anchor_pose, model: noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.001));
54
55 // Add two landmark measurements, differing in range
56 Vector4 measurement0(-1.0, 0.0, 0.0, 3.0);
57 Vector4 measurement1(-1.0, 0.0, 0.0, 1.0);
58 LocalOrientedPlane3Factor factor0(
59 measurement0, noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1), X(j: 0), X(j: 1), P(j: 0));
60 LocalOrientedPlane3Factor factor1(
61 measurement1, noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1), X(j: 0), X(j: 1), P(j: 0));
62 graph.add(factorOrContainer: factor0);
63 graph.add(factorOrContainer: factor1);
64
65 // Initial Estimate
66 Values values;
67 values.insert(j: X(j: 0), val: init_pose);
68 values.insert(j: X(j: 1), val: anchor_pose);
69 values.insert(j: P(j: 0), val: test_lm0);
70
71 // Optimize
72 ISAM2 isam2;
73 isam2.update(newFactors: graph, newTheta: values);
74 Values result_values = isam2.calculateEstimate();
75 auto optimized_plane_landmark = result_values.at<OrientedPlane3>(j: P(j: 0));
76
77 // Given two noisy measurements of equal weight, expect result between the two
78 OrientedPlane3 expected_plane_landmark(-1.0, 0.0, 0.0, 2.0);
79 EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
80}
81
82// *************************************************************************
83// TODO As described in PR #564 after correcting the derivatives in
84// OrientedPlane3Factor this test fails. It should be debugged and re-enabled.
85/*
86TEST (LocalOrientedPlane3Factor, lm_rotation_error) {
87 // Tests one pose, two measurements of the landmark that differ in angle only.
88 // Normal along -x, 3m away
89 OrientedPlane3 test_lm0(-1.0/sqrt(1.01), -0.1/sqrt(1.01), 0.0, 3.0);
90
91 NonlinearFactorGraph graph;
92
93 // Init pose and prior. Pose Prior is needed since a single plane measurement
94 // does not fully constrain the pose
95 Pose3 init_pose = Pose3::Identity();
96 graph.addPrior(X(0), init_pose, noiseModel::Isotropic::Sigma(6, 0.001));
97
98 // Add two landmark measurements, differing in angle
99 Vector4 measurement0(-1.0, 0.0, 0.0, 3.0);
100 Vector4 measurement1(0.0, -1.0, 0.0, 3.0);
101 LocalOrientedPlane3Factor factor0(measurement0,
102 noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0));
103 LocalOrientedPlane3Factor factor1(measurement1,
104 noiseModel::Isotropic::Sigma(3, 0.1), X(0), X(0), P(0));
105 graph.add(factor0);
106 graph.add(factor1);
107
108 // Initial Estimate
109 Values values;
110 values.insert(X(0), init_pose);
111 values.insert(P(0), test_lm0);
112
113 // Optimize
114 ISAM2 isam2;
115 isam2.update(graph, values);
116 Values result_values = isam2.calculateEstimate();
117 isam2.getDelta().print();
118
119 auto optimized_plane_landmark = result_values.at<OrientedPlane3>(P(0));
120
121 values.print();
122 result_values.print();
123
124 // Given two noisy measurements of equal weight, expect result between the two
125 OrientedPlane3 expected_plane_landmark(-sqrt(2.0) / 2.0, -sqrt(2.0) / 2.0,
126 0.0, 3.0);
127 EXPECT(assert_equal(optimized_plane_landmark, expected_plane_landmark));
128}
129*/
130
131// *************************************************************************
132TEST(LocalOrientedPlane3Factor, Derivatives) {
133 // Measurement
134 OrientedPlane3 p(sqrt(x: 2)/2, -sqrt(x: 2)/2, 0, 5);
135
136 // Linearisation point
137 OrientedPlane3 pLin(sqrt(x: 3)/3, -sqrt(x: 3)/3, sqrt(x: 3)/3, 7);
138 Pose3 poseLin(Rot3::RzRyRx(x: 0.5*M_PI, y: -0.3*M_PI, z: 1.4*M_PI), Point3(1, 2, -4));
139 Pose3 anchorPoseLin(Rot3::RzRyRx(x: -0.1*M_PI, y: 0.2*M_PI, z: 1.0), Point3(-5, 0, 1));
140
141 // Factor
142 Key planeKey(1), poseKey(2), anchorPoseKey(3);
143 SharedGaussian noise = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.1);
144 LocalOrientedPlane3Factor factor(p, noise, poseKey, anchorPoseKey, planeKey);
145
146 // Calculate numerical derivatives
147 auto f = [&factor] (const Pose3& p1, const Pose3& p2, const OrientedPlane3& a_plane) {
148 return factor.evaluateError(x: p1, x: p2, x: a_plane);
149 };
150
151 Matrix numericalH1 = numericalDerivative31<Vector3, Pose3, Pose3,
152 OrientedPlane3>(h: f, x1: poseLin, x2: anchorPoseLin, x3: pLin);
153 Matrix numericalH2 = numericalDerivative32<Vector3, Pose3, Pose3,
154 OrientedPlane3>(h: f, x1: poseLin, x2: anchorPoseLin, x3: pLin);
155 Matrix numericalH3 = numericalDerivative33<Vector3, Pose3, Pose3,
156 OrientedPlane3>(h: f, x1: poseLin, x2: anchorPoseLin, x3: pLin);
157
158 // Use the factor to calculate the derivative
159 Matrix actualH1, actualH2, actualH3;
160 factor.evaluateError(x: poseLin, x: anchorPoseLin, x: pLin, H&: actualH1, H&: actualH2,
161 H&: actualH3);
162
163 // Verify we get the expected error
164 EXPECT(assert_equal(numericalH1, actualH1, 1e-8));
165 EXPECT(assert_equal(numericalH2, actualH2, 1e-8));
166 EXPECT(assert_equal(numericalH3, actualH3, 1e-8));
167}
168
169
170/* ************************************************************************* */
171// Simplified version of the test by Marco Camurri to debug issue #561
172//
173// This test provides an example of how LocalOrientedPlane3Factor works.
174// x0 is the current sensor pose, and x1 is the local "anchor pose" - i.e.
175// a local linearisation point for the plane. The plane is representated and
176// optimized in x1 frame in the optimization. This greatly improves numerical
177// stability when the pose is far from the origin.
178//
179TEST(LocalOrientedPlane3Factor, Issue561Simplified) {
180 // Typedefs
181 using Plane = OrientedPlane3;
182
183 NonlinearFactorGraph graph;
184
185 // Setup prior factors
186 Pose3 x0(Rot3::Identity(), Vector3(100, 30, 10)); // the "sensor pose"
187 Pose3 x1(Rot3::Identity(), Vector3(90, 40, 5) ); // the "anchor pose"
188
189 auto x0_noise = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.01);
190 auto x1_noise = noiseModel::Isotropic::Sigma(dim: 6, sigma: 0.01);
191 graph.addPrior<Pose3>(key: X(j: 0), prior: x0, model: x0_noise);
192 graph.addPrior<Pose3>(key: X(j: 1), prior: x1, model: x1_noise);
193
194 // Two horizontal planes with different heights, in the world frame.
195 const Plane p1(0, 0, 1, 1), p2(0, 0, 1, 2);
196 // Transform to x1, the "anchor frame" (i.e. local frame)
197 auto p1_in_x1 = p1.transform(xr: x1);
198 auto p2_in_x1 = p2.transform(xr: x1);
199 auto p1_noise = noiseModel::Diagonal::Sigmas(sigmas: Vector3{1, 1, 5});
200 auto p2_noise = noiseModel::Diagonal::Sigmas(sigmas: Vector3{1, 1, 5});
201 graph.addPrior<Plane>(key: P(j: 1), prior: p1_in_x1, model: p1_noise);
202 graph.addPrior<Plane>(key: P(j: 2), prior: p2_in_x1, model: p2_noise);
203
204 // Add plane factors, with a local linearization point.
205 // transform p1 to pose x0 as a measurement
206 auto p1_measured_from_x0 = p1.transform(xr: x0);
207 // transform p2 to pose x0 as a measurement
208 auto p2_measured_from_x0 = p2.transform(xr: x0);
209 const auto x0_p1_noise = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.05);
210 const auto x0_p2_noise = noiseModel::Isotropic::Sigma(dim: 3, sigma: 0.05);
211 graph.emplace_shared<LocalOrientedPlane3Factor>(
212 args: p1_measured_from_x0.planeCoefficients(), args: x0_p1_noise, args: X(j: 0), args: X(j: 1), args: P(j: 1));
213 graph.emplace_shared<LocalOrientedPlane3Factor>(
214 args: p2_measured_from_x0.planeCoefficients(), args: x0_p2_noise, args: X(j: 0), args: X(j: 1), args: P(j: 2));
215
216 // Initial values
217 // Just offset the initial pose by 1m. This is what we are trying to optimize.
218 Values initialEstimate;
219 Pose3 x0_initial = x0.compose(g: Pose3(Rot3::Identity(), Vector3(1, 0, 0)));
220 initialEstimate.insert(j: P(j: 1), val: p1_in_x1);
221 initialEstimate.insert(j: P(j: 2), val: p2_in_x1);
222 initialEstimate.insert(j: X(j: 0), val: x0_initial);
223 initialEstimate.insert(j: X(j: 1), val: x1);
224
225 // Optimize
226 try {
227 ISAM2 isam2;
228 isam2.update(newFactors: graph, newTheta: initialEstimate);
229 Values result = isam2.calculateEstimate();
230 EXPECT_DOUBLES_EQUAL(0, graph.error(result), 0.1);
231 EXPECT(x0.equals(result.at<Pose3>(X(0))));
232 EXPECT(p1_in_x1.equals(result.at<Plane>(P(1))));
233 EXPECT(p2_in_x1.equals(result.at<Plane>(P(2))));
234 } catch (const IndeterminantLinearSystemException &e) {
235 cerr << "CAPTURED THE EXCEPTION: "
236 << DefaultKeyFormatter(e.nearbyVariable()) << endl;
237 EXPECT(false); // fail if this happens
238 }
239}
240
241/* ************************************************************************* */
242int main() {
243 srand(seed: time(timer: nullptr));
244 TestResult tr;
245 return TestRegistry::runAllTests(result&: tr);
246}
247/* ************************************************************************* */
248