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

一、Ceres Solver
- 1.定义问题
- 2. 添加残差
- - AddResidualBlock
  - AutoDiffCostFunction
- 3. 配置求解器
- 4. 求解
- 5. 检查结果
二、基于Ceres的最佳拟合
- 残差结构体
- 拟合主函数
三、带约束的Ceres拟合
- 残差设计
- 拟合区间限定
四、拟合结果
- best
- min
- max
五、完整代码

对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)  // NOLINT
      linear_solver_type = DENSE_QR;
#else
      linear_solver_type = SPARSE_NORMAL_CHOLESKY;
#endif

      preconditioner_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_SPARSE
      sparse_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
#endif

      num_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 ;                 // best
		return 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 ;                 // best
		residual[0] = deviation > T(0) ? deviation : T(0);     //max
		//residual[0] = deviation < T(0) ? deviation : T(0) ;     //min

		return 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;
}