如何将每个框架插入到SLAM框架中

0. 简介

LinK3D、CSF、BALM这几个都是非常方便去插入到激光SLAM框架的。这里我们会分别从多个角度来介绍如何将每个框架插入到SLAM框架中

1. LinK3D:三维LiDAR点云的

线性关键点表示

LinK3D的核心思想和基于我们的LinK3D的两个LiDAR扫描的匹配结果。绿色线是有效匹配。当前关键点（黑色，CK）的描述符用其相邻关键点来表示。描述符的每个维度对应于扇区区域。第一维度对应于当前关键点的最近关键点所在的扇区区域（蓝色和红色、CK的最近关键点），并且其他维度对应于以逆时针顺序布置的区域。如果在扇区区域中存在关键点，则搜索扇区区域中最近的关键点（紫色和橙色，扇区中CK的最近关键点）并将其用于表示描述符的对应维度。

2. Link3D数据植入

这里的Link3D数据植入其实在外围调用就这些内容，当中AggregationKeypoints_LinK3D存储的是存当前点云中的聚类后的关键点。而pCurrentFrame_LinK3D对应的则是点云帧。该函数中利用LinK3D仿函数执行了提取边缘点，聚类，计算描述子的操作。其实主要实现的都是LinK3D提取器。

    //在这里植入LinK3D，把接收到的点云数据用LinK3D提取边缘点和描述子，发布关键点数据，打印输出描述子
    //LinK3D提取器
    BoW3D::LinK3D_Extractor* pLinK3dExtractor(new BoW3D::LinK3D_Extractor(nScans, scanPeriod_LinK3D, minimumRange, distanceTh, matchTh)); 
    //创建点云帧,该函数中利用LinK3D仿函数执行了提取边缘点，聚类，计算描述子的操作
    Frame* pCurrentFrame_LinK3D(new Frame(pLinK3dExtractor, plaserCloudIn_LinK3D));
    //此时pCurrentFrame_LinK3D这个类指针中包含了边缘点，聚类，描述子的信息
//测试 输出关键点数量和第一个关键点信息 正常输出 
// cout << "------------------------" << endl << "关键点数量:" << pCurrentFrame_LinK3D->mvAggregationKeypoints.size();
// cout << "第一个关键点信息x坐标" << pCurrentFrame_LinK3D->mvAggregationKeypoints[0].x;
    //存当前点云中的聚类后的关键点
    AggregationKeypoints_LinK3D->points.insert(AggregationKeypoints_LinK3D->points.end(), pCurrentFrame_LinK3D->mvAggregationKeypoints.begin(), pCurrentFrame_LinK3D->mvAggregationKeypoints.end());
//测试 输出点云中信息 也能正常输出
// cout << "------------------------" << endl << "关键点数量:" << AggregationKeypoints_LinK3D->points.size();
// cout << "第一个关键点信息x坐标" << AggregationKeypoints_LinK3D->points[0].x;
    // 2.对描述子进行匹配 3.使用匹配对进行帧间icp配准 pPreviousFrame是上一个link3d Frame帧 pCurrentFrame_LinK3D是当前link3d Frame帧
    // 获取上一帧和当前帧之间的匹配索引
     vector> vMatchedIndex; 
    pLinK3dExtractor->match(pCurrentFrame_LinK3D->mvAggregationKeypoints, pPreviousFrame->mvAggregationKeypoints, pCurrentFrame_LinK3D->mDescriptors, pPreviousFrame->mDescriptors, vMatchedIndex);
    //仿照BoW3D函数写一个帧间ICP匹配函数求出R,t
    int returnValue = 0;
    // 进行帧间ICP匹配 求当前帧到上一帧的位姿变换
    // 这里求的R t是当前帧点云到上一帧点云的位姿变换
    returnValue = pose_estimation_3d3d(pCurrentFrame_LinK3D, pPreviousFrame, vMatchedIndex, RelativeR, Relativet, pLinK3dExtractor);
    //至此获得了当前帧点云到上一帧点云的位姿变换


    //当前帧Frame用完以后，赋值给上一帧Frame,赋值前先把要丢掉的帧内存释放
    //这里Frame里有成员指针，析构函数里delete成员指针
    delete pPreviousFrame;
    pPreviousFrame = pCurrentFrame_LinK3D;
    //LinK3D 植入结束

点云帧配置如下，其实可以看到这里面没有太多的内容，主要还是调用Link3D中的void LinK3D_Extractor::Ptr pLaserCloudIn, vector &keyPoints, cv::Mat &descriptors, ScanEdgePoints &validCluster)函数。并获取关键点、描述子还有聚类信息。具体的实现与具体论文保持一致，可以看Github内容

  //静态（全局？）变量要在这里初始化
  long unsigned int Frame::nNextId = 0;


  Frame::Frame(LinK3D_Extractor* pLink3dExtractor, pcl::PointCloud::Ptr pLaserCloudIn):mpLink3dExtractor(pLink3dExtractor)
  {
    mnId = nNextId++; 


    (*mpLink3dExtractor)(pLaserCloudIn, mvAggregationKeypoints, mDescriptors, mClusterEdgeKeypoints);
  }

2. CSF“布料”滤波算法

然后下面就是CSF的处理，这里其实可以将内容加在BALM当中，Github。因为CSF其实作用是区分地面点的作用

 // 
 CSF csf;
 csf.params.iterations = 600;
 csf.params.time_step = 0.95;
 csf.params.cloth_resolution = 3;
 csf.params.bSloopSmooth = false;


 csf.setPointCloud(*laserCloudSurfLast);
 // pcl::savePCDFileBinary(map_save_directory, *SurfFrame);


 std::vector groundIndexes, offGroundIndexes;
 // 输出的是vector类型的地面点和非地面点索引
 pcl::PointCloud::Ptr groundFrame(new pcl::PointCloud);
 pcl::PointCloud::Ptr offGroundFrame(new pcl::PointCloud);
 csf.do_filtering(groundIndexes, offGroundIndexes);
 pcl::copyPointCloud(*laserCloudSurfLast, groundIndexes, *groundFrame);
 pcl::copyPointCloud(*laserCloudSurfLast, offGroundIndexes, *offGroundFrame);

实际用一句话：把点云翻过来，罩上一块不同材质的布料，就可以得到地面了。参考实际物理的布，布料上的点之间存在不同的作用力，详细可以参考这篇文章：https://www.guyuehome.com/40977

3. BALM

对于BALM其实主要分为三块，balm_front、scan2map、balm_back这三个流程。我们一开始使用的是ALOAM的前端来提取激光里程计信息，然后scan2map中加入CSF来进一步区分地面点。最后分成三类传入到BA约束当中https://blog.csdn.net/lovely_yoshino/article/details/133940976。

while(n.ok())
  {
    ros::spinOnce();
    if(corn_buf.empty() || ground_buf.empty() || odom_buf.empty() || offground_buf.empty())
    {
      continue;
    }


    mBuf.lock();
    uint64_t time_corn = corn_buf.front()->header.stamp.toNSec();
    uint64_t time_ground = ground_buf.front()->header.stamp.toNSec();
    uint64_t time_odom = odom_buf.front()->header.stamp.toNSec();
    uint64_t time_offground = offground_buf.front()->header.stamp.toNSec();
    if(time_odom != time_corn)
    {
      time_odom < time_corn ? odom_buf.pop() : corn_buf.pop();
            mBuf.unlock();
            continue;
        }


        if(time_odom != time_ground)
        {
            time_odom < time_ground ? odom_buf.pop() : ground_buf.pop();
            mBuf.unlock();
            continue;
        }


        if(time_odom != time_offground)
        {
            time_odom < time_ground ? odom_buf.pop() : ground_buf.pop();
            mBuf.unlock();
            continue;
        }


        ros::Time ct(ground_buf.front()->header.stamp);
    pcl::PointCloud::Ptr pl_ground_temp(new pcl::PointCloud);
    pcl::PointCloud::Ptr pl_edge_temp(new pcl::PointCloud);
    pcl::PointCloud::Ptr pl_offground_temp(new pcl::PointCloud);


    rosmsg2ptype(*ground_buf.front(), *pl_ground);
    rosmsg2ptype(*corn_buf.front(), *pl_corn);
    rosmsg2ptype(*offground_buf.front(), *pl_offground);


    //pcl::savePCDFileBinary("/home/wb/FALOAMBA_WS/wb/Map/map.pcd", *pl_ground);


    //pl_ground还有用，所以这里复制出一个新点云
    *pl_ground_temp = *pl_ground;
    *pl_edge_temp = *pl_corn;
    *pl_offground_temp = *pl_offground;
    corn_buf.pop(); ground_buf.pop(); offground_buf.pop();


    q_odom.w() = odom_buf.front()->pose.pose.orientation.w;
    q_odom.x() = odom_buf.front()->pose.pose.orientation.x;
    q_odom.y() = odom_buf.front()->pose.pose.orientation.y;
    q_odom.z() = odom_buf.front()->pose.pose.orientation.z;
    t_odom.x() = odom_buf.front()->pose.pose.position.x;
    t_odom.y() = odom_buf.front()->pose.pose.position.y;
    t_odom.z() = odom_buf.front()->pose.pose.position.z;
    odom_buf.pop();
    mBuf.unlock();


    // T_curr2last = T_curr2w * T_last2w¯¹
    Eigen::Vector3d delta_t(q_last.matrix().transpose()*(t_odom-t_last));
    Eigen::Quaterniond delta_q(q_last.matrix().transpose() * q_odom.matrix());
    q_last = q_odom;
    t_last = t_odom;


    // T_curr2last * I
    t_gather_pose = t_gather_pose + q_gather_pose * delta_t;
    q_gather_pose = q_gather_pose * delta_q;
    if(jump_flag < skip_num)
        {
            jump_flag++;
            continue;
        }
        jump_flag = 0;


        if(plcount == 0)// 第一帧
        {
            // 第一帧：T_curr2w = T_curr2last
            q_poses.push_back(q_gather_pose);
            t_poses.push_back(t_gather_pose);
        }
        else// 第二帧
        {
            // T_1_2_0 * T_2_2_1 = T_2_2_0
            // T_2_2_0 * T_3_2_2 = T_3_2_0
            q_poses.push_back(q_poses[plcount-1]*q_gather_pose);
            t_poses.push_back(t_poses[plcount-1] + q_poses[plcount-1] * t_gather_pose);
        }


        parray.header.stamp = ct;
        geometry_msgs::Pose apose;
        apose.orientation.w = q_poses[plcount].w();
        apose.orientation.x = q_poses[plcount].x();
        apose.orientation.y = q_poses[plcount].y();
        apose.orientation.z = q_poses[plcount].z();
        apose.position.x = t_poses[plcount].x();
        apose.position.y = t_poses[plcount].y();
        apose.position.z = t_poses[plcount].z();


        // ---------------------------- 当前帧位姿(优化前的)--------------------------------
        nav_msgs::Odometry laser_odom;
        laser_odom.header.frame_id = "camera_init";
        laser_odom.child_frame_id = "aft_BA";
        laser_odom.header.stamp = ct;
        laser_odom.pose.pose.orientation.x = apose.orientation.x;
        laser_odom.pose.pose.orientation.y = apose.orientation.y;
        laser_odom.pose.pose.orientation.z = apose.orientation.z;
        laser_odom.pose.pose.orientation.w = apose.orientation.w;
        laser_odom.pose.pose.position.x = apose.position.x;
        laser_odom.pose.pose.position.y = apose.position.y;
        laser_odom.pose.pose.position.z = apose.position.z;
        //发布优化前的位姿
        pub_odom.publish(laser_odom);
        //发布坐标关系
        static tf::TransformBroadcaster br;
        tf::Transform transform;
        tf::Quaternion q;
        transform.setOrigin(tf::Vector3(apose.position.x, apose.position.y, apose.position.z));
        q.setW(apose.orientation.w);
        q.setX(apose.orientation.x);
        q.setY(apose.orientation.y);
        q.setZ(apose.orientation.z);
        transform.setRotation(q);
        br.sendTransform(tf::StampedTransform(transform, laser_odom.header.stamp, "camera_init", "aft_BA"));
        parray.poses.push_back(apose);


        // 发布优化前的位姿
        pub_pose.publish(parray);


        pl_ground_buf.push_back(pl_ground_temp);
        pl_edge_buf.push_back(pl_edge_temp);
        pl_offground_buf.push_back(pl_offground_temp);

审核编辑：黄飞