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;
}
其他形状的拟合如球体等同理。
打完收工。
扫一扫
1471
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
