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@google.com (Sameer Agarwal) 30 31 #include "ceres/residual_block_utils.h" 32 33 #include <cmath> 34 #include <cstddef> 35 #include <limits> 36 #include "ceres/array_utils.h" 37 #include "ceres/internal/eigen.h" 38 #include "ceres/internal/port.h" 39 #include "ceres/parameter_block.h" 40 #include "ceres/residual_block.h" 41 #include "ceres/stringprintf.h" 42 #include "glog/logging.h" 43 44 namespace ceres { 45 namespace internal { 46 InvalidateEvaluation(const ResidualBlock & block,double * cost,double * residuals,double ** jacobians)47 void InvalidateEvaluation(const ResidualBlock& block, 48 double* cost, 49 double* residuals, 50 double** jacobians) { 51 const int num_parameter_blocks = block.NumParameterBlocks(); 52 const int num_residuals = block.NumResiduals(); 53 54 InvalidateArray(1, cost); 55 InvalidateArray(num_residuals, residuals); 56 if (jacobians != NULL) { 57 for (int i = 0; i < num_parameter_blocks; ++i) { 58 const int parameter_block_size = block.parameter_blocks()[i]->Size(); 59 InvalidateArray(num_residuals * parameter_block_size, jacobians[i]); 60 } 61 } 62 } 63 EvaluationToString(const ResidualBlock & block,double const * const * parameters,double * cost,double * residuals,double ** jacobians)64 string EvaluationToString(const ResidualBlock& block, 65 double const* const* parameters, 66 double* cost, 67 double* residuals, 68 double** jacobians) { 69 CHECK_NOTNULL(cost); 70 CHECK_NOTNULL(residuals); 71 72 const int num_parameter_blocks = block.NumParameterBlocks(); 73 const int num_residuals = block.NumResiduals(); 74 string result = ""; 75 76 StringAppendF(&result, 77 "Residual Block size: %d parameter blocks x %d residuals\n\n", 78 num_parameter_blocks, num_residuals); 79 result += 80 "For each parameter block, the value of the parameters are printed in the first column \n" // NOLINT 81 "and the value of the jacobian under the corresponding residual. If a ParameterBlock was \n" // NOLINT 82 "held constant then the corresponding jacobian is printed as 'Not Computed'. If an entry \n" // NOLINT 83 "of the Jacobian/residual array was requested but was not written to by user code, it is \n" // NOLINT 84 "indicated by 'Uninitialized'. This is an error. Residuals or Jacobian values evaluating \n" // NOLINT 85 "to Inf or NaN is also an error. \n\n"; // NOLINT 86 87 string space = "Residuals: "; 88 result += space; 89 AppendArrayToString(num_residuals, residuals, &result); 90 StringAppendF(&result, "\n\n"); 91 92 for (int i = 0; i < num_parameter_blocks; ++i) { 93 const int parameter_block_size = block.parameter_blocks()[i]->Size(); 94 StringAppendF( 95 &result, "Parameter Block %d, size: %d\n", i, parameter_block_size); 96 StringAppendF(&result, "\n"); 97 for (int j = 0; j < parameter_block_size; ++j) { 98 AppendArrayToString(1, parameters[i] + j, &result); 99 StringAppendF(&result, "| "); 100 for (int k = 0; k < num_residuals; ++k) { 101 AppendArrayToString(1, 102 (jacobians != NULL && jacobians[i] != NULL) 103 ? jacobians[i] + k * parameter_block_size + j 104 : NULL, 105 &result); 106 } 107 StringAppendF(&result, "\n"); 108 } 109 StringAppendF(&result, "\n"); 110 } 111 StringAppendF(&result, "\n"); 112 return result; 113 } 114 IsEvaluationValid(const ResidualBlock & block,double const * const * parameters,double * cost,double * residuals,double ** jacobians)115 bool IsEvaluationValid(const ResidualBlock& block, 116 double const* const* parameters, 117 double* cost, 118 double* residuals, 119 double** jacobians) { 120 const int num_parameter_blocks = block.NumParameterBlocks(); 121 const int num_residuals = block.NumResiduals(); 122 123 if (!IsArrayValid(num_residuals, residuals)) { 124 return false; 125 } 126 127 if (jacobians != NULL) { 128 for (int i = 0; i < num_parameter_blocks; ++i) { 129 const int parameter_block_size = block.parameter_blocks()[i]->Size(); 130 if (!IsArrayValid(num_residuals * parameter_block_size, jacobians[i])) { 131 return false; 132 } 133 } 134 } 135 136 return true; 137 } 138 139 } // namespace internal 140 } // namespace ceres 141