← Back to branch summary

~ubuntu-branches/ubuntu/raring/ceres-solver/raring

« back to all changes in this revision

Viewing changes to internal/ceres/autodiff_test.cc

  • Committer: Package Import Robot
  • Author(s): Koichi Akabe
  • Date: 2012-06-04 07:15:43 UTC
  • Revision ID: package-import@ubuntu.com-20120604071543-zx6uthupvmtqn3k2
Tags: upstream-1.1.1
Import upstream version 1.1.1

Show diffs side-by-side

added added

removed removed

Lines of Context:
internal/ceres/autodiff_test.cc
 
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: keir@google.com (Keir Mierle)
 
30
 
 
31
#include "ceres/internal/autodiff.h"
 
32
 
 
33
#include "gtest/gtest.h"
 
34
#include "ceres/random.h"
 
35
 
 
36
namespace ceres {
 
37
namespace internal {
 
38
 
 
39
template <typename T> inline
 
40
T &RowMajorAccess(T *base, int rows, int cols, int i, int j) {
 
41
  return base[cols * i + j];
 
42
}
 
43
 
 
44
// Do (symmetric) finite differencing using the given function object 'b' of
 
45
// type 'B' and scalar type 'T' with step size 'del'.
 
46
//
 
47
// The type B should have a signature
 
48
//
 
49
//   bool operator()(T const *, T *) const;
 
50
//
 
51
// which maps a vector of parameters to a vector of outputs.
 
52
template <typename B, typename T, int M, int N> inline
 
53
bool SymmetricDiff(const B& b,
 
54
                   const T par[N],
 
55
                   T del,           // step size.
 
56
                   T fun[M],
 
57
                   T jac[M * N]) {  // row-major.
 
58
  if (!b(par, fun)) {
 
59
    return false;
 
60
  }
 
61
 
 
62
  // Temporary parameter vector.
 
63
  T tmp_par[N];
 
64
  for (int j = 0; j < N; ++j) {
 
65
    tmp_par[j] = par[j];
 
66
  }
 
67
 
 
68
  // For each dimension, we do one forward step and one backward step in
 
69
  // parameter space, and store the output vector vectors in these vectors.
 
70
  T fwd_fun[M];
 
71
  T bwd_fun[M];
 
72
 
 
73
  for (int j = 0; j < N; ++j) {
 
74
    // Forward step.
 
75
    tmp_par[j] = par[j] + del;
 
76
    if (!b(tmp_par, fwd_fun)) {
 
77
      return false;
 
78
    }
 
79
 
 
80
    // Backward step.
 
81
    tmp_par[j] = par[j] - del;
 
82
    if (!b(tmp_par, bwd_fun)) {
 
83
      return false;
 
84
    }
 
85
 
 
86
    // Symmetric differencing:
 
87
    //   f'(a) = (f(a + h) - f(a - h)) / (2 h)
 
88
    for (int i = 0; i < M; ++i) {
 
89
      RowMajorAccess(jac, M, N, i, j) =
 
90
          (fwd_fun[i] - bwd_fun[i]) / (T(2) * del);
 
91
    }
 
92
 
 
93
    // Restore our temporary vector.
 
94
    tmp_par[j] = par[j];
 
95
  }
 
96
 
 
97
  return true;
 
98
}
 
99
 
 
100
template <typename A> inline
 
101
void QuaternionToScaledRotation(A const q[4], A R[3 * 3]) {
 
102
  // Make convenient names for elements of q.
 
103
  A a = q[0];
 
104
  A b = q[1];
 
105
  A c = q[2];
 
106
  A d = q[3];
 
107
  // This is not to eliminate common sub-expression, but to
 
108
  // make the lines shorter so that they fit in 80 columns!
 
109
  A aa = a*a;
 
110
  A ab = a*b;
 
111
  A ac = a*c;
 
112
  A ad = a*d;
 
113
  A bb = b*b;
 
114
  A bc = b*c;
 
115
  A bd = b*d;
 
116
  A cc = c*c;
 
117
  A cd = c*d;
 
118
  A dd = d*d;
 
119
#define R(i, j) RowMajorAccess(R, 3, 3, (i), (j))
 
120
  R(0, 0) =  aa+bb-cc-dd; R(0, 1) = A(2)*(bc-ad); R(0, 2) = A(2)*(ac+bd);  // NOLINT
 
121
  R(1, 0) = A(2)*(ad+bc); R(1, 1) =  aa-bb+cc-dd; R(1, 2) = A(2)*(cd-ab);  // NOLINT
 
122
  R(2, 0) = A(2)*(bd-ac); R(2, 1) = A(2)*(ab+cd); R(2, 2) =  aa-bb-cc+dd;  // NOLINT
 
123
#undef R
 
124
}
 
125
 
 
126
// A structure for projecting a 3x4 camera matrix and a
 
127
// homogeneous 3D point, to a 2D inhomogeneous point.
 
128
struct Projective {
 
129
  // Function that takes P and X as separate vectors:
 
130
  //   P, X -> x
 
131
  template <typename A>
 
132
  bool operator()(A const P[12], A const X[4], A x[2]) const {
 
133
    A PX[3];
 
134
    for (int i = 0; i < 3; ++i) {
 
135
      PX[i] = RowMajorAccess(P, 3, 4, i, 0) * X[0] +
 
136
              RowMajorAccess(P, 3, 4, i, 1) * X[1] +
 
137
              RowMajorAccess(P, 3, 4, i, 2) * X[2] +
 
138
              RowMajorAccess(P, 3, 4, i, 3) * X[3];
 
139
    }
 
140
    if (PX[2] != 0.0) {
 
141
      x[0] = PX[0] / PX[2];
 
142
      x[1] = PX[1] / PX[2];
 
143
      return true;
 
144
    }
 
145
    return false;
 
146
  }
 
147
 
 
148
  // Version that takes P and X packed in one vector:
 
149
  //
 
150
  //   (P, X) -> x
 
151
  //
 
152
  template <typename A>
 
153
  bool operator()(A const P_X[12 + 4], A x[2]) const {
 
154
    return operator()(P_X + 0, P_X + 12, x);
 
155
  }
 
156
};
 
157
 
 
158
// Test projective camera model projector.
 
159
TEST(AutoDiff, ProjectiveCameraModel) {
 
160
  srand(5);
 
161
  double const tol = 1e-10;  // floating-point tolerance.
 
162
  double const del = 1e-4;   // finite-difference step.
 
163
  double const err = 1e-6;   // finite-difference tolerance.
 
164
 
 
165
  Projective b;
 
166
 
 
167
  // Make random P and X, in a single vector.
 
168
  double PX[12 + 4];
 
169
  for (int i = 0; i < 12 + 4; ++i) {
 
170
    PX[i] = RandDouble();
 
171
  }
 
172
 
 
173
  // Handy names for the P and X parts.
 
174
  double *P = PX + 0;
 
175
  double *X = PX + 12;
 
176
 
 
177
  // Apply the mapping, to get image point b_x.
 
178
  double b_x[2];
 
179
  b(P, X, b_x);
 
180
 
 
181
  // Use finite differencing to estimate the Jacobian.
 
182
  double fd_x[2];
 
183
  double fd_J[2 * (12 + 4)];
 
184
  ASSERT_TRUE((SymmetricDiff<Projective, double, 2, 12 + 4>(b, PX, del,
 
185
                                                            fd_x, fd_J)));
 
186
 
 
187
  for (int i = 0; i < 2; ++i) {
 
188
    ASSERT_EQ(fd_x[i], b_x[i]);
 
189
  }
 
190
 
 
191
  // Use automatic differentiation to compute the Jacobian.
 
192
  double ad_x1[2];
 
193
  double J_PX[2 * (12 + 4)];
 
194
  {
 
195
    double *parameters[] = { PX };
 
196
    double *jacobians[] = { J_PX };
 
197
    ASSERT_TRUE((AutoDiff<Projective, double, 12 + 4>::Differentiate(
 
198
        b, parameters, 2, ad_x1, jacobians)));
 
199
 
 
200
    for (int i = 0; i < 2; ++i) {
 
201
      ASSERT_NEAR(ad_x1[i], b_x[i], tol);
 
202
    }
 
203
  }
 
204
 
 
205
  // Use automatic differentiation (again), with two arguments.
 
206
  {
 
207
    double ad_x2[2];
 
208
    double J_P[2 * 12];
 
209
    double J_X[2 * 4];
 
210
    double *parameters[] = { P, X };
 
211
    double *jacobians[] = { J_P, J_X };
 
212
    ASSERT_TRUE((AutoDiff<Projective, double, 12, 4>::Differentiate(
 
213
        b, parameters, 2, ad_x2, jacobians)));
 
214
 
 
215
    for (int i = 0; i < 2; ++i) {
 
216
      ASSERT_NEAR(ad_x2[i], b_x[i], tol);
 
217
    }
 
218
 
 
219
    // Now compare the jacobians we got.
 
220
    for (int i = 0; i < 2; ++i) {
 
221
      for (int j = 0; j < 12 + 4; ++j) {
 
222
        ASSERT_NEAR(J_PX[(12 + 4) * i + j], fd_J[(12 + 4) * i + j], err);
 
223
      }
 
224
 
 
225
      for (int j = 0; j < 12; ++j) {
 
226
        ASSERT_NEAR(J_PX[(12 + 4) * i + j], J_P[12 * i + j], tol);
 
227
      }
 
228
      for (int j = 0; j < 4; ++j) {
 
229
        ASSERT_NEAR(J_PX[(12 + 4) * i + 12 + j], J_X[4 * i + j], tol);
 
230
      }
 
231
    }
 
232
  }
 
233
}
 
234
 
 
235
// Object to implement the projection by a calibrated camera.
 
236
struct Metric {
 
237
  // The mapping is
 
238
  //
 
239
  //   q, c, X -> x = dehomg(R(q) (X - c))
 
240
  //
 
241
  // where q is a quaternion and c is the center of projection.
 
242
  //
 
243
  // This function takes three input vectors.
 
244
  template <typename A>
 
245
  bool operator()(A const q[4], A const c[3], A const X[3], A x[2]) const {
 
246
    A R[3 * 3];
 
247
    QuaternionToScaledRotation(q, R);
 
248
 
 
249
    // Convert the quaternion mapping all the way to projective matrix.
 
250
    A P[3 * 4];
 
251
 
 
252
    // Set P(:, 1:3) = R
 
253
    for (int i = 0; i < 3; ++i) {
 
254
      for (int j = 0; j < 3; ++j) {
 
255
        RowMajorAccess(P, 3, 4, i, j) = RowMajorAccess(R, 3, 3, i, j);
 
256
      }
 
257
    }
 
258
 
 
259
    // Set P(:, 4) = - R c
 
260
    for (int i = 0; i < 3; ++i) {
 
261
      RowMajorAccess(P, 3, 4, i, 3) =
 
262
        - (RowMajorAccess(R, 3, 3, i, 0) * c[0] +
 
263
           RowMajorAccess(R, 3, 3, i, 1) * c[1] +
 
264
           RowMajorAccess(R, 3, 3, i, 2) * c[2]);
 
265
    }
 
266
 
 
267
    A X1[4] = { X[0], X[1], X[2], A(1) };
 
268
    Projective p;
 
269
    return p(P, X1, x);
 
270
  }
 
271
 
 
272
  // A version that takes a single vector.
 
273
  template <typename A>
 
274
  bool operator()(A const q_c_X[4 + 3 + 3], A x[2]) const {
 
275
    return operator()(q_c_X, q_c_X + 4, q_c_X + 4 + 3, x);
 
276
  }
 
277
};
 
278
 
 
279
// This test is similar in structure to the previous one.
 
280
TEST(AutoDiff, Metric) {
 
281
  srand(5);
 
282
  double const tol = 1e-10;  // floating-point tolerance.
 
283
  double const del = 1e-4;   // finite-difference step.
 
284
  double const err = 1e-5;   // finite-difference tolerance.
 
285
 
 
286
  Metric b;
 
287
 
 
288
  // Make random parameter vector.
 
289
  double qcX[4 + 3 + 3];
 
290
  for (int i = 0; i < 4 + 3 + 3; ++i)
 
291
    qcX[i] = RandDouble();
 
292
 
 
293
  // Handy names.
 
294
  double *q = qcX;
 
295
  double *c = qcX + 4;
 
296
  double *X = qcX + 4 + 3;
 
297
 
 
298
  // Compute projection, b_x.
 
299
  double b_x[2];
 
300
  ASSERT_TRUE(b(q, c, X, b_x));
 
301
 
 
302
  // Finite differencing estimate of Jacobian.
 
303
  double fd_x[2];
 
304
  double fd_J[2 * (4 + 3 + 3)];
 
305
  ASSERT_TRUE((SymmetricDiff<Metric, double, 2, 4 + 3 + 3>(b, qcX, del,
 
306
                                                           fd_x, fd_J)));
 
307
 
 
308
  for (int i = 0; i < 2; ++i) {
 
309
    ASSERT_NEAR(fd_x[i], b_x[i], tol);
 
310
  }
 
311
 
 
312
  // Automatic differentiation.
 
313
  double ad_x[2];
 
314
  double J_q[2 * 4];
 
315
  double J_c[2 * 3];
 
316
  double J_X[2 * 3];
 
317
  double *parameters[] = { q, c, X };
 
318
  double *jacobians[] = { J_q, J_c, J_X };
 
319
  ASSERT_TRUE((AutoDiff<Metric, double, 4, 3, 3>::Differentiate(
 
320
      b, parameters, 2, ad_x, jacobians)));
 
321
 
 
322
  for (int i = 0; i < 2; ++i) {
 
323
    ASSERT_NEAR(ad_x[i], b_x[i], tol);
 
324
  }
 
325
 
 
326
  // Compare the pieces.
 
327
  for (int i = 0; i < 2; ++i) {
 
328
    for (int j = 0; j < 4; ++j) {
 
329
      ASSERT_NEAR(J_q[4 * i + j], fd_J[(4 + 3 + 3) * i + j], err);
 
330
    }
 
331
    for (int j = 0; j < 3; ++j) {
 
332
      ASSERT_NEAR(J_c[3 * i + j], fd_J[(4 + 3 + 3) * i + j + 4], err);
 
333
    }
 
334
    for (int j = 0; j < 3; ++j) {
 
335
      ASSERT_NEAR(J_X[3 * i + j], fd_J[(4 + 3 + 3) * i + j + 4 + 3], err);
 
336
    }
 
337
  }
 
338
}
 
339
 
 
340
struct VaryingResidualFunctor {
 
341
  template <typename T>
 
342
  bool operator()(const T x[2], T* y) const {
 
343
    for (int i = 0; i < num_residuals; ++i) {
 
344
      y[i] = T(i) * x[0] * x[1] * x[1];
 
345
    }
 
346
    return true;
 
347
  }
 
348
 
 
349
  int num_residuals;
 
350
};
 
351
 
 
352
TEST(AutoDiff, VaryingNumberOfResidualsForOneCostFunctorType) {
 
353
  double x[2] = { 1.0, 5.5 };
 
354
  double *parameters[] = { x };
 
355
  const int kMaxResiduals = 10;
 
356
  double J_x[2 * kMaxResiduals];
 
357
  double residuals[kMaxResiduals];
 
358
  double *jacobians[] = { J_x };
 
359
 
 
360
  // Use a single functor, but tweak it to produce different numbers of
 
361
  // residuals.
 
362
  VaryingResidualFunctor functor;
 
363
 
 
364
  for (int num_residuals = 1; num_residuals < kMaxResiduals; ++num_residuals) {
 
365
    // Tweak the number of residuals to produce.
 
366
    functor.num_residuals = num_residuals;
 
367
 
 
368
    // Run autodiff with the new number of residuals.
 
369
    ASSERT_TRUE((AutoDiff<VaryingResidualFunctor, double, 2>::Differentiate(
 
370
        functor, parameters, num_residuals, residuals, jacobians)));
 
371
 
 
372
    const double kTolerance = 1e-14;
 
373
    for (int i = 0; i < num_residuals; ++i) {
 
374
      EXPECT_NEAR(J_x[2 * i + 0], i * x[1] * x[1], kTolerance) << "i: " << i;
 
375
      EXPECT_NEAR(J_x[2 * i + 1], 2 * i * x[0] * x[1], kTolerance) << "i: " << i;
 
376
    }
 
377
  }
 
378
}
 
379
 
 
380
}  // namespace internal
 
381
}  // namespace ceres