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 // Tests for the conditioned cost function.
32
33 #include "ceres/conditioned_cost_function.h"
34
35 #include "ceres/internal/eigen.h"
36 #include "ceres/normal_prior.h"
37 #include "ceres/types.h"
38 #include "gtest/gtest.h"
39
40 namespace ceres {
41 namespace internal {
42
43 // The size of the cost functions we build.
44 static const int kTestCostFunctionSize = 3;
45
46 // A simple cost function: return ax + b.
47 class LinearCostFunction : public CostFunction {
48 public:
LinearCostFunction(double a,double b)49 LinearCostFunction(double a, double b) : a_(a), b_(b) {
50 set_num_residuals(1);
51 mutable_parameter_block_sizes()->push_back(1);
52 }
53
Evaluate(double const * const * parameters,double * residuals,double ** jacobians) const54 virtual bool Evaluate(double const* const* parameters,
55 double* residuals,
56 double** jacobians) const {
57 *residuals = **parameters * a_ + b_;
58 if (jacobians && *jacobians) {
59 **jacobians = a_;
60 }
61
62 return true;
63 }
64
65 private:
66 const double a_, b_;
67 };
68
69 // Tests that ConditionedCostFunction does what it's supposed to.
TEST(CostFunctionTest,ConditionedCostFunction)70 TEST(CostFunctionTest, ConditionedCostFunction) {
71 double v1[kTestCostFunctionSize], v2[kTestCostFunctionSize],
72 jac[kTestCostFunctionSize * kTestCostFunctionSize],
73 result[kTestCostFunctionSize];
74
75 for (int i = 0; i < kTestCostFunctionSize; i++) {
76 v1[i] = i;
77 v2[i] = i * 10;
78 // Seed a few garbage values in the Jacobian matrix, to make sure that
79 // they're overwritten.
80 jac[i * 2] = i * i;
81 result[i] = i * i * i;
82 }
83
84 // Make a cost function that computes x - v2
85 VectorRef v2_vector(v2, kTestCostFunctionSize, 1);
86 Matrix identity(kTestCostFunctionSize, kTestCostFunctionSize);
87 identity.setIdentity();
88 NormalPrior* difference_cost_function = new NormalPrior(identity, v2_vector);
89
90 vector<CostFunction*> conditioners;
91 for (int i = 0; i < kTestCostFunctionSize; i++) {
92 conditioners.push_back(new LinearCostFunction(i + 2, i * 7));
93 }
94
95 ConditionedCostFunction conditioned_cost_function(difference_cost_function,
96 conditioners,
97 TAKE_OWNERSHIP);
98 EXPECT_EQ(difference_cost_function->num_residuals(),
99 conditioned_cost_function.num_residuals());
100 EXPECT_EQ(difference_cost_function->parameter_block_sizes(),
101 conditioned_cost_function.parameter_block_sizes());
102
103 double *parameters[1];
104 parameters[0] = v1;
105 double *jacs[1];
106 jacs[0] = jac;
107
108 conditioned_cost_function.Evaluate(parameters, result, jacs);
109 for (int i = 0; i < kTestCostFunctionSize; i++) {
110 EXPECT_DOUBLE_EQ((i + 2) * (v1[i] - v2[i]) + i * 7, result[i]);
111 }
112
113 for (int i = 0; i < kTestCostFunctionSize; i++) {
114 for (int j = 0; j < kTestCostFunctionSize; j++) {
115 double actual = jac[i * kTestCostFunctionSize + j];
116 if (i != j) {
117 EXPECT_DOUBLE_EQ(0, actual);
118 } else {
119 EXPECT_DOUBLE_EQ(i + 2, actual);
120 }
121 }
122 }
123 }
124
125 } // namespace internal
126 } // namespace ceres
127