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
42namespace ceres {
43namespace internal {
44
45bool 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
57void 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(&parameters[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
90bool 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
102void 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(&parameters[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