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: wjr@google.com (William Rucklidge)
30 //
31 // This file contains the implementation of the conditioned cost function.
32
33 #include "ceres/conditioned_cost_function.h"
34
35 #include <cstddef>
36
37 #include "ceres/internal/eigen.h"
38 #include "ceres/stl_util.h"
39 #include "ceres/types.h"
40 #include "glog/logging.h"
41
42 namespace ceres {
43
44 // This cost function has the same dimensions (parameters, residuals) as
45 // the one it's wrapping.
ConditionedCostFunction(CostFunction * wrapped_cost_function,const vector<CostFunction * > & conditioners,Ownership ownership)46 ConditionedCostFunction::ConditionedCostFunction(
47 CostFunction* wrapped_cost_function,
48 const vector<CostFunction*>& conditioners,
49 Ownership ownership)
50 : wrapped_cost_function_(wrapped_cost_function),
51 conditioners_(conditioners),
52 ownership_(ownership) {
53 // Set up our dimensions.
54 set_num_residuals(wrapped_cost_function_->num_residuals());
55 *mutable_parameter_block_sizes() =
56 wrapped_cost_function_->parameter_block_sizes();
57
58 // Sanity-check the conditioners' dimensions.
59 CHECK_EQ(wrapped_cost_function_->num_residuals(), conditioners_.size());
60 for (int i = 0; i < wrapped_cost_function_->num_residuals(); i++) {
61 if (conditioners[i]) {
62 CHECK_EQ(1, conditioners[i]->num_residuals());
63 CHECK_EQ(1, conditioners[i]->parameter_block_sizes().size());
64 CHECK_EQ(1, conditioners[i]->parameter_block_sizes()[0]);
65 }
66 }
67 }
68
~ConditionedCostFunction()69 ConditionedCostFunction::~ConditionedCostFunction() {
70 if (ownership_ == TAKE_OWNERSHIP) {
71 STLDeleteElements(&conditioners_);
72 } else {
73 wrapped_cost_function_.release();
74 }
75 }
76
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const77 bool ConditionedCostFunction::Evaluate(double const* const* parameters,
78 double* residuals,
79 double** jacobians) const {
80 bool success = wrapped_cost_function_->Evaluate(parameters, residuals,
81 jacobians);
82 if (!success) {
83 return false;
84 }
85
86 for (int r = 0; r < wrapped_cost_function_->num_residuals(); r++) {
87 // On output, we want to have
88 // residuals[r] = conditioners[r](wrapped_residuals[r])
89 // For parameter block i, column c,
90 // jacobians[i][r*parameter_block_size_[i] + c] =
91 // = d residual[r] / d parameters[i][c]
92 // = conditioners[r]'(wrapped_residuals[r]) *
93 // d wrapped_residuals[r] / d parameters[i][c]
94 if (conditioners_[r]) {
95 double conditioner_derivative;
96 double* conditioner_derivative_pointer = &conditioner_derivative;
97 double** conditioner_derivative_pointer2 =
98 &conditioner_derivative_pointer;
99 if (!jacobians) {
100 conditioner_derivative_pointer2 = NULL;
101 }
102
103 double unconditioned_residual = residuals[r];
104 double* parameter_pointer = &unconditioned_residual;
105 success = conditioners_[r]->Evaluate(¶meter_pointer,
106 &residuals[r],
107 conditioner_derivative_pointer2);
108 if (!success) {
109 return false;
110 }
111
112 if (jacobians) {
113 for (int i = 0;
114 i < wrapped_cost_function_->parameter_block_sizes().size();
115 i++) {
116 if (jacobians[i]) {
117 int parameter_block_size =
118 wrapped_cost_function_->parameter_block_sizes()[i];
119 VectorRef jacobian_row(jacobians[i] + r * parameter_block_size,
120 parameter_block_size, 1);
121 jacobian_row *= conditioner_derivative;
122 }
123 }
124 }
125 }
126 }
127 return true;
128 }
129
130 } // namespace ceres
131