Vehicle-cpp/zjw/ceres-solver-master/internal/ceres/c_api.cc

// Ceres Solver - A fast non-linear least squares minimizer
// Copyright 2023 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 <iostream>
#include <memory>
#include <string>
#include <vector>

#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 CERES_NO_EXPORT CallbackCostFunction final : 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]);
    }
  }

  bool Evaluate(double const* const* parameters,
                double* residuals,
                double** jacobians) const final {
    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 final : public ceres::LossFunction {
 public:
  explicit CallbackLossFunction(ceres_loss_function_t loss_function,
                                void* user_data)
      : loss_function_(loss_function), user_data_(user_data) {}
  void Evaluate(double sq_norm, double* rho) const final {
    (*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) {
  auto* ceres_problem = reinterpret_cast<Problem*>(problem);

  auto callback_cost_function =
      std::make_unique<CallbackCostFunction>(cost_function,
                                             cost_function_data,
                                             num_residuals,
                                             num_parameter_blocks,
                                             parameter_block_sizes);

  std::unique_ptr<ceres::LossFunction> callback_loss_function;
  if (loss_function != nullptr) {
    callback_loss_function = std::make_unique<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.release(),
                                      callback_loss_function.release(),
                                      parameter_blocks));
}

void ceres_solve(ceres_problem_t* c_problem) {
  auto* 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";
}