如何將每個框架插入到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ˉ1
    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);

審核編輯：黃飛