| 123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250 | // Ceres Solver - A fast non-linear least squares minimizer// Copyright 2015 Google Inc. All rights reserved.// http://ceres-solver.org///// Redistribution and use in source and binary forms, with or without// modification, are permitted provided that the following conditions are met://// * Redistributions of source code must retain the above copyright notice,//   this list of conditions and the following disclaimer.// * Redistributions in binary form must reproduce the above copyright notice,//   this list of conditions and the following disclaimer in the documentation//   and/or other materials provided with the distribution.// * Neither the name of Google Inc. nor the names of its contributors may be//   used to endorse or promote products derived from this software without//   specific prior written permission.//// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE// POSSIBILITY OF SUCH DAMAGE.//// Author: sameeragarwal@google.com (Sameer Agarwal)#include "ceres/local_parameterization.h"#include "ceres/householder_vector.h"#include "ceres/internal/eigen.h"#include "ceres/rotation.h"#include "glog/logging.h"namespace ceres {using std::vector;LocalParameterization::~LocalParameterization() {}bool LocalParameterization::MultiplyByJacobian(const double* x,                                               const int num_rows,                                               const double* global_matrix,                                               double* local_matrix) const {  Matrix jacobian(GlobalSize(), LocalSize());  if (!ComputeJacobian(x, jacobian.data())) {    return false;  }  MatrixRef(local_matrix, num_rows, LocalSize()) =      ConstMatrixRef(global_matrix, num_rows, GlobalSize()) * jacobian;  return true;}IdentityParameterization::IdentityParameterization(const int size)    : size_(size) {  CHECK_GT(size, 0);}bool IdentityParameterization::Plus(const double* x,                                    const double* delta,                                    double* x_plus_delta) const {  VectorRef(x_plus_delta, size_) =      ConstVectorRef(x, size_) + ConstVectorRef(delta, size_);  return true;}bool IdentityParameterization::ComputeJacobian(const double* x,                                               double* jacobian) const {  MatrixRef(jacobian, size_, size_) = Matrix::Identity(size_, size_);  return true;}bool IdentityParameterization::MultiplyByJacobian(const double* x,                                                  const int num_cols,                                                  const double* global_matrix,                                                  double* local_matrix) const {  std::copy(global_matrix,            global_matrix + num_cols * GlobalSize(),            local_matrix);  return true;}SubsetParameterization::SubsetParameterization(    int size,    const vector<int>& constant_parameters)    : local_size_(size - constant_parameters.size()),      constancy_mask_(size, 0) {  CHECK_GT(constant_parameters.size(), 0)      << "The set of constant parameters should contain at least "      << "one element. If you do not wish to hold any parameters "      << "constant, then do not use a SubsetParameterization";  vector<int> constant = constant_parameters;  sort(constant.begin(), constant.end());  CHECK(unique(constant.begin(), constant.end()) == constant.end())      << "The set of constant parameters cannot contain duplicates";  CHECK_LT(constant_parameters.size(), size)      << "Number of parameters held constant should be less "      << "than the size of the parameter block. If you wish "      << "to hold the entire parameter block constant, then a "      << "efficient way is to directly mark it as constant "      << "instead of using a LocalParameterization to do so.";  CHECK_GE(*min_element(constant.begin(), constant.end()), 0);  CHECK_LT(*max_element(constant.begin(), constant.end()), size);  for (int i = 0; i < constant_parameters.size(); ++i) {    constancy_mask_[constant_parameters[i]] = 1;  }}bool SubsetParameterization::Plus(const double* x,                                  const double* delta,                                  double* x_plus_delta) const {  for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) {    if (constancy_mask_[i]) {      x_plus_delta[i] = x[i];    } else {      x_plus_delta[i] = x[i] + delta[j++];    }  }  return true;}bool SubsetParameterization::ComputeJacobian(const double* x,                                             double* jacobian) const {  MatrixRef m(jacobian, constancy_mask_.size(), local_size_);  m.setZero();  for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) {    if (!constancy_mask_[i]) {      m(i, j++) = 1.0;    }  }  return true;}bool SubsetParameterization::MultiplyByJacobian(const double* x,                                               const int num_rows,                                               const double* global_matrix,                                               double* local_matrix) const {  for (int row = 0; row < num_rows; ++row) {    for (int col = 0, j = 0; col < constancy_mask_.size(); ++col) {      if (!constancy_mask_[col]) {        local_matrix[row * LocalSize() + j++] =            global_matrix[row * GlobalSize() + col];      }    }  }  return true;}bool QuaternionParameterization::Plus(const double* x,                                      const double* delta,                                      double* x_plus_delta) const {  const double norm_delta =      sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);  if (norm_delta > 0.0) {    const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);    double q_delta[4];    q_delta[0] = cos(norm_delta);    q_delta[1] = sin_delta_by_delta * delta[0];    q_delta[2] = sin_delta_by_delta * delta[1];    q_delta[3] = sin_delta_by_delta * delta[2];    QuaternionProduct(q_delta, x, x_plus_delta);  } else {    for (int i = 0; i < 4; ++i) {      x_plus_delta[i] = x[i];    }  }  return true;}bool QuaternionParameterization::ComputeJacobian(const double* x,                                                 double* jacobian) const {  jacobian[0] = -x[1]; jacobian[1]  = -x[2]; jacobian[2]  = -x[3];  // NOLINT  jacobian[3] =  x[0]; jacobian[4]  =  x[3]; jacobian[5]  = -x[2];  // NOLINT  jacobian[6] = -x[3]; jacobian[7]  =  x[0]; jacobian[8]  =  x[1];  // NOLINT  jacobian[9] =  x[2]; jacobian[10] = -x[1]; jacobian[11] =  x[0];  // NOLINT  return true;}HomogeneousVectorParameterization::HomogeneousVectorParameterization(int size)    : size_(size) {  CHECK_GT(size_, 1) << "The size of the homogeneous vector needs to be "                     << "greater than 1.";}bool HomogeneousVectorParameterization::Plus(const double* x_ptr,                                             const double* delta_ptr,                                             double* x_plus_delta_ptr) const {  ConstVectorRef x(x_ptr, size_);  ConstVectorRef delta(delta_ptr, size_ - 1);  VectorRef x_plus_delta(x_plus_delta_ptr, size_);  const double norm_delta = delta.norm();  if (norm_delta == 0.0) {    x_plus_delta = x;    return true;  }  // Map the delta from the minimum representation to the over parameterized  // homogeneous vector. See section A6.9.2 on page 624 of Hartley & Zisserman  // (2nd Edition) for a detailed description.  Note there is a typo on Page  // 625, line 4 so check the book errata.  const double norm_delta_div_2 = 0.5 * norm_delta;  const double sin_delta_by_delta = sin(norm_delta_div_2) /      norm_delta_div_2;  Vector y(size_);  y.head(size_ - 1) = 0.5 * sin_delta_by_delta * delta;  y(size_ - 1) = cos(norm_delta_div_2);  Vector v(size_);  double beta;  internal::ComputeHouseholderVector<double>(x, &v, &beta);  // Apply the delta update to remain on the unit sphere. See section A6.9.3  // on page 625 of Hartley & Zisserman (2nd Edition) for a detailed  // description.  x_plus_delta = x.norm() * (y -  v * (beta * (v.transpose() * y)));  return true;}bool HomogeneousVectorParameterization::ComputeJacobian(    const double* x_ptr, double* jacobian_ptr) const {  ConstVectorRef x(x_ptr, size_);  MatrixRef jacobian(jacobian_ptr, size_, size_ - 1);  Vector v(size_);  double beta;  internal::ComputeHouseholderVector<double>(x, &v, &beta);  // The Jacobian is equal to J = 0.5 * H.leftCols(size_ - 1) where H is the  // Householder matrix (H = I - beta * v * v').  for (int i = 0; i < size_ - 1; ++i) {    jacobian.col(i) = -0.5 * beta * v(i) * v;    jacobian.col(i)(i) += 0.5;  }  jacobian *= x.norm();  return true;}}  // namespace ceres
 |