1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2015 Google Inc. All rights reserved.
3 // http://ceres-solver.org/
4 //
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 //
8 // * Redistributions of source code must retain the above copyright notice,
9 // this list of conditions and the following disclaimer.
10 // * Redistributions in binary form must reproduce the above copyright notice,
11 // this list of conditions and the following disclaimer in the documentation
12 // and/or other materials provided with the distribution.
13 // * Neither the name of Google Inc. nor the names of its contributors may be
14 // used to endorse or promote products derived from this software without
15 // specific prior written permission.
16 //
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27 // POSSIBILITY OF SUCH DAMAGE.
28 //
29 // Author: sameeragarwal@google.com (Sameer Agarwal)
30
31 #include "ceres/local_parameterization.h"
32
33 #include <cmath>
34 #include <limits>
35 #include <memory>
36
37 #include "Eigen/Geometry"
38 #include "ceres/autodiff_local_parameterization.h"
39 #include "ceres/internal/autodiff.h"
40 #include "ceres/internal/eigen.h"
41 #include "ceres/internal/householder_vector.h"
42 #include "ceres/random.h"
43 #include "ceres/rotation.h"
44 #include "gtest/gtest.h"
45
46 namespace ceres {
47 namespace internal {
48
TEST(IdentityParameterization,EverythingTest)49 TEST(IdentityParameterization, EverythingTest) {
50 IdentityParameterization parameterization(3);
51 EXPECT_EQ(parameterization.GlobalSize(), 3);
52 EXPECT_EQ(parameterization.LocalSize(), 3);
53
54 double x[3] = {1.0, 2.0, 3.0};
55 double delta[3] = {0.0, 1.0, 2.0};
56 double x_plus_delta[3] = {0.0, 0.0, 0.0};
57 parameterization.Plus(x, delta, x_plus_delta);
58 EXPECT_EQ(x_plus_delta[0], 1.0);
59 EXPECT_EQ(x_plus_delta[1], 3.0);
60 EXPECT_EQ(x_plus_delta[2], 5.0);
61
62 double jacobian[9];
63 parameterization.ComputeJacobian(x, jacobian);
64 int k = 0;
65 for (int i = 0; i < 3; ++i) {
66 for (int j = 0; j < 3; ++j, ++k) {
67 EXPECT_EQ(jacobian[k], (i == j) ? 1.0 : 0.0);
68 }
69 }
70
71 Matrix global_matrix = Matrix::Ones(10, 3);
72 Matrix local_matrix = Matrix::Zero(10, 3);
73 parameterization.MultiplyByJacobian(
74 x, 10, global_matrix.data(), local_matrix.data());
75 EXPECT_EQ((local_matrix - global_matrix).norm(), 0.0);
76 }
77
TEST(SubsetParameterization,EmptyConstantParameters)78 TEST(SubsetParameterization, EmptyConstantParameters) {
79 std::vector<int> constant_parameters;
80 SubsetParameterization parameterization(3, constant_parameters);
81 EXPECT_EQ(parameterization.GlobalSize(), 3);
82 EXPECT_EQ(parameterization.LocalSize(), 3);
83 double x[3] = {1, 2, 3};
84 double delta[3] = {4, 5, 6};
85 double x_plus_delta[3] = {-1, -2, -3};
86 parameterization.Plus(x, delta, x_plus_delta);
87 EXPECT_EQ(x_plus_delta[0], x[0] + delta[0]);
88 EXPECT_EQ(x_plus_delta[1], x[1] + delta[1]);
89 EXPECT_EQ(x_plus_delta[2], x[2] + delta[2]);
90
91 Matrix jacobian(3, 3);
92 Matrix expected_jacobian(3, 3);
93 expected_jacobian.setIdentity();
94 parameterization.ComputeJacobian(x, jacobian.data());
95 EXPECT_EQ(jacobian, expected_jacobian);
96
97 Matrix global_matrix(3, 5);
98 global_matrix.setRandom();
99 Matrix local_matrix(3, 5);
100 parameterization.MultiplyByJacobian(
101 x, 5, global_matrix.data(), local_matrix.data());
102 EXPECT_EQ(global_matrix, local_matrix);
103 }
104
TEST(SubsetParameterization,NegativeParameterIndexDeathTest)105 TEST(SubsetParameterization, NegativeParameterIndexDeathTest) {
106 std::vector<int> constant_parameters;
107 constant_parameters.push_back(-1);
108 EXPECT_DEATH_IF_SUPPORTED(
109 SubsetParameterization parameterization(2, constant_parameters),
110 "greater than equal to zero");
111 }
112
TEST(SubsetParameterization,GreaterThanSizeParameterIndexDeathTest)113 TEST(SubsetParameterization, GreaterThanSizeParameterIndexDeathTest) {
114 std::vector<int> constant_parameters;
115 constant_parameters.push_back(2);
116 EXPECT_DEATH_IF_SUPPORTED(
117 SubsetParameterization parameterization(2, constant_parameters),
118 "less than the size");
119 }
120
TEST(SubsetParameterization,DuplicateParametersDeathTest)121 TEST(SubsetParameterization, DuplicateParametersDeathTest) {
122 std::vector<int> constant_parameters;
123 constant_parameters.push_back(1);
124 constant_parameters.push_back(1);
125 EXPECT_DEATH_IF_SUPPORTED(
126 SubsetParameterization parameterization(2, constant_parameters),
127 "duplicates");
128 }
129
TEST(SubsetParameterization,ProductParameterizationWithZeroLocalSizeSubsetParameterization1)130 TEST(SubsetParameterization,
131 ProductParameterizationWithZeroLocalSizeSubsetParameterization1) {
132 std::vector<int> constant_parameters;
133 constant_parameters.push_back(0);
134 LocalParameterization* subset_param =
135 new SubsetParameterization(1, constant_parameters);
136 LocalParameterization* identity_param = new IdentityParameterization(2);
137 ProductParameterization product_param(subset_param, identity_param);
138 EXPECT_EQ(product_param.GlobalSize(), 3);
139 EXPECT_EQ(product_param.LocalSize(), 2);
140 double x[] = {1.0, 1.0, 1.0};
141 double delta[] = {2.0, 3.0};
142 double x_plus_delta[] = {0.0, 0.0, 0.0};
143 EXPECT_TRUE(product_param.Plus(x, delta, x_plus_delta));
144 EXPECT_EQ(x_plus_delta[0], x[0]);
145 EXPECT_EQ(x_plus_delta[1], x[1] + delta[0]);
146 EXPECT_EQ(x_plus_delta[2], x[2] + delta[1]);
147
148 Matrix actual_jacobian(3, 2);
149 EXPECT_TRUE(product_param.ComputeJacobian(x, actual_jacobian.data()));
150 }
151
TEST(SubsetParameterization,ProductParameterizationWithZeroLocalSizeSubsetParameterization2)152 TEST(SubsetParameterization,
153 ProductParameterizationWithZeroLocalSizeSubsetParameterization2) {
154 std::vector<int> constant_parameters;
155 constant_parameters.push_back(0);
156 LocalParameterization* subset_param =
157 new SubsetParameterization(1, constant_parameters);
158 LocalParameterization* identity_param = new IdentityParameterization(2);
159 ProductParameterization product_param(identity_param, subset_param);
160 EXPECT_EQ(product_param.GlobalSize(), 3);
161 EXPECT_EQ(product_param.LocalSize(), 2);
162 double x[] = {1.0, 1.0, 1.0};
163 double delta[] = {2.0, 3.0};
164 double x_plus_delta[] = {0.0, 0.0, 0.0};
165 EXPECT_TRUE(product_param.Plus(x, delta, x_plus_delta));
166 EXPECT_EQ(x_plus_delta[0], x[0] + delta[0]);
167 EXPECT_EQ(x_plus_delta[1], x[1] + delta[1]);
168 EXPECT_EQ(x_plus_delta[2], x[2]);
169
170 Matrix actual_jacobian(3, 2);
171 EXPECT_TRUE(product_param.ComputeJacobian(x, actual_jacobian.data()));
172 }
173
TEST(SubsetParameterization,NormalFunctionTest)174 TEST(SubsetParameterization, NormalFunctionTest) {
175 const int kGlobalSize = 4;
176 const int kLocalSize = 3;
177
178 double x[kGlobalSize] = {1.0, 2.0, 3.0, 4.0};
179 for (int i = 0; i < kGlobalSize; ++i) {
180 std::vector<int> constant_parameters;
181 constant_parameters.push_back(i);
182 SubsetParameterization parameterization(kGlobalSize, constant_parameters);
183 double delta[kLocalSize] = {1.0, 2.0, 3.0};
184 double x_plus_delta[kGlobalSize] = {0.0, 0.0, 0.0};
185
186 parameterization.Plus(x, delta, x_plus_delta);
187 int k = 0;
188 for (int j = 0; j < kGlobalSize; ++j) {
189 if (j == i) {
190 EXPECT_EQ(x_plus_delta[j], x[j]);
191 } else {
192 EXPECT_EQ(x_plus_delta[j], x[j] + delta[k++]);
193 }
194 }
195
196 double jacobian[kGlobalSize * kLocalSize];
197 parameterization.ComputeJacobian(x, jacobian);
198 int delta_cursor = 0;
199 int jacobian_cursor = 0;
200 for (int j = 0; j < kGlobalSize; ++j) {
201 if (j != i) {
202 for (int k = 0; k < kLocalSize; ++k, jacobian_cursor++) {
203 EXPECT_EQ(jacobian[jacobian_cursor], delta_cursor == k ? 1.0 : 0.0);
204 }
205 ++delta_cursor;
206 } else {
207 for (int k = 0; k < kLocalSize; ++k, jacobian_cursor++) {
208 EXPECT_EQ(jacobian[jacobian_cursor], 0.0);
209 }
210 }
211 }
212
213 Matrix global_matrix = Matrix::Ones(10, kGlobalSize);
214 for (int row = 0; row < kGlobalSize; ++row) {
215 for (int col = 0; col < kGlobalSize; ++col) {
216 global_matrix(row, col) = col;
217 }
218 }
219
220 Matrix local_matrix = Matrix::Zero(10, kLocalSize);
221 parameterization.MultiplyByJacobian(
222 x, 10, global_matrix.data(), local_matrix.data());
223 Matrix expected_local_matrix =
224 global_matrix * MatrixRef(jacobian, kGlobalSize, kLocalSize);
225 EXPECT_EQ((local_matrix - expected_local_matrix).norm(), 0.0);
226 }
227 }
228
229 // Functor needed to implement automatically differentiated Plus for
230 // quaternions.
231 struct QuaternionPlus {
232 template <typename T>
operator ()ceres::internal::QuaternionPlus233 bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
234 const T squared_norm_delta =
235 delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
236
237 T q_delta[4];
238 if (squared_norm_delta > T(0.0)) {
239 T norm_delta = sqrt(squared_norm_delta);
240 const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
241 q_delta[0] = cos(norm_delta);
242 q_delta[1] = sin_delta_by_delta * delta[0];
243 q_delta[2] = sin_delta_by_delta * delta[1];
244 q_delta[3] = sin_delta_by_delta * delta[2];
245 } else {
246 // We do not just use q_delta = [1,0,0,0] here because that is a
247 // constant and when used for automatic differentiation will
248 // lead to a zero derivative. Instead we take a first order
249 // approximation and evaluate it at zero.
250 q_delta[0] = T(1.0);
251 q_delta[1] = delta[0];
252 q_delta[2] = delta[1];
253 q_delta[3] = delta[2];
254 }
255
256 QuaternionProduct(q_delta, x, x_plus_delta);
257 return true;
258 }
259 };
260
261 template <typename Parameterization, typename Plus>
QuaternionParameterizationTestHelper(const double * x,const double * delta,const double * x_plus_delta_ref)262 void QuaternionParameterizationTestHelper(const double* x,
263 const double* delta,
264 const double* x_plus_delta_ref) {
265 const int kGlobalSize = 4;
266 const int kLocalSize = 3;
267
268 const double kTolerance = 1e-14;
269
270 double x_plus_delta[kGlobalSize] = {0.0, 0.0, 0.0, 0.0};
271 Parameterization parameterization;
272 parameterization.Plus(x, delta, x_plus_delta);
273 for (int i = 0; i < kGlobalSize; ++i) {
274 EXPECT_NEAR(x_plus_delta[i], x_plus_delta[i], kTolerance);
275 }
276
277 const double x_plus_delta_norm = sqrt(
278 x_plus_delta[0] * x_plus_delta[0] + x_plus_delta[1] * x_plus_delta[1] +
279 x_plus_delta[2] * x_plus_delta[2] + x_plus_delta[3] * x_plus_delta[3]);
280
281 EXPECT_NEAR(x_plus_delta_norm, 1.0, kTolerance);
282
283 double jacobian_ref[12];
284 double zero_delta[kLocalSize] = {0.0, 0.0, 0.0};
285 const double* parameters[2] = {x, zero_delta};
286 double* jacobian_array[2] = {NULL, jacobian_ref};
287
288 // Autodiff jacobian at delta_x = 0.
289 internal::AutoDifferentiate<kGlobalSize,
290 StaticParameterDims<kGlobalSize, kLocalSize>>(
291 Plus(), parameters, kGlobalSize, x_plus_delta, jacobian_array);
292
293 double jacobian[12];
294 parameterization.ComputeJacobian(x, jacobian);
295 for (int i = 0; i < 12; ++i) {
296 EXPECT_TRUE(IsFinite(jacobian[i]));
297 EXPECT_NEAR(jacobian[i], jacobian_ref[i], kTolerance)
298 << "Jacobian mismatch: i = " << i << "\n Expected \n"
299 << ConstMatrixRef(jacobian_ref, kGlobalSize, kLocalSize)
300 << "\n Actual \n"
301 << ConstMatrixRef(jacobian, kGlobalSize, kLocalSize);
302 }
303
304 Matrix global_matrix = Matrix::Random(10, kGlobalSize);
305 Matrix local_matrix = Matrix::Zero(10, kLocalSize);
306 parameterization.MultiplyByJacobian(
307 x, 10, global_matrix.data(), local_matrix.data());
308 Matrix expected_local_matrix =
309 global_matrix * MatrixRef(jacobian, kGlobalSize, kLocalSize);
310 EXPECT_NEAR((local_matrix - expected_local_matrix).norm(),
311 0.0,
312 10.0 * std::numeric_limits<double>::epsilon());
313 }
314
315 template <int N>
Normalize(double * x)316 void Normalize(double* x) {
317 VectorRef(x, N).normalize();
318 }
319
TEST(QuaternionParameterization,ZeroTest)320 TEST(QuaternionParameterization, ZeroTest) {
321 double x[4] = {0.5, 0.5, 0.5, 0.5};
322 double delta[3] = {0.0, 0.0, 0.0};
323 double q_delta[4] = {1.0, 0.0, 0.0, 0.0};
324 double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0};
325 QuaternionProduct(q_delta, x, x_plus_delta);
326 QuaternionParameterizationTestHelper<QuaternionParameterization,
327 QuaternionPlus>(x, delta, x_plus_delta);
328 }
329
TEST(QuaternionParameterization,NearZeroTest)330 TEST(QuaternionParameterization, NearZeroTest) {
331 double x[4] = {0.52, 0.25, 0.15, 0.45};
332 Normalize<4>(x);
333
334 double delta[3] = {0.24, 0.15, 0.10};
335 for (int i = 0; i < 3; ++i) {
336 delta[i] = delta[i] * 1e-14;
337 }
338
339 double q_delta[4];
340 q_delta[0] = 1.0;
341 q_delta[1] = delta[0];
342 q_delta[2] = delta[1];
343 q_delta[3] = delta[2];
344
345 double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0};
346 QuaternionProduct(q_delta, x, x_plus_delta);
347 QuaternionParameterizationTestHelper<QuaternionParameterization,
348 QuaternionPlus>(x, delta, x_plus_delta);
349 }
350
TEST(QuaternionParameterization,AwayFromZeroTest)351 TEST(QuaternionParameterization, AwayFromZeroTest) {
352 double x[4] = {0.52, 0.25, 0.15, 0.45};
353 Normalize<4>(x);
354
355 double delta[3] = {0.24, 0.15, 0.10};
356 const double delta_norm =
357 sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
358 double q_delta[4];
359 q_delta[0] = cos(delta_norm);
360 q_delta[1] = sin(delta_norm) / delta_norm * delta[0];
361 q_delta[2] = sin(delta_norm) / delta_norm * delta[1];
362 q_delta[3] = sin(delta_norm) / delta_norm * delta[2];
363
364 double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0};
365 QuaternionProduct(q_delta, x, x_plus_delta);
366 QuaternionParameterizationTestHelper<QuaternionParameterization,
367 QuaternionPlus>(x, delta, x_plus_delta);
368 }
369
370 // Functor needed to implement automatically differentiated Plus for
371 // Eigen's quaternion.
372 struct EigenQuaternionPlus {
373 template <typename T>
operator ()ceres::internal::EigenQuaternionPlus374 bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
375 const T norm_delta =
376 sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
377
378 Eigen::Quaternion<T> q_delta;
379 if (norm_delta > T(0.0)) {
380 const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
381 q_delta.coeffs() << sin_delta_by_delta * delta[0],
382 sin_delta_by_delta * delta[1], sin_delta_by_delta * delta[2],
383 cos(norm_delta);
384 } else {
385 // We do not just use q_delta = [0,0,0,1] here because that is a
386 // constant and when used for automatic differentiation will
387 // lead to a zero derivative. Instead we take a first order
388 // approximation and evaluate it at zero.
389 q_delta.coeffs() << delta[0], delta[1], delta[2], T(1.0);
390 }
391
392 Eigen::Map<Eigen::Quaternion<T>> x_plus_delta_ref(x_plus_delta);
393 Eigen::Map<const Eigen::Quaternion<T>> x_ref(x);
394 x_plus_delta_ref = q_delta * x_ref;
395 return true;
396 }
397 };
398
TEST(EigenQuaternionParameterization,ZeroTest)399 TEST(EigenQuaternionParameterization, ZeroTest) {
400 Eigen::Quaterniond x(0.5, 0.5, 0.5, 0.5);
401 double delta[3] = {0.0, 0.0, 0.0};
402 Eigen::Quaterniond q_delta(1.0, 0.0, 0.0, 0.0);
403 Eigen::Quaterniond x_plus_delta = q_delta * x;
404 QuaternionParameterizationTestHelper<EigenQuaternionParameterization,
405 EigenQuaternionPlus>(
406 x.coeffs().data(), delta, x_plus_delta.coeffs().data());
407 }
408
TEST(EigenQuaternionParameterization,NearZeroTest)409 TEST(EigenQuaternionParameterization, NearZeroTest) {
410 Eigen::Quaterniond x(0.52, 0.25, 0.15, 0.45);
411 x.normalize();
412
413 double delta[3] = {0.24, 0.15, 0.10};
414 for (int i = 0; i < 3; ++i) {
415 delta[i] = delta[i] * 1e-14;
416 }
417
418 // Note: w is first in the constructor.
419 Eigen::Quaterniond q_delta(1.0, delta[0], delta[1], delta[2]);
420
421 Eigen::Quaterniond x_plus_delta = q_delta * x;
422 QuaternionParameterizationTestHelper<EigenQuaternionParameterization,
423 EigenQuaternionPlus>(
424 x.coeffs().data(), delta, x_plus_delta.coeffs().data());
425 }
426
TEST(EigenQuaternionParameterization,AwayFromZeroTest)427 TEST(EigenQuaternionParameterization, AwayFromZeroTest) {
428 Eigen::Quaterniond x(0.52, 0.25, 0.15, 0.45);
429 x.normalize();
430
431 double delta[3] = {0.24, 0.15, 0.10};
432 const double delta_norm =
433 sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
434
435 // Note: w is first in the constructor.
436 Eigen::Quaterniond q_delta(cos(delta_norm),
437 sin(delta_norm) / delta_norm * delta[0],
438 sin(delta_norm) / delta_norm * delta[1],
439 sin(delta_norm) / delta_norm * delta[2]);
440
441 Eigen::Quaterniond x_plus_delta = q_delta * x;
442 QuaternionParameterizationTestHelper<EigenQuaternionParameterization,
443 EigenQuaternionPlus>(
444 x.coeffs().data(), delta, x_plus_delta.coeffs().data());
445 }
446
447 // Functor needed to implement automatically differentiated Plus for
448 // homogeneous vectors.
449 template <int Dim>
450 struct HomogeneousVectorParameterizationPlus {
451 template <typename Scalar>
operator ()ceres::internal::HomogeneousVectorParameterizationPlus452 bool operator()(const Scalar* p_x,
453 const Scalar* p_delta,
454 Scalar* p_x_plus_delta) const {
455 Eigen::Map<const Eigen::Matrix<Scalar, Dim, 1>> x(p_x);
456 Eigen::Map<const Eigen::Matrix<Scalar, Dim - 1, 1>> delta(p_delta);
457 Eigen::Map<Eigen::Matrix<Scalar, Dim, 1>> x_plus_delta(p_x_plus_delta);
458
459 const Scalar squared_norm_delta = delta.squaredNorm();
460
461 Eigen::Matrix<Scalar, Dim, 1> y;
462 Scalar one_half(0.5);
463 if (squared_norm_delta > Scalar(0.0)) {
464 Scalar norm_delta = sqrt(squared_norm_delta);
465 Scalar norm_delta_div_2 = 0.5 * norm_delta;
466 const Scalar sin_delta_by_delta =
467 sin(norm_delta_div_2) / norm_delta_div_2;
468 y.template head<Dim - 1>() = sin_delta_by_delta * one_half * delta;
469 y[Dim - 1] = cos(norm_delta_div_2);
470
471 } else {
472 // We do not just use y = [0,0,0,1] here because that is a
473 // constant and when used for automatic differentiation will
474 // lead to a zero derivative. Instead we take a first order
475 // approximation and evaluate it at zero.
476 y.template head<Dim - 1>() = delta * one_half;
477 y[Dim - 1] = Scalar(1.0);
478 }
479
480 Eigen::Matrix<Scalar, Dim, 1> v;
481 Scalar beta;
482
483 // NOTE: The explicit template arguments are needed here because
484 // ComputeHouseholderVector is templated and some versions of MSVC
485 // have trouble deducing the type of v automatically.
486 internal::ComputeHouseholderVector<
487 Eigen::Map<const Eigen::Matrix<Scalar, Dim, 1>>,
488 Scalar,
489 Dim>(x, &v, &beta);
490
491 x_plus_delta = x.norm() * (y - v * (beta * v.dot(y)));
492
493 return true;
494 }
495 };
496
HomogeneousVectorParameterizationHelper(const double * x,const double * delta)497 static void HomogeneousVectorParameterizationHelper(const double* x,
498 const double* delta) {
499 const double kTolerance = 1e-14;
500
501 HomogeneousVectorParameterization homogeneous_vector_parameterization(4);
502
503 // Ensure the update maintains the norm.
504 double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0};
505 homogeneous_vector_parameterization.Plus(x, delta, x_plus_delta);
506
507 const double x_plus_delta_norm = sqrt(
508 x_plus_delta[0] * x_plus_delta[0] + x_plus_delta[1] * x_plus_delta[1] +
509 x_plus_delta[2] * x_plus_delta[2] + x_plus_delta[3] * x_plus_delta[3]);
510
511 const double x_norm =
512 sqrt(x[0] * x[0] + x[1] * x[1] + x[2] * x[2] + x[3] * x[3]);
513
514 EXPECT_NEAR(x_plus_delta_norm, x_norm, kTolerance);
515
516 // Autodiff jacobian at delta_x = 0.
517 AutoDiffLocalParameterization<HomogeneousVectorParameterizationPlus<4>, 4, 3>
518 autodiff_jacobian;
519
520 double jacobian_autodiff[12];
521 double jacobian_analytic[12];
522
523 homogeneous_vector_parameterization.ComputeJacobian(x, jacobian_analytic);
524 autodiff_jacobian.ComputeJacobian(x, jacobian_autodiff);
525
526 for (int i = 0; i < 12; ++i) {
527 EXPECT_TRUE(ceres::IsFinite(jacobian_analytic[i]));
528 EXPECT_NEAR(jacobian_analytic[i], jacobian_autodiff[i], kTolerance)
529 << "Jacobian mismatch: i = " << i << ", " << jacobian_analytic[i] << " "
530 << jacobian_autodiff[i];
531 }
532 }
533
TEST(HomogeneousVectorParameterization,ZeroTest)534 TEST(HomogeneousVectorParameterization, ZeroTest) {
535 double x[4] = {0.0, 0.0, 0.0, 1.0};
536 Normalize<4>(x);
537 double delta[3] = {0.0, 0.0, 0.0};
538
539 HomogeneousVectorParameterizationHelper(x, delta);
540 }
541
TEST(HomogeneousVectorParameterization,NearZeroTest1)542 TEST(HomogeneousVectorParameterization, NearZeroTest1) {
543 double x[4] = {1e-5, 1e-5, 1e-5, 1.0};
544 Normalize<4>(x);
545 double delta[3] = {0.0, 1.0, 0.0};
546
547 HomogeneousVectorParameterizationHelper(x, delta);
548 }
549
TEST(HomogeneousVectorParameterization,NearZeroTest2)550 TEST(HomogeneousVectorParameterization, NearZeroTest2) {
551 double x[4] = {0.001, 0.0, 0.0, 0.0};
552 double delta[3] = {0.0, 1.0, 0.0};
553
554 HomogeneousVectorParameterizationHelper(x, delta);
555 }
556
TEST(HomogeneousVectorParameterization,AwayFromZeroTest1)557 TEST(HomogeneousVectorParameterization, AwayFromZeroTest1) {
558 double x[4] = {0.52, 0.25, 0.15, 0.45};
559 Normalize<4>(x);
560 double delta[3] = {0.0, 1.0, -0.5};
561
562 HomogeneousVectorParameterizationHelper(x, delta);
563 }
564
TEST(HomogeneousVectorParameterization,AwayFromZeroTest2)565 TEST(HomogeneousVectorParameterization, AwayFromZeroTest2) {
566 double x[4] = {0.87, -0.25, -0.34, 0.45};
567 Normalize<4>(x);
568 double delta[3] = {0.0, 0.0, -0.5};
569
570 HomogeneousVectorParameterizationHelper(x, delta);
571 }
572
TEST(HomogeneousVectorParameterization,AwayFromZeroTest3)573 TEST(HomogeneousVectorParameterization, AwayFromZeroTest3) {
574 double x[4] = {0.0, 0.0, 0.0, 2.0};
575 double delta[3] = {0.0, 0.0, 0};
576
577 HomogeneousVectorParameterizationHelper(x, delta);
578 }
579
TEST(HomogeneousVectorParameterization,AwayFromZeroTest4)580 TEST(HomogeneousVectorParameterization, AwayFromZeroTest4) {
581 double x[4] = {0.2, -1.0, 0.0, 2.0};
582 double delta[3] = {1.4, 0.0, -0.5};
583
584 HomogeneousVectorParameterizationHelper(x, delta);
585 }
586
TEST(HomogeneousVectorParameterization,AwayFromZeroTest5)587 TEST(HomogeneousVectorParameterization, AwayFromZeroTest5) {
588 double x[4] = {2.0, 0.0, 0.0, 0.0};
589 double delta[3] = {1.4, 0.0, -0.5};
590
591 HomogeneousVectorParameterizationHelper(x, delta);
592 }
593
TEST(HomogeneousVectorParameterization,DeathTests)594 TEST(HomogeneousVectorParameterization, DeathTests) {
595 EXPECT_DEATH_IF_SUPPORTED(HomogeneousVectorParameterization x(1), "size");
596 }
597
598 // Functor needed to implement automatically differentiated Plus for
599 // line parameterization.
600 template <int AmbientSpaceDim>
601 struct LineParameterizationPlus {
602 template <typename Scalar>
operator ()ceres::internal::LineParameterizationPlus603 bool operator()(const Scalar* p_x,
604 const Scalar* p_delta,
605 Scalar* p_x_plus_delta) const {
606 static constexpr int kTangetSpaceDim = AmbientSpaceDim - 1;
607 Eigen::Map<const Eigen::Matrix<Scalar, AmbientSpaceDim, 1>> origin_point(
608 p_x);
609 Eigen::Map<const Eigen::Matrix<Scalar, AmbientSpaceDim, 1>> dir(
610 p_x + AmbientSpaceDim);
611 Eigen::Map<const Eigen::Matrix<Scalar, kTangetSpaceDim, 1>>
612 delta_origin_point(p_delta);
613 Eigen::Map<Eigen::Matrix<Scalar, AmbientSpaceDim, 1>>
614 origin_point_plus_delta(p_x_plus_delta);
615
616 HomogeneousVectorParameterizationPlus<AmbientSpaceDim> dir_plus;
617 dir_plus(dir.data(),
618 p_delta + kTangetSpaceDim,
619 p_x_plus_delta + AmbientSpaceDim);
620
621 Eigen::Matrix<Scalar, AmbientSpaceDim, 1> v;
622 Scalar beta;
623
624 // NOTE: The explicit template arguments are needed here because
625 // ComputeHouseholderVector is templated and some versions of MSVC
626 // have trouble deducing the type of v automatically.
627 internal::ComputeHouseholderVector<
628 Eigen::Map<const Eigen::Matrix<Scalar, AmbientSpaceDim, 1>>,
629 Scalar,
630 AmbientSpaceDim>(dir, &v, &beta);
631
632 Eigen::Matrix<Scalar, AmbientSpaceDim, 1> y;
633 y << 0.5 * delta_origin_point, Scalar(0.0);
634 origin_point_plus_delta = origin_point + y - v * (beta * v.dot(y));
635
636 return true;
637 }
638 };
639
640 template <int AmbientSpaceDim>
LineParameterizationHelper(const double * x_ptr,const double * delta)641 static void LineParameterizationHelper(const double* x_ptr,
642 const double* delta) {
643 const double kTolerance = 1e-14;
644
645 static constexpr int ParameterDim = 2 * AmbientSpaceDim;
646 static constexpr int TangientParameterDim = 2 * (AmbientSpaceDim - 1);
647
648 LineParameterization<AmbientSpaceDim> line_parameterization;
649
650 using ParameterVector = Eigen::Matrix<double, ParameterDim, 1>;
651 ParameterVector x_plus_delta = ParameterVector::Zero();
652 line_parameterization.Plus(x_ptr, delta, x_plus_delta.data());
653
654 // Ensure the update maintains the norm for the line direction.
655 Eigen::Map<const ParameterVector> x(x_ptr);
656 const double dir_plus_delta_norm =
657 x_plus_delta.template tail<AmbientSpaceDim>().norm();
658 const double dir_norm = x.template tail<AmbientSpaceDim>().norm();
659 EXPECT_NEAR(dir_plus_delta_norm, dir_norm, kTolerance);
660
661 // Ensure the update of the origin point is perpendicular to the line
662 // direction.
663 const double dot_prod_val = x.template tail<AmbientSpaceDim>().dot(
664 x_plus_delta.template head<AmbientSpaceDim>() -
665 x.template head<AmbientSpaceDim>());
666 EXPECT_NEAR(dot_prod_val, 0.0, kTolerance);
667
668 // Autodiff jacobian at delta_x = 0.
669 AutoDiffLocalParameterization<LineParameterizationPlus<AmbientSpaceDim>,
670 ParameterDim,
671 TangientParameterDim>
672 autodiff_jacobian;
673
674 using JacobianMatrix = Eigen::
675 Matrix<double, ParameterDim, TangientParameterDim, Eigen::RowMajor>;
676 constexpr double kNaN = std::numeric_limits<double>::quiet_NaN();
677 JacobianMatrix jacobian_autodiff = JacobianMatrix::Constant(kNaN);
678 JacobianMatrix jacobian_analytic = JacobianMatrix::Constant(kNaN);
679
680 autodiff_jacobian.ComputeJacobian(x_ptr, jacobian_autodiff.data());
681 line_parameterization.ComputeJacobian(x_ptr, jacobian_analytic.data());
682
683 EXPECT_FALSE(jacobian_autodiff.hasNaN());
684 EXPECT_FALSE(jacobian_analytic.hasNaN());
685 EXPECT_TRUE(jacobian_autodiff.isApprox(jacobian_analytic))
686 << "auto diff:\n"
687 << jacobian_autodiff << "\n"
688 << "analytic diff:\n"
689 << jacobian_analytic;
690 }
691
TEST(LineParameterization,ZeroTest3D)692 TEST(LineParameterization, ZeroTest3D) {
693 double x[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
694 double delta[4] = {0.0, 0.0, 0.0, 0.0};
695
696 LineParameterizationHelper<3>(x, delta);
697 }
698
TEST(LineParameterization,ZeroTest4D)699 TEST(LineParameterization, ZeroTest4D) {
700 double x[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
701 double delta[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0};
702
703 LineParameterizationHelper<4>(x, delta);
704 }
705
TEST(LineParameterization,ZeroOriginPointTest3D)706 TEST(LineParameterization, ZeroOriginPointTest3D) {
707 double x[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
708 double delta[4] = {0.0, 0.0, 1.0, 2.0};
709
710 LineParameterizationHelper<3>(x, delta);
711 }
712
TEST(LineParameterization,ZeroOriginPointTest4D)713 TEST(LineParameterization, ZeroOriginPointTest4D) {
714 double x[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
715 double delta[6] = {0.0, 0.0, 0.0, 1.0, 2.0, 3.0};
716
717 LineParameterizationHelper<4>(x, delta);
718 }
719
TEST(LineParameterization,ZeroDirTest3D)720 TEST(LineParameterization, ZeroDirTest3D) {
721 double x[6] = {0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
722 double delta[4] = {3.0, 2.0, 0.0, 0.0};
723
724 LineParameterizationHelper<3>(x, delta);
725 }
726
TEST(LineParameterization,ZeroDirTest4D)727 TEST(LineParameterization, ZeroDirTest4D) {
728 double x[8] = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0};
729 double delta[6] = {3.0, 2.0, 1.0, 0.0, 0.0, 0.0};
730
731 LineParameterizationHelper<4>(x, delta);
732 }
733
TEST(LineParameterization,AwayFromZeroTest3D1)734 TEST(LineParameterization, AwayFromZeroTest3D1) {
735 Eigen::Matrix<double, 6, 1> x;
736 x.head<3>() << 1.54, 2.32, 1.34;
737 x.tail<3>() << 0.52, 0.25, 0.15;
738 x.tail<3>().normalize();
739
740 double delta[4] = {4.0, 7.0, 1.0, -0.5};
741
742 LineParameterizationHelper<3>(x.data(), delta);
743 }
744
TEST(LineParameterization,AwayFromZeroTest4D1)745 TEST(LineParameterization, AwayFromZeroTest4D1) {
746 Eigen::Matrix<double, 8, 1> x;
747 x.head<4>() << 1.54, 2.32, 1.34, 3.23;
748 x.tail<4>() << 0.52, 0.25, 0.15, 0.45;
749 x.tail<4>().normalize();
750
751 double delta[6] = {4.0, 7.0, -3.0, 0.0, 1.0, -0.5};
752
753 LineParameterizationHelper<4>(x.data(), delta);
754 }
755
TEST(LineParameterization,AwayFromZeroTest3D2)756 TEST(LineParameterization, AwayFromZeroTest3D2) {
757 Eigen::Matrix<double, 6, 1> x;
758 x.head<3>() << 7.54, -2.81, 8.63;
759 x.tail<3>() << 2.52, 5.25, 4.15;
760
761 double delta[4] = {4.0, 7.0, 1.0, -0.5};
762
763 LineParameterizationHelper<3>(x.data(), delta);
764 }
765
TEST(LineParameterization,AwayFromZeroTest4D2)766 TEST(LineParameterization, AwayFromZeroTest4D2) {
767 Eigen::Matrix<double, 8, 1> x;
768 x.head<4>() << 7.54, -2.81, 8.63, 6.93;
769 x.tail<4>() << 2.52, 5.25, 4.15, 1.45;
770
771 double delta[6] = {4.0, 7.0, -3.0, 2.0, 1.0, -0.5};
772
773 LineParameterizationHelper<4>(x.data(), delta);
774 }
775
776 class ProductParameterizationTest : public ::testing::Test {
777 protected:
SetUp()778 void SetUp() final {
779 const int global_size1 = 5;
780 std::vector<int> constant_parameters1;
781 constant_parameters1.push_back(2);
782 param1_.reset(
783 new SubsetParameterization(global_size1, constant_parameters1));
784
785 const int global_size2 = 3;
786 std::vector<int> constant_parameters2;
787 constant_parameters2.push_back(0);
788 constant_parameters2.push_back(1);
789 param2_.reset(
790 new SubsetParameterization(global_size2, constant_parameters2));
791
792 const int global_size3 = 4;
793 std::vector<int> constant_parameters3;
794 constant_parameters3.push_back(1);
795 param3_.reset(
796 new SubsetParameterization(global_size3, constant_parameters3));
797
798 const int global_size4 = 2;
799 std::vector<int> constant_parameters4;
800 constant_parameters4.push_back(1);
801 param4_.reset(
802 new SubsetParameterization(global_size4, constant_parameters4));
803 }
804
805 std::unique_ptr<LocalParameterization> param1_;
806 std::unique_ptr<LocalParameterization> param2_;
807 std::unique_ptr<LocalParameterization> param3_;
808 std::unique_ptr<LocalParameterization> param4_;
809 };
810
TEST_F(ProductParameterizationTest,LocalAndGlobalSize2)811 TEST_F(ProductParameterizationTest, LocalAndGlobalSize2) {
812 LocalParameterization* param1 = param1_.release();
813 LocalParameterization* param2 = param2_.release();
814
815 ProductParameterization product_param(param1, param2);
816 EXPECT_EQ(product_param.LocalSize(),
817 param1->LocalSize() + param2->LocalSize());
818 EXPECT_EQ(product_param.GlobalSize(),
819 param1->GlobalSize() + param2->GlobalSize());
820 }
821
TEST_F(ProductParameterizationTest,LocalAndGlobalSize3)822 TEST_F(ProductParameterizationTest, LocalAndGlobalSize3) {
823 LocalParameterization* param1 = param1_.release();
824 LocalParameterization* param2 = param2_.release();
825 LocalParameterization* param3 = param3_.release();
826
827 ProductParameterization product_param(param1, param2, param3);
828 EXPECT_EQ(product_param.LocalSize(),
829 param1->LocalSize() + param2->LocalSize() + param3->LocalSize());
830 EXPECT_EQ(product_param.GlobalSize(),
831 param1->GlobalSize() + param2->GlobalSize() + param3->GlobalSize());
832 }
833
TEST_F(ProductParameterizationTest,LocalAndGlobalSize4)834 TEST_F(ProductParameterizationTest, LocalAndGlobalSize4) {
835 LocalParameterization* param1 = param1_.release();
836 LocalParameterization* param2 = param2_.release();
837 LocalParameterization* param3 = param3_.release();
838 LocalParameterization* param4 = param4_.release();
839
840 ProductParameterization product_param(param1, param2, param3, param4);
841 EXPECT_EQ(product_param.LocalSize(),
842 param1->LocalSize() + param2->LocalSize() + param3->LocalSize() +
843 param4->LocalSize());
844 EXPECT_EQ(product_param.GlobalSize(),
845 param1->GlobalSize() + param2->GlobalSize() + param3->GlobalSize() +
846 param4->GlobalSize());
847 }
848
TEST_F(ProductParameterizationTest,Plus)849 TEST_F(ProductParameterizationTest, Plus) {
850 LocalParameterization* param1 = param1_.release();
851 LocalParameterization* param2 = param2_.release();
852 LocalParameterization* param3 = param3_.release();
853 LocalParameterization* param4 = param4_.release();
854
855 ProductParameterization product_param(param1, param2, param3, param4);
856 std::vector<double> x(product_param.GlobalSize(), 0.0);
857 std::vector<double> delta(product_param.LocalSize(), 0.0);
858 std::vector<double> x_plus_delta_expected(product_param.GlobalSize(), 0.0);
859 std::vector<double> x_plus_delta(product_param.GlobalSize(), 0.0);
860
861 for (int i = 0; i < product_param.GlobalSize(); ++i) {
862 x[i] = RandNormal();
863 }
864
865 for (int i = 0; i < product_param.LocalSize(); ++i) {
866 delta[i] = RandNormal();
867 }
868
869 EXPECT_TRUE(product_param.Plus(&x[0], &delta[0], &x_plus_delta_expected[0]));
870 int x_cursor = 0;
871 int delta_cursor = 0;
872
873 EXPECT_TRUE(param1->Plus(
874 &x[x_cursor], &delta[delta_cursor], &x_plus_delta[x_cursor]));
875 x_cursor += param1->GlobalSize();
876 delta_cursor += param1->LocalSize();
877
878 EXPECT_TRUE(param2->Plus(
879 &x[x_cursor], &delta[delta_cursor], &x_plus_delta[x_cursor]));
880 x_cursor += param2->GlobalSize();
881 delta_cursor += param2->LocalSize();
882
883 EXPECT_TRUE(param3->Plus(
884 &x[x_cursor], &delta[delta_cursor], &x_plus_delta[x_cursor]));
885 x_cursor += param3->GlobalSize();
886 delta_cursor += param3->LocalSize();
887
888 EXPECT_TRUE(param4->Plus(
889 &x[x_cursor], &delta[delta_cursor], &x_plus_delta[x_cursor]));
890 x_cursor += param4->GlobalSize();
891 delta_cursor += param4->LocalSize();
892
893 for (int i = 0; i < x.size(); ++i) {
894 EXPECT_EQ(x_plus_delta[i], x_plus_delta_expected[i]);
895 }
896 }
897
TEST_F(ProductParameterizationTest,ComputeJacobian)898 TEST_F(ProductParameterizationTest, ComputeJacobian) {
899 LocalParameterization* param1 = param1_.release();
900 LocalParameterization* param2 = param2_.release();
901 LocalParameterization* param3 = param3_.release();
902 LocalParameterization* param4 = param4_.release();
903
904 ProductParameterization product_param(param1, param2, param3, param4);
905 std::vector<double> x(product_param.GlobalSize(), 0.0);
906
907 for (int i = 0; i < product_param.GlobalSize(); ++i) {
908 x[i] = RandNormal();
909 }
910
911 Matrix jacobian =
912 Matrix::Random(product_param.GlobalSize(), product_param.LocalSize());
913 EXPECT_TRUE(product_param.ComputeJacobian(&x[0], jacobian.data()));
914 int x_cursor = 0;
915 int delta_cursor = 0;
916
917 Matrix jacobian1(param1->GlobalSize(), param1->LocalSize());
918 EXPECT_TRUE(param1->ComputeJacobian(&x[x_cursor], jacobian1.data()));
919 jacobian.block(
920 x_cursor, delta_cursor, param1->GlobalSize(), param1->LocalSize()) -=
921 jacobian1;
922 x_cursor += param1->GlobalSize();
923 delta_cursor += param1->LocalSize();
924
925 Matrix jacobian2(param2->GlobalSize(), param2->LocalSize());
926 EXPECT_TRUE(param2->ComputeJacobian(&x[x_cursor], jacobian2.data()));
927 jacobian.block(
928 x_cursor, delta_cursor, param2->GlobalSize(), param2->LocalSize()) -=
929 jacobian2;
930 x_cursor += param2->GlobalSize();
931 delta_cursor += param2->LocalSize();
932
933 Matrix jacobian3(param3->GlobalSize(), param3->LocalSize());
934 EXPECT_TRUE(param3->ComputeJacobian(&x[x_cursor], jacobian3.data()));
935 jacobian.block(
936 x_cursor, delta_cursor, param3->GlobalSize(), param3->LocalSize()) -=
937 jacobian3;
938 x_cursor += param3->GlobalSize();
939 delta_cursor += param3->LocalSize();
940
941 Matrix jacobian4(param4->GlobalSize(), param4->LocalSize());
942 EXPECT_TRUE(param4->ComputeJacobian(&x[x_cursor], jacobian4.data()));
943 jacobian.block(
944 x_cursor, delta_cursor, param4->GlobalSize(), param4->LocalSize()) -=
945 jacobian4;
946 x_cursor += param4->GlobalSize();
947 delta_cursor += param4->LocalSize();
948
949 EXPECT_NEAR(jacobian.norm(), 0.0, std::numeric_limits<double>::epsilon());
950 }
951
952 } // namespace internal
953 } // namespace ceres
954