非线性优化Ceres拟合圆

最新推荐文章于 2023-07-07 17:20:42 发布

火星机器人life

最新推荐文章于 2023-07-07 17:20:42 发布

阅读量2k

点赞数

分类专栏： SLAM 文章标签： ceres

本文链接：https://blog.csdn.net/ktigerhero3/article/details/86307599

版权

SLAM 专栏收录该内容

46 篇文章 22 订阅

订阅专栏

1.问题描述

已知圆方程
$r^2=(x-x_0)^2+(y-y_0)^2$
其中x0,y0,r为曲线待拟合参数。
假设我们有N个关于x,y的观测数据点，想根据这些数据点求出曲线的参数。那么可以求解下面的最小二乘问题。
为了拟合圆方程，直接用y为x的函数
$\min_{a,b}\frac{1}{2}\sum_{i=1}^{N}\left \| y_{i}-f(x_i))) \right \|^{2}$
会牵涉到正负号的问题。
定义残差函数如下（这个如何表示欢迎指正）
$\min_{a,b}\frac{1}{2}\sum_{i=1}^{N}\left \|r^2-(x-x_0)^2+(y-y_0)^2)) \right \|^{2}$
实现代码如下
代码

#include <iostream>
#include <opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include <chrono>

using namespace std;


struct CICLE_FITTING_COST
{
    CICLE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
    // 残差的计算
    template <typename T>
    bool operator() (
        const T* const x0y0r,     // 模型参数，有3维
        T* residual ) const     // 残差
    {
        //(x-x0)^2+(y-y0)^2-r^2
        residual[0]=x0y0r[2]-(T ( _x )-x0y0r[0])*(T ( _x )-x0y0r[0])-(T ( _y )-x0y0r[1])*(T ( _y )-x0y0r[1]);
        return true;
    }
    const double _x, _y;    // x,y数据
};




int main ( int argc, char** argv )
{
    double x0=-3.0, y0=-4.0, r=2.0;         // 真实参数值
    int N=100;                          // 数据点
    double w_sigma=0.001;                 // 噪声Sigma值
    cv::RNG rng;                        // OpenCV随机数产生器
    double x0y0r[3] = {0,0,0};            // abc参数的估计值

    vector<double> x_data, y_data;      // 数据

    cout<<"generating data: "<<endl;

    for ( int i=0; i<N; i++ )
    {
        double x = x0+r*cos(i*M_PI/50);
        double noise_y=y0+r*sin(i*M_PI/50)+rng.gaussian ( w_sigma );
        x_data.push_back ( x );
        y_data.push_back (noise_y);
        cout<<x_data[i]<<" "<<y_data[i]<<endl;
    }
    ceres::Problem problem;
    for ( int i=0; i<N; i++ )
    {
        problem.AddResidualBlock (     // 向问题中添加误差项
        // 使用自动求导，模板参数：误差类型，输出维度，输入维度，维数要与前面struct中一致
            new ceres::AutoDiffCostFunction<CICLE_FITTING_COST, 1, 3> (
                new CICLE_FITTING_COST ( x_data[i], y_data[i] )
            ),
            nullptr,            // 核函数，这里不使用，为空
            x0y0r                 // 待估计参数
        );
    }
    // 配置求解器
    ceres::Solver::Options options;     // 这里有很多配置项可以填
    options.linear_solver_type = ceres::DENSE_QR;  // 增量方程如何求解
    options.minimizer_progress_to_stdout = true;   // 输出到cout

    ceres::Solver::Summary summary;                // 优化信息
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    ceres::Solve ( options, &problem, &summary );  // 开始优化
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1 );
    cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;
    // 输出结果
    cout<<summary.BriefReport() <<endl;
//    cout<<"estimated x0,y0,r = ";
    cout<<"x0 "<<x0y0r[0]<<"y0= "<<x0y0r[1]<<" r= "<<sqrt(x0y0r[2]);
    return 0;
}