3246 lines
112 KiB
C++
3246 lines
112 KiB
C++
/* Ceres Wrapper - Extended C API Implementation
|
|
* Copyright 2024 RobotNet10. All rights reserved.
|
|
*
|
|
* This implementation provides additional C APIs that are missing from the
|
|
* official Ceres C API but are required for Cartographer integration.
|
|
*/
|
|
|
|
#include "ceres_wrapper.h"
|
|
|
|
#include <cstring>
|
|
#include <memory>
|
|
#include <string>
|
|
#include <vector>
|
|
#include <algorithm>
|
|
#include <functional>
|
|
#include <map>
|
|
#include <cstdio>
|
|
#include <cmath>
|
|
|
|
#include "ceres/ceres.h"
|
|
#include "ceres/cubic_interpolation.h"
|
|
#include "ceres/manifold.h"
|
|
#include "ceres/problem.h"
|
|
#include "ceres/solver.h"
|
|
#include "ceres/sphere_manifold.h"
|
|
#include "ceres/line_manifold.h"
|
|
#include "ceres/dynamic_autodiff_cost_function.h"
|
|
#include "ceres/product_manifold.h"
|
|
#include "ceres/iteration_callback.h"
|
|
#include "ceres/numeric_diff_cost_function.h"
|
|
#include "ceres/dynamic_numeric_diff_cost_function.h"
|
|
#include "ceres/covariance.h"
|
|
#include "ceres/gradient_checker.h"
|
|
#include "ceres/context.h"
|
|
#include "ceres/loss_function.h"
|
|
#include "ceres/autodiff_manifold.h"
|
|
#include "ceres/version.h"
|
|
#include <limits>
|
|
#include <mutex>
|
|
#include <sstream>
|
|
|
|
// ============================================================================
|
|
// Error Code Utilities
|
|
// ============================================================================
|
|
|
|
const char* ceres_wrapper_get_error_message(ceres_wrapper_error_code_t error_code) {
|
|
switch (error_code) {
|
|
case CERES_WRAPPER_SUCCESS:
|
|
return "Success";
|
|
case CERES_WRAPPER_ERROR_NULL_POINTER:
|
|
return "Null pointer argument";
|
|
case CERES_WRAPPER_ERROR_INVALID_PARAMETER:
|
|
return "Invalid parameter value";
|
|
case CERES_WRAPPER_ERROR_INVALID_ENUM:
|
|
return "Invalid enum value";
|
|
case CERES_WRAPPER_ERROR_EXCEPTION:
|
|
return "C++ exception occurred";
|
|
case CERES_WRAPPER_ERROR_OUT_OF_MEMORY:
|
|
return "Memory allocation failed";
|
|
case CERES_WRAPPER_ERROR_INVALID_OPERATION:
|
|
return "Invalid operation for current state";
|
|
case CERES_WRAPPER_ERROR_BUFFER_TOO_SMALL:
|
|
return "Output buffer too small";
|
|
case CERES_WRAPPER_ERROR_NOT_FOUND:
|
|
return "Resource not found";
|
|
case CERES_WRAPPER_ERROR_ALREADY_EXISTS:
|
|
return "Resource already exists";
|
|
default:
|
|
return "Unknown error";
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Library Information and Version
|
|
// ============================================================================
|
|
|
|
const char* ceres_wrapper_get_version_string() {
|
|
return CERES_VERSION_STRING;
|
|
}
|
|
|
|
int ceres_wrapper_get_version_major() {
|
|
return CERES_VERSION_MAJOR;
|
|
}
|
|
|
|
int ceres_wrapper_get_version_minor() {
|
|
return CERES_VERSION_MINOR;
|
|
}
|
|
|
|
int ceres_wrapper_get_version_revision() {
|
|
return CERES_VERSION_REVISION;
|
|
}
|
|
|
|
ceres_wrapper_error_code_t ceres_wrapper_get_detailed_version_string(
|
|
char* buffer,
|
|
int buffer_size) {
|
|
if (!buffer || buffer_size <= 0) {
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
|
|
try {
|
|
std::ostringstream oss;
|
|
oss << CERES_VERSION_STRING;
|
|
|
|
// Add Eigen version
|
|
#if defined(EIGEN_WORLD_VERSION) && defined(EIGEN_MAJOR_VERSION) && defined(EIGEN_MINOR_VERSION)
|
|
oss << "-eigen-("
|
|
<< EIGEN_WORLD_VERSION << "."
|
|
<< EIGEN_MAJOR_VERSION << "."
|
|
<< EIGEN_MINOR_VERSION << ")";
|
|
#else
|
|
oss << "-eigen-(unknown)";
|
|
#endif
|
|
|
|
// Add LAPACK info
|
|
#ifdef CERES_NO_LAPACK
|
|
oss << "-no_lapack";
|
|
#else
|
|
oss << "-lapack";
|
|
#endif
|
|
|
|
// Add SuiteSparse info
|
|
#ifndef CERES_NO_SUITESPARSE
|
|
#ifdef CERES_SUITESPARSE_VERSION
|
|
oss << "-suitesparse-(" << CERES_SUITESPARSE_VERSION << ")";
|
|
#endif
|
|
#endif
|
|
|
|
// Add METIS info
|
|
#if !defined(CERES_NO_EIGEN_METIS) || !defined(CERES_NO_CHOLMOD_PARTITION)
|
|
#ifdef CERES_METIS_VERSION
|
|
oss << "-metis-(" << CERES_METIS_VERSION << ")";
|
|
#endif
|
|
#endif
|
|
|
|
// Add AccelerateSparse info
|
|
#ifndef CERES_NO_ACCELERATE_SPARSE
|
|
oss << "-acceleratesparse";
|
|
#endif
|
|
|
|
// Add EigenSparse info
|
|
#ifdef CERES_USE_EIGEN_SPARSE
|
|
oss << "-eigensparse";
|
|
#endif
|
|
|
|
// Add Schur specializations info
|
|
#ifdef CERES_RESTRUCT_SCHUR_SPECIALIZATIONS
|
|
oss << "-no_schur_specializations";
|
|
#endif
|
|
|
|
// Add custom BLAS info
|
|
#ifdef CERES_NO_CUSTOM_BLAS
|
|
oss << "-no_custom_blas";
|
|
#endif
|
|
|
|
// Add CUDA info
|
|
#ifndef CERES_NO_CUDA
|
|
#ifdef CUDART_VERSION
|
|
oss << "-cuda-(" << CUDART_VERSION << ")";
|
|
#endif
|
|
#endif
|
|
|
|
std::string version_str = oss.str();
|
|
int copy_size = std::min(static_cast<int>(version_str.size()), buffer_size - 1);
|
|
std::strncpy(buffer, version_str.c_str(), copy_size);
|
|
buffer[copy_size] = '\0';
|
|
|
|
return CERES_WRAPPER_SUCCESS;
|
|
} catch (...) {
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
}
|
|
}
|
|
|
|
ceres_wrapper_error_code_t ceres_wrapper_get_library_info(
|
|
char* buffer,
|
|
int buffer_size) {
|
|
if (!buffer || buffer_size <= 0) {
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
|
|
try {
|
|
std::ostringstream oss;
|
|
|
|
oss << "Ceres Solver Library Information\n";
|
|
oss << "================================\n\n";
|
|
|
|
// Version information
|
|
oss << "Version: " << CERES_VERSION_STRING << "\n";
|
|
oss << " Major: " << CERES_VERSION_MAJOR << "\n";
|
|
oss << " Minor: " << CERES_VERSION_MINOR << "\n";
|
|
oss << " Revision: " << CERES_VERSION_REVISION << "\n\n";
|
|
|
|
// Build configuration
|
|
oss << "Build Configuration:\n";
|
|
|
|
// Eigen version
|
|
#if defined(EIGEN_WORLD_VERSION) && defined(EIGEN_MAJOR_VERSION) && defined(EIGEN_MINOR_VERSION)
|
|
oss << " Eigen: "
|
|
<< EIGEN_WORLD_VERSION << "."
|
|
<< EIGEN_MAJOR_VERSION << "."
|
|
<< EIGEN_MINOR_VERSION << "\n";
|
|
#else
|
|
oss << " Eigen: Unknown\n";
|
|
#endif
|
|
|
|
// LAPACK
|
|
#ifdef CERES_NO_LAPACK
|
|
oss << " LAPACK: Disabled\n";
|
|
#else
|
|
oss << " LAPACK: Enabled\n";
|
|
#endif
|
|
|
|
// SuiteSparse
|
|
#ifdef CERES_NO_SUITESPARSE
|
|
oss << " SuiteSparse: Disabled\n";
|
|
#else
|
|
oss << " SuiteSparse: Enabled";
|
|
#ifdef CERES_SUITESPARSE_VERSION
|
|
oss << " (version " << CERES_SUITESPARSE_VERSION << ")";
|
|
#endif
|
|
oss << "\n";
|
|
#endif
|
|
|
|
// METIS
|
|
#if defined(CERES_NO_EIGEN_METIS) && defined(CERES_NO_CHOLMOD_PARTITION)
|
|
oss << " METIS: Disabled\n";
|
|
#else
|
|
oss << " METIS: Enabled";
|
|
#ifdef CERES_METIS_VERSION
|
|
oss << " (version " << CERES_METIS_VERSION << ")";
|
|
#endif
|
|
oss << "\n";
|
|
#endif
|
|
|
|
// AccelerateSparse
|
|
#ifdef CERES_NO_ACCELERATE_SPARSE
|
|
oss << " AccelerateSparse: Disabled\n";
|
|
#else
|
|
oss << " AccelerateSparse: Enabled\n";
|
|
#endif
|
|
|
|
// EigenSparse
|
|
#ifdef CERES_USE_EIGEN_SPARSE
|
|
oss << " EigenSparse: Enabled\n";
|
|
#else
|
|
oss << " EigenSparse: Disabled\n";
|
|
#endif
|
|
|
|
// Schur specializations
|
|
#ifdef CERES_RESTRUCT_SCHUR_SPECIALIZATIONS
|
|
oss << " Schur Specializations: Disabled\n";
|
|
#else
|
|
oss << " Schur Specializations: Enabled\n";
|
|
#endif
|
|
|
|
// Custom BLAS
|
|
#ifdef CERES_NO_CUSTOM_BLAS
|
|
oss << " Custom BLAS: Disabled\n";
|
|
#else
|
|
oss << " Custom BLAS: Enabled\n";
|
|
#endif
|
|
|
|
// CUDA
|
|
#ifdef CERES_NO_CUDA
|
|
oss << " CUDA: Disabled\n";
|
|
#else
|
|
oss << " CUDA: Enabled";
|
|
#ifdef CUDART_VERSION
|
|
oss << " (version " << CUDART_VERSION << ")";
|
|
#endif
|
|
oss << "\n";
|
|
#endif
|
|
|
|
std::string info_str = oss.str();
|
|
int copy_size = std::min(static_cast<int>(info_str.size()), buffer_size - 1);
|
|
std::strncpy(buffer, info_str.c_str(), copy_size);
|
|
buffer[copy_size] = '\0';
|
|
|
|
return CERES_WRAPPER_SUCCESS;
|
|
} catch (...) {
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Forward declarations - Define structs early for use in classes
|
|
// ============================================================================
|
|
|
|
// Define ceres_solver_summary_s early so it can be used in IterationCallbackWrapper
|
|
struct ceres_solver_summary_s {
|
|
ceres::Solver::Summary summary;
|
|
};
|
|
typedef struct ceres_solver_summary_s ceres_solver_summary_t;
|
|
|
|
// Wrapper to bridge C callback to Ceres IterationCallback
|
|
// Defined here so it can be used in ceres_solver_options_s
|
|
class IterationCallbackWrapper : public ceres::IterationCallback {
|
|
public:
|
|
IterationCallbackWrapper(ceres_iteration_callback_t callback, void* user_data)
|
|
: callback_(callback), user_data_(user_data) {}
|
|
|
|
ceres::CallbackReturnType operator()(const ceres::IterationSummary& summary) override {
|
|
// Validate callback pointer (user_data_ may legitimately be NULL)
|
|
if (!callback_) {
|
|
return ceres::SOLVER_TERMINATE_SUCCESSFULLY;
|
|
}
|
|
// Create a temporary solver summary wrapper for the callback
|
|
// Note: This is a simplified version - we only pass basic info
|
|
ceres_solver_summary_t temp_summary;
|
|
// IterationSummary doesn't have termination_type, use default
|
|
temp_summary.summary.termination_type = ceres::TerminationType::NO_CONVERGENCE;
|
|
temp_summary.summary.initial_cost = summary.cost;
|
|
temp_summary.summary.final_cost = summary.cost;
|
|
temp_summary.summary.iterations.clear();
|
|
temp_summary.summary.num_successful_steps = summary.step_is_successful ? 1 : 0;
|
|
temp_summary.summary.num_unsuccessful_steps = summary.step_is_successful ? 0 : 1;
|
|
|
|
int result = callback_(user_data_, &temp_summary);
|
|
|
|
// Convert return value: 0 = continue, non-zero = stop
|
|
return (result == 0) ? ceres::SOLVER_CONTINUE : ceres::SOLVER_TERMINATE_SUCCESSFULLY;
|
|
}
|
|
|
|
private:
|
|
ceres_iteration_callback_t callback_;
|
|
void* user_data_;
|
|
};
|
|
|
|
// ============================================================================
|
|
// Solver Options
|
|
// ============================================================================
|
|
|
|
struct ceres_solver_options_s {
|
|
ceres::Solver::Options options;
|
|
std::unique_ptr<IterationCallbackWrapper> callback_wrapper; // Store callback wrapper
|
|
};
|
|
|
|
ceres_solver_options_t* ceres_wrapper_create_solver_options() {
|
|
return new ceres_solver_options_t;
|
|
}
|
|
|
|
void ceres_wrapper_free_solver_options(ceres_solver_options_t* options) {
|
|
delete options;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_linear_solver_type(
|
|
ceres_solver_options_t* options, int linear_solver_type) {
|
|
if (!options) return;
|
|
options->options.linear_solver_type =
|
|
static_cast<ceres::LinearSolverType>(linear_solver_type);
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_linear_solver_type(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0;
|
|
return static_cast<int>(options->options.linear_solver_type);
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_minimizer_type(
|
|
ceres_solver_options_t* options, int minimizer_type) {
|
|
if (!options) return;
|
|
options->options.minimizer_type =
|
|
static_cast<ceres::MinimizerType>(minimizer_type);
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_minimizer_type(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0;
|
|
return static_cast<int>(options->options.minimizer_type);
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_max_num_iterations(
|
|
ceres_solver_options_t* options, int max_num_iterations) {
|
|
if (!options) return;
|
|
options->options.max_num_iterations = max_num_iterations;
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_max_num_iterations(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0;
|
|
return options->options.max_num_iterations;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_num_threads(
|
|
ceres_solver_options_t* options, int num_threads) {
|
|
if (!options) return;
|
|
options->options.num_threads = num_threads;
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_num_threads(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0;
|
|
return options->options.num_threads;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_function_tolerance(
|
|
ceres_solver_options_t* options, double function_tolerance) {
|
|
if (!options) return;
|
|
options->options.function_tolerance = function_tolerance;
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_function_tolerance(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0.0;
|
|
return options->options.function_tolerance;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_gradient_tolerance(
|
|
ceres_solver_options_t* options, double gradient_tolerance) {
|
|
if (!options) return;
|
|
options->options.gradient_tolerance = gradient_tolerance;
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_gradient_tolerance(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0.0;
|
|
return options->options.gradient_tolerance;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_parameter_tolerance(
|
|
ceres_solver_options_t* options, double parameter_tolerance) {
|
|
if (!options) return;
|
|
options->options.parameter_tolerance = parameter_tolerance;
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_parameter_tolerance(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0.0;
|
|
return options->options.parameter_tolerance;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_initial_trust_region_radius(
|
|
ceres_solver_options_t* options, double radius) {
|
|
if (!options) return;
|
|
options->options.initial_trust_region_radius = radius;
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_initial_trust_region_radius(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0.0;
|
|
return options->options.initial_trust_region_radius;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_max_trust_region_radius(
|
|
ceres_solver_options_t* options, double radius) {
|
|
if (!options) return;
|
|
options->options.max_trust_region_radius = radius;
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_max_trust_region_radius(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0.0;
|
|
return options->options.max_trust_region_radius;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_min_trust_region_radius(
|
|
ceres_solver_options_t* options, double radius) {
|
|
if (!options) return;
|
|
options->options.min_trust_region_radius = radius;
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_min_trust_region_radius(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0.0;
|
|
return options->options.min_trust_region_radius;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_preconditioner_type(
|
|
ceres_solver_options_t* options, int preconditioner_type) {
|
|
if (!options) return;
|
|
options->options.preconditioner_type =
|
|
static_cast<ceres::PreconditionerType>(preconditioner_type);
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_preconditioner_type(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0;
|
|
return static_cast<int>(options->options.preconditioner_type);
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_trust_region_strategy_type(
|
|
ceres_solver_options_t* options, int trust_region_strategy_type) {
|
|
if (!options) return;
|
|
options->options.trust_region_strategy_type =
|
|
static_cast<ceres::TrustRegionStrategyType>(trust_region_strategy_type);
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_trust_region_strategy_type(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0;
|
|
return static_cast<int>(options->options.trust_region_strategy_type);
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_dogleg_type(
|
|
ceres_solver_options_t* options, int dogleg_type) {
|
|
if (!options) return;
|
|
options->options.dogleg_type =
|
|
static_cast<ceres::DoglegType>(dogleg_type);
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_dogleg_type(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0;
|
|
return static_cast<int>(options->options.dogleg_type);
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_use_nonmonotonic_steps(
|
|
ceres_solver_options_t* options, int use_nonmonotonic_steps) {
|
|
if (!options) return;
|
|
options->options.use_nonmonotonic_steps = (use_nonmonotonic_steps != 0);
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_use_nonmonotonic_steps(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0;
|
|
return options->options.use_nonmonotonic_steps ? 1 : 0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_max_consecutive_nonmonotonic_steps(
|
|
ceres_solver_options_t* options, int max_steps) {
|
|
if (!options) return;
|
|
options->options.max_consecutive_nonmonotonic_steps = max_steps;
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_max_consecutive_nonmonotonic_steps(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0;
|
|
return options->options.max_consecutive_nonmonotonic_steps;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_max_num_consecutive_invalid_steps(
|
|
ceres_solver_options_t* options, int max_steps) {
|
|
if (!options) return;
|
|
options->options.max_num_consecutive_invalid_steps = max_steps;
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_max_num_consecutive_invalid_steps(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0;
|
|
return options->options.max_num_consecutive_invalid_steps;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_min_relative_decrease(
|
|
ceres_solver_options_t* options, double min_relative_decrease) {
|
|
if (!options) return;
|
|
options->options.min_relative_decrease = min_relative_decrease;
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_min_relative_decrease(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0.0;
|
|
return options->options.min_relative_decrease;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_logging_type(
|
|
ceres_solver_options_t* options, int logging_type) {
|
|
if (!options) return;
|
|
options->options.logging_type =
|
|
static_cast<ceres::LoggingType>(logging_type);
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_logging_type(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0;
|
|
return static_cast<int>(options->options.logging_type);
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_minimizer_progress_to_stdout(
|
|
ceres_solver_options_t* options, int minimizer_progress_to_stdout) {
|
|
if (!options) return;
|
|
options->options.minimizer_progress_to_stdout = (minimizer_progress_to_stdout != 0);
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_minimizer_progress_to_stdout(
|
|
const ceres_solver_options_t* options) {
|
|
if (!options) return 0;
|
|
return options->options.minimizer_progress_to_stdout ? 1 : 0;
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_is_valid(
|
|
const ceres_solver_options_t* options, char* error_message, int error_message_size) {
|
|
if (!options) return 0;
|
|
std::string error;
|
|
bool valid = options->options.IsValid(&error);
|
|
if (!valid && error_message && error_message_size > 0) {
|
|
int copy_size = std::min(static_cast<int>(error.size()), error_message_size - 1);
|
|
std::strncpy(error_message, error.c_str(), copy_size);
|
|
error_message[copy_size] = '\0';
|
|
}
|
|
return valid ? 1 : 0;
|
|
}
|
|
|
|
// ============================================================================
|
|
// Solver Summary
|
|
// ============================================================================
|
|
|
|
// Note: ceres_solver_summary_s is defined earlier in the file
|
|
|
|
ceres_solver_summary_t* ceres_wrapper_create_solver_summary() {
|
|
return new ceres_solver_summary_t;
|
|
}
|
|
|
|
void ceres_wrapper_free_solver_summary(ceres_solver_summary_t* summary) {
|
|
delete summary;
|
|
}
|
|
|
|
int ceres_wrapper_solver_summary_get_termination_type(
|
|
const ceres_solver_summary_t* summary) {
|
|
if (!summary) return 0;
|
|
return static_cast<int>(summary->summary.termination_type);
|
|
}
|
|
|
|
void ceres_wrapper_solver_summary_get_message(
|
|
const ceres_solver_summary_t* summary, char* message, int message_size) {
|
|
if (!summary) return;
|
|
if (message && message_size > 0) {
|
|
const std::string& msg = summary->summary.message;
|
|
int copy_size = std::min(static_cast<int>(msg.size()), message_size - 1);
|
|
std::strncpy(message, msg.c_str(), copy_size);
|
|
message[copy_size] = '\0';
|
|
}
|
|
}
|
|
|
|
double ceres_wrapper_solver_summary_get_initial_cost(
|
|
const ceres_solver_summary_t* summary) {
|
|
if (!summary) return 0.0;
|
|
return summary->summary.initial_cost;
|
|
}
|
|
|
|
double ceres_wrapper_solver_summary_get_final_cost(
|
|
const ceres_solver_summary_t* summary) {
|
|
if (!summary) return 0.0;
|
|
return summary->summary.final_cost;
|
|
}
|
|
|
|
int ceres_wrapper_solver_summary_get_iterations(
|
|
const ceres_solver_summary_t* summary) {
|
|
if (!summary) return 0;
|
|
return summary->summary.iterations.size();
|
|
}
|
|
|
|
int ceres_wrapper_solver_summary_get_num_successful_steps(
|
|
const ceres_solver_summary_t* summary) {
|
|
if (!summary) return 0;
|
|
return summary->summary.num_successful_steps;
|
|
}
|
|
|
|
int ceres_wrapper_solver_summary_get_num_unsuccessful_steps(
|
|
const ceres_solver_summary_t* summary) {
|
|
if (!summary) return 0;
|
|
return summary->summary.num_unsuccessful_steps;
|
|
}
|
|
|
|
int ceres_wrapper_solver_summary_get_num_inner_iteration_steps(
|
|
const ceres_solver_summary_t* summary) {
|
|
if (!summary) return 0;
|
|
return summary->summary.num_inner_iteration_steps;
|
|
}
|
|
|
|
double ceres_wrapper_solver_summary_get_total_time_in_seconds(
|
|
const ceres_solver_summary_t* summary) {
|
|
if (!summary) return 0.0;
|
|
return summary->summary.total_time_in_seconds;
|
|
}
|
|
|
|
double ceres_wrapper_solver_summary_get_preprocessor_time_in_seconds(
|
|
const ceres_solver_summary_t* summary) {
|
|
if (!summary) return 0.0;
|
|
return summary->summary.preprocessor_time_in_seconds;
|
|
}
|
|
|
|
double ceres_wrapper_solver_summary_get_minimizer_time_in_seconds(
|
|
const ceres_solver_summary_t* summary) {
|
|
if (!summary) return 0.0;
|
|
return summary->summary.minimizer_time_in_seconds;
|
|
}
|
|
|
|
double ceres_wrapper_solver_summary_get_postprocessor_time_in_seconds(
|
|
const ceres_solver_summary_t* summary) {
|
|
if (!summary) return 0.0;
|
|
return summary->summary.postprocessor_time_in_seconds;
|
|
}
|
|
|
|
double ceres_wrapper_solver_summary_get_linear_solver_time_in_seconds(
|
|
const ceres_solver_summary_t* summary) {
|
|
if (!summary) return 0.0;
|
|
return summary->summary.linear_solver_time_in_seconds;
|
|
}
|
|
|
|
void ceres_wrapper_solver_summary_get_full_report(
|
|
const ceres_solver_summary_t* summary, char* report, int report_size) {
|
|
if (!summary) return;
|
|
if (report && report_size > 0) {
|
|
std::string full_report = summary->summary.FullReport();
|
|
int copy_size = std::min(static_cast<int>(full_report.size()), report_size - 1);
|
|
std::strncpy(report, full_report.c_str(), copy_size);
|
|
report[copy_size] = '\0';
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Solve with Options and Summary
|
|
// ============================================================================
|
|
|
|
ceres_wrapper_error_code_t ceres_wrapper_solve(
|
|
ceres_problem_t* problem,
|
|
const ceres_solver_options_t* options,
|
|
ceres_solver_summary_t* summary,
|
|
char* error_message,
|
|
int error_message_size) {
|
|
if (!problem) {
|
|
fprintf(stdout, "E20251230 ceres_wrapper_solve: ERROR - problem is null\n");
|
|
fflush(stdout);
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "problem is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (!options) {
|
|
fprintf(stdout, "E20251230 ceres_wrapper_solve: ERROR - options is null\n");
|
|
fflush(stdout);
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "options is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (!summary) {
|
|
fprintf(stdout, "E20251230 ceres_wrapper_solve: ERROR - summary is null\n");
|
|
fflush(stdout);
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "summary is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
|
|
try {
|
|
auto* ceres_problem = reinterpret_cast<ceres::Problem*>(problem);
|
|
|
|
// Forward call to Ceres - with detailed logging
|
|
ceres::Solve(options->options, ceres_problem, &summary->summary);
|
|
|
|
return CERES_WRAPPER_SUCCESS;
|
|
} catch (const std::exception& e) {
|
|
fprintf(stdout, "E20251230 ceres_wrapper_solve: std::exception caught - what()=%s\n", e.what());
|
|
fflush(stdout);
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, e.what(), error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
} catch (...) {
|
|
fprintf(stdout, "E20251230 ceres_wrapper_solve: Unknown exception caught\n");
|
|
fflush(stdout);
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "Unknown exception during solve", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Problem Creation (Official C API)
|
|
// ============================================================================
|
|
|
|
ceres_problem_t* ceres_create_problem() {
|
|
try {
|
|
return reinterpret_cast<ceres_problem_t*>(new ceres::Problem());
|
|
} catch (...) {
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
void ceres_free_problem(ceres_problem_t* problem) {
|
|
if (problem) {
|
|
delete reinterpret_cast<ceres::Problem*>(problem);
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Parameter Blocks Management
|
|
// ============================================================================
|
|
|
|
ceres_wrapper_error_code_t ceres_wrapper_problem_add_parameter_block(
|
|
ceres_problem_t* problem,
|
|
double* parameters,
|
|
int size,
|
|
char* error_message,
|
|
int error_message_size) {
|
|
if (!problem) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "problem is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (!parameters) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "parameters is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (size <= 0) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "size must be positive", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_INVALID_PARAMETER;
|
|
}
|
|
|
|
try {
|
|
auto* ceres_problem = reinterpret_cast<ceres::Problem*>(problem);
|
|
ceres_problem->AddParameterBlock(parameters, size);
|
|
return CERES_WRAPPER_SUCCESS;
|
|
} catch (const std::exception& e) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, e.what(), error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
} catch (...) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "Unknown exception", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
}
|
|
}
|
|
|
|
void ceres_wrapper_problem_set_parameter_block_constant(
|
|
ceres_problem_t* problem,
|
|
double* parameters) {
|
|
if (!problem || !parameters) return;
|
|
try {
|
|
auto* ceres_problem = reinterpret_cast<ceres::Problem*>(problem);
|
|
ceres_problem->SetParameterBlockConstant(parameters);
|
|
} catch (...) {
|
|
// Prevent C++ exception from crossing extern "C" boundary
|
|
}
|
|
}
|
|
|
|
void ceres_wrapper_problem_set_parameter_block_variable(
|
|
ceres_problem_t* problem,
|
|
double* parameters) {
|
|
if (!problem || !parameters) return;
|
|
try {
|
|
auto* ceres_problem = reinterpret_cast<ceres::Problem*>(problem);
|
|
ceres_problem->SetParameterBlockVariable(parameters);
|
|
} catch (...) {
|
|
// Prevent C++ exception from crossing extern "C" boundary
|
|
}
|
|
}
|
|
|
|
ceres_wrapper_error_code_t ceres_wrapper_problem_remove_parameter_block(
|
|
ceres_problem_t* problem,
|
|
double* parameters,
|
|
char* error_message,
|
|
int error_message_size) {
|
|
if (!problem) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "problem is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (!parameters) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "parameters is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
|
|
try {
|
|
auto* ceres_problem = reinterpret_cast<ceres::Problem*>(problem);
|
|
ceres_problem->RemoveParameterBlock(parameters);
|
|
return CERES_WRAPPER_SUCCESS;
|
|
} catch (const std::exception& e) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, e.what(), error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
} catch (...) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "Unknown exception", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
}
|
|
}
|
|
|
|
ceres_wrapper_error_code_t ceres_wrapper_problem_remove_residual_block(
|
|
ceres_problem_t* problem,
|
|
ceres_residual_block_id_t* residual_block_id,
|
|
char* error_message,
|
|
int error_message_size) {
|
|
if (!problem) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "problem is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (!residual_block_id) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "residual_block_id is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
|
|
try {
|
|
auto* ceres_problem = reinterpret_cast<ceres::Problem*>(problem);
|
|
auto* block_id = reinterpret_cast<ceres::ResidualBlockId>(residual_block_id);
|
|
ceres_problem->RemoveResidualBlock(block_id);
|
|
return CERES_WRAPPER_SUCCESS;
|
|
} catch (const std::exception& e) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, e.what(), error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
} catch (...) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "Unknown exception", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Loss Functions (defined early for use in AddResidualBlock)
|
|
// ============================================================================
|
|
|
|
struct ceres_wrapper_loss_function_s {
|
|
std::unique_ptr<ceres::LossFunction> loss;
|
|
};
|
|
|
|
ceres_wrapper_error_code_t ceres_wrapper_problem_add_residual_block(
|
|
ceres_problem_t* problem,
|
|
void* cost_function,
|
|
void* loss_function,
|
|
double** parameter_blocks,
|
|
int num_parameter_blocks,
|
|
ceres_residual_block_id_t** residual_block_id,
|
|
char* error_message,
|
|
int error_message_size) {
|
|
if (!problem) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "problem is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (!cost_function) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "cost_function is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (!parameter_blocks) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "parameter_blocks is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (num_parameter_blocks <= 0) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "num_parameter_blocks must be positive", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_INVALID_PARAMETER;
|
|
}
|
|
|
|
try {
|
|
auto* ceres_problem = reinterpret_cast<ceres::Problem*>(problem);
|
|
auto* ceres_cost = reinterpret_cast<ceres::CostFunction*>(cost_function);
|
|
|
|
// Handle loss function
|
|
ceres::LossFunction* ceres_loss = nullptr;
|
|
if (loss_function) {
|
|
auto* loss_wrapper = reinterpret_cast<ceres_wrapper_loss_function_t*>(loss_function);
|
|
ceres_loss = loss_wrapper->loss.get(); // Don't release yet
|
|
}
|
|
|
|
// Add residual block
|
|
// Problem takes ownership of cost_function and loss_function by default
|
|
auto* block_id = ceres_problem->AddResidualBlock(
|
|
ceres_cost, ceres_loss, parameter_blocks, num_parameter_blocks);
|
|
|
|
// Only release ownership AFTER successful AddResidualBlock
|
|
if (loss_function) {
|
|
auto* loss_wrapper = reinterpret_cast<ceres_wrapper_loss_function_t*>(loss_function);
|
|
loss_wrapper->loss.release(); // Now safe to release
|
|
}
|
|
|
|
// Set output parameter if provided
|
|
if (residual_block_id) {
|
|
*residual_block_id = reinterpret_cast<ceres_residual_block_id_t*>(block_id);
|
|
}
|
|
|
|
return CERES_WRAPPER_SUCCESS;
|
|
} catch (const std::exception& e) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, e.what(), error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
} catch (...) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "Unknown exception", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_problem_num_parameter_blocks(
|
|
const ceres_problem_t* problem) {
|
|
if (!problem) return 0;
|
|
const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
|
|
return ceres_problem->NumParameterBlocks();
|
|
}
|
|
|
|
int ceres_wrapper_problem_num_residual_blocks(
|
|
const ceres_problem_t* problem) {
|
|
if (!problem) return 0;
|
|
const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
|
|
return ceres_problem->NumResidualBlocks();
|
|
}
|
|
|
|
int ceres_wrapper_problem_num_parameters(
|
|
const ceres_problem_t* problem) {
|
|
if (!problem) return 0;
|
|
const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
|
|
return ceres_problem->NumParameters();
|
|
}
|
|
|
|
int ceres_wrapper_problem_num_residuals(
|
|
const ceres_problem_t* problem) {
|
|
if (!problem) return 0;
|
|
const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
|
|
return ceres_problem->NumResiduals();
|
|
}
|
|
|
|
// ============================================================================
|
|
// Loss Functions (struct already defined above for AddResidualBlock)
|
|
// ============================================================================
|
|
|
|
ceres_wrapper_loss_function_t* ceres_wrapper_create_huber_loss(double a) {
|
|
try {
|
|
auto* loss = new ceres_wrapper_loss_function_s;
|
|
loss->loss = std::make_unique<ceres::HuberLoss>(a);
|
|
return loss;
|
|
} catch (...) {
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
ceres_wrapper_loss_function_t* ceres_wrapper_create_trivial_loss() {
|
|
try {
|
|
auto* loss = new ceres_wrapper_loss_function_s;
|
|
loss->loss = std::make_unique<ceres::TrivialLoss>();
|
|
return loss;
|
|
} catch (...) {
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
ceres_wrapper_loss_function_t* ceres_wrapper_create_cauchy_loss(double a) {
|
|
try {
|
|
auto* loss = new ceres_wrapper_loss_function_s;
|
|
loss->loss = std::make_unique<ceres::CauchyLoss>(a);
|
|
return loss;
|
|
} catch (...) {
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
ceres_wrapper_loss_function_t* ceres_wrapper_create_softl1_loss(double a) {
|
|
try {
|
|
auto* loss = new ceres_wrapper_loss_function_s;
|
|
loss->loss = std::make_unique<ceres::SoftLOneLoss>(a);
|
|
return loss;
|
|
} catch (...) {
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
ceres_wrapper_loss_function_t* ceres_wrapper_create_arctan_loss(double a) {
|
|
try {
|
|
auto* loss = new ceres_wrapper_loss_function_s;
|
|
loss->loss = std::make_unique<ceres::ArctanLoss>(a);
|
|
return loss;
|
|
} catch (...) {
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
ceres_wrapper_loss_function_t* ceres_wrapper_create_tolerant_loss(double a, double b) {
|
|
try {
|
|
auto* loss = new ceres_wrapper_loss_function_s;
|
|
loss->loss = std::make_unique<ceres::TolerantLoss>(a, b);
|
|
return loss;
|
|
} catch (...) {
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
void ceres_wrapper_free_loss_function(ceres_wrapper_loss_function_t* loss) {
|
|
delete loss;
|
|
}
|
|
|
|
ceres_wrapper_loss_function_t* ceres_wrapper_create_composed_loss(
|
|
ceres_wrapper_loss_function_t* f,
|
|
int ownership_f,
|
|
ceres_wrapper_loss_function_t* g,
|
|
int ownership_g) {
|
|
if (!f || !g) {
|
|
return nullptr;
|
|
}
|
|
|
|
try {
|
|
auto* loss = new ceres_wrapper_loss_function_s;
|
|
|
|
ceres::LossFunction* ceres_f = f->loss.release();
|
|
ceres::LossFunction* ceres_g = g->loss.release();
|
|
|
|
ceres::Ownership ownership_f_enum = ownership_f ? ceres::TAKE_OWNERSHIP : ceres::DO_NOT_TAKE_OWNERSHIP;
|
|
ceres::Ownership ownership_g_enum = ownership_g ? ceres::TAKE_OWNERSHIP : ceres::DO_NOT_TAKE_OWNERSHIP;
|
|
|
|
try {
|
|
loss->loss = std::make_unique<ceres::ComposedLoss>(
|
|
ceres_f, ownership_f_enum, ceres_g, ownership_g_enum);
|
|
} catch (...) {
|
|
// If ownership was TAKE_OWNERSHIP, ComposedLoss ctor would have taken ownership
|
|
// But if it threw before, we need to clean up
|
|
if (ownership_f_enum == ceres::DO_NOT_TAKE_OWNERSHIP) delete ceres_f;
|
|
if (ownership_g_enum == ceres::DO_NOT_TAKE_OWNERSHIP) delete ceres_g;
|
|
delete loss;
|
|
delete f;
|
|
delete g;
|
|
return nullptr;
|
|
}
|
|
|
|
// Free the wrapper structures (we always release the unique_ptr above)
|
|
delete f;
|
|
delete g;
|
|
|
|
return loss;
|
|
} catch (...) {
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
ceres_wrapper_loss_function_t* ceres_wrapper_create_scaled_loss(
|
|
ceres_wrapper_loss_function_t* rho,
|
|
double a,
|
|
int ownership) {
|
|
try {
|
|
auto* loss = new ceres_wrapper_loss_function_s;
|
|
|
|
// Extract loss function from wrapper
|
|
// Note: We release from unique_ptr because ScaledLoss will manage ownership
|
|
ceres::LossFunction* ceres_rho = nullptr;
|
|
if (rho) {
|
|
ceres_rho = rho->loss.release();
|
|
}
|
|
|
|
// Create ScaledLoss
|
|
// If ownership = 1, ScaledLoss takes ownership (will delete when done)
|
|
// If ownership = 0, ScaledLoss does not take ownership (caller manages)
|
|
ceres::Ownership ownership_enum = ownership ? ceres::TAKE_OWNERSHIP : ceres::DO_NOT_TAKE_OWNERSHIP;
|
|
loss->loss = std::make_unique<ceres::ScaledLoss>(ceres_rho, a, ownership_enum);
|
|
|
|
// Free the wrapper structure (we always release the unique_ptr above if rho was provided)
|
|
if (rho) {
|
|
delete rho;
|
|
}
|
|
|
|
return loss;
|
|
} catch (...) {
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Manifolds
|
|
// ============================================================================
|
|
|
|
// Custom deleter type for manifolds (allows non-owning pointers)
|
|
using ManifoldDeleter = std::function<void(ceres::Manifold*)>;
|
|
using ManifoldPtr = std::unique_ptr<ceres::Manifold, ManifoldDeleter>;
|
|
|
|
struct ceres_manifold_s {
|
|
ManifoldPtr manifold;
|
|
};
|
|
|
|
ceres_manifold_t* ceres_wrapper_create_quaternion_manifold() {
|
|
auto* m = new ceres_manifold_t;
|
|
// Create with default deleter (owning)
|
|
auto ptr = std::make_unique<ceres::QuaternionManifold>();
|
|
m->manifold = ManifoldPtr(ptr.release(), [](ceres::Manifold* p) { delete p; });
|
|
return m;
|
|
}
|
|
|
|
ceres_manifold_t* ceres_wrapper_create_sphere_manifold(int dimension) {
|
|
auto* m = new ceres_manifold_t;
|
|
// Create with default deleter (owning)
|
|
auto ptr = std::make_unique<ceres::SphereManifold<ceres::DYNAMIC>>(dimension);
|
|
m->manifold = ManifoldPtr(ptr.release(), [](ceres::Manifold* p) { delete p; });
|
|
return m;
|
|
}
|
|
|
|
ceres_manifold_t* ceres_wrapper_create_line_manifold(int dimension) {
|
|
auto* m = new ceres_manifold_t;
|
|
// LineManifold is a template class, use DYNAMIC for runtime dimension
|
|
// Create with default deleter (owning)
|
|
auto ptr = std::make_unique<ceres::LineManifold<ceres::DYNAMIC>>(dimension);
|
|
m->manifold = ManifoldPtr(ptr.release(), [](ceres::Manifold* p) { delete p; });
|
|
return m;
|
|
}
|
|
|
|
ceres_manifold_t* ceres_wrapper_create_euclidean_manifold(int dimension) {
|
|
auto* m = new ceres_manifold_t;
|
|
// EuclideanManifold is a template class, use DYNAMIC for runtime dimension
|
|
auto ptr = std::make_unique<ceres::EuclideanManifold<ceres::DYNAMIC>>(dimension);
|
|
m->manifold = ManifoldPtr(ptr.release(), [](ceres::Manifold* p) { delete p; });
|
|
return m;
|
|
}
|
|
|
|
ceres_manifold_t* ceres_wrapper_create_subset_manifold(
|
|
const int* constant_subset,
|
|
int constant_subset_size,
|
|
int ambient_size) {
|
|
if (!constant_subset || constant_subset_size <= 0 || ambient_size <= 0) {
|
|
return nullptr;
|
|
}
|
|
|
|
auto* m = new ceres_manifold_t;
|
|
// Convert C array to std::vector
|
|
std::vector<int> constant_indices(constant_subset, constant_subset + constant_subset_size);
|
|
// SubsetManifold constructor: SubsetManifold(int size, const std::vector<int>& constant_parameters)
|
|
auto ptr = std::make_unique<ceres::SubsetManifold>(
|
|
ambient_size, constant_indices);
|
|
m->manifold = ManifoldPtr(ptr.release(), [](ceres::Manifold* p) { delete p; });
|
|
return m;
|
|
}
|
|
|
|
void ceres_wrapper_free_manifold(ceres_manifold_t* manifold) {
|
|
delete manifold;
|
|
}
|
|
|
|
void ceres_wrapper_problem_set_manifold(
|
|
ceres_problem_t* problem,
|
|
double* parameters,
|
|
ceres_manifold_t* manifold) {
|
|
auto* ceres_problem = reinterpret_cast<ceres::Problem*>(problem);
|
|
// Problem takes ownership of the manifold (TAKE_OWNERSHIP by default)
|
|
// We need to release the unique_ptr before passing to Problem
|
|
ceres::Manifold* manifold_ptr = manifold->manifold.release();
|
|
ceres_problem->SetManifold(parameters, manifold_ptr);
|
|
// Note: Problem now owns the manifold, it will be freed when problem is destroyed
|
|
// We can still free the wrapper struct, but the manifold itself is owned by Problem
|
|
}
|
|
|
|
int ceres_wrapper_manifold_ambient_size(const ceres_manifold_t* manifold) {
|
|
if (!manifold || !manifold->manifold) return 0;
|
|
return manifold->manifold->AmbientSize();
|
|
}
|
|
|
|
int ceres_wrapper_manifold_tangent_size(const ceres_manifold_t* manifold) {
|
|
if (!manifold || !manifold->manifold) return 0;
|
|
return manifold->manifold->TangentSize();
|
|
}
|
|
|
|
// ============================================================================
|
|
// AutoDiff Manifold
|
|
// ============================================================================
|
|
|
|
// Wrapper class to bridge C callbacks to Ceres AutoDiffManifold
|
|
// Since AutoDiffManifold is a template class, we create a custom Manifold
|
|
// that uses callbacks and numeric differentiation for Jacobians
|
|
class AutoDiffManifoldWrapper : public ceres::Manifold {
|
|
public:
|
|
AutoDiffManifoldWrapper(
|
|
int ambient_size,
|
|
int tangent_size,
|
|
ceres_autodiff_manifold_plus_t plus_callback,
|
|
ceres_autodiff_manifold_minus_t minus_callback,
|
|
void* user_data)
|
|
: ambient_size_(ambient_size),
|
|
tangent_size_(tangent_size),
|
|
plus_callback_(plus_callback),
|
|
minus_callback_(minus_callback),
|
|
user_data_(user_data) {
|
|
if (ambient_size <= 0 || tangent_size <= 0) {
|
|
throw std::invalid_argument("Ambient size and tangent size must be positive");
|
|
}
|
|
}
|
|
|
|
int AmbientSize() const override { return ambient_size_; }
|
|
int TangentSize() const override { return tangent_size_; }
|
|
|
|
bool Plus(const double* x,
|
|
const double* delta,
|
|
double* x_plus_delta) const override {
|
|
if (!plus_callback_) {
|
|
return false;
|
|
}
|
|
// user_data_ may legitimately be NULL if callback doesn't need state
|
|
int result = plus_callback_(user_data_, x, delta, x_plus_delta);
|
|
return (result != 0);
|
|
}
|
|
|
|
bool PlusJacobian(const double* x, double* jacobian) const override {
|
|
// Use numeric differentiation for PlusJacobian
|
|
const double kEpsilon = 1e-8;
|
|
std::vector<double> zero_delta(tangent_size_, 0.0);
|
|
std::vector<double> x_plus_delta(ambient_size_);
|
|
std::vector<double> perturbed_delta(tangent_size_);
|
|
std::vector<double> perturbed_x_plus_delta(ambient_size_);
|
|
|
|
// Compute Plus(x, 0) as baseline
|
|
if (!Plus(x, zero_delta.data(), x_plus_delta.data())) {
|
|
return false;
|
|
}
|
|
|
|
// Compute Jacobian column by column
|
|
for (int j = 0; j < tangent_size_; ++j) {
|
|
// Perturb delta[j]
|
|
std::copy(zero_delta.data(), zero_delta.data() + tangent_size_, perturbed_delta.data());
|
|
perturbed_delta[j] = kEpsilon;
|
|
|
|
// Compute Plus(x, perturbed_delta)
|
|
if (!Plus(x, perturbed_delta.data(), perturbed_x_plus_delta.data())) {
|
|
return false;
|
|
}
|
|
|
|
// Compute finite difference: (Plus(x, perturbed_delta) - Plus(x, 0)) / epsilon
|
|
for (int i = 0; i < ambient_size_; ++i) {
|
|
jacobian[i * tangent_size_ + j] =
|
|
(perturbed_x_plus_delta[i] - x_plus_delta[i]) / kEpsilon;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool Minus(const double* y,
|
|
const double* x,
|
|
double* y_minus_x) const override {
|
|
if (!minus_callback_) {
|
|
return false;
|
|
}
|
|
// user_data_ may legitimately be NULL if callback doesn't need state
|
|
int result = minus_callback_(user_data_, y, x, y_minus_x);
|
|
return (result != 0);
|
|
}
|
|
|
|
bool MinusJacobian(const double* x, double* jacobian) const override {
|
|
// Use numeric differentiation for MinusJacobian
|
|
// MinusJacobian is D_1 Minus(x, x), so we compute derivative w.r.t. first argument
|
|
const double kEpsilon = 1e-8;
|
|
std::vector<double> y_minus_x(tangent_size_);
|
|
std::vector<double> perturbed_y(ambient_size_);
|
|
std::vector<double> perturbed_y_minus_x(tangent_size_);
|
|
|
|
// Compute Minus(x, x) as baseline (should be zero)
|
|
if (!Minus(x, x, y_minus_x.data())) {
|
|
return false;
|
|
}
|
|
|
|
// Compute Jacobian column by column
|
|
for (int j = 0; j < ambient_size_; ++j) {
|
|
// Perturb y[j] (first argument)
|
|
std::copy(x, x + ambient_size_, perturbed_y.data());
|
|
perturbed_y[j] += kEpsilon;
|
|
|
|
// Compute Minus(perturbed_y, x)
|
|
if (!Minus(perturbed_y.data(), x, perturbed_y_minus_x.data())) {
|
|
return false;
|
|
}
|
|
|
|
// Compute finite difference: (Minus(perturbed_y, x) - Minus(x, x)) / epsilon
|
|
for (int i = 0; i < tangent_size_; ++i) {
|
|
jacobian[i * ambient_size_ + j] =
|
|
(perturbed_y_minus_x[i] - y_minus_x[i]) / kEpsilon;
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
private:
|
|
int ambient_size_;
|
|
int tangent_size_;
|
|
ceres_autodiff_manifold_plus_t plus_callback_;
|
|
ceres_autodiff_manifold_minus_t minus_callback_;
|
|
void* user_data_;
|
|
};
|
|
|
|
ceres_manifold_t* ceres_wrapper_create_autodiff_manifold(
|
|
int ambient_size,
|
|
int tangent_size,
|
|
ceres_autodiff_manifold_plus_t plus_callback,
|
|
ceres_autodiff_manifold_minus_t minus_callback,
|
|
void* user_data) {
|
|
if (ambient_size <= 0 || tangent_size <= 0) {
|
|
return nullptr;
|
|
}
|
|
if (!plus_callback || !minus_callback) {
|
|
return nullptr;
|
|
}
|
|
|
|
try {
|
|
auto* m = new ceres_manifold_t;
|
|
auto wrapper = std::make_unique<AutoDiffManifoldWrapper>(
|
|
ambient_size, tangent_size, plus_callback, minus_callback, user_data);
|
|
m->manifold = ManifoldPtr(wrapper.release(), [](ceres::Manifold* p) { delete p; });
|
|
return m;
|
|
} catch (...) {
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
void ceres_wrapper_free_autodiff_manifold(ceres_manifold_t* manifold) {
|
|
// Same as free_manifold - just delete the wrapper struct
|
|
// The manifold itself might be owned by Problem
|
|
delete manifold;
|
|
}
|
|
|
|
// ============================================================================
|
|
// BiCubic Interpolator
|
|
// ============================================================================
|
|
|
|
// Adapter for 2D grid data to work with Ceres BiCubicInterpolator
|
|
class GridArrayAdapter {
|
|
public:
|
|
enum { DATA_DIMENSION = 1 };
|
|
|
|
GridArrayAdapter(const double* data, int rows, int cols)
|
|
: data_(data), rows_(rows), cols_(cols) {}
|
|
|
|
void GetValue(const int row, const int column, double* const value) const {
|
|
if (row < 0 || column < 0 || row >= rows_ || column >= cols_) {
|
|
*value = 0.0; // Out of bounds - return default value
|
|
} else {
|
|
*value = data_[row * cols_ + column];
|
|
}
|
|
}
|
|
|
|
int NumRows() const { return rows_; }
|
|
int NumCols() const { return cols_; }
|
|
|
|
private:
|
|
const double* data_;
|
|
int rows_;
|
|
int cols_;
|
|
};
|
|
|
|
struct ceres_bicubic_interpolator_s {
|
|
std::unique_ptr<ceres::BiCubicInterpolator<GridArrayAdapter>> interpolator;
|
|
std::unique_ptr<GridArrayAdapter> adapter;
|
|
std::vector<double> data; // Keep data alive
|
|
};
|
|
|
|
ceres_bicubic_interpolator_t* ceres_wrapper_create_bicubic_interpolator(
|
|
const double* data,
|
|
int rows,
|
|
int cols) {
|
|
if (!data || rows <= 0 || cols <= 0) {
|
|
return nullptr;
|
|
}
|
|
auto* interp = new ceres_bicubic_interpolator_t;
|
|
|
|
// Copy data to keep it alive
|
|
interp->data.assign(data, data + rows * cols);
|
|
|
|
// Create adapter with copied data
|
|
interp->adapter = std::make_unique<GridArrayAdapter>(
|
|
interp->data.data(), rows, cols);
|
|
|
|
// Create interpolator with adapter
|
|
interp->interpolator = std::make_unique<ceres::BiCubicInterpolator<GridArrayAdapter>>(
|
|
*interp->adapter);
|
|
|
|
return interp;
|
|
}
|
|
|
|
void ceres_wrapper_free_bicubic_interpolator(
|
|
ceres_bicubic_interpolator_t* interpolator) {
|
|
delete interpolator;
|
|
}
|
|
|
|
void ceres_wrapper_bicubic_interpolator_evaluate(
|
|
const ceres_bicubic_interpolator_t* interpolator,
|
|
double x,
|
|
double y,
|
|
double* value,
|
|
double* gradient_x,
|
|
double* gradient_y) {
|
|
double val = 0.0;
|
|
double grad_r = 0.0; // gradient with respect to row
|
|
double grad_c = 0.0; // gradient with respect to column
|
|
|
|
// Evaluate: (row, column) = (y, x)
|
|
// Note: Ceres uses (r, c) = (row, column) convention
|
|
interpolator->interpolator->Evaluate(y, x, &val, &grad_r, &grad_c);
|
|
|
|
if (value) *value = val;
|
|
// Map gradients: grad_r (row) -> grad_y, grad_c (column) -> grad_x
|
|
if (gradient_x) *gradient_x = grad_c;
|
|
if (gradient_y) *gradient_y = grad_r;
|
|
}
|
|
|
|
// ============================================================================
|
|
// AutoDiff Cost Function Wrapper
|
|
// ============================================================================
|
|
|
|
// Wrapper class to bridge C callback to Ceres AutoDiffCostFunction
|
|
class AutoDiffCostFunctionWrapper : public ceres::CostFunction {
|
|
public:
|
|
AutoDiffCostFunctionWrapper(
|
|
ceres_autodiff_cost_function_callback_t callback,
|
|
void* user_data,
|
|
int num_residuals,
|
|
int num_parameter_blocks,
|
|
const int* parameter_block_sizes)
|
|
: callback_(callback), 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 override {
|
|
// Validate callback pointer (user_data_ may legitimately be NULL)
|
|
if (!callback_) {
|
|
return false;
|
|
}
|
|
// Call the C callback to get residuals
|
|
int result = callback_(user_data_, parameters, residuals);
|
|
if (result == 0) {
|
|
return false;
|
|
}
|
|
|
|
// Basic validation: check for NaN/Inf in residuals
|
|
for (int i = 0; i < num_residuals(); ++i) {
|
|
if (std::isnan(residuals[i]) || std::isinf(residuals[i])) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// If jacobians are requested, compute them using finite differences
|
|
// Note: True AutoDiff requires template magic, so we use numerical differentiation
|
|
if (jacobians != nullptr) {
|
|
const double kEpsilon = 1e-4;
|
|
std::vector<double> perturbed_residuals(num_residuals());
|
|
int num_blocks = static_cast<int>(parameter_block_sizes().size());
|
|
|
|
for (int i = 0; i < num_blocks; ++i) {
|
|
if (jacobians[i] != nullptr) {
|
|
int param_size = parameter_block_sizes()[i];
|
|
std::vector<double> perturbed_params(param_size);
|
|
|
|
// Create array of pointers to parameter blocks
|
|
// All blocks point to original parameters except block i which points to perturbed_params
|
|
std::vector<const double*> perturbed_param_ptrs(num_blocks);
|
|
for (int k = 0; k < num_blocks; ++k) {
|
|
perturbed_param_ptrs[k] = parameters[k];
|
|
}
|
|
|
|
for (int j = 0; j < param_size; ++j) {
|
|
// Perturb parameter
|
|
std::copy(parameters[i], parameters[i] + param_size, perturbed_params.data());
|
|
perturbed_params[j] += kEpsilon;
|
|
|
|
// Update pointer for block i to point to perturbed data
|
|
perturbed_param_ptrs[i] = perturbed_params.data();
|
|
|
|
// Evaluate with perturbed parameter (pass array of all parameter block pointers)
|
|
int perturb_result = callback_(user_data_, perturbed_param_ptrs.data(), perturbed_residuals.data());
|
|
if (perturb_result == 0) {
|
|
return false;
|
|
}
|
|
|
|
// Check for NaN/Inf in perturbed residuals
|
|
for (int k = 0; k < num_residuals(); ++k) {
|
|
if (std::isnan(perturbed_residuals[k]) || std::isinf(perturbed_residuals[k])) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// Compute finite difference
|
|
for (int k = 0; k < num_residuals(); ++k) {
|
|
double diff = (perturbed_residuals[k] - residuals[k]) / kEpsilon;
|
|
if (std::isnan(diff) || std::isinf(diff)) {
|
|
return false;
|
|
}
|
|
jacobians[i][k * param_size + j] = diff;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
private:
|
|
ceres_autodiff_cost_function_callback_t callback_;
|
|
void* user_data_;
|
|
};
|
|
|
|
void* ceres_wrapper_create_autodiff_cost_function(
|
|
ceres_autodiff_cost_function_callback_t callback,
|
|
void* user_data,
|
|
int num_residuals,
|
|
int num_parameter_blocks,
|
|
const int* parameter_block_sizes) {
|
|
return new AutoDiffCostFunctionWrapper(
|
|
callback, user_data, num_residuals, num_parameter_blocks, parameter_block_sizes);
|
|
}
|
|
|
|
void ceres_wrapper_free_autodiff_cost_function(void* cost_function) {
|
|
delete reinterpret_cast<AutoDiffCostFunctionWrapper*>(cost_function);
|
|
}
|
|
|
|
// ============================================================================
|
|
// Dynamic AutoDiff Cost Function Wrapper
|
|
// ============================================================================
|
|
|
|
// Wrapper class to bridge C callback to Ceres DynamicAutoDiffCostFunction
|
|
class DynamicAutoDiffCostFunctionWrapper : public ceres::DynamicCostFunction {
|
|
public:
|
|
DynamicAutoDiffCostFunctionWrapper(
|
|
ceres_autodiff_cost_function_callback_t callback,
|
|
void* user_data,
|
|
int num_residuals,
|
|
int num_parameter_blocks,
|
|
const int* parameter_block_sizes)
|
|
: callback_(callback), 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 override {
|
|
// Validate callback pointer (user_data_ may legitimately be NULL)
|
|
if (!callback_) {
|
|
return false;
|
|
}
|
|
// Call the C callback to get residuals
|
|
int result = callback_(user_data_, parameters, residuals);
|
|
if (result == 0) {
|
|
return false;
|
|
}
|
|
|
|
// Basic validation: check for NaN/Inf in residuals
|
|
for (int i = 0; i < num_residuals(); ++i) {
|
|
if (std::isnan(residuals[i]) || std::isinf(residuals[i])) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// If jacobians are requested, compute them using finite differences
|
|
if (jacobians != nullptr) {
|
|
const double kEpsilon = 1e-4;
|
|
std::vector<double> perturbed_residuals(num_residuals());
|
|
|
|
for (int i = 0; i < static_cast<int>(parameter_block_sizes().size()); ++i) {
|
|
if (jacobians[i] != nullptr) {
|
|
int param_size = parameter_block_sizes()[i];
|
|
std::vector<double> perturbed_params(param_size);
|
|
|
|
// Create array of pointers for all parameter blocks
|
|
std::vector<const double*> param_ptrs(parameter_block_sizes().size());
|
|
for (int k = 0; k < static_cast<int>(parameter_block_sizes().size()); ++k) {
|
|
param_ptrs[k] = parameters[k];
|
|
}
|
|
|
|
for (int j = 0; j < param_size; ++j) {
|
|
// Perturb parameter
|
|
std::copy(parameters[i], parameters[i] + param_size, perturbed_params.data());
|
|
perturbed_params[j] += kEpsilon;
|
|
param_ptrs[i] = perturbed_params.data();
|
|
|
|
// Evaluate with perturbed parameter
|
|
int perturb_result = callback_(user_data_, param_ptrs.data(), perturbed_residuals.data());
|
|
if (perturb_result == 0) {
|
|
return false;
|
|
}
|
|
|
|
// Check for NaN/Inf in perturbed residuals
|
|
for (int k = 0; k < num_residuals(); ++k) {
|
|
if (std::isnan(perturbed_residuals[k]) || std::isinf(perturbed_residuals[k])) {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
// Compute finite difference
|
|
for (int k = 0; k < num_residuals(); ++k) {
|
|
double diff = (perturbed_residuals[k] - residuals[k]) / kEpsilon;
|
|
if (std::isnan(diff) || std::isinf(diff)) {
|
|
return false;
|
|
}
|
|
jacobians[i][k * param_size + j] = diff;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
private:
|
|
ceres_autodiff_cost_function_callback_t callback_;
|
|
void* user_data_;
|
|
};
|
|
|
|
void* ceres_wrapper_create_dynamic_autodiff_cost_function(
|
|
ceres_autodiff_cost_function_callback_t callback,
|
|
void* user_data,
|
|
int num_residuals,
|
|
int num_parameter_blocks,
|
|
const int* parameter_block_sizes) {
|
|
return new DynamicAutoDiffCostFunctionWrapper(
|
|
callback, user_data, num_residuals, num_parameter_blocks, parameter_block_sizes);
|
|
}
|
|
|
|
void ceres_wrapper_free_dynamic_autodiff_cost_function(void* cost_function) {
|
|
delete reinterpret_cast<DynamicAutoDiffCostFunctionWrapper*>(cost_function);
|
|
}
|
|
|
|
// ============================================================================
|
|
// Product Manifold
|
|
// ============================================================================
|
|
|
|
// Custom manifold wrapper for runtime ProductManifold
|
|
// Since Ceres ProductManifold requires compile-time template parameters,
|
|
// we create a custom manifold that wraps multiple manifolds at runtime
|
|
class RuntimeProductManifold : public ceres::Manifold {
|
|
public:
|
|
RuntimeProductManifold(std::vector<ManifoldPtr> manifolds)
|
|
: manifolds_(std::move(manifolds)) {
|
|
// Calculate sizes
|
|
ambient_size_ = 0;
|
|
tangent_size_ = 0;
|
|
for (const auto& m : manifolds_) {
|
|
ambient_sizes_.push_back(m->AmbientSize());
|
|
tangent_sizes_.push_back(m->TangentSize());
|
|
ambient_size_ += ambient_sizes_.back();
|
|
tangent_size_ += tangent_sizes_.back();
|
|
}
|
|
|
|
// Calculate offsets
|
|
ambient_offsets_.push_back(0);
|
|
tangent_offsets_.push_back(0);
|
|
for (size_t i = 0; i < manifolds_.size(); ++i) {
|
|
ambient_offsets_.push_back(ambient_offsets_.back() + ambient_sizes_[i]);
|
|
tangent_offsets_.push_back(tangent_offsets_.back() + tangent_sizes_[i]);
|
|
}
|
|
}
|
|
|
|
int AmbientSize() const override { return ambient_size_; }
|
|
int TangentSize() const override { return tangent_size_; }
|
|
|
|
bool Plus(const double* x, const double* delta, double* x_plus_delta) const override {
|
|
for (size_t i = 0; i < manifolds_.size(); ++i) {
|
|
// Validate manifold pointer to prevent use-after-free
|
|
if (!manifolds_[i]) {
|
|
return false;
|
|
}
|
|
const double* x_i = x + ambient_offsets_[i];
|
|
const double* delta_i = delta + tangent_offsets_[i];
|
|
double* x_plus_delta_i = x_plus_delta + ambient_offsets_[i];
|
|
|
|
if (!manifolds_[i]->Plus(x_i, delta_i, x_plus_delta_i)) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool Minus(const double* y, const double* x, double* y_minus_x) const override {
|
|
for (size_t i = 0; i < manifolds_.size(); ++i) {
|
|
// Validate manifold pointer to prevent use-after-free
|
|
if (!manifolds_[i]) {
|
|
return false;
|
|
}
|
|
const double* y_i = y + ambient_offsets_[i];
|
|
const double* x_i = x + ambient_offsets_[i];
|
|
double* y_minus_x_i = y_minus_x + tangent_offsets_[i];
|
|
|
|
if (!manifolds_[i]->Minus(y_i, x_i, y_minus_x_i)) {
|
|
return false;
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool PlusJacobian(const double* x, double* jacobian) const override {
|
|
// Compute Jacobian for Plus operation
|
|
// For product manifold, Jacobian is block diagonal
|
|
// Jacobian is AmbientSize() x TangentSize() matrix (row-major)
|
|
// Each block is ambient_sizes_[i] x tangent_sizes_[i]
|
|
|
|
// Initialize to zero
|
|
std::fill(jacobian, jacobian + ambient_size_ * tangent_size_, 0.0);
|
|
|
|
for (size_t i = 0; i < manifolds_.size(); ++i) {
|
|
const double* x_i = x + ambient_offsets_[i];
|
|
|
|
// Get sub-jacobian for this manifold
|
|
std::vector<double> sub_jacobian(ambient_sizes_[i] * tangent_sizes_[i]);
|
|
if (!manifolds_[i]->PlusJacobian(x_i, sub_jacobian.data())) {
|
|
return false;
|
|
}
|
|
|
|
// Copy sub-jacobian to correct position in block diagonal
|
|
for (int row = 0; row < ambient_sizes_[i]; ++row) {
|
|
for (int col = 0; col < tangent_sizes_[i]; ++col) {
|
|
jacobian[(ambient_offsets_[i] + row) * tangent_size_ + tangent_offsets_[i] + col] =
|
|
sub_jacobian[row * tangent_sizes_[i] + col];
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool MinusJacobian(const double* x, double* jacobian) const override {
|
|
// Compute Jacobian for Minus operation
|
|
// For product manifold, Jacobian is block diagonal
|
|
// Jacobian is TangentSize() x AmbientSize() matrix (row-major)
|
|
// Each block is tangent_sizes_[i] x ambient_sizes_[i]
|
|
|
|
// Initialize to zero
|
|
std::fill(jacobian, jacobian + tangent_size_ * ambient_size_, 0.0);
|
|
|
|
for (size_t i = 0; i < manifolds_.size(); ++i) {
|
|
const double* x_i = x + ambient_offsets_[i];
|
|
// Get sub-jacobian for this manifold
|
|
std::vector<double> sub_jacobian(tangent_sizes_[i] * ambient_sizes_[i]);
|
|
if (!manifolds_[i]->MinusJacobian(x_i, sub_jacobian.data())) {
|
|
return false;
|
|
}
|
|
|
|
// Copy sub-jacobian to correct position in block diagonal
|
|
for (int row = 0; row < tangent_sizes_[i]; ++row) {
|
|
for (int col = 0; col < ambient_sizes_[i]; ++col) {
|
|
jacobian[(tangent_offsets_[i] + row) * ambient_size_ + ambient_offsets_[i] + col] =
|
|
sub_jacobian[row * ambient_sizes_[i] + col];
|
|
}
|
|
}
|
|
}
|
|
return true;
|
|
}
|
|
|
|
private:
|
|
std::vector<ManifoldPtr> manifolds_;
|
|
int ambient_size_;
|
|
int tangent_size_;
|
|
std::vector<int> ambient_sizes_;
|
|
std::vector<int> tangent_sizes_;
|
|
std::vector<int> ambient_offsets_;
|
|
std::vector<int> tangent_offsets_;
|
|
};
|
|
|
|
ceres_manifold_t* ceres_wrapper_create_product_manifold(
|
|
const ceres_manifold_t** manifolds,
|
|
int num_manifolds) {
|
|
if (!manifolds || num_manifolds <= 0) {
|
|
return nullptr;
|
|
}
|
|
|
|
// Extract and clone Ceres manifolds from wrappers
|
|
// We need to clone because we'll own them
|
|
std::vector<ManifoldPtr> manifold_clones;
|
|
for (int i = 0; i < num_manifolds; ++i) {
|
|
if (!manifolds[i] || !manifolds[i]->manifold) {
|
|
return nullptr; // Invalid manifold
|
|
}
|
|
|
|
// We need to clone the manifold, but since we can't clone generically,
|
|
// we'll just reference the original. This means the original manifold
|
|
// must outlive the product manifold.
|
|
// For a proper implementation, we'd need to clone based on type.
|
|
// For now, we'll use a non-owning unique_ptr with custom deleter
|
|
// WARNING: Non-owning reference - the original manifold wrappers must outlive
|
|
// this product manifold. Freeing a component manifold while the product manifold
|
|
// is alive will result in undefined behavior (dangling pointer).
|
|
ceres::Manifold* raw_ptr = manifolds[i]->manifold.get();
|
|
manifold_clones.push_back(ManifoldPtr(
|
|
raw_ptr,
|
|
[](ceres::Manifold*) {})); // Non-owning deleter
|
|
}
|
|
|
|
// Create runtime product manifold
|
|
auto* wrapped = new ceres_manifold_t;
|
|
// Create with default deleter (owning)
|
|
auto ptr = std::make_unique<RuntimeProductManifold>(std::move(manifold_clones));
|
|
wrapped->manifold = ManifoldPtr(ptr.release(), [](ceres::Manifold* p) { delete p; });
|
|
return wrapped;
|
|
}
|
|
|
|
// ============================================================================
|
|
// Problem Query Methods
|
|
// ============================================================================
|
|
|
|
int ceres_wrapper_problem_has_parameter_block(
|
|
const ceres_problem_t* problem,
|
|
const double* parameters) {
|
|
if (!problem || !parameters) {
|
|
return 0;
|
|
}
|
|
const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
|
|
return ceres_problem->HasParameterBlock(parameters) ? 1 : 0;
|
|
}
|
|
|
|
int ceres_wrapper_problem_is_parameter_block_constant(
|
|
const ceres_problem_t* problem,
|
|
const double* parameters) {
|
|
if (!problem || !parameters) {
|
|
return 0;
|
|
}
|
|
const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
|
|
return ceres_problem->IsParameterBlockConstant(parameters) ? 1 : 0;
|
|
}
|
|
|
|
int ceres_wrapper_problem_get_parameter_block_size(
|
|
const ceres_problem_t* problem,
|
|
const double* parameters) {
|
|
if (!problem || !parameters) {
|
|
return -1;
|
|
}
|
|
const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
|
|
if (!ceres_problem->HasParameterBlock(parameters)) {
|
|
return -1;
|
|
}
|
|
return ceres_problem->ParameterBlockSize(parameters);
|
|
}
|
|
|
|
int ceres_wrapper_problem_get_parameter_block_tangent_size(
|
|
const ceres_problem_t* problem,
|
|
const double* parameters) {
|
|
if (!problem || !parameters) {
|
|
return -1;
|
|
}
|
|
const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
|
|
if (!ceres_problem->HasParameterBlock(parameters)) {
|
|
return -1;
|
|
}
|
|
return ceres_problem->ParameterBlockTangentSize(parameters);
|
|
}
|
|
|
|
int ceres_wrapper_problem_has_manifold(
|
|
const ceres_problem_t* problem,
|
|
const double* parameters) {
|
|
if (!problem || !parameters) {
|
|
return 0;
|
|
}
|
|
const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
|
|
return ceres_problem->HasManifold(parameters) ? 1 : 0;
|
|
}
|
|
|
|
ceres_manifold_t* ceres_wrapper_problem_get_manifold(
|
|
const ceres_problem_t* problem,
|
|
const double* parameters) {
|
|
if (!problem || !parameters) {
|
|
return nullptr;
|
|
}
|
|
const auto* ceres_problem = reinterpret_cast<const ceres::Problem*>(problem);
|
|
const ceres::Manifold* ceres_manifold = ceres_problem->GetManifold(parameters);
|
|
|
|
if (!ceres_manifold) {
|
|
return nullptr;
|
|
}
|
|
|
|
// Wrap the manifold (note: we don't own it, so we create a non-owning wrapper)
|
|
// However, since Problem owns the manifold, we need to be careful
|
|
// For now, we'll create a wrapper that doesn't own the pointer
|
|
// This is a limitation - the returned manifold should not be freed
|
|
auto* wrapped = new ceres_manifold_t;
|
|
// Create a unique_ptr that doesn't delete (since Problem owns it)
|
|
wrapped->manifold = ManifoldPtr(
|
|
const_cast<ceres::Manifold*>(ceres_manifold),
|
|
[](ceres::Manifold*) {}); // No-op deleter
|
|
return wrapped;
|
|
}
|
|
|
|
// ============================================================================
|
|
// Iteration Callback
|
|
// ============================================================================
|
|
|
|
void ceres_wrapper_solver_options_set_iteration_callback(
|
|
ceres_solver_options_t* options,
|
|
ceres_iteration_callback_t callback,
|
|
void* user_data) {
|
|
if (!options) {
|
|
return;
|
|
}
|
|
|
|
// Clear existing callback
|
|
options->options.callbacks.clear();
|
|
options->callback_wrapper.reset();
|
|
|
|
if (callback) {
|
|
// Create callback wrapper and store it in options
|
|
options->callback_wrapper = std::make_unique<IterationCallbackWrapper>(callback, user_data);
|
|
options->options.callbacks.push_back(options->callback_wrapper.get());
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Numeric Differentiation Cost Functions (Phase 2)
|
|
// ============================================================================
|
|
|
|
// Note: ConvertNumericDiffMethod is no longer needed since we use template specialization
|
|
// Keeping it commented out in case it's needed in the future
|
|
// static ceres::NumericDiffMethodType ConvertNumericDiffMethod(ceres_numeric_diff_method_t method) {
|
|
// switch (method) {
|
|
// case CERES_NUMERIC_DIFF_FORWARD:
|
|
// return ceres::FORWARD;
|
|
// case CERES_NUMERIC_DIFF_CENTRAL:
|
|
// return ceres::CENTRAL;
|
|
// case CERES_NUMERIC_DIFF_RIDDERS:
|
|
// return ceres::RIDDERS;
|
|
// default:
|
|
// return ceres::CENTRAL;
|
|
// }
|
|
// }
|
|
|
|
// Cost functor wrapper for NumericDiffCostFunction
|
|
// This functor bridges the C callback to Ceres' template-based NumericDiffCostFunction
|
|
struct NumericDiffCostFunctor {
|
|
NumericDiffCostFunctor(ceres_numeric_diff_cost_function_t callback, void* user_data)
|
|
: callback_(callback), user_data_(user_data) {}
|
|
|
|
// Template operator for Ceres - converts T* to double* for C callback
|
|
// For numeric diff, T is always double, so we can safely cast
|
|
template <typename T>
|
|
bool operator()(const T* const* parameters, T* residuals) const {
|
|
// Validate callback pointer (user_data_ may legitimately be NULL)
|
|
if (!callback_) {
|
|
return false;
|
|
}
|
|
|
|
// For numeric diff, T is always double, so we can safely cast
|
|
// Convert const T* const* to double* const* for C callback
|
|
// Note: C callback expects double* const* (non-const pointers to const data)
|
|
// We use const_cast which is safe here since numeric diff only reads
|
|
double* const* params_for_callback = const_cast<double* const*>(reinterpret_cast<const double* const*>(parameters));
|
|
double* double_residuals = reinterpret_cast<double*>(residuals);
|
|
|
|
int result = callback_(params_for_callback, double_residuals, user_data_);
|
|
|
|
return (result != 0);
|
|
}
|
|
|
|
private:
|
|
ceres_numeric_diff_cost_function_t callback_;
|
|
void* user_data_;
|
|
};
|
|
|
|
void* ceres_wrapper_create_numeric_diff_cost_function(
|
|
const ceres_numeric_diff_options_t* options,
|
|
ceres_numeric_diff_method_t method) {
|
|
if (!options || !options->callback) {
|
|
return nullptr;
|
|
}
|
|
|
|
// Create functor wrapper
|
|
auto* functor = new NumericDiffCostFunctor(options->callback, options->user_data);
|
|
|
|
// Use DynamicNumericDiffCostFunction for flexibility with variable parameter block sizes
|
|
// The method type is a template parameter, so we need to create the appropriate specialization
|
|
ceres::NumericDiffOptions numeric_diff_options;
|
|
|
|
// Create cost function based on method type using template specialization
|
|
ceres::CostFunction* cost_function = nullptr;
|
|
|
|
switch (method) {
|
|
case CERES_NUMERIC_DIFF_FORWARD: {
|
|
auto* cf = new ceres::DynamicNumericDiffCostFunction<NumericDiffCostFunctor, ceres::FORWARD>(
|
|
functor, ceres::TAKE_OWNERSHIP, numeric_diff_options);
|
|
cf->SetNumResiduals(options->num_residuals);
|
|
for (int i = 0; i < options->num_parameter_blocks; ++i) {
|
|
cf->AddParameterBlock(options->parameter_block_sizes[i]);
|
|
}
|
|
cost_function = cf;
|
|
break;
|
|
}
|
|
case CERES_NUMERIC_DIFF_CENTRAL: {
|
|
auto* cf = new ceres::DynamicNumericDiffCostFunction<NumericDiffCostFunctor, ceres::CENTRAL>(
|
|
functor, ceres::TAKE_OWNERSHIP, numeric_diff_options);
|
|
cf->SetNumResiduals(options->num_residuals);
|
|
for (int i = 0; i < options->num_parameter_blocks; ++i) {
|
|
cf->AddParameterBlock(options->parameter_block_sizes[i]);
|
|
}
|
|
cost_function = cf;
|
|
break;
|
|
}
|
|
case CERES_NUMERIC_DIFF_RIDDERS: {
|
|
auto* cf = new ceres::DynamicNumericDiffCostFunction<NumericDiffCostFunctor, ceres::RIDDERS>(
|
|
functor, ceres::TAKE_OWNERSHIP, numeric_diff_options);
|
|
cf->SetNumResiduals(options->num_residuals);
|
|
for (int i = 0; i < options->num_parameter_blocks; ++i) {
|
|
cf->AddParameterBlock(options->parameter_block_sizes[i]);
|
|
}
|
|
cost_function = cf;
|
|
break;
|
|
}
|
|
default: {
|
|
// Default to CENTRAL
|
|
auto* cf = new ceres::DynamicNumericDiffCostFunction<NumericDiffCostFunctor, ceres::CENTRAL>(
|
|
functor, ceres::TAKE_OWNERSHIP, numeric_diff_options);
|
|
cf->SetNumResiduals(options->num_residuals);
|
|
for (int i = 0; i < options->num_parameter_blocks; ++i) {
|
|
cf->AddParameterBlock(options->parameter_block_sizes[i]);
|
|
}
|
|
cost_function = cf;
|
|
break;
|
|
}
|
|
}
|
|
|
|
return cost_function;
|
|
}
|
|
|
|
void* ceres_wrapper_create_dynamic_numeric_diff_cost_function(
|
|
const ceres_numeric_diff_options_t* options,
|
|
ceres_numeric_diff_method_t method) {
|
|
// Same implementation - DynamicNumericDiffCostFunction already handles dynamic sizes
|
|
return ceres_wrapper_create_numeric_diff_cost_function(options, method);
|
|
}
|
|
|
|
void ceres_wrapper_free_numeric_diff_cost_function(void* cost_function) {
|
|
if (cost_function) {
|
|
// Delete the CostFunction (base class)
|
|
// Note: The functor is owned by the cost function, so it will be deleted automatically
|
|
delete reinterpret_cast<ceres::CostFunction*>(cost_function);
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Additional SolverOptions (Phase 2)
|
|
// ============================================================================
|
|
|
|
void ceres_wrapper_solver_options_set_line_search_type(
|
|
ceres_solver_options_t* options, int line_search_type) {
|
|
if (options) {
|
|
options->options.line_search_type = static_cast<ceres::LineSearchType>(line_search_type);
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_line_search_type(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? static_cast<int>(options->options.line_search_type) : 0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_line_search_direction_type(
|
|
ceres_solver_options_t* options, int line_search_direction_type) {
|
|
if (options) {
|
|
options->options.line_search_direction_type =
|
|
static_cast<ceres::LineSearchDirectionType>(line_search_direction_type);
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_line_search_direction_type(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? static_cast<int>(options->options.line_search_direction_type) : 0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_nonlinear_conjugate_gradient_type(
|
|
ceres_solver_options_t* options, int ncg_type) {
|
|
if (options) {
|
|
options->options.nonlinear_conjugate_gradient_type =
|
|
static_cast<ceres::NonlinearConjugateGradientType>(ncg_type);
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_nonlinear_conjugate_gradient_type(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? static_cast<int>(options->options.nonlinear_conjugate_gradient_type) : 0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_max_lbfgs_rank(
|
|
ceres_solver_options_t* options, int max_lbfgs_rank) {
|
|
if (options) {
|
|
options->options.max_lbfgs_rank = max_lbfgs_rank;
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_max_lbfgs_rank(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? options->options.max_lbfgs_rank : 0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_max_line_search_step_contraction(
|
|
ceres_solver_options_t* options, double max_step_contraction) {
|
|
if (options) {
|
|
options->options.max_line_search_step_contraction = max_step_contraction;
|
|
}
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_max_line_search_step_contraction(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? options->options.max_line_search_step_contraction : 0.0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_min_line_search_step_contraction(
|
|
ceres_solver_options_t* options, double min_step_contraction) {
|
|
if (options) {
|
|
options->options.min_line_search_step_contraction = min_step_contraction;
|
|
}
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_min_line_search_step_contraction(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? options->options.min_line_search_step_contraction : 0.0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_max_num_line_search_step_size_iterations(
|
|
ceres_solver_options_t* options, int max_iterations) {
|
|
if (options) {
|
|
options->options.max_num_line_search_step_size_iterations = max_iterations;
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_max_num_line_search_step_size_iterations(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? options->options.max_num_line_search_step_size_iterations : 0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_max_num_line_search_direction_restarts(
|
|
ceres_solver_options_t* options, int max_restarts) {
|
|
if (options) {
|
|
options->options.max_num_line_search_direction_restarts = max_restarts;
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_max_num_line_search_direction_restarts(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? options->options.max_num_line_search_direction_restarts : 0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_line_search_sufficient_function_decrease(
|
|
ceres_solver_options_t* options, double sufficient_decrease) {
|
|
if (options) {
|
|
options->options.line_search_sufficient_function_decrease = sufficient_decrease;
|
|
}
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_line_search_sufficient_function_decrease(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? options->options.line_search_sufficient_function_decrease : 0.0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_line_search_sufficient_curvature_decrease(
|
|
ceres_solver_options_t* options, double sufficient_curvature_decrease) {
|
|
if (options) {
|
|
options->options.line_search_sufficient_curvature_decrease = sufficient_curvature_decrease;
|
|
}
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_line_search_sufficient_curvature_decrease(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? options->options.line_search_sufficient_curvature_decrease : 0.0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_max_line_search_step_expansion(
|
|
ceres_solver_options_t* options, double max_step_expansion) {
|
|
if (options) {
|
|
options->options.max_line_search_step_expansion = max_step_expansion;
|
|
}
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_max_line_search_step_expansion(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? options->options.max_line_search_step_expansion : 0.0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_max_lm_diagonal(
|
|
ceres_solver_options_t* options, double max_lm_diagonal) {
|
|
if (options) {
|
|
options->options.max_lm_diagonal = max_lm_diagonal;
|
|
}
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_max_lm_diagonal(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? options->options.max_lm_diagonal : 0.0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_min_lm_diagonal(
|
|
ceres_solver_options_t* options, double min_lm_diagonal) {
|
|
if (options) {
|
|
options->options.min_lm_diagonal = min_lm_diagonal;
|
|
}
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_min_lm_diagonal(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? options->options.min_lm_diagonal : 0.0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_sparse_linear_algebra_library_type(
|
|
ceres_solver_options_t* options, int sparse_library_type) {
|
|
if (options) {
|
|
options->options.sparse_linear_algebra_library_type =
|
|
static_cast<ceres::SparseLinearAlgebraLibraryType>(sparse_library_type);
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_sparse_linear_algebra_library_type(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? static_cast<int>(options->options.sparse_linear_algebra_library_type) : 0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_dense_linear_algebra_library_type(
|
|
ceres_solver_options_t* options, int dense_library_type) {
|
|
if (options) {
|
|
options->options.dense_linear_algebra_library_type =
|
|
static_cast<ceres::DenseLinearAlgebraLibraryType>(dense_library_type);
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_dense_linear_algebra_library_type(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? static_cast<int>(options->options.dense_linear_algebra_library_type) : 0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_max_linear_solver_iterations(
|
|
ceres_solver_options_t* options, int max_iterations) {
|
|
if (options) {
|
|
options->options.max_linear_solver_iterations = max_iterations;
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_max_linear_solver_iterations(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? options->options.max_linear_solver_iterations : 0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_min_linear_solver_iterations(
|
|
ceres_solver_options_t* options, int min_iterations) {
|
|
if (options) {
|
|
options->options.min_linear_solver_iterations = min_iterations;
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_min_linear_solver_iterations(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? options->options.min_linear_solver_iterations : 0;
|
|
}
|
|
|
|
// Note: linear_solver_tolerance doesn't exist in Ceres Solver::Options
|
|
// This is a placeholder - remove or comment out if not available
|
|
void ceres_wrapper_solver_options_set_linear_solver_tolerance(
|
|
ceres_solver_options_t* options, double tolerance) {
|
|
// This option doesn't exist in Ceres - no-op
|
|
(void)options;
|
|
(void)tolerance;
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_linear_solver_tolerance(
|
|
const ceres_solver_options_t* options) {
|
|
// This option doesn't exist in Ceres - return 0
|
|
(void)options;
|
|
return 0.0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_use_inner_iterations(
|
|
ceres_solver_options_t* options, int use_inner_iterations) {
|
|
if (options) {
|
|
options->options.use_inner_iterations = (use_inner_iterations != 0);
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_use_inner_iterations(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? (options->options.use_inner_iterations ? 1 : 0) : 0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_inner_iteration_tolerance(
|
|
ceres_solver_options_t* options, double tolerance) {
|
|
if (options) {
|
|
options->options.inner_iteration_tolerance = tolerance;
|
|
}
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_inner_iteration_tolerance(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? options->options.inner_iteration_tolerance : 0.0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_max_solver_time_in_seconds(
|
|
ceres_solver_options_t* options, double max_time) {
|
|
if (options) {
|
|
options->options.max_solver_time_in_seconds = max_time;
|
|
}
|
|
}
|
|
|
|
double ceres_wrapper_solver_options_get_max_solver_time_in_seconds(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? options->options.max_solver_time_in_seconds : 0.0;
|
|
}
|
|
|
|
void ceres_wrapper_solver_options_set_update_state_every_iteration(
|
|
ceres_solver_options_t* options, int update_every_iteration) {
|
|
if (options) {
|
|
options->options.update_state_every_iteration = (update_every_iteration != 0);
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_solver_options_get_update_state_every_iteration(
|
|
const ceres_solver_options_t* options) {
|
|
return options ? (options->options.update_state_every_iteration ? 1 : 0) : 0;
|
|
}
|
|
|
|
// ============================================================================
|
|
// Problem Options (Phase 2)
|
|
// ============================================================================
|
|
|
|
struct ceres_problem_options_s {
|
|
ceres::Problem::Options options;
|
|
};
|
|
|
|
// Evaluation callback wrapper class
|
|
// Note: We need to store the wrapper to keep it alive
|
|
// We'll use a map to track callbacks per options object
|
|
// Thread-safe: protected by mutex
|
|
static std::mutex evaluation_callbacks_mutex;
|
|
static std::map<ceres_problem_options_t*, std::unique_ptr<ceres::EvaluationCallback>> evaluation_callbacks;
|
|
|
|
class EvaluationCallbackWrapper : public ceres::EvaluationCallback {
|
|
public:
|
|
EvaluationCallbackWrapper(
|
|
ceres_evaluation_callback_t callback,
|
|
void* user_data)
|
|
: callback_(callback), user_data_(user_data) {}
|
|
|
|
void PrepareForEvaluation(bool evaluate_jacobians,
|
|
bool new_evaluation_point) override {
|
|
(void)evaluate_jacobians;
|
|
(void)new_evaluation_point;
|
|
// Validate callback and user_data_ to prevent use-after-free
|
|
if (callback_ && user_data_) {
|
|
// Note: We don't have num_residuals and parameter_block_sizes here
|
|
// This is a limitation - we'll call with dummy values
|
|
// In practice, the callback should be set up with proper context
|
|
callback_(user_data_, 0, 0, nullptr);
|
|
}
|
|
}
|
|
|
|
private:
|
|
ceres_evaluation_callback_t callback_;
|
|
void* user_data_;
|
|
};
|
|
|
|
ceres_problem_options_t* ceres_wrapper_create_problem_options() {
|
|
return new ceres_problem_options_t;
|
|
}
|
|
|
|
void ceres_wrapper_free_problem_options(ceres_problem_options_t* options) {
|
|
// Remove callback from map if it exists (thread-safe)
|
|
{
|
|
std::lock_guard<std::mutex> lock(evaluation_callbacks_mutex);
|
|
evaluation_callbacks.erase(options);
|
|
}
|
|
delete options;
|
|
}
|
|
|
|
void ceres_wrapper_problem_options_set_cost_function_ownership(
|
|
ceres_problem_options_t* options, int ownership) {
|
|
if (options) {
|
|
options->options.cost_function_ownership =
|
|
static_cast<ceres::Ownership>(ownership);
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_problem_options_get_cost_function_ownership(
|
|
const ceres_problem_options_t* options) {
|
|
return options ? static_cast<int>(options->options.cost_function_ownership) : 0;
|
|
}
|
|
|
|
void ceres_wrapper_problem_options_set_loss_function_ownership(
|
|
ceres_problem_options_t* options, int ownership) {
|
|
if (options) {
|
|
options->options.loss_function_ownership =
|
|
static_cast<ceres::Ownership>(ownership);
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_problem_options_get_loss_function_ownership(
|
|
const ceres_problem_options_t* options) {
|
|
return options ? static_cast<int>(options->options.loss_function_ownership) : 0;
|
|
}
|
|
|
|
void ceres_wrapper_problem_options_set_manifold_ownership(
|
|
ceres_problem_options_t* options, int ownership) {
|
|
if (options) {
|
|
options->options.manifold_ownership =
|
|
static_cast<ceres::Ownership>(ownership);
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_problem_options_get_manifold_ownership(
|
|
const ceres_problem_options_t* options) {
|
|
return options ? static_cast<int>(options->options.manifold_ownership) : 0;
|
|
}
|
|
|
|
void ceres_wrapper_problem_options_set_enable_fast_removal(
|
|
ceres_problem_options_t* options, int enable) {
|
|
if (options) {
|
|
options->options.enable_fast_removal = (enable != 0);
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_problem_options_get_enable_fast_removal(
|
|
const ceres_problem_options_t* options) {
|
|
return options ? (options->options.enable_fast_removal ? 1 : 0) : 0;
|
|
}
|
|
|
|
void ceres_wrapper_problem_options_set_disable_all_safety_checks(
|
|
ceres_problem_options_t* options, int disable) {
|
|
if (options) {
|
|
options->options.disable_all_safety_checks = (disable != 0);
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_problem_options_get_disable_all_safety_checks(
|
|
const ceres_problem_options_t* options) {
|
|
return options ? (options->options.disable_all_safety_checks ? 1 : 0) : 0;
|
|
}
|
|
|
|
void ceres_wrapper_problem_options_set_evaluation_callback(
|
|
ceres_problem_options_t* options,
|
|
ceres_evaluation_callback_t callback,
|
|
void* user_data) {
|
|
if (options) {
|
|
std::lock_guard<std::mutex> lock(evaluation_callbacks_mutex);
|
|
if (callback) {
|
|
// Create wrapper and store it in the map (thread-safe)
|
|
evaluation_callbacks[options] = std::make_unique<EvaluationCallbackWrapper>(callback, user_data);
|
|
options->options.evaluation_callback = evaluation_callbacks[options].get();
|
|
} else {
|
|
evaluation_callbacks.erase(options);
|
|
options->options.evaluation_callback = nullptr;
|
|
}
|
|
}
|
|
}
|
|
|
|
ceres_problem_t* ceres_wrapper_create_problem_with_options(
|
|
const ceres_problem_options_t* options) {
|
|
if (!options) {
|
|
return reinterpret_cast<ceres_problem_t*>(new ceres::Problem);
|
|
}
|
|
return reinterpret_cast<ceres_problem_t*>(new ceres::Problem(options->options));
|
|
}
|
|
|
|
// ============================================================================
|
|
// Parameter Bounds (Phase 2)
|
|
// ============================================================================
|
|
|
|
void ceres_wrapper_problem_set_parameter_lower_bound(
|
|
ceres_problem_t* problem,
|
|
double* parameters,
|
|
int index,
|
|
double lower_bound) {
|
|
if (problem) {
|
|
reinterpret_cast<ceres::Problem*>(problem)->SetParameterLowerBound(parameters, index, lower_bound);
|
|
}
|
|
}
|
|
|
|
void ceres_wrapper_problem_set_parameter_upper_bound(
|
|
ceres_problem_t* problem,
|
|
double* parameters,
|
|
int index,
|
|
double upper_bound) {
|
|
if (problem) {
|
|
reinterpret_cast<ceres::Problem*>(problem)->SetParameterUpperBound(parameters, index, upper_bound);
|
|
}
|
|
}
|
|
|
|
double ceres_wrapper_problem_get_parameter_lower_bound(
|
|
const ceres_problem_t* problem,
|
|
const double* parameters,
|
|
int index) {
|
|
if (!problem) {
|
|
return -std::numeric_limits<double>::infinity();
|
|
}
|
|
return reinterpret_cast<const ceres::Problem*>(problem)->GetParameterLowerBound(parameters, index);
|
|
}
|
|
|
|
double ceres_wrapper_problem_get_parameter_upper_bound(
|
|
const ceres_problem_t* problem,
|
|
const double* parameters,
|
|
int index) {
|
|
if (!problem) {
|
|
return std::numeric_limits<double>::infinity();
|
|
}
|
|
return reinterpret_cast<const ceres::Problem*>(problem)->GetParameterUpperBound(parameters, index);
|
|
}
|
|
|
|
// ============================================================================
|
|
// Additional SolverSummary Fields (Phase 2)
|
|
// ============================================================================
|
|
// Note: Timing functions are already defined above (lines 355-378)
|
|
// Only add new functions that don't exist yet
|
|
|
|
int ceres_wrapper_solver_summary_get_num_parameter_blocks(
|
|
const ceres_solver_summary_t* summary) {
|
|
return summary ? summary->summary.num_parameter_blocks : 0;
|
|
}
|
|
|
|
int ceres_wrapper_solver_summary_get_num_parameters(
|
|
const ceres_solver_summary_t* summary) {
|
|
return summary ? summary->summary.num_parameters : 0;
|
|
}
|
|
|
|
int ceres_wrapper_solver_summary_get_num_effective_parameters(
|
|
const ceres_solver_summary_t* summary) {
|
|
return summary ? summary->summary.num_effective_parameters : 0;
|
|
}
|
|
|
|
int ceres_wrapper_solver_summary_get_num_residual_blocks(
|
|
const ceres_solver_summary_t* summary) {
|
|
return summary ? summary->summary.num_residual_blocks : 0;
|
|
}
|
|
|
|
int ceres_wrapper_solver_summary_get_num_residuals(
|
|
const ceres_solver_summary_t* summary) {
|
|
return summary ? summary->summary.num_residuals : 0;
|
|
}
|
|
|
|
double ceres_wrapper_solver_summary_get_cost_change(
|
|
const ceres_solver_summary_t* summary) {
|
|
if (!summary) {
|
|
return 0.0;
|
|
}
|
|
return summary->summary.initial_cost - summary->summary.final_cost;
|
|
}
|
|
|
|
// ============================================================================
|
|
// Covariance Estimation (Phase 3 - HIGH Priority)
|
|
// ============================================================================
|
|
|
|
struct ceres_covariance_options_s {
|
|
ceres::Covariance::Options options;
|
|
};
|
|
|
|
ceres_covariance_options_t* ceres_wrapper_create_covariance_options() {
|
|
return new ceres_covariance_options_t;
|
|
}
|
|
|
|
void ceres_wrapper_free_covariance_options(ceres_covariance_options_t* options) {
|
|
delete options;
|
|
}
|
|
|
|
void ceres_wrapper_covariance_options_set_num_threads(
|
|
ceres_covariance_options_t* options, int num_threads) {
|
|
if (options) {
|
|
options->options.num_threads = num_threads;
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_covariance_options_get_num_threads(
|
|
const ceres_covariance_options_t* options) {
|
|
return options ? options->options.num_threads : 0;
|
|
}
|
|
|
|
void ceres_wrapper_covariance_options_set_sparse_linear_algebra_library_type(
|
|
ceres_covariance_options_t* options, int library_type) {
|
|
if (options) {
|
|
options->options.sparse_linear_algebra_library_type =
|
|
static_cast<ceres::SparseLinearAlgebraLibraryType>(library_type);
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_covariance_options_get_sparse_linear_algebra_library_type(
|
|
const ceres_covariance_options_t* options) {
|
|
return options ? static_cast<int>(options->options.sparse_linear_algebra_library_type) : 0;
|
|
}
|
|
|
|
void ceres_wrapper_covariance_options_set_algorithm_type(
|
|
ceres_covariance_options_t* options, int algorithm_type) {
|
|
if (options) {
|
|
options->options.algorithm_type =
|
|
static_cast<ceres::CovarianceAlgorithmType>(algorithm_type);
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_covariance_options_get_algorithm_type(
|
|
const ceres_covariance_options_t* options) {
|
|
return options ? static_cast<int>(options->options.algorithm_type) : 0;
|
|
}
|
|
|
|
void ceres_wrapper_covariance_options_set_min_reciprocal_condition_number(
|
|
ceres_covariance_options_t* options, double min_reciprocal_condition_number) {
|
|
if (options) {
|
|
options->options.min_reciprocal_condition_number = min_reciprocal_condition_number;
|
|
}
|
|
}
|
|
|
|
double ceres_wrapper_covariance_options_get_min_reciprocal_condition_number(
|
|
const ceres_covariance_options_t* options) {
|
|
return options ? options->options.min_reciprocal_condition_number : 0.0;
|
|
}
|
|
|
|
void ceres_wrapper_covariance_options_set_null_space_rank(
|
|
ceres_covariance_options_t* options, int null_space_rank) {
|
|
if (options) {
|
|
options->options.null_space_rank = null_space_rank;
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_covariance_options_get_null_space_rank(
|
|
const ceres_covariance_options_t* options) {
|
|
return options ? options->options.null_space_rank : 0;
|
|
}
|
|
|
|
void ceres_wrapper_covariance_options_set_apply_loss_function(
|
|
ceres_covariance_options_t* options, int apply_loss_function) {
|
|
if (options) {
|
|
options->options.apply_loss_function = (apply_loss_function != 0);
|
|
}
|
|
}
|
|
|
|
int ceres_wrapper_covariance_options_get_apply_loss_function(
|
|
const ceres_covariance_options_t* options) {
|
|
return options ? (options->options.apply_loss_function ? 1 : 0) : 0;
|
|
}
|
|
|
|
struct ceres_covariance_s {
|
|
std::unique_ptr<ceres::Covariance> covariance;
|
|
|
|
// Constructor with default options
|
|
ceres_covariance_s() : covariance(std::make_unique<ceres::Covariance>(ceres::Covariance::Options())) {}
|
|
|
|
// Constructor with custom options
|
|
ceres_covariance_s(const ceres::Covariance::Options& options) : covariance(std::make_unique<ceres::Covariance>(options)) {}
|
|
};
|
|
|
|
ceres_covariance_t* ceres_wrapper_create_covariance() {
|
|
return new ceres_covariance_t;
|
|
}
|
|
|
|
// Create covariance with options
|
|
ceres_covariance_t* ceres_wrapper_create_covariance_with_options(
|
|
const ceres_covariance_options_t* options) {
|
|
if (!options) {
|
|
return new ceres_covariance_t;
|
|
}
|
|
return new ceres_covariance_t(options->options);
|
|
}
|
|
|
|
void ceres_wrapper_free_covariance(ceres_covariance_t* covariance) {
|
|
delete covariance;
|
|
}
|
|
|
|
ceres_wrapper_error_code_t ceres_wrapper_covariance_compute(
|
|
ceres_covariance_t* covariance,
|
|
const ceres_problem_t* problem,
|
|
const ceres_covariance_options_t* options,
|
|
const double** parameter_blocks,
|
|
int num_parameter_blocks,
|
|
char* error_message,
|
|
int error_message_size) {
|
|
if (!covariance) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "covariance is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (!problem) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "problem is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (!parameter_blocks) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "parameter_blocks is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (num_parameter_blocks <= 0) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "num_parameter_blocks must be positive", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_INVALID_PARAMETER;
|
|
}
|
|
|
|
try {
|
|
// Covariance::Compute requires non-const Problem*, so we need to cast
|
|
ceres::Problem* ceres_problem = const_cast<ceres::Problem*>(
|
|
reinterpret_cast<const ceres::Problem*>(problem));
|
|
std::vector<const double*> param_blocks(parameter_blocks, parameter_blocks + num_parameter_blocks);
|
|
|
|
// Note: Options are set when creating covariance object
|
|
// If different options are needed, user should create a new covariance object
|
|
(void)options; // Options parameter kept for API compatibility but not used here
|
|
|
|
bool success = covariance->covariance->Compute(param_blocks, ceres_problem);
|
|
|
|
if (!success) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "Covariance computation failed", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_INVALID_OPERATION;
|
|
}
|
|
|
|
return CERES_WRAPPER_SUCCESS;
|
|
} catch (const std::exception& e) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, e.what(), error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
} catch (...) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "Unknown exception during covariance computation", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
}
|
|
}
|
|
|
|
ceres_wrapper_error_code_t ceres_wrapper_covariance_get_covariance_block(
|
|
const ceres_covariance_t* covariance,
|
|
const double* parameter_block1,
|
|
const double* parameter_block2,
|
|
double* covariance_block,
|
|
char* error_message,
|
|
int error_message_size) {
|
|
if (!covariance) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "covariance is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (!parameter_block1) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "parameter_block1 is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (!parameter_block2) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "parameter_block2 is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (!covariance_block) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "covariance_block is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
|
|
try {
|
|
// GetCovarianceBlock requires the covariance_block array to be pre-allocated
|
|
// with size = block1_size * block2_size
|
|
// Caller is responsible for providing correctly sized array
|
|
bool success = covariance->covariance->GetCovarianceBlock(
|
|
parameter_block1, parameter_block2, covariance_block);
|
|
|
|
if (!success) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "Failed to get covariance block", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NOT_FOUND;
|
|
}
|
|
|
|
return CERES_WRAPPER_SUCCESS;
|
|
} catch (const std::exception& e) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, e.what(), error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
} catch (...) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "Unknown exception", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
}
|
|
}
|
|
|
|
ceres_wrapper_error_code_t ceres_wrapper_covariance_get_covariance_matrix(
|
|
const ceres_covariance_t* covariance,
|
|
const double** parameter_blocks,
|
|
int num_parameter_blocks,
|
|
double* covariance_matrix,
|
|
char* error_message,
|
|
int error_message_size) {
|
|
if (!covariance) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "covariance is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (!parameter_blocks) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "parameter_blocks is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (!covariance_matrix) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "covariance_matrix is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (num_parameter_blocks <= 0) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "num_parameter_blocks must be positive", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_INVALID_PARAMETER;
|
|
}
|
|
|
|
try {
|
|
std::vector<const double*> param_blocks(parameter_blocks, parameter_blocks + num_parameter_blocks);
|
|
|
|
bool success = covariance->covariance->GetCovarianceMatrix(
|
|
param_blocks, covariance_matrix);
|
|
|
|
if (!success) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "Failed to get covariance matrix", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_INVALID_OPERATION;
|
|
}
|
|
|
|
return CERES_WRAPPER_SUCCESS;
|
|
} catch (const std::exception& e) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, e.what(), error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
} catch (...) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "Unknown exception", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Gradient Checker (Phase 3 - MEDIUM Priority)
|
|
// ============================================================================
|
|
|
|
struct ceres_gradient_checker_options_s {
|
|
ceres::NumericDiffOptions numeric_diff_options;
|
|
double gradient_check_relative_precision; // Stored separately, used in Probe()
|
|
};
|
|
|
|
ceres_gradient_checker_options_t* ceres_wrapper_create_gradient_checker_options() {
|
|
auto* opts = new ceres_gradient_checker_options_s;
|
|
opts->gradient_check_relative_precision = 1e-4; // Default value
|
|
return opts;
|
|
}
|
|
|
|
void ceres_wrapper_free_gradient_checker_options(ceres_gradient_checker_options_t* options) {
|
|
delete options;
|
|
}
|
|
|
|
void ceres_wrapper_gradient_checker_options_set_gradient_check_relative_precision(
|
|
ceres_gradient_checker_options_t* options, double precision) {
|
|
if (options) {
|
|
options->gradient_check_relative_precision = precision;
|
|
}
|
|
}
|
|
|
|
double ceres_wrapper_gradient_checker_options_get_gradient_check_relative_precision(
|
|
const ceres_gradient_checker_options_t* options) {
|
|
if (!options) return 0.0;
|
|
return options->gradient_check_relative_precision;
|
|
}
|
|
|
|
void ceres_wrapper_gradient_checker_options_set_gradient_check_numeric_derivative_relative_step_size(
|
|
ceres_gradient_checker_options_t* options, double step_size) {
|
|
if (options) {
|
|
options->numeric_diff_options.relative_step_size = step_size;
|
|
}
|
|
}
|
|
|
|
double ceres_wrapper_gradient_checker_options_get_gradient_check_numeric_derivative_relative_step_size(
|
|
const ceres_gradient_checker_options_t* options) {
|
|
if (!options) return 0.0;
|
|
return options->numeric_diff_options.relative_step_size;
|
|
}
|
|
|
|
struct ceres_gradient_checker_s {
|
|
std::unique_ptr<ceres::GradientChecker> checker;
|
|
};
|
|
|
|
ceres_gradient_checker_t* ceres_wrapper_create_gradient_checker(
|
|
void* cost_function,
|
|
const ceres_manifold_t** manifolds,
|
|
int num_manifolds,
|
|
const ceres_gradient_checker_options_t* options) {
|
|
if (!cost_function) {
|
|
return nullptr;
|
|
}
|
|
|
|
try {
|
|
ceres::CostFunction* ceres_cost_function = reinterpret_cast<ceres::CostFunction*>(cost_function);
|
|
|
|
std::vector<const ceres::Manifold*> ceres_manifolds;
|
|
if (manifolds && num_manifolds > 0) {
|
|
for (int i = 0; i < num_manifolds; ++i) {
|
|
if (manifolds[i]) {
|
|
ceres_manifolds.push_back(manifolds[i]->manifold.get());
|
|
}
|
|
}
|
|
}
|
|
|
|
ceres::NumericDiffOptions numeric_diff_options;
|
|
if (options) {
|
|
numeric_diff_options = options->numeric_diff_options;
|
|
}
|
|
|
|
// GradientChecker constructor takes pointer to vector or nullptr
|
|
const std::vector<const ceres::Manifold*>* manifolds_ptr =
|
|
ceres_manifolds.empty() ? nullptr : &ceres_manifolds;
|
|
|
|
auto* checker = new ceres_gradient_checker_s;
|
|
checker->checker = std::make_unique<ceres::GradientChecker>(
|
|
ceres_cost_function, manifolds_ptr, numeric_diff_options);
|
|
|
|
return checker;
|
|
} catch (...) {
|
|
return nullptr;
|
|
}
|
|
}
|
|
|
|
void ceres_wrapper_free_gradient_checker(ceres_gradient_checker_t* checker) {
|
|
delete checker;
|
|
}
|
|
|
|
ceres_wrapper_error_code_t ceres_wrapper_gradient_checker_probe(
|
|
const ceres_gradient_checker_t* checker,
|
|
const double* const* parameters,
|
|
double relative_precision,
|
|
char* error_message,
|
|
int error_message_size) {
|
|
if (!checker) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "checker is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
if (!parameters) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "parameters is null", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_NULL_POINTER;
|
|
}
|
|
|
|
try {
|
|
ceres::GradientChecker::ProbeResults results;
|
|
bool success = checker->checker->Probe(parameters, relative_precision, &results);
|
|
|
|
if (error_message && error_message_size > 0) {
|
|
if (!results.error_log.empty()) {
|
|
int copy_size = std::min(static_cast<int>(results.error_log.length()), error_message_size - 1);
|
|
std::strncpy(error_message, results.error_log.c_str(), copy_size);
|
|
error_message[copy_size] = '\0';
|
|
} else if (success) {
|
|
error_message[0] = '\0';
|
|
} else {
|
|
std::strncpy(error_message, "Gradient check failed", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
}
|
|
|
|
if (!success) {
|
|
return CERES_WRAPPER_ERROR_INVALID_PARAMETER; // Gradients don't match
|
|
}
|
|
|
|
return CERES_WRAPPER_SUCCESS;
|
|
} catch (const std::exception& e) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, e.what(), error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
} catch (...) {
|
|
if (error_message && error_message_size > 0) {
|
|
std::strncpy(error_message, "Unknown exception during gradient check", error_message_size - 1);
|
|
error_message[error_message_size - 1] = '\0';
|
|
}
|
|
return CERES_WRAPPER_ERROR_EXCEPTION;
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Context (Phase 3 - MEDIUM Priority)
|
|
// ============================================================================
|
|
|
|
struct ceres_context_s {
|
|
ceres::Context* context;
|
|
|
|
ceres_context_s() : context(ceres::Context::Create()) {}
|
|
|
|
~ceres_context_s() {
|
|
delete context;
|
|
}
|
|
};
|
|
|
|
ceres_context_t* ceres_wrapper_create_context() {
|
|
return new ceres_context_s;
|
|
}
|
|
|
|
void ceres_wrapper_free_context(ceres_context_t* context) {
|
|
delete context;
|
|
}
|
|
|
|
void ceres_wrapper_problem_options_set_context(
|
|
ceres_problem_options_t* options,
|
|
ceres_context_t* context) {
|
|
if (options && context) {
|
|
// Context::Create() returns ContextImpl* which is the correct type
|
|
options->options.context = context->context;
|
|
}
|
|
}
|
|
|
|
// ============================================================================
|
|
// Cubic Interpolator (1D) (Phase 3 - MEDIUM Priority)
|
|
// ============================================================================
|
|
|
|
// Adapter for 1D array data to work with Ceres CubicInterpolator
|
|
class Array1DAdapter {
|
|
public:
|
|
enum { DATA_DIMENSION = 1 };
|
|
|
|
Array1DAdapter(const double* data, int size)
|
|
: data_(data), size_(size) {}
|
|
|
|
void GetValue(const int index, double* const value) const {
|
|
if (index < 0 || index >= size_) {
|
|
*value = 0.0; // Out of bounds - return default value
|
|
} else {
|
|
*value = data_[index];
|
|
}
|
|
}
|
|
|
|
int NumValues() const { return size_; }
|
|
|
|
private:
|
|
const double* data_;
|
|
int size_;
|
|
};
|
|
|
|
struct ceres_cubic_interpolator_s {
|
|
std::unique_ptr<ceres::CubicInterpolator<Array1DAdapter>> interpolator;
|
|
std::unique_ptr<Array1DAdapter> adapter;
|
|
std::vector<double> data; // Keep data alive
|
|
};
|
|
|
|
ceres_cubic_interpolator_t* ceres_wrapper_create_cubic_interpolator(
|
|
const double* data,
|
|
int num_values) {
|
|
if (!data || num_values <= 0) {
|
|
return nullptr;
|
|
}
|
|
|
|
auto* interp = new ceres_cubic_interpolator_t;
|
|
|
|
// Copy data to keep it alive
|
|
interp->data.assign(data, data + num_values);
|
|
|
|
// Create adapter with copied data
|
|
interp->adapter = std::make_unique<Array1DAdapter>(
|
|
interp->data.data(), num_values);
|
|
|
|
// Create interpolator with adapter
|
|
interp->interpolator = std::make_unique<ceres::CubicInterpolator<Array1DAdapter>>(
|
|
*interp->adapter);
|
|
|
|
return interp;
|
|
}
|
|
|
|
void ceres_wrapper_free_cubic_interpolator(
|
|
ceres_cubic_interpolator_t* interpolator) {
|
|
delete interpolator;
|
|
}
|
|
|
|
void ceres_wrapper_cubic_interpolator_evaluate(
|
|
const ceres_cubic_interpolator_t* interpolator,
|
|
double x,
|
|
double* value,
|
|
double* gradient) {
|
|
if (!interpolator) {
|
|
if (value) *value = 0.0;
|
|
if (gradient) *gradient = 0.0;
|
|
return;
|
|
}
|
|
|
|
double val = 0.0;
|
|
double grad = 0.0;
|
|
|
|
// Evaluate at index x (clamped to valid range)
|
|
double clamped_x = std::max(0.0, std::min(x, static_cast<double>(interpolator->adapter->NumValues() - 1)));
|
|
interpolator->interpolator->Evaluate(clamped_x, &val, &grad);
|
|
|
|
if (value) *value = val;
|
|
if (gradient) *gradient = grad;
|
|
}
|
|
|