激光SLAM理论与实践 第四次作业（帧间匹配算法，imls-icp和csm）

最新推荐文章于 2022-04-21 17:09:26 发布
最新推荐文章于 2022-04-21 17:09:26 发布
阅读量1.9k 收藏 31
点赞数 3
分类专栏： 激光SLAM理论与实践 文章标签： slam
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/zzr1024/article/details/118683780
版权

有对视频ppt讲解不清楚的，可参考下面这个链接（对ICP、PL-ICP、NICP、IMLS-ICP匹配算法的解析）。

首先，第四章的作业之间编译会报错，是应为cmakelist未添加nabo库，添加即可。

报错如下：

CMakeFiles/imlsMatcher_node.dir/src/imls_icp.cpp.o：在函数‘IMLSICPMatcher::setTargetPointCloud(std::vector<Eigen::Matrix<double, 2, 1, 0, 2, 1>, std::allocator<Eigen::Matrix<double, 2, 1, 0, 2, 1> > >&)’中：
/home/zzr/桌面/HW4/imlsMatcherProject/src/imlsMatcher/src/imls_icp.cpp:118：对‘Nabo::NearestNeighbourSearch<double, Eigen::Matrix<double, -1, -1, 0, -1, -1> >::createKDTreeLinearHeap(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, unsigned int, Nabo::Parameters const&)’未定义的引用
CMakeFiles/imlsMatcher_node.dir/src/imls_icp.cpp.o：在函数‘IMLSICPMatcher::ImplicitMLSFunction(Eigen::Matrix<double, 2, 1, 0, 2, 1>, double&)’中：
/home/zzr/桌面/HW4/imlsMatcherProject/src/imlsMatcher/src/imls_icp.cpp:159：对‘Nabo::NearestNeighbourSearch<double, Eigen::Matrix<double, -1, -1, 0, -1, -1> >::knn(Eigen::Matrix<double, -1, 1, 0, -1, 1> const&, Eigen::Matrix<int, -1, 1, 0, -1, 1>&, Eigen::Matrix<double, -1, 1, 0, -1, 1>&, int, double, unsigned int, double) const’未定义的引用
/home/zzr/桌面/HW4/imlsMatcherProject/src/imlsMatcher/src/imls_icp.cpp:144：对‘Nabo::NearestNeighbourSearch<double, Eigen::Matrix<double, -1, -1, 0, -1, -1> >::createKDTreeLinearHeap(Eigen::Matrix<double, -1, -1, 0, -1, -1> const&, int, unsigned int, Nabo::Parameters const&)’未定义的引用

 修改方式如下：

 关于上面eigen库的使用，可参考官网教程（超级详细），

链接：http://eigen.tuxfamily.org/dox/group__TutorialLinearAlgebra.html

运行效果图：绿色为里程计轨迹，红色为匹配后的轨迹

 具体代码文章末

 第二题只需要在main函数中添加SetPIICPParams()、laserScanToLDP()、PIICPBetweenTwoFrames()，同时修改回调函数championLaserScanCallback()即可；

 第一步：添加SetPIICPParams()

第二步：添加laserScanToLDP()

第三步：添加PIICPBetweenTwoFrames()

 第四步：修改championLaserScanCallback()

 具体代码文章末

运行截图：

3.阅读 ICP 相关论文，总结课上所学的几种 ICP 及其相关变型并简述其异同(ICPICP，PL-ICP，NICP, IMLSICP)

1）区别

 2）思路相同

4. 现在你已经了解了多种 ICP 算法，你是否也能提出一种改进的 ICP 算法，或能提升 ICP 总体匹配精度或速度的技巧？请简述你的改进策略

提升ICP方法精度和速度的一些考虑
精度上：
1）使用其他传感器或方法（如里程计、IMU）提供初值（coarse to fine）。
效率上：
1）选取合适的采样点。IMLS-ICP的论文里就提到了一些去除误差点的方法。
2）使用合适的数据结构（TD-tree），提高程序的运行效率。

第一题源代码：

#include "imls_icp.h"

//pcl::visualization::CloudViewer g_cloudViewer("view-view");

IMLSICPMatcher::IMLSICPMatcher(void )
{
    m_r = 0.03;
    m_h = 0.10;
    m_Iterations = 10;

    m_PtID = 0;

    m_pTargetKDTree = m_pSourceKDTree = NULL;
}

//构造函数．
IMLSICPMatcher::IMLSICPMatcher(double _r,double _h,int _iter)
{
    m_r = _r;
    m_h = _h;
    m_Iterations = _iter;
}

//析构函数，释放内存．
IMLSICPMatcher::~IMLSICPMatcher(void )
{
    if(m_pTargetKDTree != NULL)
        delete m_pTargetKDTree;

    if(m_pSourceKDTree != NULL)
        delete m_pSourceKDTree;
}

void IMLSICPMatcher::setIterations(int _iter)
{
    m_Iterations = _iter;
}

//去除非法数据
void IMLSICPMatcher::RemoveNANandINFData(std::vector<Eigen::Vector2d> &_input)
{
    //去除非法数据．
    for(std::vector<Eigen::Vector2d>::iterator it = _input.begin();it != _input.end();)
    {
        Eigen::Vector2d tmpPt = *it;
        if(std::isnan(tmpPt(0)) || std::isnan(tmpPt(1)) ||
                std::isinf(tmpPt(0)) || std::isinf(tmpPt(1)))
        {
            it = _input.erase(it);
        }
        else
        {
            it++;
        }
    }
}

void IMLSICPMatcher::setSourcePointCloud(pcl::PointCloud<pcl::PointXYZ> &pcl_cloud)
{
    std::vector<Eigen::Vector2d> source_ptCloud;
    for(int i =0; i < pcl_cloud.size();i++)
    {
        source_ptCloud.push_back(Eigen::Vector2d(pcl_cloud[i].x,pcl_cloud[i].y));
    }
    setSourcePointCloud(source_ptCloud);
}

void IMLSICPMatcher::setSourcePointCloud(std::vector<Eigen::Vector2d> &_source_pcloud)
{
    m_sourcePointCloud = _source_pcloud;

    //去除非法数据
    RemoveNANandINFData(m_sourcePointCloud);

    // std::cout <<"Source Pt Cloud:"<<m_sourcePointCloud.size()<<std::endl;
}


void IMLSICPMatcher::setSourcePointCloudNormals(std::vector<Eigen::Vector2d> &_normals)
{
    m_sourcePtCloudNormals = _normals;
}

//设置目标点云．
void IMLSICPMatcher::setTargetPointCloud(pcl::PointCloud<pcl::PointXYZ> &pcl_cloud)
{
    std::vector<Eigen::Vector2d> target_ptCloud;
    for(int i =0; i < pcl_cloud.size();i++)
    {
        target_ptCloud.push_back(Eigen::Vector2d(pcl_cloud[i].x,pcl_cloud[i].y));
    }
    setTargetPointCloud(target_ptCloud);
}

//本函数会自动重新生成kd树．
void IMLSICPMatcher::setTargetPointCloud(std::vector<Eigen::Vector2d> &_target_pcloud)
{
    m_targetPointCloud = _target_pcloud;

    if(m_pTargetKDTree != NULL)
    {
        delete m_pTargetKDTree;
        m_pTargetKDTree = NULL;
    }

    RemoveNANandINFData(m_targetPointCloud);


    //构建kd树．
    if(m_pTargetKDTree == NULL)
    {
        m_targetKDTreeDataBase.resize(2,m_targetPointCloud.size());
        for(int i = 0; i < m_targetPointCloud.size();i++)
        {
            m_targetKDTreeDataBase(0,i) = m_targetPointCloud[i](0);
            m_targetKDTreeDataBase(1,i) = m_targetPointCloud[i](1);
        }
        m_pTargetKDTree = Nabo::NNSearchD::createKDTreeLinearHeap(m_targetKDTreeDataBase);
    }

    //设置需要重新计算法向量
    m_isGetNormals = false;
}


//IMLS函数，主要用来进行曲面投影．
//可以认为是xi在曲面上的高度．
//用target_sourcePtcloud构造一个kd树．
bool IMLSICPMatcher::ImplicitMLSFunction(Eigen::Vector2d x,
                                         double& height)
{
    double weightSum = 0.0;
    double projSum = 0.0;

    //创建KD树
    if(m_pTargetKDTree == NULL)
    {
        m_targetKDTreeDataBase.resize(2,m_targetPointCloud.size());
        for(int i = 0; i < m_targetPointCloud.size();i++)
        {
            m_targetKDTreeDataBase(0,i) = m_targetPointCloud[i](0);
            m_targetKDTreeDataBase(1,i) = m_targetPointCloud[i](1);
        }
        m_pTargetKDTree = Nabo::NNSearchD::createKDTreeLinearHeap(m_targetKDTreeDataBase);
    }

    // 找到位于点x附近(m_r)的所有的点云
    int searchNumber = 20;
    Eigen::VectorXi nearIndices(searchNumber);
    Eigen::VectorXd nearDist2(searchNumber);

    //找到某一个点的最近邻．
    //搜索searchNumber个最近邻
    //下标储存在nearIndices中，距离储存在nearDist2中．
    //最大搜索距离为m_r
    m_pTargetKDTree->knn(x,nearIndices,nearDist2,searchNumber,0,
                         Nabo::NNSearchD::SORT_RESULTS | Nabo::NNSearchD::ALLOW_SELF_MATCH|
                         Nabo::NNSearchD::TOUCH_STATISTICS,
                         m_h);

    std::vector<Eigen::Vector2d> nearPoints;
    std::vector<Eigen::Vector2d> nearNormals;
    for(int i = 0; i < searchNumber;i++)
    {
        //说明最近邻是合法的．
        if(nearDist2(i) < std::numeric_limits<double>::infinity() &&
                std::isinf(nearDist2(i)) == false &&
                std::isnan(nearDist2(i)) == false)
        {
            //该最近邻在原始数据中的下标．
            int index = nearIndices(i);

            Eigen::Vector2d tmpPt(m_targetKDTreeDataBase(0,index),m_targetKDTreeDataBase(1,index));

            //是否为inf
            if(std::isinf(tmpPt(0))||std::isinf(tmpPt(1))||
                    std::isnan(tmpPt(0))||std::isnan(tmpPt(1)))
            {
                continue;
            }

            Eigen::Vector2d normal;
            normal = m_targetPtCloudNormals[index];

            //如果对应的点没有法向量，则不要．
            if(std::isinf(normal(0))||std::isinf(normal(1))||
                    std::isnan(normal(0))||std::isnan(normal(1)))
            {
                continue;
            }

            nearPoints.push_back(tmpPt);
            nearNormals.push_back(normal);
        }
        else
        {
            break;
        }
    }

    //如果nearPoints小于３个，则认为没有匹配点．
    if(nearPoints.size() < 3)
    {
        return false;
    }

    //根据函数进行投影．计算height，即ppt中的I(x)
    double mh2 = m_h * m_h;
    for(int i = 0; i < nearPoints.size(); ++i)
    {
        Eigen::Vector2d delta_p = x - nearPoints[i];
        double weight = std::exp(-delta_p.squaredNorm()/mh2);//向量的平方范数
        projSum += weight * delta_p.dot(nearNormals[i]); //向量的点积 或者 矩阵积
        weightSum += weight;
    }
    height = projSum / (weightSum + 0.000001);//+ 0.000001防止分母未零

    return true;
}



/**
 * @brief IMLSICPMatcher::projSourcePtToSurface
 * 此函数的目的是把source_cloud中的点云，投影到对应的surface上．
   即为每一个当前帧点云in_cloud的激光点计算匹配点和对应的法向量
   即in_cloud和out_cloud进行匹配，同时得到out_normal
   注意：本函数应该删除in_cloud内部找不到匹配值的点．
 * @param in_cloud          当前帧点云
 * @param out_cloud         当前帧的匹配点云
 * @param out_normal        匹配点云对应的法向量．
 */
void IMLSICPMatcher::projSourcePtToSurface(
        std::vector<Eigen::Vector2d> &in_cloud,
        std::vector<Eigen::Vector2d> &out_cloud,
        std::vector<Eigen::Vector2d> &out_normal)
{
    out_cloud.clear();
    out_normal.clear();

    for(std::vector<Eigen::Vector2d>::iterator it = in_cloud.begin(); it != in_cloud.end();)
    {
        Eigen::Vector2d xi = *it;

        //找到在target_cloud中的最近邻
        //包括该点和下标．
        int K = 1;
        Eigen::VectorXi indices(K);
        Eigen::VectorXd dist2(K);
        m_pTargetKDTree->knn(xi,indices,dist2);

        Eigen::Vector2d nearXi = m_targetKDTreeDataBase.col(indices(0));

        //为最近邻计算法向量．－－进行投影的时候，使用统一的法向量．
        Eigen::Vector2d nearNormal = m_targetPtCloudNormals[indices(0)];

        //如果对应的点没有法向量，也认为没有匹配点．因此直接不考虑．
        if(std::isinf(nearNormal(0))||std::isinf(nearNormal(1))||
                std::isnan(nearNormal(0))||std::isnan(nearNormal(1)))
        {
            it = in_cloud.erase(it);
            continue;
        }

        //如果距离太远，则说明没有匹配点．因此可以不需要进行投影，直接去除．
        if(dist2(0) > m_h * m_h )
        {
            it = in_cloud.erase(it);
            continue;
        }

        //进行匹配
        double height;
        if(ImplicitMLSFunction(xi,height) == false)
        {
            it = in_cloud.erase(it);
            continue;
        }

        if(std::isnan(height))
        {
            std::cout <<"proj:this is not possible"<<std::endl;
            it = in_cloud.erase(it);
            continue;
        }

        if(std::isinf(height))
        {
            std::cout <<"proj:this is inf,not possible"<<std::endl;
            it = in_cloud.erase(it);
            continue;
        }

        Eigen::Vector2d yi;
        //计算yi．
        yi = xi - height * nearNormal;

        out_cloud.push_back(yi);
        out_normal.push_back(nearNormal);

        it++;
    }
}


//已知对应点对和法向量的时候，求解相对位姿．
//source_cloud在ref_cloud下的位姿．
//具体的算法，可以参考point-to-line ICP论文．
bool IMLSICPMatcher::SolveMotionEstimationProblem(std::vector<Eigen::Vector2d> &source_cloud,
                                                  std::vector<Eigen::Vector2d> &ref_cloud,
                                                  std::vector<Eigen::Vector2d> &ref_normals,
                                                  Eigen::Matrix3d& deltaTrans)
{
    Eigen::Matrix4d M;
    M.setZero();
    Eigen::Matrix<double,1,4> gt; //gt是个行向量．
    gt.setZero();

    for(int i = 0; i < source_cloud.size();i++)
    {
        //点p-source point
        Eigen::Vector2d p = source_cloud[i];

        //target-point
        Eigen::Vector2d refPt = ref_cloud[i];

        //ref对应的normal
        Eigen::Vector2d ni = ref_normals[i];

        //加权矩阵
        //对于p-p来说，Ci =wi * I
        //对于point-line来说，Ci =wi *  n*n^T
        Eigen::Matrix2d Ci =ni * ni.transpose();

        //构造M矩阵
        Eigen::Matrix<double,2,4> Mi;
        Mi <<   1,0,p(0),-p(1),
                0,1,p(1), p(0);
        M += Mi.transpose() * Ci * Mi;

        Eigen::Matrix<double,1,4> gti;
        gti  = -2 * refPt.transpose() * Ci * Mi;

        gt += gti;
    }

    //g是个列向量
    Eigen::Matrix<double,4,1> g = gt.transpose();

    //构建完了M矩阵和g向量．
    //在后续的求解过程中，基本上都使用的是2*M,因此直接令M = 2*M
    M = 2*M;

    //M(实际是2*M,下面等同)矩阵能分为４个部分
    Eigen::Matrix2d A,B,D;
    A = M.block(0,0,2,2);
    B = M.block(0,2,2,2);
    D = M.block(2,2,2,2);

    //论文中还引入了S和SA矩阵．
    Eigen::Matrix2d S,SA;
    S = D - B.transpose() * A.inverse() * B;
    SA = S.determinant() * S.inverse();

    //目前所有的式子已经可以构建多项式系数了．
    //式31右边p(\lambda)的系数
    //p(\lambda)的系数为：
    Eigen::Vector3d p_coffcient;
    p_coffcient << S.determinant(),2*(S(0,0)+S(1,1)) ,4;

    //论文中式(31)左边的系数(a x^2 + b x + c)为：
    double a,b,c;
    Eigen::Matrix4d tmpMatrix;
    tmpMatrix.block(0,0,2,2) = A.inverse() * B  * SA  * SA.transpose()* B.transpose() * A.inverse().transpose();
    tmpMatrix.block(0,2,2,2) = -A.inverse() * B  * SA * SA.transpose();
    tmpMatrix.block(2,0,2,2) = tmpMatrix.block(0,2,2,2).transpose();
    tmpMatrix.block(2,2,2,2) = SA * SA.transpose() ;

    c = g.transpose() * tmpMatrix * g;

    tmpMatrix.block(0,0,2,2) = A.inverse() * B * SA * B.transpose() * A.inverse().transpose();
    tmpMatrix.block(0,2,2,2) = -A.inverse() * B * SA;
    tmpMatrix.block(2,0,2,2) = tmpMatrix.block(0,2,2,2).transpose();
    tmpMatrix.block(2,2,2,2) = SA;
    b = 4 * g.transpose() * tmpMatrix * g;

    tmpMatrix.block(0,0,2,2) = A.inverse() * B * B.transpose() * A.inverse().transpose();
    tmpMatrix.block(0,2,2,2) = -A.inverse() * B;
    tmpMatrix.block(2,0,2,2) = tmpMatrix.block(0,2,2,2).transpose();
    tmpMatrix.block(2,2,2,2) = Eigen::Matrix2d::Identity();
    a = 4 * g.transpose() * tmpMatrix * g;

    //把式31的等式两边进行合并，得到一个4次多项式．５个系数．
    Eigen::VectorXd poly_coffi(5);
    poly_coffi(0) = c - p_coffcient(0) * p_coffcient(0);
    poly_coffi(1) = b - 2 * p_coffcient(0) * p_coffcient(1);
    poly_coffi(2) = a - (p_coffcient(1)*p_coffcient(1) + 2*p_coffcient(0)*p_coffcient(2));
    poly_coffi(3) = -2 * p_coffcient(1)*p_coffcient(2);
    poly_coffi(4) = - p_coffcient(2) * p_coffcient(2);

    for(int i = 0; i < 5;i++)
    {
        if(std::isnan(poly_coffi(i)))
        {
            std::cout <<"Error, This should not happen"<<std::endl;
        }
    }


    //进行多项式的求解，得到对应的lambda．
    double lambda;
    if(SolverFourthOrderPolynomial(poly_coffi,lambda) == false)
    {
        std::cout <<"Polynomial Solve Failed"<<std::endl;
        return false;
    }

    //得到lambda之后，根据式24．
    Eigen::Matrix4d W;
    W.setZero();
    W.block(2,2,2,2) = Eigen::Matrix2d::Identity();

    //Eigen::Vector4d res = -(M + 2 *lambda * W).inverse().transpose() * g;
    Eigen::Vector4d res = -(M + 2 * lambda * W).inverse() * g;

    //转换成旋转矩阵
    double theta = std::atan2(res(3),res(2));
    deltaTrans << cos(theta),-sin(theta),res(0),
            sin(theta), cos(theta),res(1),
            0,          0,     1;
    return true;
}

/**
 * @brief IMLSICPMatcher::ComputeNormal
 * 计算法向量
 * @param nearPoints    某个点周围的所有激光点
 * @return
 */
Eigen::Vector2d IMLSICPMatcher::ComputeNormal(std::vector<Eigen::Vector2d> &nearPoints)
{
    Eigen::Vector2d normal;
    //根据周围的激光点计算法向量，参考ppt中NICP计算法向量的方法
    if(nearPoints.size() < 3)
    {
        normal(0) = normal(1) = std::numeric_limits<double>::infinity();
        return normal;
    }

    Eigen::Vector2d centroid(0,0);
    for(auto point : nearPoints){
        centroid += point;
    }
    centroid /= double(nearPoints.size());

    Eigen::Matrix2d sigma = Eigen::Matrix2d::Zero();
    for(auto point : nearPoints){
        Eigen::Vector2d tmp = point - centroid;
        sigma = tmp * tmp.transpose();
    }
    sigma /= double(nearPoints.size());
    // 计算法向量，第一个特征向量即为最小特征值对应的特征向量，矩阵的第一列
    Eigen::SelfAdjointEigenSolver<Eigen::Matrix2d> eig(sigma);
    Eigen::Matrix2d eigVectors = eig.eigenvectors();
    normal = eigVectors.col(0);

    return normal;
}


/**
 * @brief IMLSICPMatcher::Match
 * 最终使用的ICP匹配函数．
 * @param finalResult
 * @param covariance
 * @return
 */
bool IMLSICPMatcher::Match(Eigen::Matrix3d& finalResult,
                           Eigen::Matrix3d& covariance)
{
    //如果没有设置法向量，则自动进行计算．
    if(m_isGetNormals == false)
    {
        //自动计算target pointcloud中每个点的法向量
        m_targetPtCloudNormals.clear();
        for(int i = 0; i < m_targetPointCloud.size();i++)
        {
            Eigen::Vector2d xi = m_targetPointCloud[i];

            int K = 20;
            Eigen::VectorXi indices(K);
            Eigen::VectorXd dist2(K);
            int num = m_pTargetKDTree->knn(xi,indices,dist2,K,0.0,
                                           Nabo::NNSearchD::SORT_RESULTS | Nabo::NNSearchD::ALLOW_SELF_MATCH|
                                           Nabo::NNSearchD::TOUCH_STATISTICS,
                                           0.15);

            std::vector<Eigen::Vector2d> nearPoints;
            for(int ix = 0; ix < K;ix++)
            {
                if(dist2(ix) < std::numeric_limits<double>::infinity() &&
                        std::isinf(dist2(ix)) == false)
                {
                    nearPoints.push_back(m_targetKDTreeDataBase.col(indices(ix)));
                }
                else break;
            }

            //计算法向量
            Eigen::Vector2d normal;
            if(nearPoints.size() > 3)
            {
                //计算法向量
                normal = ComputeNormal(nearPoints);
            }
            else
            {
                normal(0) = normal(1) = std::numeric_limits<double>::infinity();
            }
            m_targetPtCloudNormals.push_back(normal);
        }
    }

    //初始化估计值．
    Eigen::Matrix3d result;
    result.setIdentity();
    covariance.setIdentity();

    for(int i = 0; i < m_Iterations;i++)
    {
        //根据当前估计的位姿对原始点云进行转换．
        std::vector<Eigen::Vector2d> in_cloud;
        for(int ix = 0; ix < m_sourcePointCloud.size();ix++)
        {
            Eigen::Vector3d origin_pt;
            origin_pt << m_sourcePointCloud[ix],1;

            Eigen::Vector3d now_pt = result * origin_pt;
            in_cloud.push_back(Eigen::Vector2d(now_pt(0),now_pt(1)));
        }

        //把sourceCloud中的点投影到targetCloud组成的平面上
        //对应的投影点即为sourceCloud的匹配点．
        //每次转换完毕之后，都需要重新计算匹配点．
        //这个函数会得到对应的匹配点．
        //本次匹配会自动删除in_cloud内部的一些找不到匹配点的点．
        //因此，这个函数出来之后，in_cloud和ref_cloud是一一匹配的．
        std::vector<Eigen::Vector2d> ref_cloud;
        std::vector<Eigen::Vector2d> ref_normal;
        projSourcePtToSurface(in_cloud,
                              ref_cloud,
                              ref_normal);

        if(in_cloud.size() < 5 || ref_cloud.size() < 5)
        {
            std::cout <<"Not Enough Correspondence:"<<in_cloud.size()<<","<<ref_cloud.size()<<std::endl;
            std::cout <<"ICP Iterations Failed!!"<<std::endl;
            return false;
        }

        //计算帧间位移．从当前的source -> target
        Eigen::Matrix3d deltaTrans;
        bool flag = SolveMotionEstimationProblem(in_cloud,
                                                 ref_cloud,
                                                 ref_normal,
                                                 deltaTrans);

        if(flag == false)
        {
            std::cout <<"ICP Iterations Failed!!!!"<<std::endl;
            return false;
        }

        //更新位姿．
        result = deltaTrans * result;

        //迭代条件是否满足．
        double deltadist = std::sqrt( std::pow(deltaTrans(0,2),2) + std::pow(deltaTrans(1,2),2));
        double deltaAngle = std::atan2(deltaTrans(1,0),deltaTrans(0,0));

        //如果迭代条件允许，则直接退出．
        if(deltadist < 0.001 && deltaAngle < (0.01/57.295))
        {
            break;
        }
    }

    finalResult = result;
    return true;
}

//调用Eigen求解四次多项式的第一个非零实根
bool IMLSICPMatcher::SolverFourthOrderPolynomial(Eigen::VectorXd&p_coffi,
                                                 double &lambda)
{
    Eigen::PolynomialSolver<double,4> polySolve(p_coffi);

    Eigen::Matrix<std::complex<double>,4,1,0,4,1> roots = polySolve.roots();

    bool isAssigned = false;
    double finalRoot = 0.0;

    //找到第一个非零实根--有可能不止一个，因为有优化空间．
    for(int i = 0; i < roots.size();i++)
    {
        //如果是虚根，则不要．
        if(roots(i).imag() != 0 )continue;

        //如果是非零实根，则选择．
        if(isAssigned == false ||  roots(i).real() > finalRoot)
        {
            isAssigned = true;
            finalRoot = roots(i).real();
        }
    }

    lambda = finalRoot;
    return isAssigned;
}

第二题源代码：

#include "imls_icp.h"
#include <ros/ros.h>
#include "tf/transform_listener.h"
#include "sensor_msgs/PointCloud.h"
#include "geometry_msgs/Point32.h"
#include "visualization_msgs/Marker.h"
#include "visualization_msgs/MarkerArray.h"
#include "champion_nav_msgs/ChampionNavLaserScan.h"
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <rosbag/bag.h>
#include <rosbag/view.h>
#include <boost/foreach.hpp>
#include <csm/csm_all.h>
using namespace Eigen;
//pcl::visualization::CloudViewer g_cloudViewer("cloud_viewer");
//此处bag包的地址需要自行修改
std::string bagfile = "/home/zzr/HW4/imlsMatcherProject/src/bag/imls_icp.bag";

class imlsDebug
{
    public:
        imlsDebug()
        {
            SetPIICPParams();

            m_imlsPathPub = m_node.advertise<nav_msgs::Path>("imls_path_pub_",1,true);
            m_imlsPath.header.stamp = ros::Time::now();
            m_imlsPath.header.frame_id = "odom";
            m_odomPathPub = m_node.advertise<nav_msgs::Path>("odom_path_pub_",1,true);
            m_odomPath.header.stamp = ros::Time::now();
            m_odomPath.header.frame_id = "odom";

            m_isFirstFrame = true;

            rosbag::Bag bag;
            bag.open(bagfile, rosbag::bagmode::Read);

            std::vector<std::string> topics;
            topics.push_back(std::string("/sick_scan"));
            topics.push_back(std::string("/odom"));
            rosbag::View view(bag, rosbag::TopicQuery(topics));
            //按顺序读取bag内激光的消息和里程计的消息
            BOOST_FOREACH(rosbag::MessageInstance const m, view)
            {
                champion_nav_msgs::ChampionNavLaserScanConstPtr scan = m.instantiate<champion_nav_msgs::ChampionNavLaserScan>();
                if(scan != NULL)
                    championLaserScanCallback(scan);

                nav_msgs::OdometryConstPtr odom = m.instantiate<nav_msgs::Odometry>();
                if(odom != NULL)
                    odomCallback(odom);

                ros::spinOnce();
                if(!ros::ok())
                    break;
            }
            // m_laserscanSub = m_nh.subscribe("sick_scan",5,&imlsDebug::championLaserScanCallback,this);
        }

        //将激光消息转换为激光坐标系下的二维点云
        void ConvertChampionLaserScanToEigenPointCloud(const champion_nav_msgs::ChampionNavLaserScanConstPtr& msg,
                std::vector<Eigen::Vector2d>& eigen_pts)
        {
            eigen_pts.clear();
            for(int i = 0; i < msg->ranges.size(); ++i)
            {
                if(msg->ranges[i] < msg->range_min || msg->ranges[i] > msg->range_max)
                    continue;

                double lx = msg->ranges[i] * std::cos(msg->angles[i]);
                double ly = msg->ranges[i] * std::sin(msg->angles[i]);

                if(std::isnan(lx) || std::isinf(ly) ||
                        std::isnan(ly) || std::isinf(ly))
                    continue;

                eigen_pts.push_back(Eigen::Vector2d(lx,ly));
            }
        }

        void championLaserScanCallback(const champion_nav_msgs::ChampionNavLaserScanConstPtr& msg)
        {
            if(m_isFirstFrame == true)
            {
                std::cout <<"First Frame"<<std::endl;
                m_isFirstFrame = false;
                m_prevLaserPose = Eigen::Vector3d(0, 0, 0);
                pubPath(m_prevLaserPose, m_imlsPath, m_imlsPathPub);
#if 0
                ConvertChampionLaserScanToEigenPointCloud(msg, m_prevPointCloud);
#else
                LaserScanToLDP(msg,m_prevLDP);
#endif
                return ;
            }

#if 0
            std::vector<Eigen::Vector2d> nowPts;
            ConvertChampionLaserScanToEigenPointCloud(msg, nowPts);

            //调用imls进行icp匹配，并输出结果．
            m_imlsMatcher.setSourcePointCloud(nowPts);
            m_imlsMatcher.setTargetPointCloud(m_prevPointCloud);

            Eigen::Matrix3d rPose,rCovariance;
            if(m_imlsMatcher.Match(rPose,rCovariance))
            {
                std::cout <<"IMLS Match Successful:"<<rPose(0,2)<<","<<rPose(1,2)<<","<<atan2(rPose(1,0),rPose(0,0))*57.295<<std::endl;
                Eigen::Matrix3d lastPose;
                lastPose << cos(m_prevLaserPose(2)), -sin(m_prevLaserPose(2)), m_prevLaserPose(0),
                         sin(m_prevLaserPose(2)),  cos(m_prevLaserPose(2)), m_prevLaserPose(1),
                         0, 0, 1;
                Eigen::Matrix3d nowPose = lastPose * rPose;
                m_prevLaserPose << nowPose(0, 2), nowPose(1, 2), atan2(nowPose(1,0), nowPose(0,0));
                pubPath(m_prevLaserPose, m_imlsPath, m_imlsPathPub);
            }
            else
            {
                std::cout <<"IMLS Match Failed!!!!"<<std::endl;
            }

            m_prevPointCloud = nowPts;
#else
            LDP currentLDP;
            LaserScanToLDP(msg,currentLDP);
            Vector3d d_point_scan = PIICPBetweenTwoFrames(currentLDP, Vector3d(0,0,0));
            Matrix3d lastPose, rPose;
            // 构造旋转矩阵 生成三种位姿

            lastPose << cos(m_prevLaserPose(2)), -sin(m_prevLaserPose(2)), m_prevLaserPose(0),
                     sin(m_prevLaserPose(2)),  cos(m_prevLaserPose(2)), m_prevLaserPose(1),
                     0, 0, 1;

            rPose << cos(d_point_scan(2)), -sin(d_point_scan(2)), d_point_scan(0),
                  sin(d_point_scan(2)),  cos(d_point_scan(2)), d_point_scan(1),
                  0, 0, 1;

            Eigen::Matrix3d nowPose = lastPose * rPose;
            m_prevLaserPose << nowPose(0, 2), nowPose(1, 2), atan2(nowPose(1,0), nowPose(0,0));
            pubPath(m_prevLaserPose, m_imlsPath, m_imlsPathPub);

#endif

        }

        void odomCallback(const nav_msgs::OdometryConstPtr& msg)
        {
            if(m_isFirstFrame == true)
                return;

            pubPath(msg, m_odomPath, m_odomPathPub);
        }

        //发布路径消息
        void pubPath(Eigen::Vector3d& pose, nav_msgs::Path &path, ros::Publisher &mcu_path_pub_)
        {
            ros::Time current_time = ros::Time::now();
            geometry_msgs::PoseStamped this_pose_stamped;
            this_pose_stamped.pose.position.x = pose(0);
            this_pose_stamped.pose.position.y = pose(1);

            geometry_msgs::Quaternion goal_quat = tf::createQuaternionMsgFromYaw(pose(2));
            this_pose_stamped.pose.orientation.x = goal_quat.x;
            this_pose_stamped.pose.orientation.y = goal_quat.y;
            this_pose_stamped.pose.orientation.z = goal_quat.z;
            this_pose_stamped.pose.orientation.w = goal_quat.w;

            this_pose_stamped.header.stamp = current_time;
            this_pose_stamped.header.frame_id = "odom";
            path.poses.push_back(this_pose_stamped);
            mcu_path_pub_.publish(path);
        }

        void pubPath(const nav_msgs::OdometryConstPtr& msg, nav_msgs::Path &path, ros::Publisher &mcu_path_pub_)
        {
            ros::Time current_time = ros::Time::now();
            geometry_msgs::PoseStamped this_pose_stamped;
            this_pose_stamped.pose.position.x = msg->pose.pose.position.x;
            this_pose_stamped.pose.position.y = msg->pose.pose.position.y;

            this_pose_stamped.pose.orientation.x = msg->pose.pose.orientation.x;
            this_pose_stamped.pose.orientation.y = msg->pose.pose.orientation.y;
            this_pose_stamped.pose.orientation.z = msg->pose.pose.orientation.z;
            this_pose_stamped.pose.orientation.w = msg->pose.pose.orientation.w;

            this_pose_stamped.header.stamp = current_time;
            this_pose_stamped.header.frame_id = "odom";
            path.poses.push_back(this_pose_stamped);
            mcu_path_pub_.publish(path);
        }

        bool m_isFirstFrame;
        ros::NodeHandle m_nh;
        IMLSICPMatcher m_imlsMatcher;
        Eigen::Vector3d m_prevLaserPose;
        std::vector<Eigen::Vector2d> m_prevPointCloud;
        nav_msgs::Path m_imlsPath;
        nav_msgs::Path m_odomPath;

        tf::TransformListener m_tfListener;
        ros::NodeHandle m_node;

        ros::Subscriber m_laserscanSub;
        ros::Publisher m_imlsPathPub;
        ros::Publisher m_odomPathPub;

        //进行PI-ICP需要的变量
        LDP m_prevLDP;
        sm_params m_PIICPParams;
        sm_result m_OutputResult;

        //进行pl-icp的相关函数.
        void SetPIICPParams()
        {
            //设置激光的范围
            m_PIICPParams.min_reading = 0.1;
            m_PIICPParams.max_reading = 20;

            //设置位姿最大的变化范围
            m_PIICPParams.max_angular_correction_deg = 20.0;
            m_PIICPParams.max_linear_correction = 1;

            //设置迭代停止的条件
            m_PIICPParams.max_iterations = 50;
            m_PIICPParams.epsilon_xy = 0.000001;
            m_PIICPParams.epsilon_theta = 0.0000001;

            //设置correspondence相关参数
            m_PIICPParams.max_correspondence_dist = 1;
            m_PIICPParams.sigma = 0.01;
            m_PIICPParams.use_corr_tricks = 1;

            //设置restart过程，因为不需要restart所以可以不管
            m_PIICPParams.restart = 0;
            m_PIICPParams.restart_threshold_mean_error = 0.01;
            m_PIICPParams.restart_dt = 1.0;
            m_PIICPParams.restart_dtheta = 0.1;

            //设置聚类参数
            m_PIICPParams.clustering_threshold = 0.2;

            //用最近的10个点来估计方向
            m_PIICPParams.orientation_neighbourhood = 10;

            //设置使用PI-ICP
            m_PIICPParams.use_point_to_line_distance = 1;

            //不进行alpha_test
            m_PIICPParams.do_alpha_test = 0;
            m_PIICPParams.do_alpha_test_thresholdDeg = 5;

            //设置trimmed参数 用来进行outlier remove
            m_PIICPParams.outliers_maxPerc = 0.9;
            m_PIICPParams.outliers_adaptive_order = 0.7;
            m_PIICPParams.outliers_adaptive_mult = 2.0;

            //进行visibility_test 和 remove double
            m_PIICPParams.do_visibility_test = 1;
            m_PIICPParams.outliers_remove_doubles = 1;
            m_PIICPParams.do_compute_covariance = 0;
            m_PIICPParams.debug_verify_tricks = 0;
            m_PIICPParams.use_ml_weights = 0;
            m_PIICPParams.use_sigma_weights = 0;


        }

        void LaserScanToLDP(const champion_nav_msgs::ChampionNavLaserScanConstPtr& msg, LDP& ldp)
        {
            int nPts = msg->ranges.size();
            ldp = ld_alloc_new(nPts);

            for(int i = 0;i < nPts;i++)
            {
                double dist = msg->ranges[i];
                if(dist > 0.1 && dist < 20)
                {
                    ldp->valid[i] = 1;
                    ldp->readings[i] = dist;
                }
                else
                {
                    ldp->valid[i] = 0;
                    ldp->readings[i] = -1;
                }
                ldp->theta[i] = msg->angles[i];
            }
            ldp->min_theta = ldp->theta[0];
            ldp->max_theta = ldp->theta[nPts-1];

            ldp->odometry[0] = 0.0;
            ldp->odometry[1] = 0.0;
            ldp->odometry[2] = 0.0;

            ldp->true_pose[0] = 0.0;
            ldp->true_pose[1] = 0.0;
            ldp->true_pose[2] = 0.0;
        }

        Eigen::Vector3d  PIICPBetweenTwoFrames(LDP& currentLDPScan,
                Eigen::Vector3d tmprPose)
        {
            m_prevLDP->odometry[0] = 0.0;
            m_prevLDP->odometry[1] = 0.0;
            m_prevLDP->odometry[2] = 0.0;

            m_prevLDP->estimate[0] = 0.0;
            m_prevLDP->estimate[1] = 0.0;
            m_prevLDP->estimate[2] = 0.0;

            m_prevLDP->true_pose[0] = 0.0;
            m_prevLDP->true_pose[1] = 0.0;
            m_prevLDP->true_pose[2] = 0.0;

            //设置匹配的参数值
            m_PIICPParams.laser_ref = m_prevLDP;
            m_PIICPParams.laser_sens = currentLDPScan;

            m_PIICPParams.first_guess[0] = tmprPose(0);
            m_PIICPParams.first_guess[1] = tmprPose(1);
            m_PIICPParams.first_guess[2] = tmprPose(2);

            m_OutputResult.cov_x_m = 0;
            m_OutputResult.dx_dy1_m = 0;
            m_OutputResult.dx_dy2_m = 0;

            sm_icp(&m_PIICPParams,&m_OutputResult);

            //nowPose在lastPose中的坐标
            Eigen::Vector3d  rPose;
            if(m_OutputResult.valid)
            {
                //得到两帧激光之间的相对位姿
                rPose(0)=(m_OutputResult.x[0]);
                rPose(1)=(m_OutputResult.x[1]);
                rPose(2)=(m_OutputResult.x[2]);
            }
            else
            {
                std::cout <<"PI ICP Failed!!!!!!!"<<std::endl;
                rPose = tmprPose;
            }

            //更新
            m_prevLDP = currentLDPScan;

            return rPose;

        }

};


int main(int argc, char** argv)
{
    ros::init(argc, argv, "imls_debug");

    imlsDebug imls_debug;

    ros::spin();

    return (0);
}

zzr1024
关注
  • 3
    点赞
  • 踩
  • 31
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
专栏目录
icp的源代码
11-03
著名的迭代就近点算法（icp）的源代码，在20世纪80年代中期，很多学者开始对点集数据的配准进行了大量研究。1987年，Horn[1]、Arun[2]等人用四元数法提出点集对点集配准方法。这种点集与点集坐标系匹配算法通过实践证明是一个解决复杂配准问题的关键方法。1992年，计算机视觉研究者Besl和Mckay[3]介绍了一种高层次的基于自由形态曲面的配准方法，也称为迭代就近点法ICP（Iterative Closest Point）。以点集对点集（PSTPS）配准方法为基础，他们阐述了一种曲面拟合算法，该算法是基于四元数的点集到点集配准方法。从测量点集中确定其对应的就近点点集后，运用Faugera和Hebert提出的方法计算新的就近点点集。用该方法进行迭代计算，直到残差平方和所构成的目标函数值不变，结束迭代过程。ICP配准法主要用于解决基于自由形态曲面的配准问题。
3D激光SLAM的帧间匹配方法： Point-to-Plane ICP、NDT、Feature-based Method
努力努力努力
10-18 3237
0、前言 在说Point-to-Plane ICP 之前，先介绍一下ICP： 经典ICP 形如求相机的旋转矩阵一样，ICP的目的很简单，就是求解两堆点云之间的变换关系。具体的做法是，既然不知道 R和t (针对刚体运动)，那我们就假设为未知量呗，然后通过某些方法求解。 假设有两堆点云，分别记为两个集合 X=x1,x2,…,xm 和 Y=y1,y2,…,ym（m并不总是等于n）。然后呢，我们不失一般性的，假设两个点云之间的变换为 R（旋转变换）和t（平移变换），这个问题描述成最小化均方误差： ...
激光SLAM中的帧间匹配算法
weixin_42038167的博客
11-13 2378
激光SLAM中的帧间匹配算法 帧间匹配 1.1. 什么是帧间匹配 1.2. 常用的帧间匹配 也先占个坑
激光SLAM理论与实践 第四次作业（-帧间匹配算法，imls-icp和csm）
努力是明天快乐的源泉
01-19 2332
模板2
SLAM　前端匹配算法　IMLS-ICP
one_skr的博客
07-29 1606
实际点云中隐藏着真实的曲面，如果能把这个曲面重构出来，然后选用代表性高的激光点（曲率、法向量过滤）与曲面进行匹配，则匹配精度会非常好，这就是IMLS　ICP。 １.基本思想 IMLS(implict ) 可以分为两步，一是选取激光点，二是重构曲面。 选取激光点：选取具有代表性的激光点进行匹配，与NICP类似，采用曲率、法向量来对激光点进行过滤，最好的激光点是结构化的点，也就是曲率最小的点。既能减少计算量，又能避免激光点云数据分布不均匀导致计算结果出现偏移 曲面：隐藏在点云中的曲面，不需要用表达式来求解曲面
激光SLAM理论与实践第2章
07-07
《激光SLAM理论与实践》第二章主要探讨了激光SLAM（Simultaneous Localization And Mapping，同时定位与建图）的基础概念和技术流程。这一章在深入理解SLAM的核心原理上，为后续章节的实践应用奠定了基础。以下是本...
激光SLAM理论与实践第一章
07-07
《激光SLAM理论与实践第一章》是关于机器人定位与建图技术的重要篇章，主要探讨了在现实环境中如何利用激光传感器进行自主导航和环境理解。在这个章节中，我们首先会接触到一个核心概念——“变换”，它是SLAM算法中...
激光SLAM理论与实践第7章
07-07
总结来说，《激光SLAM理论与实践第7章》将深入讲解占用网格映射的原理、算法以及实际应用中的挑战与解决方案，这对于理解和实施基于激光雷达的机器人自主导航系统至关重要。通过对这一章的学习，我们可以更好地理解...
第四课-帧间匹配算法-V4.01
08-04
在本课程“第四课-帧间匹配算法-V4.01”中，重点介绍了几种主流的帧间匹配算法，包括ICP（Iterative Closest Point）、PL-ICP、NICP（Normal Iterative Closest Point）以及IMLS-ICP。 1. ICP匹配方法： ICP是一种...
激光SLAM理论与实践
05-29
《激光SLAM理论与实践》课程作业和资料涵盖了激光SLAM（Simultaneous Localization And Mapping，同时定位与建图）这一重要领域的深度学习。激光SLAM是机器人领域的一个关键技术，主要用于移动机器人在未知环境中的...
ICP PL-ICP NICP IMLSICP
dwell548560的博客
07-02 457
imlsicp:最小化 这里用了point to plane的概念 xj是源点云中的点，yj是投影到目标点云隐藏的曲面的投影点 最小化 源点云的点到其投影点之间的距离在法向量上的投影，这个法向量与xj 最近的目标点云中点的法向量 固定一个迭代次数和收敛精度。每求出一次R t ，将这个转换矩阵作用于源点云即新到点云更新，然后再进入循环进行又一次迭代计算，直到到达收敛精度退出或迭代次数退出 xj:源点云点 yj:xj投影在目标点云隐藏曲面上的投影点 nj: 距离xj最近的目标点云中的那个...
IMLS：用于3D重构的深层隐式移动最小二乘函数（CVPR2021）
3D视觉工坊
10-28 1225
点击上方“3D视觉工坊”，选择“星标”干货第一时间送达作者丨paopaoslam来源丨泡泡机器人SLAM标题：Deep Implicit Moving Least-Squares Func...
ROS kinetic下laser_scan_matcher 的安装
Ocean_JH的博客
03-30 2369
ROS kinectic下scan_tool的安装csm部分scan_tool部分 参考 ：https://blog.csdn.net/zhu751191958/article/details/80165925 在开始只下载scan_tool的时候，执行catkin_make，会出现一堆报错，其中就有找不到csm包。经过查询实验，只用解决csm包就能正确catkin_make。整个步骤大概分为三步...
激光SLAM从理论到实践学习——第四节（激光SLAM的前端配准方法）
摆正心态
11-20 3260
激光的前端配准算法（帧间匹配算法） 前端配准在视觉内叫做Tracking或者帧间匹配，对激光SLAM是有非常大的影响的 帧间匹配不一定说的是前后两帧进行匹配，也可以是任意帧之间进匹配 是一个Map—>Scan的过程，一个Scan和一个Map去匹配的过程 常用的方法有下面四种 ICP匹配方法（对上节ICP算法的证明） PL-ICP匹配方法（与ICP算法相似，只是说误差是之间的...
karto算法论文阅读及代码讲解-扫描匹配（correlative scan matching,csm）
zhangkkit的博客
03-21 4849
扫描匹配算法csm，correlative scan matching1.论文阅读   本博客前部分论文阅读，是对论文的高度概括。 1.论文阅读 图2 概率扫描匹配的图形模型   根据图2，已知x𝑖-1 和z𝑖-1 ，也测量到了u 和 z𝑖 ，如何尽可能准确的求出x𝑖 ？   数学模型： p(xi|xi-1,u,zi-1,zi)   应用高斯分布：p(xi|xi-1,u,zi-1,zi) 近似等于p(z|xi,m) p(xi|xi-1,u) 。其中p(xi|xi-1,u) 是我们熟悉的运动
激光SLAM的发展和应用趋势
weixin_46777885的博客
04-21 4218
激光SLAM的发展和应用趋势
深蓝学院激光SLAM学习（一）介绍、发展
a063225的博客
12-10 2139
激光SLAM介绍 输入 IMU ： 角度信息 里程计 ： 距离信息 2D激光雷达 输出 栅格地图 机器人轨迹 帧间匹配方法 PI-ICP ：较常用的匹配算法，ICP基本不算使用 CSM ： cartographer使用 梯度优化方法： hector-slam使用的就是这个 使用效果比较好的是CSM（粗匹配）+梯度优化（细匹配），cartographer使用的就是组合 回环检测方法 s...
激光slam学习——第二章（里程计标定）习题
lievech的博客
04-12 2446
深蓝学院激光slam学习——第二章（里程计标定）习题题目描述可能出现的问题第二种解决思路：第三种解决思路： 题目描述 本次的作业为用直接线性方法来对机器人的里程计进行校正。 给出的文件中包含有本次作业使用的bag数据，路径为odom_ws/bag/odom.bag。 本次的作业中，需要实现三个函数，分别为： Main.cpp，第340行中的cal_delta_distance()函数，该函数的功能为给定两个里程计位姿，计算这两个位姿之间的位姿差。 Odom_Calib.cpp，第23行Add_Data()
EAI激光雷达X4使用gmapping与laser_scan_matcher建图（一）
zhu751191958的博客
05-02 6626
由于底盘部分没处理好，想了个偏门的法子，就是利用雷达扫描与周围环境匹配实现位姿估计，将位姿估计当做里程计提供给gmapping进行建图，经过博主测试，使用gmapping+laser_scan_matcher建图在环境特征比较明显的地方建图效果特别好，尤其是在比较复杂的室内环境，凌乱一点也行，缺点呢，在长廊这种特征环境不是特别明显的地方，建图的时候位姿估计得不是很精确，因为环境特征比较单一，容易出...
激光slam理论与实践 第一周作业
最新发布
10-19
激光SLAM（Simultaneous Localization and Mapping）是指通过利用激光扫描仪的数据同时实现机器人的自我定位和环境建图的过程。这项技术已经成为机器人领域中重要的研究内容之一。 在激光SLAM理论方面，主要有几个关键的概念。首先是地图构建，机器人通过扫描周围环境，将获取到的激光点云数据转化为一幅地图。同时，激光SLAM也需要实现机器人的同时自我定位，也就是在未知环境中，机器人通过分析激光数据推算出其自身的位置。 实践方面，激光SLAM需要激光传感器进行环境测量。激光传感器会在扫描过程中发射激光束，然后通过接收反射回来的激光束，来计算击中目标物体的位置。机器人通过不断地旋转或移动激光传感器，以此来获取周围环境的激光点云数据。 激光SLAM的实施过程主要包括建图、定位和配准等步骤。建图过程中，机器人通过收集周围环境的激光数据，将其转化为一幅地图。定位过程中，机器人通过对比当前获得的激光数据和已有的地图数据，从而推算出自身的位置。配准是指将不同位置、角度下获取的激光数据进行融合，从而得到整体一致性的地图。 总的来说，激光SLAM理论与实践是通过利用激光扫描仪的数据实现机器人定位以及地图构建的过程。通过激光传感器扫描环境，将激光数据转化为地图，并实现机器人的同时自我定位。激光SLAM技术在无人驾驶、工业自动化等领域有重要的应用价值。
评论 1
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

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

抵扣说明：

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

余额充值