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: mierle@gmail.com (Keir Mierle)
30//
31// An incomplete C API for Ceres.
32//
33// TODO(keir): Figure out why logging does not seem to work.
34
35#include "ceres/c_api.h"
36
37#include <vector>
38#include <iostream>
39#include <string>
40#include "ceres/cost_function.h"
41#include "ceres/loss_function.h"
42#include "ceres/problem.h"
43#include "ceres/solver.h"
44#include "ceres/types.h"  // for std
45#include "glog/logging.h"
46
47using ceres::Problem;
48
49void ceres_init() {
50  // This is not ideal, but it's not clear what to do if there is no gflags and
51  // no access to command line arguments.
52  char message[] = "<unknown>";
53  google::InitGoogleLogging(message);
54}
55
56ceres_problem_t* ceres_create_problem() {
57  return reinterpret_cast<ceres_problem_t*>(new Problem);
58}
59
60void ceres_free_problem(ceres_problem_t* problem) {
61  delete reinterpret_cast<Problem*>(problem);
62}
63
64// This cost function wraps a C-level function pointer from the user, to bridge
65// between C and C++.
66class CallbackCostFunction : public ceres::CostFunction {
67 public:
68  CallbackCostFunction(ceres_cost_function_t cost_function,
69                       void* user_data,
70                       int num_residuals,
71                       int num_parameter_blocks,
72                       int* parameter_block_sizes)
73      : cost_function_(cost_function),
74        user_data_(user_data) {
75    set_num_residuals(num_residuals);
76    for (int i = 0; i < num_parameter_blocks; ++i) {
77      mutable_parameter_block_sizes()->push_back(parameter_block_sizes[i]);
78    }
79  }
80
81  virtual ~CallbackCostFunction() {}
82
83  virtual bool Evaluate(double const* const* parameters,
84                        double* residuals,
85                        double** jacobians) const {
86    return (*cost_function_)(user_data_,
87                             const_cast<double**>(parameters),
88                             residuals,
89                             jacobians);
90  }
91
92 private:
93  ceres_cost_function_t cost_function_;
94  void* user_data_;
95};
96
97// This loss function wraps a C-level function pointer from the user, to bridge
98// between C and C++.
99class CallbackLossFunction : public ceres::LossFunction {
100 public:
101  explicit CallbackLossFunction(ceres_loss_function_t loss_function,
102                                void* user_data)
103    : loss_function_(loss_function), user_data_(user_data) {}
104  virtual void Evaluate(double sq_norm, double* rho) const {
105    (*loss_function_)(user_data_, sq_norm, rho);
106  }
107
108 private:
109  ceres_loss_function_t loss_function_;
110  void* user_data_;
111};
112
113// Wrappers for the stock loss functions.
114void* ceres_create_huber_loss_function_data(double a) {
115  return new ceres::HuberLoss(a);
116}
117void* ceres_create_softl1_loss_function_data(double a) {
118  return new ceres::SoftLOneLoss(a);
119}
120void* ceres_create_cauchy_loss_function_data(double a) {
121  return new ceres::CauchyLoss(a);
122}
123void* ceres_create_arctan_loss_function_data(double a) {
124  return new ceres::ArctanLoss(a);
125}
126void* ceres_create_tolerant_loss_function_data(double a, double b) {
127  return new ceres::TolerantLoss(a, b);
128}
129
130void ceres_free_stock_loss_function_data(void* loss_function_data) {
131  delete reinterpret_cast<ceres::LossFunction*>(loss_function_data);
132}
133
134void ceres_stock_loss_function(void* user_data,
135                               double squared_norm,
136                               double out[3]) {
137  reinterpret_cast<ceres::LossFunction*>(user_data)
138      ->Evaluate(squared_norm, out);
139}
140
141ceres_residual_block_id_t* ceres_problem_add_residual_block(
142    ceres_problem_t* problem,
143    ceres_cost_function_t cost_function,
144    void* cost_function_data,
145    ceres_loss_function_t loss_function,
146    void* loss_function_data,
147    int num_residuals,
148    int num_parameter_blocks,
149    int* parameter_block_sizes,
150    double** parameters) {
151  Problem* ceres_problem = reinterpret_cast<Problem*>(problem);
152
153  ceres::CostFunction* callback_cost_function =
154      new CallbackCostFunction(cost_function,
155                               cost_function_data,
156                               num_residuals,
157                               num_parameter_blocks,
158                               parameter_block_sizes);
159
160  ceres::LossFunction* callback_loss_function = NULL;
161  if (loss_function != NULL) {
162    callback_loss_function = new CallbackLossFunction(loss_function,
163                                                      loss_function_data);
164  }
165
166  std::vector<double*> parameter_blocks(parameters,
167                                        parameters + num_parameter_blocks);
168  return reinterpret_cast<ceres_residual_block_id_t*>(
169      ceres_problem->AddResidualBlock(callback_cost_function,
170                                      callback_loss_function,
171                                      parameter_blocks));
172}
173
174void ceres_solve(ceres_problem_t* c_problem) {
175  Problem* problem = reinterpret_cast<Problem*>(c_problem);
176
177  // TODO(keir): Obviously, this way of setting options won't scale or last.
178  // Instead, figure out a way to specify some of the options without
179  // duplicating everything.
180  ceres::Solver::Options options;
181  options.max_num_iterations = 100;
182  options.linear_solver_type = ceres::DENSE_QR;
183  options.minimizer_progress_to_stdout = true;
184
185  ceres::Solver::Summary summary;
186  ceres::Solve(options, problem, &summary);
187  std::cout << summary.FullReport() << "\n";
188}
189