如何將每個框架插入到SLAM框架中

0. 簡介

LinK3D、CSF、BALM這幾個都是非常方便去插入到激光SLAM框架的。這里我們會分別從多個角度來介紹如何將每個框架插入到SLAM框架中

1. LinK3D:三維LiDAR點云的

線性關(guān)鍵點表示

LinK3D的核心思想和基于我們的LinK3D的兩個LiDAR掃描的匹配結(jié)果。綠色線是有效匹配。當(dāng)前關(guān)鍵點（黑色，CK）的描述符用其相鄰關(guān)鍵點來表示。描述符的每個維度對應(yīng)于扇區(qū)區(qū)域。第一維度對應(yīng)于當(dāng)前關(guān)鍵點的最近關(guān)鍵點所在的扇區(qū)區(qū)域（藍(lán)色和紅色、CK的最近關(guān)鍵點），并且其他維度對應(yīng)于以逆時針順序布置的區(qū)域。如果在扇區(qū)區(qū)域中存在關(guān)鍵點，則搜索扇區(qū)區(qū)域中最近的關(guān)鍵點（紫色和橙色，扇區(qū)中CK的最近關(guān)鍵點）并將其用于表示描述符的對應(yīng)維度。

2. Link3D數(shù)據(jù)植入

這里的Link3D數(shù)據(jù)植入其實在外圍調(diào)用就這些內(nèi)容，當(dāng)中AggregationKeypoints_LinK3D存儲的是存當(dāng)前點云中的聚類后的關(guān)鍵點。而pCurrentFrame_LinK3D對應(yīng)的則是點云幀。該函數(shù)中利用LinK3D仿函數(shù)執(zhí)行了提取邊緣點，聚類，計算描述子的操作。其實主要實現(xiàn)的都是LinK3D提取器。

    //在這里植入LinK3D，把接收到的點云數(shù)據(jù)用LinK3D提取邊緣點和描述子，發(fā)布關(guān)鍵點數(shù)據(jù)，打印輸出描述子
    //LinK3D提取器
    BoW3D::LinK3D_Extractor* pLinK3dExtractor(new BoW3D::LinK3D_Extractor(nScans, scanPeriod_LinK3D, minimumRange, distanceTh, matchTh)); 
    //創(chuàng)建點云幀,該函數(shù)中利用LinK3D仿函數(shù)執(zhí)行了提取邊緣點，聚類，計算描述子的操作
    Frame* pCurrentFrame_LinK3D(new Frame(pLinK3dExtractor, plaserCloudIn_LinK3D));
    //此時pCurrentFrame_LinK3D這個類指針中包含了邊緣點，聚類，描述子的信息
//測試 輸出關(guān)鍵點數(shù)量和第一個關(guān)鍵點信息 正常輸出 
// cout << "------------------------" << endl << "關(guān)鍵點數(shù)量:" << pCurrentFrame_LinK3D->mvAggregationKeypoints.size();
// cout << "第一個關(guān)鍵點信息x坐標(biāo)" << pCurrentFrame_LinK3D->mvAggregationKeypoints[0].x;
    //存當(dāng)前點云中的聚類后的關(guān)鍵點
    AggregationKeypoints_LinK3D->points.insert(AggregationKeypoints_LinK3D->points.end(), pCurrentFrame_LinK3D->mvAggregationKeypoints.begin(), pCurrentFrame_LinK3D->mvAggregationKeypoints.end());
//測試 輸出點云中信息 也能正常輸出
// cout << "------------------------" << endl << "關(guān)鍵點數(shù)量:" << AggregationKeypoints_LinK3D->points.size();
// cout << "第一個關(guān)鍵點信息x坐標(biāo)" << AggregationKeypoints_LinK3D->points[0].x;
    // 2.對描述子進行匹配 3.使用匹配對進行幀間icp配準(zhǔn) pPreviousFrame是上一個link3d Frame幀 pCurrentFrame_LinK3D是當(dāng)前l(fā)ink3d Frame幀
    // 獲取上一幀和當(dāng)前幀之間的匹配索引
     vector> vMatchedIndex; 
    pLinK3dExtractor->match(pCurrentFrame_LinK3D->mvAggregationKeypoints, pPreviousFrame->mvAggregationKeypoints, pCurrentFrame_LinK3D->mDescriptors, pPreviousFrame->mDescriptors, vMatchedIndex);
    //仿照BoW3D函數(shù)寫一個幀間ICP匹配函數(shù)求出R,t
    int returnValue = 0;
    // 進行幀間ICP匹配 求當(dāng)前幀到上一幀的位姿變換
    // 這里求的R t是當(dāng)前幀點云到上一幀點云的位姿變換
    returnValue = pose_estimation_3d3d(pCurrentFrame_LinK3D, pPreviousFrame, vMatchedIndex, RelativeR, Relativet, pLinK3dExtractor);
    //至此獲得了當(dāng)前幀點云到上一幀點云的位姿變換


    //當(dāng)前幀F(xiàn)rame用完以后，賦值給上一幀F(xiàn)rame,賦值前先把要丟掉的幀內(nèi)存釋放
    //這里Frame里有成員指針，析構(gòu)函數(shù)里delete成員指針
    delete pPreviousFrame;
    pPreviousFrame = pCurrentFrame_LinK3D;
    //LinK3D 植入結(jié)束

點云幀配置如下，其實可以看到這里面沒有太多的內(nèi)容，主要還是調(diào)用Link3D中的void LinK3D_Extractor::Ptr pLaserCloudIn, vector &keyPoints, cv::Mat &descriptors, ScanEdgePoints &validCluster)函數(shù)。并獲取關(guān)鍵點、描述子還有聚類信息。具體的實現(xiàn)與具體論文保持一致，可以看Github內(nèi)容

  //靜態(tài)（全局？）變量要在這里初始化
  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的處理，這里其實可以將內(nèi)容加在BALM當(dāng)中，Github。因為CSF其實作用是區(qū)分地面點的作用

 // 
 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);

實際用一句話：把點云翻過來，罩上一塊不同材質(zhì)的布料，就可以得到地面了。參考實際物理的布，布料上的點之間存在不同的作用力，詳細(xì)可以參考這篇文章：https://www.guyuehome.com/40977

3. BALM

對于BALM其實主要分為三塊，balm_front、scan2map、balm_back這三個流程。我們一開始使用的是ALOAM的前端來提取激光里程計信息，然后scan2map中加入CSF來進一步區(qū)分地面點。最后分成三類傳入到BA約束當(dāng)中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還有用，所以這里復(fù)制出一個新點云
    *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();


 ? ? ? ?// ---------------------------- 當(dāng)前幀位姿(優(yōu)化前的)--------------------------------
 ? ? ? ?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;
 ? ? ? ?//發(fā)布優(yōu)化前的位姿
 ? ? ? ?pub_odom.publish(laser_odom);
 ? ? ? ?//發(fā)布坐標(biāo)關(guān)系
 ? ? ? ?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);


 ? ? ? ?// 發(fā)布優(yōu)化前的位姿
 ? ? ? ?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);

審核編輯：黃飛