Ceres: Update to the latest actual version
[blender.git] / extern / ceres / internal / ceres / local_parameterization.cc
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 <algorithm>
34 #include "Eigen/Geometry"
35 #include "ceres/householder_vector.h"
36 #include "ceres/internal/eigen.h"
37 #include "ceres/internal/fixed_array.h"
38 #include "ceres/rotation.h"
39 #include "glog/logging.h"
40
41 namespace ceres {
42
43 using std::vector;
44
45 LocalParameterization::~LocalParameterization() {
46 }
47
48 bool LocalParameterization::MultiplyByJacobian(const double* x,
49                                                const int num_rows,
50                                                const double* global_matrix,
51                                                double* local_matrix) const {
52   Matrix jacobian(GlobalSize(), LocalSize());
53   if (!ComputeJacobian(x, jacobian.data())) {
54     return false;
55   }
56
57   MatrixRef(local_matrix, num_rows, LocalSize()) =
58       ConstMatrixRef(global_matrix, num_rows, GlobalSize()) * jacobian;
59   return true;
60 }
61
62 IdentityParameterization::IdentityParameterization(const int size)
63     : size_(size) {
64   CHECK_GT(size, 0);
65 }
66
67 bool IdentityParameterization::Plus(const double* x,
68                                     const double* delta,
69                                     double* x_plus_delta) const {
70   VectorRef(x_plus_delta, size_) =
71       ConstVectorRef(x, size_) + ConstVectorRef(delta, size_);
72   return true;
73 }
74
75 bool IdentityParameterization::ComputeJacobian(const double* x,
76                                                double* jacobian) const {
77   MatrixRef(jacobian, size_, size_) = Matrix::Identity(size_, size_);
78   return true;
79 }
80
81 bool IdentityParameterization::MultiplyByJacobian(const double* x,
82                                                   const int num_cols,
83                                                   const double* global_matrix,
84                                                   double* local_matrix) const {
85   std::copy(global_matrix,
86             global_matrix + num_cols * GlobalSize(),
87             local_matrix);
88   return true;
89 }
90
91 SubsetParameterization::SubsetParameterization(
92     int size, const vector<int>& constant_parameters)
93     : local_size_(size - constant_parameters.size()), constancy_mask_(size, 0) {
94   vector<int> constant = constant_parameters;
95   std::sort(constant.begin(), constant.end());
96   CHECK_GE(constant.front(), 0)
97       << "Indices indicating constant parameter must be greater than zero.";
98   CHECK_LT(constant.back(), size)
99       << "Indices indicating constant parameter must be less than the size "
100       << "of the parameter block.";
101   CHECK(std::adjacent_find(constant.begin(), constant.end()) == constant.end())
102       << "The set of constant parameters cannot contain duplicates";
103   for (int i = 0; i < constant_parameters.size(); ++i) {
104     constancy_mask_[constant_parameters[i]] = 1;
105   }
106 }
107
108 bool SubsetParameterization::Plus(const double* x,
109                                   const double* delta,
110                                   double* x_plus_delta) const {
111   for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) {
112     if (constancy_mask_[i]) {
113       x_plus_delta[i] = x[i];
114     } else {
115       x_plus_delta[i] = x[i] + delta[j++];
116     }
117   }
118   return true;
119 }
120
121 bool SubsetParameterization::ComputeJacobian(const double* x,
122                                              double* jacobian) const {
123   if (local_size_ == 0) {
124     return true;
125   }
126
127   MatrixRef m(jacobian, constancy_mask_.size(), local_size_);
128   m.setZero();
129   for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) {
130     if (!constancy_mask_[i]) {
131       m(i, j++) = 1.0;
132     }
133   }
134   return true;
135 }
136
137 bool SubsetParameterization::MultiplyByJacobian(const double* x,
138                                                const int num_rows,
139                                                const double* global_matrix,
140                                                double* local_matrix) const {
141   if (local_size_ == 0) {
142     return true;
143   }
144
145   for (int row = 0; row < num_rows; ++row) {
146     for (int col = 0, j = 0; col < constancy_mask_.size(); ++col) {
147       if (!constancy_mask_[col]) {
148         local_matrix[row * LocalSize() + j++] =
149             global_matrix[row * GlobalSize() + col];
150       }
151     }
152   }
153   return true;
154 }
155
156 bool QuaternionParameterization::Plus(const double* x,
157                                       const double* delta,
158                                       double* x_plus_delta) const {
159   const double norm_delta =
160       sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
161   if (norm_delta > 0.0) {
162     const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);
163     double q_delta[4];
164     q_delta[0] = cos(norm_delta);
165     q_delta[1] = sin_delta_by_delta * delta[0];
166     q_delta[2] = sin_delta_by_delta * delta[1];
167     q_delta[3] = sin_delta_by_delta * delta[2];
168     QuaternionProduct(q_delta, x, x_plus_delta);
169   } else {
170     for (int i = 0; i < 4; ++i) {
171       x_plus_delta[i] = x[i];
172     }
173   }
174   return true;
175 }
176
177 bool QuaternionParameterization::ComputeJacobian(const double* x,
178                                                  double* jacobian) const {
179   jacobian[0] = -x[1]; jacobian[1]  = -x[2]; jacobian[2]  = -x[3];  // NOLINT
180   jacobian[3] =  x[0]; jacobian[4]  =  x[3]; jacobian[5]  = -x[2];  // NOLINT
181   jacobian[6] = -x[3]; jacobian[7]  =  x[0]; jacobian[8]  =  x[1];  // NOLINT
182   jacobian[9] =  x[2]; jacobian[10] = -x[1]; jacobian[11] =  x[0];  // NOLINT
183   return true;
184 }
185
186 bool EigenQuaternionParameterization::Plus(const double* x_ptr,
187                                            const double* delta,
188                                            double* x_plus_delta_ptr) const {
189   Eigen::Map<Eigen::Quaterniond> x_plus_delta(x_plus_delta_ptr);
190   Eigen::Map<const Eigen::Quaterniond> x(x_ptr);
191
192   const double norm_delta =
193       sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
194   if (norm_delta > 0.0) {
195     const double sin_delta_by_delta = sin(norm_delta) / norm_delta;
196
197     // Note, in the constructor w is first.
198     Eigen::Quaterniond delta_q(cos(norm_delta),
199                                sin_delta_by_delta * delta[0],
200                                sin_delta_by_delta * delta[1],
201                                sin_delta_by_delta * delta[2]);
202     x_plus_delta = delta_q * x;
203   } else {
204     x_plus_delta = x;
205   }
206
207   return true;
208 }
209
210 bool EigenQuaternionParameterization::ComputeJacobian(const double* x,
211                                                       double* jacobian) const {
212   jacobian[0] =  x[3]; jacobian[1]  =  x[2]; jacobian[2]  = -x[1];  // NOLINT
213   jacobian[3] = -x[2]; jacobian[4]  =  x[3]; jacobian[5]  =  x[0];  // NOLINT
214   jacobian[6] =  x[1]; jacobian[7]  = -x[0]; jacobian[8]  =  x[3];  // NOLINT
215   jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2];  // NOLINT
216   return true;
217 }
218
219 HomogeneousVectorParameterization::HomogeneousVectorParameterization(int size)
220     : size_(size) {
221   CHECK_GT(size_, 1) << "The size of the homogeneous vector needs to be "
222                      << "greater than 1.";
223 }
224
225 bool HomogeneousVectorParameterization::Plus(const double* x_ptr,
226                                              const double* delta_ptr,
227                                              double* x_plus_delta_ptr) const {
228   ConstVectorRef x(x_ptr, size_);
229   ConstVectorRef delta(delta_ptr, size_ - 1);
230   VectorRef x_plus_delta(x_plus_delta_ptr, size_);
231
232   const double norm_delta = delta.norm();
233
234   if (norm_delta == 0.0) {
235     x_plus_delta = x;
236     return true;
237   }
238
239   // Map the delta from the minimum representation to the over parameterized
240   // homogeneous vector. See section A6.9.2 on page 624 of Hartley & Zisserman
241   // (2nd Edition) for a detailed description.  Note there is a typo on Page
242   // 625, line 4 so check the book errata.
243   const double norm_delta_div_2 = 0.5 * norm_delta;
244   const double sin_delta_by_delta = sin(norm_delta_div_2) /
245       norm_delta_div_2;
246
247   Vector y(size_);
248   y.head(size_ - 1) = 0.5 * sin_delta_by_delta * delta;
249   y(size_ - 1) = cos(norm_delta_div_2);
250
251   Vector v(size_);
252   double beta;
253   internal::ComputeHouseholderVector<double>(x, &v, &beta);
254
255   // Apply the delta update to remain on the unit sphere. See section A6.9.3
256   // on page 625 of Hartley & Zisserman (2nd Edition) for a detailed
257   // description.
258   x_plus_delta = x.norm() * (y -  v * (beta * (v.transpose() * y)));
259
260   return true;
261 }
262
263 bool HomogeneousVectorParameterization::ComputeJacobian(
264     const double* x_ptr, double* jacobian_ptr) const {
265   ConstVectorRef x(x_ptr, size_);
266   MatrixRef jacobian(jacobian_ptr, size_, size_ - 1);
267
268   Vector v(size_);
269   double beta;
270   internal::ComputeHouseholderVector<double>(x, &v, &beta);
271
272   // The Jacobian is equal to J = 0.5 * H.leftCols(size_ - 1) where H is the
273   // Householder matrix (H = I - beta * v * v').
274   for (int i = 0; i < size_ - 1; ++i) {
275     jacobian.col(i) = -0.5 * beta * v(i) * v;
276     jacobian.col(i)(i) += 0.5;
277   }
278   jacobian *= x.norm();
279
280   return true;
281 }
282
283 ProductParameterization::ProductParameterization(
284     LocalParameterization* local_param1,
285     LocalParameterization* local_param2) {
286   local_params_.push_back(local_param1);
287   local_params_.push_back(local_param2);
288   Init();
289 }
290
291 ProductParameterization::ProductParameterization(
292     LocalParameterization* local_param1,
293     LocalParameterization* local_param2,
294     LocalParameterization* local_param3) {
295   local_params_.push_back(local_param1);
296   local_params_.push_back(local_param2);
297   local_params_.push_back(local_param3);
298   Init();
299 }
300
301 ProductParameterization::ProductParameterization(
302     LocalParameterization* local_param1,
303     LocalParameterization* local_param2,
304     LocalParameterization* local_param3,
305     LocalParameterization* local_param4) {
306   local_params_.push_back(local_param1);
307   local_params_.push_back(local_param2);
308   local_params_.push_back(local_param3);
309   local_params_.push_back(local_param4);
310   Init();
311 }
312
313 ProductParameterization::~ProductParameterization() {
314   for (int i = 0; i < local_params_.size(); ++i) {
315     delete local_params_[i];
316   }
317 }
318
319 void ProductParameterization::Init() {
320   global_size_ = 0;
321   local_size_ = 0;
322   buffer_size_ = 0;
323   for (int i = 0; i < local_params_.size(); ++i) {
324     const LocalParameterization* param = local_params_[i];
325     buffer_size_ = std::max(buffer_size_,
326                             param->LocalSize() * param->GlobalSize());
327     global_size_ += param->GlobalSize();
328     local_size_ += param->LocalSize();
329   }
330 }
331
332 bool ProductParameterization::Plus(const double* x,
333                                    const double* delta,
334                                    double* x_plus_delta) const {
335   int x_cursor = 0;
336   int delta_cursor = 0;
337   for (int i = 0; i < local_params_.size(); ++i) {
338     const LocalParameterization* param = local_params_[i];
339     if (!param->Plus(x + x_cursor,
340                      delta + delta_cursor,
341                      x_plus_delta + x_cursor)) {
342       return false;
343     }
344     delta_cursor += param->LocalSize();
345     x_cursor += param->GlobalSize();
346   }
347
348   return true;
349 }
350
351 bool ProductParameterization::ComputeJacobian(const double* x,
352                                               double* jacobian_ptr) const {
353   MatrixRef jacobian(jacobian_ptr, GlobalSize(), LocalSize());
354   jacobian.setZero();
355   internal::FixedArray<double> buffer(buffer_size_);
356
357   int x_cursor = 0;
358   int delta_cursor = 0;
359   for (int i = 0; i < local_params_.size(); ++i) {
360     const LocalParameterization* param = local_params_[i];
361     const int local_size = param->LocalSize();
362     const int global_size = param->GlobalSize();
363
364     if (!param->ComputeJacobian(x + x_cursor, buffer.get())) {
365       return false;
366     }
367     jacobian.block(x_cursor, delta_cursor, global_size, local_size)
368         = MatrixRef(buffer.get(), global_size, local_size);
369
370     delta_cursor += local_size;
371     x_cursor += global_size;
372   }
373
374   return true;
375 }
376
377 }  // namespace ceres