C++ 带约束的Ceres形状拟合

对Ceres早有耳闻，现结合一个具体需求对其进行学习。比如我需要通过一个近似圆的点集拟合一个最佳圆，除此之外，我还要拟合最小圆和最大圆，要求它们的平均偏差、最大偏差、最小偏差分别为0。完整代码在文末。

一、Ceres Solver

Ceres Solver 是一个由 Google 开发的开源库，专门用于解决非线性最小二乘问题。它被广泛应用于机器人学、计算机视觉和其他需要精确数据拟合的应用中。Ceres 提供了一种灵活的方式来定义目标函数，并允许用户指定约束条件，从而使得复杂的优化问题可以被高效地求解。

SLAM领域应该常用这个

1.定义问题

首先要定义一个需要优化的问题：

ceres::Problem problem;

定义了一个ceres::Problem对象后，在使用 Ceres 进行拟合之前，还需要：

成本函数（Cost Function）：描述了观测值与模型预测值之间的差异。
参数块（Parameter Block）：即你要优化的变量。
损失函数（Loss Function）：可选的，用于减少残差中的大误差对结果的影响。
局部参数化（Local Parameterization）：用于非欧几里得空间中的参数优化。
约束（Constraints）：可以为参数块添加额外的限制。

---

2. 添加残差

通过调用 problem.AddResidualBlock() 方法来添加成本项到问题中。每个成本项通常对应一个观测或测量(所以通常需要循环构建)。

AddResidualBlock

AddResidualBlock是ceres::Problem类的一个成员函数，它允许你将一个残差项（即误差项）添加到优化问题中。这个残差块定义了模型与观测数据之间的差异度量，是构成整个非线性最小二乘问题的基础单元。

  ResidualBlockId AddResidualBlock(CostFunction* cost_function,LossFunction* loss_function,const std::vector<double*>& parameter_blocks);

• CostFunction

这是一个指向用户定义的代价函数（残差函数）的指针。Ceres 使用这个函数来计算残差值，最终目标是使残差最小化。用户可以通过继承 Ceres 的 CostFunction 类，或者使用 Ceres 提供的自动求导工具（如 ceres::AutoDiffCostFunction）来定义自己的残差函数。

• LossFunction

这是一个指向损失函数的指针。损失函数用于对残差进行处理，特别是在鲁棒估计中，它可以帮助减少异常值的影响。如果为 nullptr，Ceres 将使用二次损失函数，即直接最小化残差的平方和。
常用的损失函数包括 Huber 损失、Cauchy 损失等，可以通过实例化 ceres::HuberLoss、ceres::CauchyLoss 等来使用。

• std::vector parameter_blocks

参数块是指向优化参数的指针数组。每个参数块对应了残差函数所需的参数。double* 类型表示每个参数块的起始地址。Ceres 允许优化多个参数块，因此你可以传入多个参数的指针。
每个参数块可以是一个标量，也可以是一个向量（例如 3D 空间中的点）。Ceres 会在优化过程中修改这些参数，使得残差函数的值尽可能地小。

AutoDiffCostFunction

AutoDiffCostFunction 是 Google Ceres Solver 中一个重要的类，它通过自动求导来简化用户定义残差函数时的梯度计算。这个类可以将用户定义的代价函数（残差计算函数）自动地转换为可以用于优化问题的 CostFunction，并自动计算其导数。

template <typename CostFunctor,int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.int N0,       // Number of parameters in block 0.int N1 = 0,   // Number of parameters in block 1.int N2 = 0,   // Number of parameters in block 2.int N3 = 0,   // Number of parameters in block 3.int N4 = 0,   // Number of parameters in block 4.int N5 = 0,   // Number of parameters in block 5.int N6 = 0,   // Number of parameters in block 6.int N7 = 0,   // Number of parameters in block 7.int N8 = 0,   // Number of parameters in block 8.int N9 = 0>   // Number of parameters in block 9.

• CostFunctor
• 用户定义的残差计算函数类。
• kNumResiduals
• 残差的数量（标量或向量的维度），即目标函数输出的维数。
• N0, N1, N2, N3…
• 各参数块的维度，N0 是第一个参数块的维度，N1 是第二个参数块的维度，依此类推。

---

3. 配置求解器

使用 ceres::Solver::Options 来配置求解器的选项，如线性求解器、非线性最小化策略、收敛标准等。

// 配置求解器ceres::Solver::Options options;options.linear_solver_type = ceres::DENSE_QR;options.minimizer_progress_to_stdout = false;

Options参数相当之多

Options() {minimizer_type = TRUST_REGION;line_search_direction_type = LBFGS;line_search_type = WOLFE;nonlinear_conjugate_gradient_type = FLETCHER_REEVES;max_lbfgs_rank = 20;use_approximate_eigenvalue_bfgs_scaling = false;line_search_interpolation_type = CUBIC;min_line_search_step_size = 1e-9;line_search_sufficient_function_decrease = 1e-4;max_line_search_step_contraction = 1e-3;min_line_search_step_contraction = 0.6;max_num_line_search_step_size_iterations = 20;max_num_line_search_direction_restarts = 5;line_search_sufficient_curvature_decrease = 0.9;max_line_search_step_expansion = 10.0;trust_region_strategy_type = LEVENBERG_MARQUARDT;dogleg_type = TRADITIONAL_DOGLEG;use_nonmonotonic_steps = false;max_consecutive_nonmonotonic_steps = 5;max_num_iterations = 50;max_solver_time_in_seconds = 1e9;num_threads = 1;initial_trust_region_radius = 1e4;max_trust_region_radius = 1e16;min_trust_region_radius = 1e-32;min_relative_decrease = 1e-3;min_lm_diagonal = 1e-6;max_lm_diagonal = 1e32;max_num_consecutive_invalid_steps = 5;function_tolerance = 1e-6;gradient_tolerance = 1e-10;parameter_tolerance = 1e-8;#if defined(CERES_NO_SUITESPARSE) && defined(CERES_NO_CXSPARSE) && !defined(CERES_ENABLE_LGPL_CODE)  // NOLINTlinear_solver_type = DENSE_QR;
#elselinear_solver_type = SPARSE_NORMAL_CHOLESKY;
#endifpreconditioner_type = JACOBI;visibility_clustering_type = CANONICAL_VIEWS;dense_linear_algebra_library_type = EIGEN;// Choose a default sparse linear algebra library in the order:////   SUITE_SPARSE > CX_SPARSE > EIGEN_SPARSE > NO_SPARSEsparse_linear_algebra_library_type = NO_SPARSE;
#if !defined(CERES_NO_SUITESPARSE)sparse_linear_algebra_library_type = SUITE_SPARSE;
#else#if !defined(CERES_NO_CXSPARSE)sparse_linear_algebra_library_type = CX_SPARSE;#else#if defined(CERES_USE_EIGEN_SPARSE)sparse_linear_algebra_library_type = EIGEN_SPARSE;#endif#endif
#endifnum_linear_solver_threads = -1;use_explicit_schur_complement = false;use_postordering = false;dynamic_sparsity = false;min_linear_solver_iterations = 0;max_linear_solver_iterations = 500;eta = 1e-1;jacobi_scaling = true;use_inner_iterations = false;inner_iteration_tolerance = 1e-3;logging_type = PER_MINIMIZER_ITERATION;minimizer_progress_to_stdout = false;trust_region_problem_dump_directory = "/tmp";trust_region_problem_dump_format_type = TEXTFILE;check_gradients = false;gradient_check_relative_precision = 1e-8;gradient_check_numeric_derivative_relative_step_size = 1e-6;update_state_every_iteration = false;evaluation_callback = NULL;}

常用的有

• linear_solver_type：
  类型：ceres::LinearSolverType
  说明：指定用于解决线性子问题的线性求解器类型。线性求解是 Ceres 优化中一个关键步骤，不同的线性求解器适用于不同的问题规模和稀疏性。如DENSE_QR: 适合小规模且致密的问题。SPARSE_NORMAL_CHOLESKY: 适合大规模稀疏问题。ITERATIVE_SCHUR: 适用于大规模稠密问题。

• max_num_iterations：
  类型：int
  说明：指定优化问题的最大迭代次数，达到此次数后求解器停止。一般设置为 50 到 500 之间的值。

• function_tolerance：
  类型：double
  说明：当目标函数值的相对变化小于这个阈值时，优化过程会终止。该值用于控制收敛精度，较小的值意味着更严格的收敛条件。默认1e-6 。

• minimizer_progress_to_stdout：
  类型：bool
  说明：是否在标准输出中打印优化过程的进展信息。如果设为 true，每次迭代会打印当前残差和其他调试信息。

---

4. 求解

配置好所有参数后，调用 ceres::Solve 对象开始求解过程。Ceres 将会尝试找到最小化所有成本项的参数值。

ceres::Solver::Summary summary;// 求解问题ceres::Solve(options, &problem, &summary);

Solve还需要一个ceres::Solver::Summary参数用来存放求解过程中产生的信息

---

5. 检查结果

ceres::Solver::Summary是一个结构体，它包含了非线性最小二乘问题求解过程的详细信息。当使用Ceres Solver完成一次优化后，可以通过Summary来获取关于求解过程的各种统计数据。

如

std::cout << summary.BriefReport() << "\n";   //概要报告
std::cout << summary.FullReport() << "\n";	  //详解报告

还有一些关键成员变量

termination_type: 表示求解器终止的原因。
iterations: 求解过程中迭代的次数。
initial_cost: 最初的成本函数值。
final_cost: 经过优化后的成本函数值。
reduced_chi2: 最终的归一化后的成本函数值。
num_successful_steps: 成功的迭代步数。
num_unsuccessful_steps: 不成功的迭代步数。
num_parameters: 参数的数量。
num_residuals: 残差的数量。
num_linear_solves: 线性求解器调用的次数。
num_threads_invoked: 被调用的线程数量。
linear_solver_time_in_seconds: 线性求解器花费的时间（秒）。
minimizer_time_in_seconds: 整个最小化过程花费的时间（秒）。

---

二、基于Ceres的最佳拟合

通常会将ceres拟合问题拆为一个残差结构体和一个拟合主函数

残差结构体

// 定义圆的残差计算结构体
struct CircleResidual
{CircleResidual(double x, double y) : x_(x), y_(y) {}template <typename T>bool operator()(const T* const center, const T* const radius, T* residual) const{T dist = sqrt((x_ - center[0]) * (x_ - center[0]) +(y_ - center[1]) * (y_ - center[1]));// 计算点到圆心的距离减去半径T deviation = dist - radius[0];residual[0] = deviation ;                 // bestreturn true;}private:const double x_, y_;
};

拟合主函数

在主函数中可以看到整个求解（solve problem）过程

void fitCircle(const std::vector<std::pair<double, double>>& data, double* center, double& radius)
{// 1.构建 Ceres 问题ceres::Problem problem;for (const auto& point : data) {  // 2.添加残差块problem.AddResidualBlock(new ceres::AutoDiffCostFunction<CircleResidual, 1, 2, 1>(new CircleResidual(point.first, point.second)),nullptr,center, &radius);}// 3.配置求解器ceres::Solver::Options options;options.linear_solver_type = ceres::DENSE_QR;options.minimizer_progress_to_stdout = true;  // 可选，设为true会打印每次迭代的基本信息// 4.求解ceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);// 5.检查结果（可选）// std::cout << summary.FullReport() << "\n";
}

---

三、带约束的Ceres拟合

经过多次探索，终于在ceres中找到了拟合最大最小圆的方法（主要是最大最小圆）。这里注意两个关键点：1. 残差的设计 2. 拟合区间限定

残差设计

以最大圆为例，最开始我想的是，如果点到圆心距离大于半径，则将其残差赋一个较大值，但结果一直不佳，怎么赋值，赋多大的值都没有可靠来源

最后发现，应该反向思考，将点到圆心距离小于半径的点的残差设为0，则其就会自动去找最大偏差点完成优化

最后正确的（或者说可行的）残差如下

//residual[0] = deviation ;                 // best
residual[0] = deviation > T(0) ? deviation : T(0);     //max
//residual[0] = deviation < T(0) ? deviation : T(0) ;     //min

其实很简单…

拟合区间限定

即使有正确的残差设计，如果不对参数加以限定，可能会出现异常值（比如半径为负）的情况，这时候就自然想到应该对其进行限定，而ceres完全支持这种参数拟合区间的限定，那么（在初始参数已知且相对可靠的情况下）为何不干脆对所有参数都来一个限定呢？

	problem.SetParameterLowerBound(&radius, 0, 0.0);problem.SetParameterUpperBound(&radius, 0, radius * 2);problem.SetParameterLowerBound(center, 0, center[0] - radius);problem.SetParameterUpperBound(center, 0, center[0] + radius);problem.SetParameterLowerBound(center, 1, center[1] - radius);problem.SetParameterUpperBound(center, 1, center[1] + radius);

在这里其实限定得非常宽松

---

四、拟合结果

综上，可得不同拟合结果如下：

best

min

max

参与拟合的点没变，三个模式下的相应偏差指标为0，且不是简单的内缩/外扩，所有点都参与了拟合过程，目标达成。

---

五、完整代码

#include <ceres/ceres.h>
#include <iostream>
#include <vector>
#include <cmath>
#include <random>
#include <limits>
#include <numeric>
#include <iomanip>
#include <fstream>
#define M_PI  3.141592653589793// 定义残差计算结构体
struct CircleResidual
{CircleResidual(double x, double y) : x_(x), y_(y) {}template <typename T>bool operator()(const T* const center, const T* const radius, T* residual) const{T dist = sqrt((x_ - center[0]) * (x_ - center[0]) +(y_ - center[1]) * (y_ - center[1]));// 计算点到圆心的距离减去半径T deviation = dist - radius[0];//residual[0] = deviation ;                 // bestresidual[0] = deviation > T(0) ? deviation : T(0);     //max//residual[0] = deviation < T(0) ? deviation : T(0) ;     //minreturn true;}private:const double x_, y_;
};void fitCircle(const std::vector<std::pair<double, double>>& data, double* center, double& radius)
{// 构建 Ceres 问题ceres::Problem problem;for (const auto& point : data) {problem.AddResidualBlock(new ceres::AutoDiffCostFunction<CircleResidual, 1, 2, 1>(new CircleResidual(point.first, point.second)),nullptr,center, &radius);}problem.SetParameterLowerBound(&radius, 0, 0.0);problem.SetParameterUpperBound(&radius, 0, radius * 2);problem.SetParameterLowerBound(center, 0, center[0] - radius);problem.SetParameterUpperBound(center, 0, center[0] + radius);problem.SetParameterLowerBound(center, 1, center[1] - radius);problem.SetParameterUpperBound(center, 1, center[1] + radius);// 配置求解器ceres::Solver::Options options;options.linear_solver_type = ceres::DENSE_QR;options.minimizer_progress_to_stdout = true;// 求解问题ceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);
}int main()
{// 已知的真实圆参数double true_center[2] = { 2.0, 3.0 };  // 圆心double true_radius = 5.0;            // 半径// 生成模拟数据：100个点，均匀分布在圆周上，并添加噪声std::vector<std::pair<double, double>> data;//std::default_random_engine generator;//std::normal_distribution<double> noise(0.0, 0.1); // 添加少量噪声//for (int i = 0; i < 100; ++i) {//	double angle = 2 * M_PI * i / 100;  // 在圆周上均匀分布//	double x = true_center[0] + true_radius * cos(angle) + noise(generator);//	double y = true_center[1] + true_radius * sin(angle) + noise(generator);//	data.emplace_back(x, y);//}//std::ofstream file("circles_pts.txt");		// 为了使用同一份数据以及可视化，将随机生成的点保存并在后续读取使用//if (file.is_open()) {//    for (const auto& point : data) {//        file << point.first << " " << point.second << "\n";//    }//    file.close();//}std::ifstream file("circles_pts.txt");if (!file.is_open()) {return false;}double x, y;std::string line;while (std::getline(file, line)) {std::istringstream stream(line);if (stream >> x >> y) {data.emplace_back(x, y);}else {std::cerr << "Error reading line: " << line << std::endl;}}file.close();// 初始猜测的圆参数double center[2] = { 2.0, 3.0 };  // 初始圆心double radius = 5.0;            // 初始半径fitCircle(data, center, radius);// 输出拟合结果//std::cout << summary.FullReport() << "\n";std::cout << "Estimated center: (" << center[0] << ", " << center[1] << ")\n";std::cout << "Estimated radius: " << radius << "\n";// 计算偏差的最大、最小和平均值double max_deviation = std::numeric_limits<double>::lowest();double min_deviation = std::numeric_limits<double>::max();double sum_deviation = 0.0;for (const auto& point : data) {double distance = sqrt((point.first - center[0]) * (point.first - center[0]) +(point.second - center[1]) * (point.second - center[1]));double deviation = distance - radius;if (deviation > max_deviation) max_deviation = deviation;if (deviation < min_deviation) min_deviation = deviation;sum_deviation += deviation;}double avg_deviation = sum_deviation / data.size();// 输出统计结果std::cout << std::fixed << std::setprecision(4);std::cout << "\nDeviation statistics:\n";std::cout << "Min deviation: " << min_deviation << "\n";std::cout << "Avg deviation: " << avg_deviation << "\n";std::cout << "Max deviation: " << max_deviation << "\n";return 0;
}

---

其他形状的拟合如球体等同理。

打完收工。