首页 > 编程语言 >C++ 带约束的Ceres形状拟合

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

时间：2024-09-17 19:49:52浏览次数：12  
标签：deviation Ceres center 残差 C++ ceres radius 拟合

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)  // 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;
}

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

打完收工。

标签：deviation,Ceres,center,残差,C++,ceres,radius,拟合
From： https://blog.csdn.net/m0_50910915/article/details/142143491

相关文章

  • C++面试题整理 2
    8.C++11新特性又哪些自动类型推导auto,智能指指针(share_ptr,unique_ptr等)，for循环简化，线程相关的（std::thread/std::mutex),空指针nullptr，lambda表达式，等等9.share_ptr是线程安全的吗share_ptr里包含引用计数和数据指针，引用计数是原子操作，线程安全的，但是改变数据指针的指向，......
  • C++前后缀分解
    相关知识点C++算法与数据结构打开打包代码的方法兼述单元测试这个算法很容易想到，学习了本文后，可以更快得想到。前后缀分解分治法的一种，将数组和字符串，拆分成前缀和后缀。字符串(数组）的前缀是字符串的前i个元素：s.substr(0,i-1)，即s[0]......
  • C++类和对象
    1.类的定义1.1类的格式(1)class为定义的类关键字，Stack为类的名字，{}中为类的主体，注意类定义结束时后面要跟分号。(2)为了区分类中的成员变量，一般我们命名的时候会在前面加上_或m开头，这个不是硬性要求。(3)C++中也兼容struct的用法，struct升级成了类，但他仍兼容C语言的用法，同......
  • 图像处理-天体中心确定及边缘拟合
    概述确定一张天体照片的中心点和边缘拟合对于天文学、空间科学以及相关领域的研究和应用具有重要意义。尤其在进行多幅天体图像的对比或组合时，精确的中心点和边缘能够帮助进行图像配准，确保图像正确对齐。本文基于一张太阳图片，介绍了定位太阳圆心及拟合边缘的过程及实现，该过......
  • C++模板函数实现类型推导
    C++模板函数实现类型推导以快读函数举例说明无法类型推导的情况template<typenameT>inlineTread(){Tx=0;intf=1;charch=getchar();while(ch<'0'||ch>'9'){if(ch=='-')f=-1;ch=getchar(......
  • 过拟合与欠拟合、批量标准化
    过拟合与欠拟合过拟合（Overfitting）1、基本概念：过拟合指的是模型在训练数据上表现很好，但在未见过的测试数据上表现较差的情况。过拟合发生的原因是模型过于复杂，能够记住训练数据的细节和噪声，而不是学习数据的通用模式。2、特征：模型在训练数据上的准确度高。模型在测试数据......
  • VScode快速配置c++（菜鸟版）
    1.vscode是什么VisualStdioCode简称VSCode，是一款跨平台的、免费且开源的现代轻量级代码编辑器，支持几乎主流开发语言的语法高亮、智能代码补全、自定义快捷键、括号匹配和颜色区分、代码片段提示、代码对比等特性，也拥有对git的开箱即用的支持。同时，它还支持插件扩展，通过丰......
  • C++内存管理详解：各类变量的存储区域
      在C++中，变量的存储位置取决于它们的类型和生命周期。那么不同的各个变量究竟存储在哪个区域呢？1.不同类型的变量我们首先从变量类型的不同来说明：1.全局变量和静态变量 -存储区：全局/静态区（静态区）-说明：全局变量（包括文件级和函数级的）和使用`static`关键字声明的变......
  • C++的类与对象下
    目录1.初始化列表2.隐式类型转换1.单参数2.多参数（C++11提供的新功能）3.static成员4.友元5.内部类6.匿名对象1.初始化列表C++祖师爷规定初始化列表是成员变量定义与初始化的地方。classTime{public: Time(inthour) :_hour(hour) { cout<<"Time()"<<......
  • 4.C++中程序中的命名空间
    咱们在前面的程序中，提到过使用usingnamespacestd;引入这个命名空间，那么std就是由编程系统提供的标准命名空间，那什么是命名空间呢？想像一下，比如一个年级的学生，在记录的时候出现了重名的情况，那么这个时候应该怎么记录呢，是不是需要加一些其它的名称，比如，一三班小李同学，一一班小李......

赞助商

阅读排行

网站主页关于我们联系我们网站地图

本网站内容转载自其他媒体，侵权联系[admin##ips99.com]。

Copyright © 2020-2023 IPS99 版权所有 IPS99