1 // Ceres Solver - A fast non-linear least squares minimizer
2 // Copyright 2013 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/numeric_diff_test_utils.h"
32
33 #include <algorithm>
34 #include <cmath>
35 #include "ceres/cost_function.h"
36 #include "ceres/internal/macros.h"
37 #include "ceres/test_util.h"
38 #include "ceres/types.h"
39 #include "gtest/gtest.h"
40
41
42 namespace ceres {
43 namespace internal {
44
operator ()(const double * x1,const double * x2,double * residuals) const45 bool EasyFunctor::operator()(const double* x1,
46 const double* x2,
47 double* residuals) const {
48 residuals[0] = residuals[1] = residuals[2] = 0;
49 for (int i = 0; i < 5; ++i) {
50 residuals[0] += x1[i] * x2[i];
51 residuals[2] += x2[i] * x2[i];
52 }
53 residuals[1] = residuals[0] * residuals[0];
54 return true;
55 }
56
ExpectCostFunctionEvaluationIsNearlyCorrect(const CostFunction & cost_function,NumericDiffMethod method) const57 void EasyFunctor::ExpectCostFunctionEvaluationIsNearlyCorrect(
58 const CostFunction& cost_function,
59 NumericDiffMethod method) const {
60 double x1[] = { 1.0, 2.0, 3.0, 4.0, 5.0 };
61 double x2[] = { 9.0, 9.0, 5.0, 5.0, 1.0 };
62 double *parameters[] = { &x1[0], &x2[0] };
63
64 double dydx1[15]; // 3 x 5, row major.
65 double dydx2[15]; // 3 x 5, row major.
66 double *jacobians[2] = { &dydx1[0], &dydx2[0] };
67
68 double residuals[3] = {-1e-100, -2e-100, -3e-100 };
69
70 ASSERT_TRUE(cost_function.Evaluate(¶meters[0],
71 &residuals[0],
72 &jacobians[0]));
73
74 EXPECT_EQ(residuals[0], 67);
75 EXPECT_EQ(residuals[1], 4489);
76 EXPECT_EQ(residuals[2], 213);
77
78 const double tolerance = (method == CENTRAL)? 3e-9 : 2e-5;
79
80 for (int i = 0; i < 5; ++i) {
81 ExpectClose(x2[i], dydx1[5 * 0 + i], tolerance); // y1
82 ExpectClose(x1[i], dydx2[5 * 0 + i], tolerance);
83 ExpectClose(2 * x2[i] * residuals[0], dydx1[5 * 1 + i], tolerance); // y2
84 ExpectClose(2 * x1[i] * residuals[0], dydx2[5 * 1 + i], tolerance);
85 ExpectClose(0.0, dydx1[5 * 2 + i], tolerance); // y3
86 ExpectClose(2 * x2[i], dydx2[5 * 2 + i], tolerance);
87 }
88 }
89
operator ()(const double * x1,const double * x2,double * residuals) const90 bool TranscendentalFunctor::operator()(const double* x1,
91 const double* x2,
92 double* residuals) const {
93 double x1x2 = 0;
94 for (int i = 0; i < 5; ++i) {
95 x1x2 += x1[i] * x2[i];
96 }
97 residuals[0] = sin(x1x2);
98 residuals[1] = exp(-x1x2 / 10);
99 return true;
100 }
101
ExpectCostFunctionEvaluationIsNearlyCorrect(const CostFunction & cost_function,NumericDiffMethod method) const102 void TranscendentalFunctor::ExpectCostFunctionEvaluationIsNearlyCorrect(
103 const CostFunction& cost_function,
104 NumericDiffMethod method) const {
105 struct {
106 double x1[5];
107 double x2[5];
108 } kTests[] = {
109 { { 1.0, 2.0, 3.0, 4.0, 5.0 }, // No zeros.
110 { 9.0, 9.0, 5.0, 5.0, 1.0 },
111 },
112 { { 0.0, 2.0, 3.0, 0.0, 5.0 }, // Some zeros x1.
113 { 9.0, 9.0, 5.0, 5.0, 1.0 },
114 },
115 { { 1.0, 2.0, 3.0, 1.0, 5.0 }, // Some zeros x2.
116 { 0.0, 9.0, 0.0, 5.0, 0.0 },
117 },
118 { { 0.0, 0.0, 0.0, 0.0, 0.0 }, // All zeros x1.
119 { 9.0, 9.0, 5.0, 5.0, 1.0 },
120 },
121 { { 1.0, 2.0, 3.0, 4.0, 5.0 }, // All zeros x2.
122 { 0.0, 0.0, 0.0, 0.0, 0.0 },
123 },
124 { { 0.0, 0.0, 0.0, 0.0, 0.0 }, // All zeros.
125 { 0.0, 0.0, 0.0, 0.0, 0.0 },
126 },
127 };
128
129 for (int k = 0; k < CERES_ARRAYSIZE(kTests); ++k) {
130 double *x1 = &(kTests[k].x1[0]);
131 double *x2 = &(kTests[k].x2[0]);
132 double *parameters[] = { x1, x2 };
133
134 double dydx1[10];
135 double dydx2[10];
136 double *jacobians[2] = { &dydx1[0], &dydx2[0] };
137
138 double residuals[2];
139
140 ASSERT_TRUE(cost_function.Evaluate(¶meters[0],
141 &residuals[0],
142 &jacobians[0]));
143 double x1x2 = 0;
144 for (int i = 0; i < 5; ++i) {
145 x1x2 += x1[i] * x2[i];
146 }
147
148 const double tolerance = (method == CENTRAL)? 3e-9 : 2e-5;
149
150 for (int i = 0; i < 5; ++i) {
151 ExpectClose( x2[i] * cos(x1x2), dydx1[5 * 0 + i], tolerance);
152 ExpectClose( x1[i] * cos(x1x2), dydx2[5 * 0 + i], tolerance);
153 ExpectClose(-x2[i] * exp(-x1x2 / 10.) / 10., dydx1[5 * 1 + i], tolerance);
154 ExpectClose(-x1[i] * exp(-x1x2 / 10.) / 10., dydx2[5 * 1 + i], tolerance);
155 }
156 }
157 }
158
159 } // namespace internal
160 } // namespace ceres
161