1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2015 Google Inc. All rights reserved.
3 // http://ceres-solver.org/
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
47 using std::string;
48
InvalidateEvaluation(const ResidualBlock & block,double * cost,double * residuals,double ** jacobians)49 void InvalidateEvaluation(const ResidualBlock& block,
50 double* cost,
51 double* residuals,
52 double** jacobians) {
53 const int num_parameter_blocks = block.NumParameterBlocks();
54 const int num_residuals = block.NumResiduals();
55
56 InvalidateArray(1, cost);
57 InvalidateArray(num_residuals, residuals);
58 if (jacobians != NULL) {
59 for (int i = 0; i < num_parameter_blocks; ++i) {
60 const int parameter_block_size = block.parameter_blocks()[i]->Size();
61 InvalidateArray(num_residuals * parameter_block_size, jacobians[i]);
62 }
63 }
64 }
65
EvaluationToString(const ResidualBlock & block,double const * const * parameters,double * cost,double * residuals,double ** jacobians)66 string EvaluationToString(const ResidualBlock& block,
67 double const* const* parameters,
68 double* cost,
69 double* residuals,
70 double** jacobians) {
71 CHECK(cost != nullptr);
72 CHECK(residuals != nullptr);
73
74 const int num_parameter_blocks = block.NumParameterBlocks();
75 const int num_residuals = block.NumResiduals();
76 string result = "";
77
78 StringAppendF(&result,
79 "Residual Block size: %d parameter blocks x %d residuals\n\n",
80 num_parameter_blocks, num_residuals);
81 result +=
82 "For each parameter block, the value of the parameters are printed in the first column \n" // NOLINT
83 "and the value of the jacobian under the corresponding residual. If a ParameterBlock was \n" // NOLINT
84 "held constant then the corresponding jacobian is printed as 'Not Computed'. If an entry \n" // NOLINT
85 "of the Jacobian/residual array was requested but was not written to by user code, it is \n" // NOLINT
86 "indicated by 'Uninitialized'. This is an error. Residuals or Jacobian values evaluating \n" // NOLINT
87 "to Inf or NaN is also an error. \n\n"; // NOLINT
88
89 string space = "Residuals: ";
90 result += space;
91 AppendArrayToString(num_residuals, residuals, &result);
92 StringAppendF(&result, "\n\n");
93
94 for (int i = 0; i < num_parameter_blocks; ++i) {
95 const int parameter_block_size = block.parameter_blocks()[i]->Size();
96 StringAppendF(
97 &result, "Parameter Block %d, size: %d\n", i, parameter_block_size);
98 StringAppendF(&result, "\n");
99 for (int j = 0; j < parameter_block_size; ++j) {
100 AppendArrayToString(1, parameters[i] + j, &result);
101 StringAppendF(&result, "| ");
102 for (int k = 0; k < num_residuals; ++k) {
103 AppendArrayToString(1,
104 (jacobians != NULL && jacobians[i] != NULL)
105 ? jacobians[i] + k * parameter_block_size + j
106 : NULL,
107 &result);
108 }
109 StringAppendF(&result, "\n");
110 }
111 StringAppendF(&result, "\n");
112 }
113 StringAppendF(&result, "\n");
114 return result;
115 }
116
IsEvaluationValid(const ResidualBlock & block,double const * const * parameters,double * cost,double * residuals,double ** jacobians)117 bool IsEvaluationValid(const ResidualBlock& block,
118 double const* const* parameters,
119 double* cost,
120 double* residuals,
121 double** jacobians) {
122 const int num_parameter_blocks = block.NumParameterBlocks();
123 const int num_residuals = block.NumResiduals();
124
125 if (!IsArrayValid(num_residuals, residuals)) {
126 return false;
127 }
128
129 if (jacobians != NULL) {
130 for (int i = 0; i < num_parameter_blocks; ++i) {
131 const int parameter_block_size = block.parameter_blocks()[i]->Size();
132 if (!IsArrayValid(num_residuals * parameter_block_size, jacobians[i])) {
133 return false;
134 }
135 }
136 }
137
138 return true;
139 }
140
141 } // namespace internal
142 } // namespace ceres
143