本文旨在幫助讀者快速了解經典的3D雷射SLAM方法：A-LOAM！筆者在閱讀時主要參考了LOAM筆記及A-LOAM源碼閱讀。廢話不多說，直接上代碼！
//基于前述的4種feature進行幀與幀的點雲特征配準，即簡單的Lidar Odometry
#include <cmath>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf/transform_datatypes.h>
#include <tf/transform_broadcaster.h>
#include <eigen3/Eigen/Dense>
#include <mutex>
#include <queue>
#include "aloam_velodyne/common.h"
#include "aloam_velodyne/tic_toc.h"
#include "lidarFactor.hpp"
#define DISTORTION 0
int corner_correspondence = 0, plane_correspondence = 0;
constexpr double SCAN_PERIOD = 0.1; //建議凡是「常量」語義的場景都使用 constexpr，隻對「隻讀」語義使用 const
constexpr double DISTANCE_SQ_THRESHOLD = 25;
constexpr double NEARBY_SCAN = 2.5;
int skipFrameNum = 5;
bool systemInited = false;
double timeCornerPointsSharp = 0;
double timeCornerPointsLessSharp = 0;
double timeSurfPointsFlat = 0;
double timeSurfPointsLessFlat = 0;
double timeLaserCloudFullRes = 0;
//建立KdtreeFLANN對象
pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeCornerLast(new pcl::KdTreeFLANN<pcl::PointXYZI>());
pcl::KdTreeFLANN<pcl::PointXYZI>::Ptr kdtreeSurfLast(new pcl::KdTreeFLANN<pcl::PointXYZI>());
//typedef pcl::PointXYZI PointType; 以下定義的4種點雲為scanRegisteration中或取到的4種特征點雲
pcl::PointCloud<PointType>::Ptr cornerPointsSharp(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr cornerPointsLessSharp(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surfPointsFlat(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr surfPointsLessFlat(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudCornerLast(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudSurfLast(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr laserCloudFullRes(new pcl::PointCloud<PointType>());
int laserCloudCornerLastNum = 0;
int laserCloudSurfLastNum = 0;
// Lidar Odometry線程估計的frame在world坐标系下的位姿P，Transformation from current frame to world frame
Eigen::Quaterniond q_w_curr(1, 0, 0, 0); //旋轉四元數q
Eigen::Vector3d t_w_curr(0, 0, 0); //位移t
// 點雲特征比對時的待優化變量， q_curr_last(x, y, z, w), t_curr_last
double para_q[4] = {0, 0, 0, 1};
double para_t[3] = {0, 0, 0};
//下面兩個分别是優化變量para_q和para_t的映射：表示的是兩個world坐标系下的位姿P之間的增量，例如，△P = P0.inverse() * P1
Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q);//Eigen::Map可以了解為映射/引用
Eigen::Map<Eigen::Vector3d> t_last_curr(para_t);
std::queue<sensor_msgs::PointCloud2ConstPtr> cornerSharpBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> cornerLessSharpBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> surfFlatBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> surfLessFlatBuf;
std::queue<sensor_msgs::PointCloud2ConstPtr> fullPointsBuf;
std::mutex mBuf; //定義互斥鎖
// undistort lidar point
void TransformToStart(PointType const *const pi, PointType *const po)
// TransformToStart：将目前幀Lidar坐标系下的點雲變換到上一幀Lidar坐标系下
{
//interpolation ratio
//如果點雲沒有去除畸變，用slerp內插補點的方式計算出每個點在fire時刻的位姿，然後進行TransformToStart的坐标變換，一方面實作了變換到上一幀Lidar坐标系下；另一方面也可以了解成點都将fire時刻統一到了開始時刻，即去除了畸變，完成了運動補償
double s;
if (DISTORTION)
s = (pi->intensity - int(pi->intensity)) / SCAN_PERIOD; //SCAN_PERIOD=0.1，intensity的整數部分存放scanID，小數部分存放歸一化後的fireID，int強轉向下取整。是以s=fireID/0.1
else
s = 1.0;
//s = 1;
Eigen::Quaterniond q_point_last = Eigen::Quaterniond::Identity().slerp(s, q_last_curr);//Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q)，表示坐标變換的旋轉增量
Eigen::Vector3d t_point_last = s * t_last_curr; //Eigen::Map<Eigen::Vector3d> t_last_curr(para_t)，表示坐标變換的位移增量
Eigen::Vector3d point(pi->x, pi->y, pi->z);
Eigen::Vector3d un_point = q_point_last * point + t_point_last;
po->x = un_point.x();
po->y = un_point.y();
po->z = un_point.z();
po->intensity = pi->intensity;
}
// transform all lidar points to the start of the next frame
//将輸入點轉為下一幀下的坐标
void TransformToEnd(PointType const *const pi, PointType *const po)
{
// undistort point first
pcl::PointXYZI un_point_tmp;
TransformToStart(pi, &un_point_tmp); //pi轉到上一幀Lidar坐标系下，轉為un_point_tmp
Eigen::Vector3d un_point(un_point_tmp.x, un_point_tmp.y, un_point_tmp.z);//un_point_tmp轉為un_point向量
Eigen::Vector3d point_end = q_last_curr.inverse() * (un_point - t_last_curr);//Eigen::Map<Eigen::Vector3d> t_last_curr(para_t)
po->x = point_end.x();
po->y = point_end.y();
po->z = point_end.z();
//Remove distortion time info
po->intensity = int(pi->intensity);
}
//接下來五個Handler函數為接受5個topic的回調函數，作用是将消息緩存到對應的queue中
//receive sharp pointcloud
void laserCloudSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsSharp2)
{
mBuf.lock(); //互斥鎖被鎖定。線程申請該互斥鎖，如果未能獲得該互斥鎖，則調用線程将阻塞(block)在該互斥鎖上；如果成功獲得該互訴鎖，該線程一直擁有該互斥鎖直到調用unlock解鎖；如果該互斥鎖已經被目前調用線程鎖住，則産生死鎖(deadlock)。
cornerSharpBuf.push(cornerPointsSharp2); //将corner_sharp點加入到cornerSharpBuf中
mBuf.unlock(); //解鎖，釋放調用線程對該互斥鎖的所有權。
}
//receive less_sharp pointcloud
void laserCloudLessSharpHandler(const sensor_msgs::PointCloud2ConstPtr &cornerPointsLessSharp2)
{
mBuf.lock();
cornerLessSharpBuf.push(cornerPointsLessSharp2);//将corner_less_sharp點加入到CornerLessSharpBuf中
mBuf.unlock();
}
//receive flat pointcloud
void laserCloudFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsFlat2)
{
mBuf.lock();
surfFlatBuf.push(surfPointsFlat2);//将surf_flat點加入到surfFlatBuf中
mBuf.unlock();
}
//receive less_flat pointcloud
void laserCloudLessFlatHandler(const sensor_msgs::PointCloud2ConstPtr &surfPointsLessFlat2)
{
mBuf.lock();
surfLessFlatBuf.push(surfPointsLessFlat2);//将surf_less_flat點加入到surfLessFireBuf中
mBuf.unlock();
}
//receive all pointcloud
void laserCloudFullResHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudFullRes2)
{
mBuf.lock();
fullPointsBuf.push(laserCloudFullRes2);//将所有點laserCloud點加入到fullPointsBuf中
mBuf.unlock();
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "laserOdometry");
ros::NodeHandle nh;
nh.param<int>("mapping_skip_frame", skipFrameNum, 2); //在節點初始化之後擷取參數伺服器的參數，并設定初始值，launch檔案中param name調用
printf("Mapping %d Hz \n", 10 / skipFrameNum);
ros::Subscriber subCornerPointsSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100, laserCloudSharpHandler);
ros::Subscriber subCornerPointsLessSharp = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100, laserCloudLessSharpHandler);
ros::Subscriber subSurfPointsFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100, laserCloudFlatHandler);
ros::Subscriber subSurfPointsLessFlat = nh.subscribe<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100, laserCloudLessFlatHandler);
ros::Subscriber subLaserCloudFullRes = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100, laserCloudFullResHandler);
ros::Publisher pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_corner_last", 100);
ros::Publisher pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 100);
ros::Publisher pubLaserCloudFullRes = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_3", 100);
ros::Publisher pubLaserOdometry = nh.advertise<nav_msgs::Odometry>("/laser_odom_to_init", 100);
ros::Publisher pubLaserPath = nh.advertise<nav_msgs::Path>("/laser_odom_path", 100);
nav_msgs::Path laserPath;
int frameCount = 0;
ros::Rate rate(100);
while (ros::ok())
{
ros::spinOnce();
if (!cornerSharpBuf.empty() && !cornerLessSharpBuf.empty() &&
!surfFlatBuf.empty() && !surfLessFlatBuf.empty() &&
!fullPointsBuf.empty())
{
//記錄四種特征點雲中的擷取到的第一個點的時間戳
timeCornerPointsSharp = cornerSharpBuf.front()->header.stamp.toSec();
timeCornerPointsLessSharp = cornerLessSharpBuf.front()->header.stamp.toSec(); //queue.front() 傳回第一個元素
timeSurfPointsFlat = surfFlatBuf.front()->header.stamp.toSec();
timeSurfPointsLessFlat = surfLessFlatBuf.front()->header.stamp.toSec();
timeLaserCloudFullRes = fullPointsBuf.front()->header.stamp.toSec();
if (timeCornerPointsSharp != timeLaserCloudFullRes ||
timeCornerPointsLessSharp != timeLaserCloudFullRes ||
timeSurfPointsFlat != timeLaserCloudFullRes ||
timeSurfPointsLessFlat != timeLaserCloudFullRes)
{
printf("unsync messeage!");
ROS_BREAK();
}
mBuf.lock();
cornerPointsSharp->clear(); //清空cornerPointSharp中的所有點雲
pcl::fromROSMsg(*cornerSharpBuf.front(), *cornerPointsSharp);//将ROS系統接收到的cornerSharpBuf中的首元素（一幀點雲）轉存到cornerPointsSharp
cornerSharpBuf.pop();//删除cornerSharpBuf中的首元素
cornerPointsLessSharp->clear();
pcl::fromROSMsg(*cornerLessSharpBuf.front(), *cornerPointsLessSharp);//将CornerLessSharpBuf中的首元素轉存到CornerPointsLessSharp
cornerLessSharpBuf.pop(); //删除CornerLessSharpBuf中的首元素
surfPointsFlat->clear();
pcl::fromROSMsg(*surfFlatBuf.front(), *surfPointsFlat);//将surfFlatBuf中的首元素轉存到surfPointsFlat
surfFlatBuf.pop();//删除surfFlatBuf的首元素
surfPointsLessFlat->clear();
pcl::fromROSMsg(*surfLessFlatBuf.front(), *surfPointsLessFlat);//将surfLessFlatBuf中的首元素轉存到surfPointsLessFlat
surfLessFlatBuf.pop();//删除surfLessFlatBuf的首元素
laserCloudFullRes->clear();
pcl::fromROSMsg(*fullPointsBuf.front(), *laserCloudFullRes);//将fullPointsBuf中的首元素轉存到laserCloudFullRes
fullPointsBuf.pop();//删除fullPointsBuf的首元素
mBuf.unlock();
//這三句：.clear()、.fromROSmsg與.pop()都處在循環之中，每次循環都會清空A，将B首元素添加到A，再删除B的首元素，這樣就實作了B的周遊并且保證每次A中隻有一個元素
TicToc t_whole;
// initializing
if (!systemInited) //第一幀不進行比對，僅僅将CornerPointsLessSharp儲存到laserCloudCornerLast，将surfPointLessFlat儲存至laserCloudSurfLast，為下次比對提供target
{
systemInited = true;
std::cout << "Initialization finished \n";
}
else
{
int cornerPointsSharpNum = cornerPointsSharp->points.size();
int surfPointsFlatNum = surfPointsFlat->points.size();
TicToc t_opt;
//點到線以及點到面的ICP，疊代兩次
for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)
{
corner_correspondence = 0;
plane_correspondence = 0;
ceres::LossFunction *loss_function = new ceres::HuberLoss(0.1); //定義ceres使用Huber損失函數
//對于四元數或者旋轉矩陣這種使用過參數化表示旋轉的方式，它們是不支援廣義的加法
//（因為使用普通的加法就會打破其constraint，比如旋轉矩陣加旋轉矩陣得到的就不再是旋轉矩陣），
//是以我們在使用ceres對其進行疊代更新的時候就需要自定義其更新方式了，具體的做法是實作一個參數本地化的子類，
//需要繼承于LocalParameterization，LocalParameterization是純虛類，是以我們繼承的時候要把所有的純虛函數都實作一遍才能使用該類生成對象.
ceres::LocalParameterization *q_parameterization = new ceres::EigenQuaternionParameterization();
ceres::Problem::Options problem_options;
ceres::Problem problem(problem_options); //定義優化問題
problem.AddParameterBlock(para_q, 4, q_parameterization); //待優化參數para_q，内含4個變量，對旋轉四元數需要自定義參數本地化的子類q_parameterization
problem.AddParameterBlock(para_t, 3);//待優化參數para_t
pcl::PointXYZI pointSel; //定義kdTree查詢點
std::vector<int> pointSearchInd; //定義kdTree搜尋到的查詢點近鄰的索引
std::vector<float> pointSearchSqDis;//定義kdTree對應近鄰點中心距離平方
TicToc t_data;
// 基于最近鄰(隻找2個最近鄰點)原理建立corner特征點之間關聯，find correspondence for corner features
for (int i = 0; i < cornerPointsSharpNum; ++i) //對于每個corner_sharp點,注意是++i
{
TransformToStart(&(cornerPointsSharp->points[i]), &pointSel); //将目前幀的corner_sharp特征點從目前幀的Lidar坐标系下變換到上一幀的Lidar坐标系下（作為kdTree的查詢點）
kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); // kdtree中的點雲是上一幀的corner_less_sharp，是以這是在上一幀
// 的corner_less_sharp中尋找目前幀corner_sharp特征點O的最近鄰點（記為A）
//pointSel：查詢點；1：鄰近個數；pointSearchInd：儲存搜尋到的近鄰點的索引；pointSearchSqDis：儲存查詢點與對應近鄰點中心距離平方
int closestPointInd = -1, minPointInd2 = -1;
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD) //如果最近鄰的corner特征點之間距離平方小于門檻值(25)，則最近鄰點A有效
{
closestPointInd = pointSearchInd[0];
int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity); //強轉int向下取整，表示scanID
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
// 向上搜尋，尋找另外一個最近鄰的點（記為B1），search in the direction of increasing scan line
for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j) //laserCloudCornerLast 來自上一幀的corner_less_sharp特征點，由于提取特征時是按照scan的順序提取的，是以laserCloudCornerLast中的點也是按照scanID遞增的順序存放的
{
// if in the same scan line, continue
if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID) //intensity整數部分存放的是scanID
continue;
// if not in nearby scans, end the loop
if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
break;
double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
(laserCloudCornerLast->points[j].x - pointSel.x) +
(laserCloudCornerLast->points[j].y - pointSel.y) *
(laserCloudCornerLast->points[j].y - pointSel.y) +
(laserCloudCornerLast->points[j].z - pointSel.z) *
(laserCloudCornerLast->points[j].z - pointSel.z);
if (pointSqDis < minPointSqDis2) //第二個最近鄰點有效，更新點B1
{
// find nearer point
minPointSqDis2 = pointSqDis;
minPointInd2 = j; //找到一個像樣的點B1，縮小門檻值，繼續搜尋更近的點B來替換上一輪搜尋的B，直到周遊所有的laserCloudCornerLast，即上一幀的corner_less_sharp點
}
}
// 向下搜尋，尋找點另外一個最近鄰的點B2，search in the direction of decreasing scan line
for (int j = closestPointInd - 1; j >= 0; --j)
{
// if in the same scan line, continue
if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)
continue;
// if not in nearby scans, end the loop
if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
break;
double pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
(laserCloudCornerLast->points[j].x - pointSel.x) +
(laserCloudCornerLast->points[j].y - pointSel.y) *
(laserCloudCornerLast->points[j].y - pointSel.y) +
(laserCloudCornerLast->points[j].z - pointSel.z) *
(laserCloudCornerLast->points[j].z - pointSel.z);
if (pointSqDis < minPointSqDis2) //第二個最近鄰點有效，更新點B
{
// find nearer point
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
}
}
if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid
{ // 即特征點0的兩個最近鄰點A和B都有效
Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x, //目前點
cornerPointsSharp->points[i].y,
cornerPointsSharp->points[i].z);
Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x, //上一幀中搜尋到的近鄰點A
laserCloudCornerLast->points[closestPointInd].y,
laserCloudCornerLast->points[closestPointInd].z);
Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x, //上一幀中搜尋到的近鄰點B
laserCloudCornerLast->points[minPointInd2].y,
laserCloudCornerLast->points[minPointInd2].z);
double s; //運動補償系數
if (DISTORTION)
s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;// fireID/0.1
else
s = 1.0;
// 用0，A，B構造點到線的距離的殘差，注意這三個點都是在上一幀的Lidar坐标系下，即，殘差=點0到直線AB的距離
//具體計算方法在lidarFactor.hpp
ceres::CostFunction *cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
corner_correspondence++;
}
}
//下面說的點符号與上述相同
//與上面建立corner特征點之間的關聯類似，尋找平面特征點的最近鄰點ABC（隻找3個最近鄰點），即基于最近鄰原理建立surf特征點之間的關聯
// find correspondence for plane features
for (int i = 0; i < surfPointsFlatNum; ++i)
{
TransformToStart(&(surfPointsFlat->points[i]), &pointSel);//将目前幀的corner_sharp特征點從目前幀的Lidar坐标系下變換到上一幀的Lidar坐标系下（作為kdTree的查詢點）
kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis); // kdtree中的點雲是上一幀的corner_less_sharp，是以這是在上一幀
// 的corner_less_sharp中尋找目前幀corner_sharp特征點O的最近鄰點（記為A）
//pointSel：查詢點；1：鄰近個數；pointSearchInd：儲存搜尋到的近鄰點的索引；pointSearchSqDis：儲存查詢點與對應近鄰點中心距離平方
int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD) //如果最近鄰的corner特征點之間距離平方小于門檻值(25)，則找到的最近鄰點A有效
{
closestPointInd = pointSearchInd[0];
// get closest point's scan ID
int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);//強轉int向下取整，表示scanID
double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;
// 向上搜尋，尋找另外一個最近鄰的點（記為B1），search in the direction of increasing scan line
for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)
{
// if not in nearby scans, end the loop
if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
break;
double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
// if in the same or lower scan line
if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis; //找到的第2個最近鄰有效，更新B，注意如果scanID準确的話，一般A與B的scanID相同
minPointInd2 = j;
}
// if in the higher scan line
else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
{
minPointSqDis3 = pointSqDis; //找到的第3個最近鄰點有效，更新點C，注意如果scanID準确的話，一般點A和點B的scanID相同，且與點C的scanID不同
minPointInd3 = j;
}
}
// search in the direction of decreasing scan line
for (int j = closestPointInd - 1; j >= 0; --j)
{
// if not in nearby scans, end the loop
if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
break;
double pointSqDis = (laserCloudSurfLast->points[j].x - pointSel.x) *
(laserCloudSurfLast->points[j].x - pointSel.x) +
(laserCloudSurfLast->points[j].y - pointSel.y) *
(laserCloudSurfLast->points[j].y - pointSel.y) +
(laserCloudSurfLast->points[j].z - pointSel.z) *
(laserCloudSurfLast->points[j].z - pointSel.z);
// if in the same or higher scan line
if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2)
{
minPointSqDis2 = pointSqDis;
minPointInd2 = j;
}
else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3)
{
// find nearer point
minPointSqDis3 = pointSqDis;
minPointInd3 = j;
}
}
if (minPointInd2 >= 0 && minPointInd3 >= 0) //A、B、C都有效
{
Eigen::Vector3d curr_point(surfPointsFlat->points[i].x, //目前點
surfPointsFlat->points[i].y,
surfPointsFlat->points[i].z);
Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x, //上一幀中的近鄰點A
laserCloudSurfLast->points[closestPointInd].y,
laserCloudSurfLast->points[closestPointInd].z);
Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x, //上一幀中的近鄰點B
laserCloudSurfLast->points[minPointInd2].y,
laserCloudSurfLast->points[minPointInd2].z);
Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x, //上一幀中的近鄰點C
laserCloudSurfLast->points[minPointInd3].y,
laserCloudSurfLast->points[minPointInd3].z);
double s;
if (DISTORTION)
s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD;
else
s = 1.0;
//用點0，A，B，C構造點到面的距離的殘差，注意這三個點都是在上一幀的Lidar坐标系下，即殘差=點0到平面ABC的距離
ceres::CostFunction *cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);
problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
plane_correspondence++;
}
}
}
//printf("coner_correspondance %d, plane_correspondence %d \n", corner_correspondence, plane_correspondence);
printf("data association time %f ms \n", t_data.toc());
if ((corner_correspondence + plane_correspondence) < 10)
{
printf("less correspondence! *************************************************\n");
}
TicToc t_solver;
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;//QR分解
options.max_num_iterations = 4;
options.minimizer_progress_to_stdout = false;
ceres::Solver::Summary summary;
//基于構造的所有殘差項，求解最優的目前幀位姿與上一幀位姿的位姿增量：para_q和para_t
ceres::Solve(options, &problem, &summary);
printf("solver time %f ms \n", t_solver.toc());
}
printf("optimization twice time %f \n", t_opt.toc());
//用最新計算出的位姿增量，更新上一幀的位姿，得到目前幀的位姿，注意這裡說的位姿都是世界坐标系下的位姿
t_w_curr = t_w_curr + q_w_curr * t_last_curr;
q_w_curr = q_w_curr * q_last_curr;
}
TicToc t_pub;
// publish odometry
//publish由目前Odometry線程計算出的目前幀在世界坐标系的位姿、corner_less_sharp特征點、surf_less_flat特征點、濾波後的點雲（原封不動的轉發上一個node處理出的目前幀點雲）
nav_msgs::Odometry laserOdometry;
laserOdometry.header.frame_id = "/camera_init";
laserOdometry.child_frame_id = "/laser_odom";
laserOdometry.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserOdometry.pose.pose.orientation.x = q_w_curr.x();
laserOdometry.pose.pose.orientation.y = q_w_curr.y();
laserOdometry.pose.pose.orientation.z = q_w_curr.z();
laserOdometry.pose.pose.orientation.w = q_w_curr.w();//目前幀的旋轉四元數
laserOdometry.pose.pose.position.x = t_w_curr.x();
laserOdometry.pose.pose.position.y = t_w_curr.y();
laserOdometry.pose.pose.position.z = t_w_curr.z();//目前幀的位移量
pubLaserOdometry.publish(laserOdometry);
geometry_msgs::PoseStamped laserPose;
laserPose.header = laserOdometry.header;
laserPose.pose = laserOdometry.pose.pose;
laserPath.header.stamp = laserOdometry.header.stamp;
laserPath.poses.push_back(laserPose);
laserPath.header.frame_id = "/camera_init";
pubLaserPath.publish(laserPath);
// transform corner features and plane features to the scan end point
//用cornerPointsLessSharp和surfPointsLessFlat更新laserCloudCornerLast和laserCloudSurfLast以及相應的kdtree，為下一次點雲特征比對提供target
if (0)
{
int cornerPointsLessSharpNum = cornerPointsLessSharp->points.size();
for (int i = 0; i < cornerPointsLessSharpNum; i++)
{
TransformToEnd(&cornerPointsLessSharp->points[i], &cornerPointsLessSharp->points[i]);
}
int surfPointsLessFlatNum = surfPointsLessFlat->points.size();
for (int i = 0; i < surfPointsLessFlatNum; i++)
{
TransformToEnd(&surfPointsLessFlat->points[i], &surfPointsLessFlat->points[i]);
}
int laserCloudFullResNum = laserCloudFullRes->points.size();
for (int i = 0; i < laserCloudFullResNum; i++)
{
TransformToEnd(&laserCloudFullRes->points[i], &laserCloudFullRes->points[i]);
}
}
pcl::PointCloud<PointType>::Ptr laserCloudTemp = cornerPointsLessSharp;
cornerPointsLessSharp = laserCloudCornerLast;
laserCloudCornerLast = laserCloudTemp; //将上一節點中擷取到的less_sharp點雲賦給laserCloudCornerLast，，這就是為啥//277行//第一幀不進行比對，僅僅将CornerPointsLessSharp儲存到laserCloudCornerLast，将surfPointLessFlat儲存至laserCloudSurfLast，為下次比對提供target
laserCloudTemp = surfPointsLessFlat;
surfPointsLessFlat = laserCloudSurfLast;
laserCloudSurfLast = laserCloudTemp;
laserCloudCornerLastNum = laserCloudCornerLast->points.size();
laserCloudSurfLastNum = laserCloudSurfLast->points.size();
// std::cout << "the size of corner last is " << laserCloudCornerLastNum << ", and the size of surf last is " << laserCloudSurfLastNum << '\n';
kdtreeCornerLast->setInputCloud(laserCloudCornerLast); //設定kdtreeCornerLast輸入點雲為上一幀的less_sharp
kdtreeSurfLast->setInputCloud(laserCloudSurfLast);//設定 kdtreeSurfLast輸入點雲為上一幀的less_flat
if (frameCount % skipFrameNum == 0)//提取關鍵幀，間隔四幀提取一次，即每五幀釋出一次話題用于建圖
{
frameCount = 0;
sensor_msgs::PointCloud2 laserCloudCornerLast2;
pcl::toROSMsg(*laserCloudCornerLast, laserCloudCornerLast2);
laserCloudCornerLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudCornerLast2.header.frame_id = "/camera";
pubLaserCloudCornerLast.publish(laserCloudCornerLast2);
sensor_msgs::PointCloud2 laserCloudSurfLast2;
pcl::toROSMsg(*laserCloudSurfLast, laserCloudSurfLast2);
laserCloudSurfLast2.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudSurfLast2.header.frame_id = "/camera";
pubLaserCloudSurfLast.publish(laserCloudSurfLast2);
sensor_msgs::PointCloud2 laserCloudFullRes3;
pcl::toROSMsg(*laserCloudFullRes, laserCloudFullRes3);
laserCloudFullRes3.header.stamp = ros::Time().fromSec(timeSurfPointsLessFlat);
laserCloudFullRes3.header.frame_id = "/camera";
pubLaserCloudFullRes.publish(laserCloudFullRes3);
}
printf("publication time %f ms \n", t_pub.toc());
printf("whole laserOdometry time %f ms \n \n", t_whole.toc());
if(t_whole.toc() > 100)
ROS_WARN("odometry process over 100ms");
frameCount++;
}
rate.sleep();
}
return 0;
}
參考：LOAM筆記及A-LOAM源碼閱讀
A-LOAM源碼位址：https://github.com/HKUST-Aerial-Robotics/A-LOAM
![使用hector構圖_如何使用均衡的構圖拍攝更清晰的照片[圖]](data:image/gif;base64,R0lGODlhAQABAIAAAP///wAAACwAAAAAAQABAAACAkQBADs=)