1// Ceres Solver - A fast non-linear least squares minimizer
2// Copyright 2010, 2011, 2012 Google Inc. All rights reserved.
3// http://code.google.com/p/ceres-solver/
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 <cmath>
32#include "ceres/fpclassify.h"
33#include "ceres/internal/autodiff.h"
34#include "ceres/internal/eigen.h"
35#include "ceres/local_parameterization.h"
36#include "ceres/rotation.h"
37#include "gtest/gtest.h"
38
39namespace ceres {
40namespace internal {
41
42TEST(IdentityParameterization, EverythingTest) {
43  IdentityParameterization parameterization(3);
44  EXPECT_EQ(parameterization.GlobalSize(), 3);
45  EXPECT_EQ(parameterization.LocalSize(), 3);
46
47  double x[3] = {1.0, 2.0, 3.0};
48  double delta[3] = {0.0, 1.0, 2.0};
49  double x_plus_delta[3] = {0.0, 0.0, 0.0};
50  parameterization.Plus(x, delta, x_plus_delta);
51  EXPECT_EQ(x_plus_delta[0], 1.0);
52  EXPECT_EQ(x_plus_delta[1], 3.0);
53  EXPECT_EQ(x_plus_delta[2], 5.0);
54
55  double jacobian[9];
56  parameterization.ComputeJacobian(x, jacobian);
57  int k = 0;
58  for (int i = 0; i < 3; ++i) {
59    for (int j = 0; j < 3; ++j, ++k) {
60      EXPECT_EQ(jacobian[k], (i == j) ? 1.0 : 0.0);
61    }
62  }
63}
64
65TEST(SubsetParameterization, DeathTests) {
66  vector<int> constant_parameters;
67  EXPECT_DEATH_IF_SUPPORTED(
68      SubsetParameterization parameterization(1, constant_parameters),
69      "at least");
70
71  constant_parameters.push_back(0);
72  EXPECT_DEATH_IF_SUPPORTED(
73      SubsetParameterization parameterization(1, constant_parameters),
74      "Number of parameters");
75
76  constant_parameters.push_back(1);
77  EXPECT_DEATH_IF_SUPPORTED(
78      SubsetParameterization parameterization(2, constant_parameters),
79      "Number of parameters");
80
81  constant_parameters.push_back(1);
82  EXPECT_DEATH_IF_SUPPORTED(
83      SubsetParameterization parameterization(2, constant_parameters),
84      "duplicates");
85}
86
87TEST(SubsetParameterization, NormalFunctionTest) {
88  double x[4] = {1.0, 2.0, 3.0, 4.0};
89  for (int i = 0; i < 4; ++i) {
90    vector<int> constant_parameters;
91    constant_parameters.push_back(i);
92    SubsetParameterization parameterization(4, constant_parameters);
93    double delta[3] = {1.0, 2.0, 3.0};
94    double x_plus_delta[4] = {0.0, 0.0, 0.0};
95
96    parameterization.Plus(x, delta, x_plus_delta);
97    int k = 0;
98    for (int j = 0; j < 4; ++j) {
99      if (j == i)  {
100        EXPECT_EQ(x_plus_delta[j], x[j]);
101      } else {
102        EXPECT_EQ(x_plus_delta[j], x[j] + delta[k++]);
103      }
104    }
105
106    double jacobian[4 * 3];
107    parameterization.ComputeJacobian(x, jacobian);
108    int delta_cursor = 0;
109    int jacobian_cursor = 0;
110    for (int j = 0; j < 4; ++j) {
111      if (j != i) {
112        for (int k = 0; k < 3; ++k, jacobian_cursor++) {
113          EXPECT_EQ(jacobian[jacobian_cursor], delta_cursor == k ? 1.0 : 0.0);
114        }
115        ++delta_cursor;
116      } else {
117        for (int k = 0; k < 3; ++k, jacobian_cursor++) {
118          EXPECT_EQ(jacobian[jacobian_cursor], 0.0);
119        }
120      }
121    }
122  };
123}
124
125// Functor needed to implement automatically differentiated Plus for
126// quaternions.
127struct QuaternionPlus {
128  template<typename T>
129  bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
130    const T squared_norm_delta =
131        delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
132
133    T q_delta[4];
134    if (squared_norm_delta > T(0.0)) {
135      T norm_delta = sqrt(squared_norm_delta);
136      const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
137      q_delta[0] = cos(norm_delta);
138      q_delta[1] = sin_delta_by_delta * delta[0];
139      q_delta[2] = sin_delta_by_delta * delta[1];
140      q_delta[3] = sin_delta_by_delta * delta[2];
141    } else {
142      // We do not just use q_delta = [1,0,0,0] here because that is a
143      // constant and when used for automatic differentiation will
144      // lead to a zero derivative. Instead we take a first order
145      // approximation and evaluate it at zero.
146      q_delta[0] = T(1.0);
147      q_delta[1] = delta[0];
148      q_delta[2] = delta[1];
149      q_delta[3] = delta[2];
150    }
151
152    QuaternionProduct(q_delta, x, x_plus_delta);
153    return true;
154  }
155};
156
157void QuaternionParameterizationTestHelper(const double* x,
158                                          const double* delta,
159                                          const double* q_delta) {
160  const double kTolerance = 1e-14;
161  double x_plus_delta_ref[4] = {0.0, 0.0, 0.0, 0.0};
162  QuaternionProduct(q_delta, x, x_plus_delta_ref);
163
164  double x_plus_delta[4] = {0.0, 0.0, 0.0, 0.0};
165  QuaternionParameterization param;
166  param.Plus(x, delta, x_plus_delta);
167  for (int i = 0; i < 4; ++i) {
168    EXPECT_NEAR(x_plus_delta[i], x_plus_delta_ref[i], kTolerance);
169  }
170
171  const double x_plus_delta_norm =
172      sqrt(x_plus_delta[0] * x_plus_delta[0] +
173           x_plus_delta[1] * x_plus_delta[1] +
174           x_plus_delta[2] * x_plus_delta[2] +
175           x_plus_delta[3] * x_plus_delta[3]);
176
177  EXPECT_NEAR(x_plus_delta_norm, 1.0, kTolerance);
178
179  double jacobian_ref[12];
180  double zero_delta[3] = {0.0, 0.0, 0.0};
181  const double* parameters[2] = {x, zero_delta};
182  double* jacobian_array[2] = { NULL, jacobian_ref };
183
184  // Autodiff jacobian at delta_x = 0.
185  internal::AutoDiff<QuaternionPlus, double, 4, 3>::Differentiate(
186      QuaternionPlus(), parameters, 4, x_plus_delta, jacobian_array);
187
188  double jacobian[12];
189  param.ComputeJacobian(x, jacobian);
190  for (int i = 0; i < 12; ++i) {
191    EXPECT_TRUE(IsFinite(jacobian[i]));
192    EXPECT_NEAR(jacobian[i], jacobian_ref[i], kTolerance)
193        << "Jacobian mismatch: i = " << i
194        << "\n Expected \n" << ConstMatrixRef(jacobian_ref, 4, 3)
195        << "\n Actual \n" << ConstMatrixRef(jacobian, 4, 3);
196  }
197}
198
199TEST(QuaternionParameterization, ZeroTest) {
200  double x[4] = {0.5, 0.5, 0.5, 0.5};
201  double delta[3] = {0.0, 0.0, 0.0};
202  double q_delta[4] = {1.0, 0.0, 0.0, 0.0};
203  QuaternionParameterizationTestHelper(x, delta, q_delta);
204}
205
206
207TEST(QuaternionParameterization, NearZeroTest) {
208  double x[4] = {0.52, 0.25, 0.15, 0.45};
209  double norm_x = sqrt(x[0] * x[0] +
210                       x[1] * x[1] +
211                       x[2] * x[2] +
212                       x[3] * x[3]);
213  for (int i = 0; i < 4; ++i) {
214    x[i] = x[i] / norm_x;
215  }
216
217  double delta[3] = {0.24, 0.15, 0.10};
218  for (int i = 0; i < 3; ++i) {
219    delta[i] = delta[i] * 1e-14;
220  }
221
222  double q_delta[4];
223  q_delta[0] = 1.0;
224  q_delta[1] = delta[0];
225  q_delta[2] = delta[1];
226  q_delta[3] = delta[2];
227
228  QuaternionParameterizationTestHelper(x, delta, q_delta);
229}
230
231TEST(QuaternionParameterization, AwayFromZeroTest) {
232  double x[4] = {0.52, 0.25, 0.15, 0.45};
233  double norm_x = sqrt(x[0] * x[0] +
234                       x[1] * x[1] +
235                       x[2] * x[2] +
236                       x[3] * x[3]);
237
238  for (int i = 0; i < 4; ++i) {
239    x[i] = x[i] / norm_x;
240  }
241
242  double delta[3] = {0.24, 0.15, 0.10};
243  const double delta_norm = sqrt(delta[0] * delta[0] +
244                                 delta[1] * delta[1] +
245                                 delta[2] * delta[2]);
246  double q_delta[4];
247  q_delta[0] = cos(delta_norm);
248  q_delta[1] = sin(delta_norm) / delta_norm * delta[0];
249  q_delta[2] = sin(delta_norm) / delta_norm * delta[1];
250  q_delta[3] = sin(delta_norm) / delta_norm * delta[2];
251
252  QuaternionParameterizationTestHelper(x, delta, q_delta);
253}
254
255
256}  // namespace internal
257}  // namespace ceres
258