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