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#include <gtsam_unstable/slam/PartialPriorFactor.h>
13#include <gtsam/inference/Symbol.h>
14#include <gtsam/geometry/Pose2.h>
15#include <gtsam/geometry/Pose3.h>
16#include <gtsam/base/numericalDerivative.h>
17#include <gtsam/base/TestableAssertions.h>
18
19#include <CppUnitLite/TestHarness.h>
20
21using namespace std::placeholders;
22using namespace std;
23using namespace gtsam;
24
25namespace NM = gtsam::noiseModel;
26
27// Pose3 tangent representation is [ Rx Ry Rz Tx Ty Tz ].
28static const int kIndexRx = 0;
29static const int kIndexRy = 1;
30static const int kIndexRz = 2;
31static const int kIndexTx = 3;
32static const int kIndexTy = 4;
33static const int kIndexTz = 5;
34
35typedef PartialPriorFactor<Pose2> TestPartialPriorFactor2;
36typedef PartialPriorFactor<Pose3> TestPartialPriorFactor3;
37typedef std::vector<size_t> Indices;
38
39/// traits
40namespace gtsam {
41template<>
42struct traits<TestPartialPriorFactor2> : public Testable<TestPartialPriorFactor2> {};
43
44template<>
45struct traits<TestPartialPriorFactor3> : public Testable<TestPartialPriorFactor3> {};
46}
47
48/* ************************************************************************* */
49TEST(PartialPriorFactor, Constructors2) {
50 Key poseKey(1);
51 Pose2 measurement(-13.1, 3.14, -0.73);
52
53 // Prior on x component of translation.
54 TestPartialPriorFactor2 factor1(poseKey, 0, measurement.x(), NM::Isotropic::Sigma(dim: 1, sigma: 0.25));
55 CHECK(assert_equal(1, factor1.prior().rows()));
56 CHECK(assert_equal(measurement.x(), factor1.prior()(0)));
57 CHECK(assert_container_equality<Indices>({ 0 }, factor1.indices()));
58
59 // Prior on full translation vector.
60 const Indices t_indices = { 0, 1 };
61 TestPartialPriorFactor2 factor2(poseKey, t_indices, measurement.translation(), NM::Isotropic::Sigma(dim: 2, sigma: 0.25));
62 CHECK(assert_equal(2, factor2.prior().rows()));
63 CHECK(assert_equal(measurement.translation(), factor2.prior()));
64 CHECK(assert_container_equality<Indices>(t_indices, factor2.indices()));
65
66 // Prior on theta.
67 TestPartialPriorFactor2 factor3(poseKey, 2, measurement.theta(), NM::Isotropic::Sigma(dim: 1, sigma: 0.1));
68 CHECK(assert_equal(1, factor3.prior().rows()));
69 CHECK(assert_equal(measurement.theta(), factor3.prior()(0)));
70 CHECK(assert_container_equality<Indices>({ 2 }, factor3.indices()));
71}
72
73/* ************************************************************************* */
74TEST(PartialPriorFactor, JacobianPartialTranslation2) {
75 Key poseKey(1);
76 Pose2 measurement(-13.1, 3.14, -0.73);
77
78 // Prior on x component of translation.
79 TestPartialPriorFactor2 factor(poseKey, 0, measurement.x(), NM::Isotropic::Sigma(dim: 1, sigma: 0.25));
80
81 Pose2 pose = measurement; // Zero-error linearization point.
82
83 // Calculate numerical derivatives.
84 Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
85 h: [&factor](const Pose2& p) { return factor.evaluateError(x: p); }, x: pose);
86
87 // Use the factor to calculate the derivative.
88 Matrix actualH1;
89 factor.evaluateError(x: pose, H&: actualH1);
90
91 // Verify we get the expected error.
92 CHECK(assert_equal(expectedH1, actualH1, 1e-5));
93}
94
95/* ************************************************************************* */
96TEST(PartialPriorFactor, JacobianFullTranslation2) {
97 Key poseKey(1);
98 Pose2 measurement(-6.0, 3.5, 0.123);
99
100 // Prior on x component of translation.
101 TestPartialPriorFactor2 factor(poseKey, {0, 1}, measurement.translation(),
102 NM::Isotropic::Sigma(dim: 2, sigma: 0.25));
103
104 Pose2 pose = measurement; // Zero-error linearization point.
105
106 // Calculate numerical derivatives.
107 Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
108 h: [&factor](const Pose2& p) { return factor.evaluateError(x: p); }, x: pose);
109
110 // Use the factor to calculate the derivative.
111 Matrix actualH1;
112 factor.evaluateError(x: pose, H&: actualH1);
113
114 // Verify we get the expected error.
115 CHECK(assert_equal(expectedH1, actualH1, 1e-5));
116}
117
118/* ************************************************************************* */
119TEST(PartialPriorFactor, JacobianTheta) {
120 Key poseKey(1);
121 Pose2 measurement(-1.0, 0.4, -2.5);
122
123 // Prior on x component of translation.
124 TestPartialPriorFactor2 factor(poseKey, 2, measurement.theta(), NM::Isotropic::Sigma(dim: 1, sigma: 0.25));
125
126 Pose2 pose = measurement; // Zero-error linearization point.
127
128 // Calculate numerical derivatives.
129 Matrix expectedH1 = numericalDerivative11<Vector, Pose2>(
130 h: [&factor](const Pose2& p) { return factor.evaluateError(x: p); }, x: pose);
131
132 // Use the factor to calculate the derivative.
133 Matrix actualH1;
134 factor.evaluateError(x: pose, H&: actualH1);
135
136 // Verify we get the expected error.
137 CHECK(assert_equal(expectedH1, actualH1, 1e-5));
138}
139
140/* ************************************************************************* */
141TEST(PartialPriorFactor, Constructors3) {
142 Key poseKey(1);
143 Pose3 measurement(Rot3::RzRyRx(x: -0.17, y: 0.567, M_PI), Point3(10.0, -2.3, 3.14));
144
145 // Single component of translation.
146 TestPartialPriorFactor3 factor1(poseKey, kIndexTy, measurement.y(),
147 NM::Isotropic::Sigma(dim: 1, sigma: 0.25));
148 CHECK(assert_equal(1, factor1.prior().rows()));
149 CHECK(assert_equal(measurement.y(), factor1.prior()(0)));
150 CHECK(assert_container_equality<Indices>({ kIndexTy }, factor1.indices()));
151
152 // Full translation vector.
153 const Indices t_indices = { kIndexTx, kIndexTy, kIndexTz };
154 TestPartialPriorFactor3 factor2(poseKey, t_indices, measurement.translation(),
155 NM::Isotropic::Sigma(dim: 3, sigma: 0.25));
156 CHECK(assert_equal(3, factor2.prior().rows()));
157 CHECK(assert_equal(measurement.translation(), factor2.prior()));
158 CHECK(assert_container_equality<Indices>(t_indices, factor2.indices()));
159
160 // Full tangent vector of rotation.
161 const Indices r_indices = { kIndexRx, kIndexRy, kIndexRz };
162 TestPartialPriorFactor3 factor3(poseKey, r_indices, Rot3::Logmap(R: measurement.rotation()),
163 NM::Isotropic::Sigma(dim: 3, sigma: 0.1));
164 CHECK(assert_equal(3, factor3.prior().rows()));
165 CHECK(assert_equal(Rot3::Logmap(measurement.rotation()), factor3.prior()));
166 CHECK(assert_container_equality<Indices>(r_indices, factor3.indices()));
167}
168
169/* ************************************************************************* */
170TEST(PartialPriorFactor, JacobianAtIdentity3) {
171 Key poseKey(1);
172 Pose3 measurement = Pose3::Identity();
173 SharedNoiseModel model = NM::Isotropic::Sigma(dim: 1, sigma: 0.25);
174
175 TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model);
176
177 Pose3 pose = measurement; // Zero-error linearization point.
178
179 // Calculate numerical derivatives.
180 Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
181 h: [&factor](const Pose3& p) { return factor.evaluateError(x: p); }, x: pose);
182
183 // Use the factor to calculate the derivative.
184 Matrix actualH1;
185 factor.evaluateError(x: pose, H&: actualH1);
186
187 // Verify we get the expected error.
188 CHECK(assert_equal(expectedH1, actualH1, 1e-5));
189}
190
191/* ************************************************************************* */
192TEST(PartialPriorFactor, JacobianPartialTranslation3) {
193 Key poseKey(1);
194 Pose3 measurement(Rot3::RzRyRx(x: 0.15, y: -0.30, z: 0.45), Point3(-5.0, 8.0, -11.0));
195 SharedNoiseModel model = NM::Isotropic::Sigma(dim: 1, sigma: 0.25);
196
197 TestPartialPriorFactor3 factor(poseKey, kIndexTy, measurement.translation().y(), model);
198
199 Pose3 pose = measurement; // Zero-error linearization point.
200
201 // Calculate numerical derivatives.
202 Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
203 h: [&factor](const Pose3& p) { return factor.evaluateError(x: p); }, x: pose);
204
205 // Use the factor to calculate the derivative.
206 Matrix actualH1;
207 factor.evaluateError(x: pose, H&: actualH1);
208
209 // Verify we get the expected error.
210 CHECK(assert_equal(expectedH1, actualH1, 1e-5));
211}
212
213/* ************************************************************************* */
214TEST(PartialPriorFactor, JacobianFullTranslation3) {
215 Key poseKey(1);
216 Pose3 measurement(Rot3::RzRyRx(x: 0.15, y: -0.30, z: 0.45), Point3(-5.0, 8.0, -11.0));
217 SharedNoiseModel model = NM::Isotropic::Sigma(dim: 3, sigma: 0.25);
218
219 std::vector<size_t> translationIndices = { kIndexTx, kIndexTy, kIndexTz };
220 TestPartialPriorFactor3 factor(poseKey, translationIndices, measurement.translation(), model);
221
222 // Create a linearization point at the zero-error point
223 Pose3 pose = measurement; // Zero-error linearization point.
224
225 // Calculate numerical derivatives.
226 Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
227 h: [&factor](const Pose3& p) { return factor.evaluateError(x: p); }, x: pose);
228
229 // Use the factor to calculate the derivative.
230 Matrix actualH1;
231 factor.evaluateError(x: pose, H&: actualH1);
232
233 // Verify we get the expected error.
234 CHECK(assert_equal(expectedH1, actualH1, 1e-5));
235}
236
237/* ************************************************************************* */
238TEST(PartialPriorFactor, JacobianTxTz3) {
239 Key poseKey(1);
240 Pose3 measurement(Rot3::RzRyRx(x: -0.17, y: 0.567, M_PI), Point3(10.0, -2.3, 3.14));
241 SharedNoiseModel model = NM::Isotropic::Sigma(dim: 2, sigma: 0.25);
242
243 std::vector<size_t> translationIndices = { kIndexTx, kIndexTz };
244 TestPartialPriorFactor3 factor(poseKey, translationIndices,
245 (Vector(2) << measurement.x(), measurement.z()).finished(), model);
246
247 Pose3 pose = measurement; // Zero-error linearization point.
248
249 // Calculate numerical derivatives.
250 Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
251 h: [&factor](const Pose3& p) { return factor.evaluateError(x: p); }, x: pose);
252
253 // Use the factor to calculate the derivative.
254 Matrix actualH1;
255 factor.evaluateError(x: pose, H&: actualH1);
256
257 // Verify we get the expected error.
258 CHECK(assert_equal(expectedH1, actualH1, 1e-5));
259}
260
261/* ************************************************************************* */
262TEST(PartialPriorFactor, JacobianFullRotation3) {
263 Key poseKey(1);
264 Pose3 measurement(Rot3::RzRyRx(x: 0.15, y: -0.30, z: 0.45), Point3(-5.0, 8.0, -11.0));
265 SharedNoiseModel model = NM::Isotropic::Sigma(dim: 3, sigma: 0.25);
266
267 std::vector<size_t> rotationIndices = { kIndexRx, kIndexRy, kIndexRz };
268 TestPartialPriorFactor3 factor(poseKey, rotationIndices, Rot3::Logmap(R: measurement.rotation()), model);
269
270 Pose3 pose = measurement; // Zero-error linearization point.
271
272 // Calculate numerical derivatives.
273 Matrix expectedH1 = numericalDerivative11<Vector, Pose3>(
274 h: [&factor](const Pose3& p) { return factor.evaluateError(x: p); }, x: pose);
275
276 // Use the factor to calculate the derivative.
277 Matrix actualH1;
278 factor.evaluateError(x: pose, H&: actualH1);
279
280 // Verify we get the expected error.
281 CHECK(assert_equal(expectedH1, actualH1, 1e-5));
282}
283
284/* ************************************************************************* */
285int main() { TestResult tr; return TestRegistry::runAllTests(result&: tr); }
286/* ************************************************************************* */
287