| 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
 
 
  |