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