Squashed 'third_party/ceres/' content from commit e51e9b4
Change-Id: I763587619d57e594d3fa158dc3a7fe0b89a1743b
git-subtree-dir: third_party/ceres
git-subtree-split: e51e9b46f6ca88ab8b2266d0e362771db6d98067
diff --git a/internal/ceres/c_api.cc b/internal/ceres/c_api.cc
new file mode 100644
index 0000000..ada8f3e
--- /dev/null
+++ b/internal/ceres/c_api.cc
@@ -0,0 +1,188 @@
+// Ceres Solver - A fast non-linear least squares minimizer
+// Copyright 2015 Google Inc. All rights reserved.
+// http://ceres-solver.org/
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright notice,
+// this list of conditions and the following disclaimer in the documentation
+// and/or other materials provided with the distribution.
+// * Neither the name of Google Inc. nor the names of its contributors may be
+// used to endorse or promote products derived from this software without
+// specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//
+// Author: mierle@gmail.com (Keir Mierle)
+//
+// An incomplete C API for Ceres.
+//
+// TODO(keir): Figure out why logging does not seem to work.
+
+#include "ceres/c_api.h"
+
+#include <vector>
+#include <iostream>
+#include <string>
+#include "ceres/cost_function.h"
+#include "ceres/loss_function.h"
+#include "ceres/problem.h"
+#include "ceres/solver.h"
+#include "ceres/types.h" // for std
+#include "glog/logging.h"
+
+using ceres::Problem;
+
+void ceres_init() {
+ // This is not ideal, but it's not clear what to do if there is no gflags and
+ // no access to command line arguments.
+ char message[] = "<unknown>";
+ google::InitGoogleLogging(message);
+}
+
+ceres_problem_t* ceres_create_problem() {
+ return reinterpret_cast<ceres_problem_t*>(new Problem);
+}
+
+void ceres_free_problem(ceres_problem_t* problem) {
+ delete reinterpret_cast<Problem*>(problem);
+}
+
+// This cost function wraps a C-level function pointer from the user, to bridge
+// between C and C++.
+class CallbackCostFunction : public ceres::CostFunction {
+ public:
+ CallbackCostFunction(ceres_cost_function_t cost_function,
+ void* user_data,
+ int num_residuals,
+ int num_parameter_blocks,
+ int* parameter_block_sizes)
+ : cost_function_(cost_function),
+ user_data_(user_data) {
+ set_num_residuals(num_residuals);
+ for (int i = 0; i < num_parameter_blocks; ++i) {
+ mutable_parameter_block_sizes()->push_back(parameter_block_sizes[i]);
+ }
+ }
+
+ virtual ~CallbackCostFunction() {}
+
+ virtual bool Evaluate(double const* const* parameters,
+ double* residuals,
+ double** jacobians) const {
+ return (*cost_function_)(user_data_,
+ const_cast<double**>(parameters),
+ residuals,
+ jacobians);
+ }
+
+ private:
+ ceres_cost_function_t cost_function_;
+ void* user_data_;
+};
+
+// This loss function wraps a C-level function pointer from the user, to bridge
+// between C and C++.
+class CallbackLossFunction : public ceres::LossFunction {
+ public:
+ explicit CallbackLossFunction(ceres_loss_function_t loss_function,
+ void* user_data)
+ : loss_function_(loss_function), user_data_(user_data) {}
+ virtual void Evaluate(double sq_norm, double* rho) const {
+ (*loss_function_)(user_data_, sq_norm, rho);
+ }
+
+ private:
+ ceres_loss_function_t loss_function_;
+ void* user_data_;
+};
+
+// Wrappers for the stock loss functions.
+void* ceres_create_huber_loss_function_data(double a) {
+ return new ceres::HuberLoss(a);
+}
+void* ceres_create_softl1_loss_function_data(double a) {
+ return new ceres::SoftLOneLoss(a);
+}
+void* ceres_create_cauchy_loss_function_data(double a) {
+ return new ceres::CauchyLoss(a);
+}
+void* ceres_create_arctan_loss_function_data(double a) {
+ return new ceres::ArctanLoss(a);
+}
+void* ceres_create_tolerant_loss_function_data(double a, double b) {
+ return new ceres::TolerantLoss(a, b);
+}
+
+void ceres_free_stock_loss_function_data(void* loss_function_data) {
+ delete reinterpret_cast<ceres::LossFunction*>(loss_function_data);
+}
+
+void ceres_stock_loss_function(void* user_data,
+ double squared_norm,
+ double out[3]) {
+ reinterpret_cast<ceres::LossFunction*>(user_data)
+ ->Evaluate(squared_norm, out);
+}
+
+ceres_residual_block_id_t* ceres_problem_add_residual_block(
+ ceres_problem_t* problem,
+ ceres_cost_function_t cost_function,
+ void* cost_function_data,
+ ceres_loss_function_t loss_function,
+ void* loss_function_data,
+ int num_residuals,
+ int num_parameter_blocks,
+ int* parameter_block_sizes,
+ double** parameters) {
+ Problem* ceres_problem = reinterpret_cast<Problem*>(problem);
+
+ ceres::CostFunction* callback_cost_function =
+ new CallbackCostFunction(cost_function,
+ cost_function_data,
+ num_residuals,
+ num_parameter_blocks,
+ parameter_block_sizes);
+
+ ceres::LossFunction* callback_loss_function = NULL;
+ if (loss_function != NULL) {
+ callback_loss_function = new CallbackLossFunction(loss_function,
+ loss_function_data);
+ }
+
+ std::vector<double*> parameter_blocks(parameters,
+ parameters + num_parameter_blocks);
+ return reinterpret_cast<ceres_residual_block_id_t*>(
+ ceres_problem->AddResidualBlock(callback_cost_function,
+ callback_loss_function,
+ parameter_blocks));
+}
+
+void ceres_solve(ceres_problem_t* c_problem) {
+ Problem* problem = reinterpret_cast<Problem*>(c_problem);
+
+ // TODO(keir): Obviously, this way of setting options won't scale or last.
+ // Instead, figure out a way to specify some of the options without
+ // duplicating everything.
+ ceres::Solver::Options options;
+ options.max_num_iterations = 100;
+ options.linear_solver_type = ceres::DENSE_QR;
+ options.minimizer_progress_to_stdout = true;
+
+ ceres::Solver::Summary summary;
+ ceres::Solve(options, problem, &summary);
+ std::cout << summary.FullReport() << "\n";
+}