如何將每個(gè)框架插入到SLAM框架中

0. 簡(jiǎn)介

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

1. LinK3D:三維LiDAR點(diǎn)云的

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

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

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

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

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


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

點(diǎn)云幀配置如下，其實(shí)可以看到這里面沒有太多的內(nèi)容，主要還是調(diào)用Link3D中的void LinK3D_Extractor::Ptr pLaserCloudIn, vector &keyPoints, cv::Mat &descriptors, ScanEdgePoints &validCluster)函數(shù)。并獲取關(guān)鍵點(diǎn)、描述子還有聚類信息。具體的實(shí)現(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的處理，這里其實(shí)可以將內(nèi)容加在BALM當(dāng)中，Github。因?yàn)镃SF其實(shí)作用是區(qū)分地面點(diǎn)的作用

 // 
 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類型的地面點(diǎn)和非地面點(diǎn)索引
 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);

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

3. BALM

對(duì)于BALM其實(shí)主要分為三塊，balm_front、scan2map、balm_back這三個(gè)流程。我們一開始使用的是ALOAM的前端來提取激光里程計(jì)信息，然后scan2map中加入CSF來進(jìn)一步區(qū)分地面點(diǎn)。最后分成三類傳入到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ù)制出一個(gè)新點(diǎn)云
    *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);

審核編輯：黃飛