SLAM学习笔记（二）——手动实现引入了Huber核函数的非线性优化点云配准代码，并研究讨论了结合渐进非凸优化策略对精度的影响

已于 2025-02-28 10:00:52 修改
阅读量291 收藏 5
点赞数 8
文章标签： 学习 笔记
于 2025-02-28 09:58:04 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.youkuaiyun.com/m0_60398577/article/details/145920103
版权

在上一节的优化代码的基础上引入了Huber核函数加权SLAM学习笔记（一）——手动实现点云配准的非线性优化。一开始想不通如何对Huber函数进行非线性优化求解，之后询问了一下Deepseek，了解到只是用Huber核函数对原来的最小二乘进行加权，那么其它鲁棒核函数也是如此。

#include <iostream>
#include <Eigen/Dense>
#include <Eigen/Core>
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/common/transforms.h>
#include <pcl/common/geometry.h>
#include <chrono>

Eigen::Matrix4f optimizeT(pcl::PointCloud<pcl::PointXYZ>::Ptr &S,pcl::PointCloud<pcl::PointXYZ>::Ptr &T,
float &threshold)
{
    Eigen::MatrixXf Jaco_total=Eigen::MatrixXf::Zero(6,6);
    Eigen::MatrixXf Jaco_e_total=Eigen::MatrixXf::Zero(6,1);
    for(int index=0;index<S->size();index++)
    {
        Eigen::MatrixXf W_i=Eigen::MatrixXf::Zero(3,3);
        Eigen::MatrixXf Jaco_index_T=Eigen::MatrixXf::Zero(3,6);
        Jaco_index_T<<1,0,0,0,S->points[index].z,-1*S->points[index].y,
                      0,1,0,-1*S->points[index].z,0,S->points[index].x,
                      0,0,1,S->points[index].y,-1*S->points[index].x,0,
                      0,0,0,0,0,0;
        Eigen::MatrixXf e_i=Eigen::MatrixXf::Zero(3,1);
        e_i<<S->points[index].x-T->points[index].x,S->points[index].y-T->points[index].y,
             S->points[index].z-T->points[index].z;
        float e_i_norm=sqrt(pow(S->points[index].x-T->points[index].x,2)+
                       pow(S->points[index].y-T->points[index].y,2)+
                       pow(S->points[index].z-T->points[index].z,2));
        if(e_i_norm>threshold)
        {
            W_i(0,0)=threshold/e_i_norm;
            W_i(1,1)=threshold/e_i_norm;
            W_i(2,2)=threshold/e_i_norm;
        }
        else
        {
            W_i(0,0)=1.0f;
            W_i(1,1)=1.0f;
            W_i(2,2)=1.0f;
        }
        Jaco_total+=Jaco_index_T.transpose()*W_i*Jaco_index_T;
        Jaco_e_total+=-1*Jaco_index_T.transpose()*W_i*e_i;
    }

    Eigen::VectorXf dx(6);
    dx=Jaco_total.ldlt().solve(Jaco_e_total);

    if(isnan(dx[0]))
    {
        std::cout<<"result is nan!"<<std::endl;
        Eigen::Matrix4f E=Eigen::Matrix4f::Identity();
        return E;
    }
    else
    {
        Eigen::Affine3d aff_mat;
		aff_mat.linear() = (Eigen::Matrix3d) Eigen::AngleAxisd(dx[5], Eigen::Vector3d::UnitZ())
			* Eigen::AngleAxisd(dx[4], Eigen::Vector3d::UnitY())
			* Eigen::AngleAxisd(dx[3], Eigen::Vector3d::UnitX());
		aff_mat.translation() = Eigen::Vector3d(dx[0], dx[1], dx[2]);

		Eigen::Matrix4f newT = aff_mat.matrix().cast<float>();

        return newT;
    }
}

int main(int argc, char** argv)
{
    //Nonlinear Optimization---------------------------------------------------------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr S_trans(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr T(new pcl::PointCloud<pcl::PointXYZ>);
    std::string SnameS = argv[1];
    std::string SnameT = argv[2];
    pcl::io::loadPCDFile(SnameS, *S_trans);
    pcl::io::loadPCDFile(SnameT, *T);
    float threshold=std::atof(argv[3]);

    std::chrono::steady_clock::time_point t1=std::chrono::steady_clock::now();
    float align_error=0;
    for(int i=0;i<S_trans->size();i++)
    {
        float dis=pcl::geometry::distance(S_trans->points[i],T->points[i]);
        align_error+=dis;
    }
    std::cout<<"before optimize: "<<align_error<<std::endl;

    Eigen::Matrix4f optimal_trans=Eigen::Matrix4f::Identity();

    for(int optimal_count=0;optimal_count<20;optimal_count++)
    {
        Eigen::Matrix4f new_T;
        new_T=optimizeT(S_trans,T,threshold);
        if(new_T==Eigen::Matrix4f::Identity())
        {
            break;
        }
        pcl::transformPointCloud(*S_trans, *S_trans, new_T);
        float after_align_error=0;
        for(int i=0;i<S_trans->size();i++)
        {
            float dis=pcl::geometry::distance(S_trans->points[i],T->points[i]);
            after_align_error+=dis;
        }
        optimal_trans=new_T*optimal_trans;
        std::cout<<optimal_count+1<<" optimize: "<<after_align_error<<std::endl;
    }
    std::chrono::steady_clock::time_point t2=std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used=std::chrono::duration_cast<std::chrono::duration<double>>(t2-t1);
    std::cout<<"optimize time: "<<time_used.count()<<"seconds"<<std::endl;
    std::cout<<"optimal transformation: "<<std::endl<<optimal_trans<<std::endl;
    //-----------------------------------------------------------------------------------------------

}

运行结果对比

//最小二乘运行结果
before optimize: 152.927
1 optimize: 13.0699
2 optimize: 5.01842
3 optimize: 5.02915
4 optimize: 5.02987
5 optimize: 5.02991
6 optimize: 5.02992
7 optimize: 5.02992
8 optimize: 5.02992
9 optimize: 5.02992
10 optimize: 5.02992
11 optimize: 5.02992
12 optimize: 5.02992
13 optimize: 5.02992
14 optimize: 5.02992
15 optimize: 5.02992
16 optimize: 5.02992
17 optimize: 5.02992
18 optimize: 5.02992
19 optimize: 5.02992
20 optimize: 5.02992
optimize time: 0.000178879seconds
optimal transformation: 
  0.987375  -0.157942  0.0119976    1.80202
  0.157542   0.987084  0.0290874    11.8185
-0.0164367   -0.02683   0.999505  -0.246407
         0          0          0          1
//基于Huber核函数加权后的运行结果
before optimize: 152.927
1 optimize: 12.5113
2 optimize: 4.65814
3 optimize: 4.16248
4 optimize: 4.26452
5 optimize: 4.25053
6 optimize: 4.25586
7 optimize: 4.25656
8 optimize: 4.25742
9 optimize: 4.25786
10 optimize: 4.25813
11 optimize: 4.2583
12 optimize: 4.25839
13 optimize: 4.25845
14 optimize: 4.25849
15 optimize: 4.25851
16 optimize: 4.25852
17 optimize: 4.25853
18 optimize: 4.25853
19 optimize: 4.25854
20 optimize: 4.25853
optimize time: 0.000599866seconds
optimal transformation: 
  0.984906  -0.172376  0.0156684    1.71422
  0.171819   0.984615  0.0317958    11.8851
-0.0209082 -0.0286237   0.999372   -0.27486
         0          0          0          1

由于Huber核函数是需要设置尺度参数\delta的，参考论文《Fast Global Registration》里的渐进非凸优化策略，对尺度参数不断调整，会提高算法的鲁棒性，避免陷入局部最优解，同时不断优化精度。

//主要在main中修改

int main(int argc, char** argv)
{
    //Nonlinear Optimization---------------------------------------------------------------------------
    pcl::PointCloud<pcl::PointXYZ>::Ptr S_trans(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr T(new pcl::PointCloud<pcl::PointXYZ>);
    std::string SnameS = argv[1];
    std::string SnameT = argv[2];
    pcl::io::loadPCDFile(SnameS, *S_trans);
    pcl::io::loadPCDFile(SnameT, *T);
    float threshold=std::atof(argv[3]);

    std::chrono::steady_clock::time_point t1=std::chrono::steady_clock::now();
    float align_error=0;
    for(int i=0;i<S_trans->size();i++)
    {
        float dis=pcl::geometry::distance(S_trans->points[i],T->points[i]);
        align_error+=dis;
    }
    std::cout<<"before optimize: "<<align_error<<std::endl;

    Eigen::Matrix4f optimal_trans=Eigen::Matrix4f::Identity();

    for(int optimal_count=0;optimal_count<20;optimal_count++)
    {
        if((optimal_count+1)%4==0)
        {
            threshold/=1.4;
        }
        Eigen::Matrix4f new_T;
        new_T=optimizeT(S_trans,T,threshold);
        if(new_T==Eigen::Matrix4f::Identity())
        {
            break;
        }
        pcl::transformPointCloud(*S_trans, *S_trans, new_T);
        float after_align_error=0;
        for(int i=0;i<S_trans->size();i++)
        {
            float dis=pcl::geometry::distance(S_trans->points[i],T->points[i]);
            after_align_error+=dis;
        }
        optimal_trans=new_T*optimal_trans;
        std::cout<<optimal_count+1<<" optimize: "<<after_align_error<<std::endl;
    }
    std::chrono::steady_clock::time_point t2=std::chrono::steady_clock::now();
    std::chrono::duration<double> time_used=std::chrono::duration_cast<std::chrono::duration<double>>(t2-t1);
    std::cout<<"optimize time: "<<time_used.count()<<"seconds"<<std::endl;
    std::cout<<"optimal transformation: "<<std::endl<<optimal_trans<<std::endl;
    //-----------------------------------------------------------------------------------------------

}

运行结果

before optimize: 152.927
1 optimize: 12.5113
2 optimize: 4.65814
3 optimize: 4.16248
4 optimize: 4.06365
5 optimize: 4.06436
6 optimize: 4.05434
7 optimize: 4.04834
8 optimize: 3.96635
9 optimize: 3.97047
10 optimize: 3.96392
11 optimize: 3.9604
12 optimize: 3.9171
13 optimize: 3.92773
14 optimize: 3.92687
15 optimize: 3.92758
16 optimize: 3.88708
17 optimize: 3.89422
18 optimize: 3.88998
19 optimize: 3.88926
20 optimize: 3.85594
optimize time: 0.000348311seconds
optimal transformation: 
  0.983349  -0.180695  0.0193308    1.63153
   0.18002   0.983132  0.0323223     11.903
-0.0248452 -0.0283042   0.999291   -0.28872
         0          0          0          1

不过可能用的数据的解是凸的，单纯是随着\delta变小精度越来越高了，后面可以加入到ICP里试一试

评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值