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 (at) google.com (Sameer Agarwal) 30 31 #include "ceres/local_parameterization.h" 32 33 #include "ceres/internal/eigen.h" 34 #include "ceres/rotation.h" 35 #include "glog/logging.h" 36 37 namespace ceres { 38 39 IdentityParameterization::IdentityParameterization(const int size) 40 : size_(size) { 41 CHECK_GT(size, 0); 42 } 43 44 bool IdentityParameterization::Plus(const double* x, 45 const double* delta, 46 double* x_plus_delta) const { 47 VectorRef(x_plus_delta, size_) = 48 ConstVectorRef(x, size_) + ConstVectorRef(delta, size_); 49 return true; 50 } 51 52 bool IdentityParameterization::ComputeJacobian(const double* x, 53 double* jacobian) const { 54 MatrixRef(jacobian, size_, size_) = Matrix::Identity(size_, size_); 55 return true; 56 } 57 58 SubsetParameterization::SubsetParameterization( 59 int size, 60 const vector<int>& constant_parameters) 61 : local_size_(size - constant_parameters.size()), 62 constancy_mask_(size, 0) { 63 CHECK_GT(constant_parameters.size(), 0) 64 << "The set of constant parameters should contain at least " 65 << "one element. If you do not wish to hold any parameters " 66 << "constant, then do not use a SubsetParameterization"; 67 68 vector<int> constant = constant_parameters; 69 sort(constant.begin(), constant.end()); 70 CHECK(unique(constant.begin(), constant.end()) == constant.end()) 71 << "The set of constant parameters cannot contain duplicates"; 72 CHECK_LT(constant_parameters.size(), size) 73 << "Number of parameters held constant should be less " 74 << "than the size of the parameter block. If you wish " 75 << "to hold the entire parameter block constant, then a " 76 << "efficient way is to directly mark it as constant " 77 << "instead of using a LocalParameterization to do so."; 78 CHECK_GE(*min_element(constant.begin(), constant.end()), 0); 79 CHECK_LT(*max_element(constant.begin(), constant.end()), size); 80 81 for (int i = 0; i < constant_parameters.size(); ++i) { 82 constancy_mask_[constant_parameters[i]] = 1; 83 } 84 } 85 86 bool SubsetParameterization::Plus(const double* x, 87 const double* delta, 88 double* x_plus_delta) const { 89 for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) { 90 if (constancy_mask_[i]) { 91 x_plus_delta[i] = x[i]; 92 } else { 93 x_plus_delta[i] = x[i] + delta[j++]; 94 } 95 } 96 return true; 97 } 98 99 bool SubsetParameterization::ComputeJacobian(const double* x, 100 double* jacobian) const { 101 MatrixRef m(jacobian, constancy_mask_.size(), local_size_); 102 m.setZero(); 103 for (int i = 0, j = 0; i < constancy_mask_.size(); ++i) { 104 if (!constancy_mask_[i]) { 105 m(i, j++) = 1.0; 106 } 107 } 108 return true; 109 } 110 111 bool QuaternionParameterization::Plus(const double* x, 112 const double* delta, 113 double* x_plus_delta) const { 114 const double norm_delta = 115 sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]); 116 if (norm_delta > 0.0) { 117 const double sin_delta_by_delta = (sin(norm_delta) / norm_delta); 118 double q_delta[4]; 119 q_delta[0] = cos(norm_delta); 120 q_delta[1] = sin_delta_by_delta * delta[0]; 121 q_delta[2] = sin_delta_by_delta * delta[1]; 122 q_delta[3] = sin_delta_by_delta * delta[2]; 123 QuaternionProduct(q_delta, x, x_plus_delta); 124 } else { 125 for (int i = 0; i < 4; ++i) { 126 x_plus_delta[i] = x[i]; 127 } 128 } 129 return true; 130 } 131 132 bool QuaternionParameterization::ComputeJacobian(const double* x, 133 double* jacobian) const { 134 jacobian[0] = -x[1]; jacobian[1] = -x[2]; jacobian[2] = -x[3]; // NOLINT 135 jacobian[3] = x[0]; jacobian[4] = x[3]; jacobian[5] = -x[2]; // NOLINT 136 jacobian[6] = -x[3]; jacobian[7] = x[0]; jacobian[8] = x[1]; // NOLINT 137 jacobian[9] = x[2]; jacobian[10] = -x[1]; jacobian[11] = x[0]; // NOLINT 138 return true; 139 } 140 141 } // namespace ceres 142
