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/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 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 64 // Utility routine to print an array of doubles to a string. If the 65 // array pointer is NULL, it is treated as an array of zeros. 66 namespace { 67 void AppendArrayToString(const int size, const double* x, string* result) { 68 for (int i = 0; i < size; ++i) { 69 if (x == NULL) { 70 StringAppendF(result, "Not Computed "); 71 } else { 72 if (x[i] == kImpossibleValue) { 73 StringAppendF(result, "Uninitialized "); 74 } else { 75 StringAppendF(result, "%12g ", x[i]); 76 } 77 } 78 } 79 } 80 } // namespace 81 82 string EvaluationToString(const ResidualBlock& block, 83 double const* const* parameters, 84 double* cost, 85 double* residuals, 86 double** jacobians) { 87 CHECK_NOTNULL(cost); 88 CHECK_NOTNULL(residuals); 89 90 const int num_parameter_blocks = block.NumParameterBlocks(); 91 const int num_residuals = block.NumResiduals(); 92 string result = ""; 93 94 StringAppendF(&result, 95 "Residual Block size: %d parameter blocks x %d residuals\n\n", 96 num_parameter_blocks, num_residuals); 97 result += 98 "For each parameter block, the value of the parameters are printed in the first column \n" // NOLINT 99 "and the value of the jacobian under the corresponding residual. If a ParameterBlock was \n" // NOLINT 100 "held constant then the corresponding jacobian is printed as 'Not Computed'. If an entry \n" // NOLINT 101 "of the Jacobian/residual array was requested but was not written to by user code, it is \n" // NOLINT 102 "indicated by 'Uninitialized'. This is an error. Residuals or Jacobian values evaluating \n" // NOLINT 103 "to Inf or NaN is also an error. \n\n"; // NOLINT 104 105 string space = "Residuals: "; 106 result += space; 107 AppendArrayToString(num_residuals, residuals, &result); 108 StringAppendF(&result, "\n\n"); 109 110 for (int i = 0; i < num_parameter_blocks; ++i) { 111 const int parameter_block_size = block.parameter_blocks()[i]->Size(); 112 StringAppendF( 113 &result, "Parameter Block %d, size: %d\n", i, parameter_block_size); 114 StringAppendF(&result, "\n"); 115 for (int j = 0; j < parameter_block_size; ++j) { 116 AppendArrayToString(1, parameters[i] + j, &result); 117 StringAppendF(&result, "| "); 118 for (int k = 0; k < num_residuals; ++k) { 119 AppendArrayToString(1, 120 (jacobians != NULL && jacobians[i] != NULL) 121 ? jacobians[i] + k * parameter_block_size + j 122 : NULL, 123 &result); 124 } 125 StringAppendF(&result, "\n"); 126 } 127 StringAppendF(&result, "\n"); 128 } 129 StringAppendF(&result, "\n"); 130 return result; 131 } 132 133 bool IsEvaluationValid(const ResidualBlock& block, 134 double const* const* parameters, 135 double* cost, 136 double* residuals, 137 double** jacobians) { 138 const int num_parameter_blocks = block.NumParameterBlocks(); 139 const int num_residuals = block.NumResiduals(); 140 141 if (!IsArrayValid(num_residuals, residuals)) { 142 return false; 143 } 144 145 if (jacobians != NULL) { 146 for (int i = 0; i < num_parameter_blocks; ++i) { 147 const int parameter_block_size = block.parameter_blocks()[i]->Size(); 148 if (!IsArrayValid(num_residuals * parameter_block_size, jacobians[i])) { 149 return false; 150 } 151 } 152 } 153 154 return true; 155 } 156 157 } // namespace internal 158 } // namespace ceres 159
