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// CostFunctionToFunctor is an adapter class that allows users to use
32// CostFunction objects in templated functors which are to be used for
33// automatic differentiation.  This allows the user to seamlessly mix
34// analytic, numeric and automatic differentiation.
35//
36// For example, let us assume that
37//
38//  class IntrinsicProjection : public SizedCostFunction<2, 5, 3> {
39//    public:
40//      IntrinsicProjection(const double* observations);
41//      virtual bool Evaluate(double const* const* parameters,
42//                            double* residuals,
43//                            double** jacobians) const;
44//  };
45//
46// is a cost function that implements the projection of a point in its
47// local coordinate system onto its image plane and subtracts it from
48// the observed point projection. It can compute its residual and
49// either via analytic or numerical differentiation can compute its
50// jacobians.
51//
52// Now we would like to compose the action of this CostFunction with
53// the action of camera extrinsics, i.e., rotation and
54// translation. Say we have a templated function
55//
56//   template<typename T>
57//   void RotateAndTranslatePoint(const T* rotation,
58//                                const T* translation,
59//                                const T* point,
60//                                T* result);
61//
62// Then we can now do the following,
63//
64// struct CameraProjection {
65//   CameraProjection(double* observation) {
66//     intrinsic_projection_.reset(
67//         new CostFunctionToFunctor<2, 5, 3>(
68//             new IntrinsicProjection(observation_)));
69//   }
70//   template <typename T>
71//   bool operator()(const T* rotation,
72//                   const T* translation,
73//                   const T* intrinsics,
74//                   const T* point,
75//                   T* residual) const {
76//     T transformed_point[3];
77//     RotateAndTranslatePoint(rotation, translation, point, transformed_point);
78//
79//     // Note that we call intrinsic_projection_, just like it was
80//     // any other templated functor.
81//
82//     return (*intrinsic_projection_)(intrinsics, transformed_point, residual);
83//   }
84//
85//  private:
86//   scoped_ptr<CostFunctionToFunctor<2,5,3> > intrinsic_projection_;
87// };
88
89#ifndef CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_
90#define CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_
91
92#include <numeric>
93#include <vector>
94
95#include "ceres/cost_function.h"
96#include "ceres/internal/fixed_array.h"
97#include "ceres/internal/port.h"
98#include "ceres/internal/scoped_ptr.h"
99
100namespace ceres {
101
102template <int kNumResiduals,
103          int N0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0,
104          int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0>
105class CostFunctionToFunctor {
106 public:
107  explicit CostFunctionToFunctor(CostFunction* cost_function)
108  : cost_function_(cost_function) {
109    CHECK_NOTNULL(cost_function);
110
111    CHECK_GE(kNumResiduals, 0);
112    CHECK_EQ(cost_function->num_residuals(), kNumResiduals);
113
114    // This block breaks the 80 column rule to keep it somewhat readable.
115    CHECK((!N1 && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
116          ((N1 > 0) && !N2 && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
117          ((N1 > 0) && (N2 > 0) && !N3 && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
118          ((N1 > 0) && (N2 > 0) && (N3 > 0) && !N4 && !N5 && !N6 && !N7 && !N8 && !N9) ||
119          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && !N5 && !N6 && !N7 && !N8 && !N9) ||
120          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && !N6 && !N7 && !N8 && !N9) ||
121          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && !N7 && !N8 && !N9) ||
122          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && !N8 && !N9) ||
123          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && !N9) ||
124          ((N1 > 0) && (N2 > 0) && (N3 > 0) && (N4 > 0) && (N5 > 0) && (N6 > 0) && (N7 > 0) && (N8 > 0) && (N9 > 0)))
125        << "Zero block cannot precede a non-zero block. Block sizes are "
126        << "(ignore trailing 0s): " << N0 << ", " << N1 << ", " << N2 << ", "
127        << N3 << ", " << N4 << ", " << N5 << ", " << N6 << ", " << N7 << ", "
128        << N8 << ", " << N9;
129
130    const vector<int16>& parameter_block_sizes =
131        cost_function->parameter_block_sizes();
132    const int num_parameter_blocks =
133        (N0 > 0) + (N1 > 0) + (N2 > 0) + (N3 > 0) + (N4 > 0) +
134        (N5 > 0) + (N6 > 0) + (N7 > 0) + (N8 > 0) + (N9 > 0);
135    CHECK_EQ(parameter_block_sizes.size(), num_parameter_blocks);
136
137    CHECK_EQ(N0, parameter_block_sizes[0]);
138    if (parameter_block_sizes.size() > 1) CHECK_EQ(N1, parameter_block_sizes[1]);  // NOLINT
139    if (parameter_block_sizes.size() > 2) CHECK_EQ(N2, parameter_block_sizes[2]);  // NOLINT
140    if (parameter_block_sizes.size() > 3) CHECK_EQ(N3, parameter_block_sizes[3]);  // NOLINT
141    if (parameter_block_sizes.size() > 4) CHECK_EQ(N4, parameter_block_sizes[4]);  // NOLINT
142    if (parameter_block_sizes.size() > 5) CHECK_EQ(N5, parameter_block_sizes[5]);  // NOLINT
143    if (parameter_block_sizes.size() > 6) CHECK_EQ(N6, parameter_block_sizes[6]);  // NOLINT
144    if (parameter_block_sizes.size() > 7) CHECK_EQ(N7, parameter_block_sizes[7]);  // NOLINT
145    if (parameter_block_sizes.size() > 8) CHECK_EQ(N8, parameter_block_sizes[8]);  // NOLINT
146    if (parameter_block_sizes.size() > 9) CHECK_EQ(N9, parameter_block_sizes[9]);  // NOLINT
147
148    CHECK_EQ(accumulate(parameter_block_sizes.begin(),
149                        parameter_block_sizes.end(), 0),
150             N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9);
151  }
152
153  bool operator()(const double* x0, double* residuals) const {
154    CHECK_NE(N0, 0);
155    CHECK_EQ(N1, 0);
156    CHECK_EQ(N2, 0);
157    CHECK_EQ(N3, 0);
158    CHECK_EQ(N4, 0);
159    CHECK_EQ(N5, 0);
160    CHECK_EQ(N6, 0);
161    CHECK_EQ(N7, 0);
162    CHECK_EQ(N8, 0);
163    CHECK_EQ(N9, 0);
164
165    return cost_function_->Evaluate(&x0, residuals, NULL);
166  }
167
168  bool operator()(const double* x0,
169                  const double* x1,
170                  double* residuals) const {
171    CHECK_NE(N0, 0);
172    CHECK_NE(N1, 0);
173    CHECK_EQ(N2, 0);
174    CHECK_EQ(N3, 0);
175    CHECK_EQ(N4, 0);
176    CHECK_EQ(N5, 0);
177    CHECK_EQ(N6, 0);
178    CHECK_EQ(N7, 0);
179    CHECK_EQ(N8, 0);
180    CHECK_EQ(N9, 0);
181    internal::FixedArray<const double*> parameter_blocks(2);
182    parameter_blocks[0] = x0;
183    parameter_blocks[1] = x1;
184    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
185  }
186
187  bool operator()(const double* x0,
188                  const double* x1,
189                  const double* x2,
190                  double* residuals) const {
191    CHECK_NE(N0, 0);
192    CHECK_NE(N1, 0);
193    CHECK_NE(N2, 0);
194    CHECK_EQ(N3, 0);
195    CHECK_EQ(N4, 0);
196    CHECK_EQ(N5, 0);
197    CHECK_EQ(N6, 0);
198    CHECK_EQ(N7, 0);
199    CHECK_EQ(N8, 0);
200    CHECK_EQ(N9, 0);
201    internal::FixedArray<const double*> parameter_blocks(3);
202    parameter_blocks[0] = x0;
203    parameter_blocks[1] = x1;
204    parameter_blocks[2] = x2;
205    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
206  }
207
208  bool operator()(const double* x0,
209                  const double* x1,
210                  const double* x2,
211                  const double* x3,
212                  double* residuals) const {
213    CHECK_NE(N0, 0);
214    CHECK_NE(N1, 0);
215    CHECK_NE(N2, 0);
216    CHECK_NE(N3, 0);
217    CHECK_EQ(N4, 0);
218    CHECK_EQ(N5, 0);
219    CHECK_EQ(N6, 0);
220    CHECK_EQ(N7, 0);
221    CHECK_EQ(N8, 0);
222    CHECK_EQ(N9, 0);
223    internal::FixedArray<const double*> parameter_blocks(4);
224    parameter_blocks[0] = x0;
225    parameter_blocks[1] = x1;
226    parameter_blocks[2] = x2;
227    parameter_blocks[3] = x3;
228    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
229  }
230
231  bool operator()(const double* x0,
232                  const double* x1,
233                  const double* x2,
234                  const double* x3,
235                  const double* x4,
236                  double* residuals) const {
237    CHECK_NE(N0, 0);
238    CHECK_NE(N1, 0);
239    CHECK_NE(N2, 0);
240    CHECK_NE(N3, 0);
241    CHECK_NE(N4, 0);
242    CHECK_EQ(N5, 0);
243    CHECK_EQ(N6, 0);
244    CHECK_EQ(N7, 0);
245    CHECK_EQ(N8, 0);
246    CHECK_EQ(N9, 0);
247    internal::FixedArray<const double*> parameter_blocks(5);
248    parameter_blocks[0] = x0;
249    parameter_blocks[1] = x1;
250    parameter_blocks[2] = x2;
251    parameter_blocks[3] = x3;
252    parameter_blocks[4] = x4;
253    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
254  }
255
256  bool operator()(const double* x0,
257                  const double* x1,
258                  const double* x2,
259                  const double* x3,
260                  const double* x4,
261                  const double* x5,
262                  double* residuals) const {
263    CHECK_NE(N0, 0);
264    CHECK_NE(N1, 0);
265    CHECK_NE(N2, 0);
266    CHECK_NE(N3, 0);
267    CHECK_NE(N4, 0);
268    CHECK_NE(N5, 0);
269    CHECK_EQ(N6, 0);
270    CHECK_EQ(N7, 0);
271    CHECK_EQ(N8, 0);
272    CHECK_EQ(N9, 0);
273    internal::FixedArray<const double*> parameter_blocks(6);
274    parameter_blocks[0] = x0;
275    parameter_blocks[1] = x1;
276    parameter_blocks[2] = x2;
277    parameter_blocks[3] = x3;
278    parameter_blocks[4] = x4;
279    parameter_blocks[5] = x5;
280    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
281  }
282
283  bool operator()(const double* x0,
284                  const double* x1,
285                  const double* x2,
286                  const double* x3,
287                  const double* x4,
288                  const double* x5,
289                  const double* x6,
290                  double* residuals) const {
291    CHECK_NE(N0, 0);
292    CHECK_NE(N1, 0);
293    CHECK_NE(N2, 0);
294    CHECK_NE(N3, 0);
295    CHECK_NE(N4, 0);
296    CHECK_NE(N5, 0);
297    CHECK_NE(N6, 0);
298    CHECK_EQ(N7, 0);
299    CHECK_EQ(N8, 0);
300    CHECK_EQ(N9, 0);
301    internal::FixedArray<const double*> parameter_blocks(7);
302    parameter_blocks[0] = x0;
303    parameter_blocks[1] = x1;
304    parameter_blocks[2] = x2;
305    parameter_blocks[3] = x3;
306    parameter_blocks[4] = x4;
307    parameter_blocks[5] = x5;
308    parameter_blocks[6] = x6;
309    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
310  }
311
312  bool operator()(const double* x0,
313                  const double* x1,
314                  const double* x2,
315                  const double* x3,
316                  const double* x4,
317                  const double* x5,
318                  const double* x6,
319                  const double* x7,
320                  double* residuals) const {
321    CHECK_NE(N0, 0);
322    CHECK_NE(N1, 0);
323    CHECK_NE(N2, 0);
324    CHECK_NE(N3, 0);
325    CHECK_NE(N4, 0);
326    CHECK_NE(N5, 0);
327    CHECK_NE(N6, 0);
328    CHECK_NE(N7, 0);
329    CHECK_EQ(N8, 0);
330    CHECK_EQ(N9, 0);
331    internal::FixedArray<const double*> parameter_blocks(8);
332    parameter_blocks[0] = x0;
333    parameter_blocks[1] = x1;
334    parameter_blocks[2] = x2;
335    parameter_blocks[3] = x3;
336    parameter_blocks[4] = x4;
337    parameter_blocks[5] = x5;
338    parameter_blocks[6] = x6;
339    parameter_blocks[7] = x7;
340    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
341  }
342
343  bool operator()(const double* x0,
344                  const double* x1,
345                  const double* x2,
346                  const double* x3,
347                  const double* x4,
348                  const double* x5,
349                  const double* x6,
350                  const double* x7,
351                  const double* x8,
352                  double* residuals) const {
353    CHECK_NE(N0, 0);
354    CHECK_NE(N1, 0);
355    CHECK_NE(N2, 0);
356    CHECK_NE(N3, 0);
357    CHECK_NE(N4, 0);
358    CHECK_NE(N5, 0);
359    CHECK_NE(N6, 0);
360    CHECK_NE(N7, 0);
361    CHECK_NE(N8, 0);
362    CHECK_EQ(N9, 0);
363    internal::FixedArray<const double*> parameter_blocks(9);
364    parameter_blocks[0] = x0;
365    parameter_blocks[1] = x1;
366    parameter_blocks[2] = x2;
367    parameter_blocks[3] = x3;
368    parameter_blocks[4] = x4;
369    parameter_blocks[5] = x5;
370    parameter_blocks[6] = x6;
371    parameter_blocks[7] = x7;
372    parameter_blocks[8] = x8;
373    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
374  }
375
376  bool operator()(const double* x0,
377                  const double* x1,
378                  const double* x2,
379                  const double* x3,
380                  const double* x4,
381                  const double* x5,
382                  const double* x6,
383                  const double* x7,
384                  const double* x8,
385                  const double* x9,
386                  double* residuals) const {
387    CHECK_NE(N0, 0);
388    CHECK_NE(N1, 0);
389    CHECK_NE(N2, 0);
390    CHECK_NE(N3, 0);
391    CHECK_NE(N4, 0);
392    CHECK_NE(N5, 0);
393    CHECK_NE(N6, 0);
394    CHECK_NE(N7, 0);
395    CHECK_NE(N8, 0);
396    CHECK_NE(N9, 0);
397    internal::FixedArray<const double*> parameter_blocks(10);
398    parameter_blocks[0] = x0;
399    parameter_blocks[1] = x1;
400    parameter_blocks[2] = x2;
401    parameter_blocks[3] = x3;
402    parameter_blocks[4] = x4;
403    parameter_blocks[5] = x5;
404    parameter_blocks[6] = x6;
405    parameter_blocks[7] = x7;
406    parameter_blocks[8] = x8;
407    parameter_blocks[9] = x9;
408    return cost_function_->Evaluate(parameter_blocks.get(), residuals, NULL);
409  }
410
411  template <typename JetT>
412  bool operator()(const JetT* x0, JetT* residuals) const {
413    CHECK_NE(N0, 0);
414    CHECK_EQ(N1, 0);
415    CHECK_EQ(N2, 0);
416    CHECK_EQ(N3, 0);
417    CHECK_EQ(N4, 0);
418    CHECK_EQ(N5, 0);
419    CHECK_EQ(N6, 0);
420    CHECK_EQ(N7, 0);
421    CHECK_EQ(N8, 0);
422    CHECK_EQ(N9, 0);
423    return EvaluateWithJets(&x0, residuals);
424  }
425
426  template <typename JetT>
427  bool operator()(const JetT* x0,
428                  const JetT* x1,
429                  JetT* residuals) const {
430    CHECK_NE(N0, 0);
431    CHECK_NE(N1, 0);
432    CHECK_EQ(N2, 0);
433    CHECK_EQ(N3, 0);
434    CHECK_EQ(N4, 0);
435    CHECK_EQ(N5, 0);
436    CHECK_EQ(N6, 0);
437    CHECK_EQ(N7, 0);
438    CHECK_EQ(N8, 0);
439    CHECK_EQ(N9, 0);
440    internal::FixedArray<const JetT*> jets(2);
441    jets[0] = x0;
442    jets[1] = x1;
443    return EvaluateWithJets(jets.get(), residuals);
444  }
445
446  template <typename JetT>
447  bool operator()(const JetT* x0,
448                  const JetT* x1,
449                  const JetT* x2,
450                  JetT* residuals) const {
451    CHECK_NE(N0, 0);
452    CHECK_NE(N1, 0);
453    CHECK_NE(N2, 0);
454    CHECK_EQ(N3, 0);
455    CHECK_EQ(N4, 0);
456    CHECK_EQ(N5, 0);
457    CHECK_EQ(N6, 0);
458    CHECK_EQ(N7, 0);
459    CHECK_EQ(N8, 0);
460    CHECK_EQ(N9, 0);
461    internal::FixedArray<const JetT*> jets(3);
462    jets[0] = x0;
463    jets[1] = x1;
464    jets[2] = x2;
465    return EvaluateWithJets(jets.get(), residuals);
466  }
467
468  template <typename JetT>
469  bool operator()(const JetT* x0,
470                  const JetT* x1,
471                  const JetT* x2,
472                  const JetT* x3,
473                  JetT* residuals) const {
474    CHECK_NE(N0, 0);
475    CHECK_NE(N1, 0);
476    CHECK_NE(N2, 0);
477    CHECK_NE(N3, 0);
478    CHECK_EQ(N4, 0);
479    CHECK_EQ(N5, 0);
480    CHECK_EQ(N6, 0);
481    CHECK_EQ(N7, 0);
482    CHECK_EQ(N8, 0);
483    CHECK_EQ(N9, 0);
484    internal::FixedArray<const JetT*> jets(4);
485    jets[0] = x0;
486    jets[1] = x1;
487    jets[2] = x2;
488    jets[3] = x3;
489    return EvaluateWithJets(jets.get(), residuals);
490  }
491
492  template <typename JetT>
493  bool operator()(const JetT* x0,
494                  const JetT* x1,
495                  const JetT* x2,
496                  const JetT* x3,
497                  const JetT* x4,
498                  JetT* residuals) const {
499    CHECK_NE(N0, 0);
500    CHECK_NE(N1, 0);
501    CHECK_NE(N2, 0);
502    CHECK_NE(N3, 0);
503    CHECK_NE(N4, 0);
504    CHECK_EQ(N5, 0);
505    CHECK_EQ(N6, 0);
506    CHECK_EQ(N7, 0);
507    CHECK_EQ(N8, 0);
508    CHECK_EQ(N9, 0);
509    internal::FixedArray<const JetT*> jets(5);
510    jets[0] = x0;
511    jets[1] = x1;
512    jets[2] = x2;
513    jets[3] = x3;
514    jets[4] = x4;
515    return EvaluateWithJets(jets.get(), residuals);
516  }
517
518  template <typename JetT>
519  bool operator()(const JetT* x0,
520                  const JetT* x1,
521                  const JetT* x2,
522                  const JetT* x3,
523                  const JetT* x4,
524                  const JetT* x5,
525                  JetT* residuals) const {
526    CHECK_NE(N0, 0);
527    CHECK_NE(N1, 0);
528    CHECK_NE(N2, 0);
529    CHECK_NE(N3, 0);
530    CHECK_NE(N4, 0);
531    CHECK_NE(N5, 0);
532    CHECK_EQ(N6, 0);
533    CHECK_EQ(N7, 0);
534    CHECK_EQ(N8, 0);
535    CHECK_EQ(N9, 0);
536    internal::FixedArray<const JetT*> jets(6);
537    jets[0] = x0;
538    jets[1] = x1;
539    jets[2] = x2;
540    jets[3] = x3;
541    jets[4] = x4;
542    jets[5] = x5;
543    return EvaluateWithJets(jets.get(), residuals);
544  }
545
546  template <typename JetT>
547  bool operator()(const JetT* x0,
548                  const JetT* x1,
549                  const JetT* x2,
550                  const JetT* x3,
551                  const JetT* x4,
552                  const JetT* x5,
553                  const JetT* x6,
554                  JetT* residuals) const {
555    CHECK_NE(N0, 0);
556    CHECK_NE(N1, 0);
557    CHECK_NE(N2, 0);
558    CHECK_NE(N3, 0);
559    CHECK_NE(N4, 0);
560    CHECK_NE(N5, 0);
561    CHECK_NE(N6, 0);
562    CHECK_EQ(N7, 0);
563    CHECK_EQ(N8, 0);
564    CHECK_EQ(N9, 0);
565    internal::FixedArray<const JetT*> jets(7);
566    jets[0] = x0;
567    jets[1] = x1;
568    jets[2] = x2;
569    jets[3] = x3;
570    jets[4] = x4;
571    jets[5] = x5;
572    jets[6] = x6;
573    return EvaluateWithJets(jets.get(), residuals);
574  }
575
576  template <typename JetT>
577  bool operator()(const JetT* x0,
578                  const JetT* x1,
579                  const JetT* x2,
580                  const JetT* x3,
581                  const JetT* x4,
582                  const JetT* x5,
583                  const JetT* x6,
584                  const JetT* x7,
585                  JetT* residuals) const {
586    CHECK_NE(N0, 0);
587    CHECK_NE(N1, 0);
588    CHECK_NE(N2, 0);
589    CHECK_NE(N3, 0);
590    CHECK_NE(N4, 0);
591    CHECK_NE(N5, 0);
592    CHECK_NE(N6, 0);
593    CHECK_NE(N7, 0);
594    CHECK_EQ(N8, 0);
595    CHECK_EQ(N9, 0);
596    internal::FixedArray<const JetT*> jets(8);
597    jets[0] = x0;
598    jets[1] = x1;
599    jets[2] = x2;
600    jets[3] = x3;
601    jets[4] = x4;
602    jets[5] = x5;
603    jets[6] = x6;
604    jets[7] = x7;
605    return EvaluateWithJets(jets.get(), residuals);
606  }
607
608  template <typename JetT>
609  bool operator()(const JetT* x0,
610                  const JetT* x1,
611                  const JetT* x2,
612                  const JetT* x3,
613                  const JetT* x4,
614                  const JetT* x5,
615                  const JetT* x6,
616                  const JetT* x7,
617                  const JetT* x8,
618                  JetT* residuals) const {
619    CHECK_NE(N0, 0);
620    CHECK_NE(N1, 0);
621    CHECK_NE(N2, 0);
622    CHECK_NE(N3, 0);
623    CHECK_NE(N4, 0);
624    CHECK_NE(N5, 0);
625    CHECK_NE(N6, 0);
626    CHECK_NE(N7, 0);
627    CHECK_NE(N8, 0);
628    CHECK_EQ(N9, 0);
629    internal::FixedArray<const JetT*> jets(9);
630    jets[0] = x0;
631    jets[1] = x1;
632    jets[2] = x2;
633    jets[3] = x3;
634    jets[4] = x4;
635    jets[5] = x5;
636    jets[6] = x6;
637    jets[7] = x7;
638    jets[8] = x8;
639    return EvaluateWithJets(jets.get(), residuals);
640  }
641
642  template <typename JetT>
643  bool operator()(const JetT* x0,
644                  const JetT* x1,
645                  const JetT* x2,
646                  const JetT* x3,
647                  const JetT* x4,
648                  const JetT* x5,
649                  const JetT* x6,
650                  const JetT* x7,
651                  const JetT* x8,
652                  const JetT* x9,
653                  JetT* residuals) const {
654    CHECK_NE(N0, 0);
655    CHECK_NE(N1, 0);
656    CHECK_NE(N2, 0);
657    CHECK_NE(N3, 0);
658    CHECK_NE(N4, 0);
659    CHECK_NE(N5, 0);
660    CHECK_NE(N6, 0);
661    CHECK_NE(N7, 0);
662    CHECK_NE(N8, 0);
663    CHECK_NE(N9, 0);
664    internal::FixedArray<const JetT*> jets(10);
665    jets[0] = x0;
666    jets[1] = x1;
667    jets[2] = x2;
668    jets[3] = x3;
669    jets[4] = x4;
670    jets[5] = x5;
671    jets[6] = x6;
672    jets[7] = x7;
673    jets[8] = x8;
674    jets[9] = x9;
675    return EvaluateWithJets(jets.get(), residuals);
676  }
677
678 private:
679  template <typename JetT>
680  bool EvaluateWithJets(const JetT** inputs, JetT* output) const {
681    const int kNumParameters =  N0 + N1 + N2 + N3 + N4 + N5 + N6 + N7 + N8 + N9;
682    const vector<int16>& parameter_block_sizes =
683        cost_function_->parameter_block_sizes();
684    const int num_parameter_blocks = parameter_block_sizes.size();
685    const int num_residuals = cost_function_->num_residuals();
686
687    internal::FixedArray<double> parameters(kNumParameters);
688    internal::FixedArray<double*> parameter_blocks(num_parameter_blocks);
689    internal::FixedArray<double> jacobians(num_residuals * kNumParameters);
690    internal::FixedArray<double*> jacobian_blocks(num_parameter_blocks);
691    internal::FixedArray<double> residuals(num_residuals);
692
693    // Build a set of arrays to get the residuals and jacobians from
694    // the CostFunction wrapped by this functor.
695    double* parameter_ptr = parameters.get();
696    double* jacobian_ptr = jacobians.get();
697    for (int i = 0; i < num_parameter_blocks; ++i) {
698      parameter_blocks[i] = parameter_ptr;
699      jacobian_blocks[i] = jacobian_ptr;
700      for (int j = 0; j < parameter_block_sizes[i]; ++j) {
701        *parameter_ptr++ = inputs[i][j].a;
702      }
703      jacobian_ptr += num_residuals * parameter_block_sizes[i];
704    }
705
706    if (!cost_function_->Evaluate(parameter_blocks.get(),
707                                  residuals.get(),
708                                  jacobian_blocks.get())) {
709      return false;
710    }
711
712    // Now that we have the incoming Jets, which are carrying the
713    // partial derivatives of each of the inputs w.r.t to some other
714    // underlying parameters. The derivative of the outputs of the
715    // cost function w.r.t to the same underlying parameters can now
716    // be computed by applying the chain rule.
717    //
718    //  d output[i]               d output[i]   d input[j]
719    //  --------------  = sum_j   ----------- * ------------
720    //  d parameter[k]            d input[j]    d parameter[k]
721    //
722    // d input[j]
723    // --------------  = inputs[j], so
724    // d parameter[k]
725    //
726    //  outputJet[i]  = sum_k jacobian[i][k] * inputJet[k]
727    //
728    // The following loop, iterates over the residuals, computing one
729    // output jet at a time.
730    for (int i = 0; i < num_residuals; ++i) {
731      output[i].a = residuals[i];
732      output[i].v.setZero();
733
734      for (int j = 0; j < num_parameter_blocks; ++j) {
735        const int16 block_size = parameter_block_sizes[j];
736        for (int k = 0; k < parameter_block_sizes[j]; ++k) {
737          output[i].v +=
738              jacobian_blocks[j][i * block_size + k] * inputs[j][k].v;
739        }
740      }
741    }
742
743    return true;
744  }
745
746 private:
747  internal::scoped_ptr<CostFunction> cost_function_;
748};
749
750}  // namespace ceres
751
752#endif  // CERES_PUBLIC_COST_FUNCTION_TO_FUNCTOR_H_
753