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 44namespace ceres { 45namespace internal { 46 47void 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 64string 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 115bool 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