【SLAM】LIO-SAM解析——数据预处理imageProjection(2)
系列文章链接：
【SLAM】LIO-SAM解析——流程图(1)
【SLAM】LIO-SAM解析——数据预处理imageProjection(2)
【SLAM】LIO-SAM解析——特征提取featureTrack(3)
【SLAM】LIO-SAM解析——IMU预计分IMU-Preintegration(4)
【SLAM】LIO-SAM解析——后端优化MapOptimization(5)
【SLAM】LIO-SAM解析——里程计融合transformFusion(6)
知识点：
如何用IMU的角加速度对lidar帧去旋转畸变，如何用里程计的平移数据对lidar帧去平移畸变，如何用IMU和里程计初始时刻的位姿给这一帧找到位姿。
这部分内容对应imageProjection.cpp，雷达帧是一定角度旋转的(例如360°)，同时机器人也在运动，这就造成了雷达在不同角度获取数据时机器人的位姿是不同的，也就是说，lidar在一帧时间范围内的每一个时刻所在的坐标系是不同的，对应获得的点云也是在不同坐标系下的。
这部分代码的作用就是把雷达旋转一周时，所有的点云都转换到同一个位姿下，也就是该帧初始时刻的lidar坐标系下。如何做到呢？初始时刻的位姿我知道，起止时间内的位姿变化我知道，每一个点云的时刻我知道，那么每一个时刻相对于初始时刻的位姿变化(平移有里程计变化量获得，旋转角由IMU变化量获得)就可以根据时间线性插值获得，然后根据这个位姿变化量把点变换过去就行了。
对每一个激光点云都这么玩一下，就实现了lidar帧的运动去畸变。
2. 功能入口
int main(int argc, char** argv)
{
ros::init(argc, argv, "lio_sam");
ImageProjection IP;
ROS_INFO("\033[1;32m----> Image Projection Started.\033[0m");
ros::MultiThreadedSpinner spinner(3);
spinner.spin();
return 0;
}核心内容就在创建ImageProjection对象时进行的，看一下他的构造函数：
ImageProjection():deskewFlag(0)
{
// 订阅原始imu数据
subImu = nh.subscribe(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
// 订阅imu里程计，由imuPreintegration积分计算得到的每时刻imu位姿
subOdom = nh.subscribe(odomTopic+"_incremental", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
// 订阅原始lidar数据
subLaserCloud = nh.subscribe(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());
// 发布当前激光帧运动畸变校正后的点云，有效点
pubExtractedCloud = nh.advertise ("lio_sam/deskew/cloud_deskewed", 1);
// 发布当前激光帧运动畸变校正后的点云信息
pubLaserCloudInfo = nh.advertise ("lio_sam/deskew/cloud_info", 1);
// 初始化
allocateMemory();
// 重置参数
resetParameters();
// pcl日志级别，只打ERROR日志
pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
}
在这里可以看到，在对象构造的时候，就创建了ROS的回调函数，这里有三个关键的函数，分别是imuHandler(),odometryHandler()和cloudHandler()，分别对应IMU数据，里程计数据和雷达数据。
2.1 回调函数imuHandler()
这部分作用是把IMU数据的坐标系转换到lidar系上，从而和lidar数据进行匹配，然后放到容器里去。
/**
* 订阅原始imu数据
* 1、imu原始测量数据转换到lidar系，加速度、角速度、RPY
*/
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
{
// imu原始测量数据转换到lidar系，加速度、角速度、RPY
sensor_msgs::Imu thisImu = imuConverter(*imuMsg);
// 上锁，添加数据的时候队列不可用
std::lock_guard lock1(imuLock);
imuQueue.push_back(thisImu);
}
imuConverter()定义在utility.h文件里，这个文件定义了全部通用的工具函数。它把线加速度和角速度转化到lidar系下，rot转化为从lidar->world的旋转。
/**
* imu原始测量数据转换到lidar系，加速度、角速度、RPY
*/
sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
{
sensor_msgs::Imu imu_out = imu_in;
// 加速度，只跟xyz坐标系的旋转有关系
Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
acc = extRot * acc;
imu_out.linear_acceleration.x = acc.x();
imu_out.linear_acceleration.y = acc.y();
imu_out.linear_acceleration.z = acc.z();
// 角速度，只跟xyz坐标系的旋转有关系
Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
gyr = extRot * gyr;
imu_out.angular_velocity.x = gyr.x();
imu_out.angular_velocity.y = gyr.y();
imu_out.angular_velocity.z = gyr.z();
// RPY
Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
Eigen::Quaterniond q_final = q_from * extQRPY;
imu_out.orientation.x = q_final.x();
imu_out.orientation.y = q_final.y();
imu_out.orientation.z = q_final.z();
imu_out.orientation.w = q_final.w();
if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1)
{
ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");
ros::shutdown();
}
return imu_out;
}2.2 回调函数odometryHandler()
直接把里程计放到buffer里去，注意这里接收的里程计都已经是lidar系在world系下的表示 T(world<-lidar)。
/**
* 订阅imu里程计，由imuPreintegration积分计算得到的每时刻imu位姿
*/
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg)
{
std::lock_guard lock2(odoLock);
odomQueue.push_back(*odometryMsg);
} 2.3 回调函数cloudHandler() 核心部分
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
// 添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性
if (!cachePointCloud(laserCloudMsg))
return;
// 当前帧起止时刻对应的imu数据、imu里程计数据处理
if (!deskewInfo())
return;
// 当前帧激光点云运动畸变校正
// 1、检查激光点距离、扫描线是否合规
// 2、激光运动畸变校正，保存激光点
projectPointCloud();
// 提取有效激光点，存extractedCloud
cloudExtraction();
// 发布当前帧校正后点云，有效点
publishClouds();
// 重置参数，接收每帧lidar数据都要重置这些参数
resetParameters();
}
2.3.1 检查lidar原始数据是否有效 cachePointCloud()
这个函数是一个辅助函数，我相信各位跑代码的时候输入数据肯定是没有问题的，这个函数干的事情就是检查输入数据格式，ring，time等信息是否是要求的格式。
bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
cloudQueue.push_back(*laserCloudMsg);
if (cloudQueue.size() <= 2)
return false;
// 取出激光点云队列中最早的一帧
currentCloudMsg = std::move(cloudQueue.front());
cloudQueue.pop_front();
if (sensor == SensorType::VELODYNE)
{
// 转换成pcl点云格式
pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);
}
else if (sensor == SensorType::OUSTER)
{
// 转换成Velodyne格式
pcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn);
laserCloudIn->points.resize(tmpOusterCloudIn->size());
laserCloudIn->is_dense = tmpOusterCloudIn->is_dense;
for (size_t i = 0; i < tmpOusterCloudIn->size(); i++)
{
auto &src = tmpOusterCloudIn->points[i];
auto &dst = laserCloudIn->points[i];
dst.x = src.x;
dst.y = src.y;
dst.z = src.z;
dst.intensity = src.intensity;
dst.ring = src.ring;
dst.time = src.t * 1e-9f;
}
}
// 当前帧头部
cloudHeader = currentCloudMsg.header;
// 当前帧起始时刻
timeScanCur = cloudHeader.stamp.toSec();
// 当前帧结束时刻，注：点云中激光点的time记录相对于当前帧第一个激光点的时差，第一个点time=0
timeScanEnd = timeScanCur + laserCloudIn->points.back().time;
// 存在无效点，Nan或者Inf
if (laserCloudIn->is_dense == false)
{
ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
ros::shutdown();
}
// 检查是否存在ring通道，注意static只检查一次
static int ringFlag = 0;
if (ringFlag == 0)
{
ringFlag = -1;
for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i)
{
if (currentCloudMsg.fields[i].name == "ring")
{
ringFlag = 1;
break;
}
}
if (ringFlag == -1)
{
ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!");
ros::shutdown();
}
}
// 检查是否存在time通道
if (deskewFlag == 0)
{
deskewFlag = -1;
for (auto &field : currentCloudMsg.fields)
{
if (field.name == "time" || field.name == "t")
{
deskewFlag = 1;
break;
}
}
if (deskewFlag == -1)
ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!");
}
return true;
}laserCloudIn的格式是pcl::PointCloud
一个VelodynePointXYZIRT点云数据记录了这个点在当前坐标系下的3D坐标，自定义的intensity，在哪个ring上，和获取的时间。
/**
* Velodyne点云结构，变量名XYZIRT是每个变量的首字母，即x,y,z,intensity,time
*/
struct VelodynePointXYZIRT
{
PCL_ADD_POINT4D // 位置
PCL_ADD_INTENSITY; // 激光点反射强度，也可以存点的索引
uint16_t ring; // 扫描线
float time; // 时间戳，记录相对于当前帧第一个激光点的时差，第一个点time=0
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16; // 内存16字节对齐，EIGEN SSE优化要求除此之外，由于一帧数据不是同一个时候获得的，对应了lidar旋转一周的时间，所以还要记录当前帧的起止时间timeScanCur和timeScanEnd。
2.3.2 deskewInfo()
这个函数的作用是获得在当前激光帧范围内的每一个时刻时的相对旋转角，初始时刻的rollpitchyaw角，位姿，和起止时刻间位姿的变化量。
bool deskewInfo()
{
std::lock_guard lock1(imuLock);
std::lock_guard lock2(odoLock);
// 要求imu数据包含激光数据，否则不往下处理了
if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd)
{
ROS_DEBUG("Waiting for IMU data ...");
return false;
}
imuDeskewInfo();
odomDeskewInfo();
return true;
} 首先是imuDeskewInfo()，这一部分的内容就是需要IMU的第一个数据和该激光帧开始时刻的时间差不能超过0.01s。另外记录了第一个时刻IMU的roll，pitch，yaw角给cloudInfo。
这是cloudInfo第一次出现的地方，它是整个cpp文件的输出信息，即去畸变后的lidar帧信息。看到这里你会发现，虽然每一个IMU都记录有roll，pitch，yaw角，但是我们只要了第一个IMU的，之后的旋转角的变化量都是代码自己计算的。
void imuDeskewInfo()
{
cloudInfo.imuAvailable = false;
// 从imu队列中删除当前激光帧0.01s前面时刻的imu数据
while (!imuQueue.empty())
{
if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
imuQueue.pop_front();
else
break;
}
if (imuQueue.empty())
return;
imuPointerCur = 0;
// 遍历当前激光帧起止时刻（前后扩展0.01s）之间的imu数据
for (int i = 0; i < (int)imuQueue.size(); ++i)
{
sensor_msgs::Imu thisImuMsg = imuQueue[i];
double currentImuTime = thisImuMsg.header.stamp.toSec();
// 提取imu姿态角RPY，作为当前lidar帧初始姿态角
if (currentImuTime <= timeScanCur)
imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);
// 超过当前激光帧结束时刻0.01s，结束
if (currentImuTime > timeScanEnd + 0.01)
break;令第一个IMU时刻的累计x，y，z方向的旋转角为0，则之后每一个IMU时刻的旋转角都是基于当前IMU角速度和之前旋转角的累加。
// 第一帧imu旋转角初始化
if (imuPointerCur == 0){
imuRotX[0] = 0;
imuRotY[0] = 0;
imuRotZ[0] = 0;
imuTime[0] = currentImuTime;
++imuPointerCur;
continue;
}
// 提取imu角速度
double angular_x, angular_y, angular_z;
imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);
// imu帧间时差
double timeDiff = currentImuTime - imuTime[imuPointerCur-1];
// 当前时刻旋转角 = 前一时刻旋转角 + 角速度 * 时差
imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff;
imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff;
imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff;
imuTime[imuPointerCur] = currentImuTime;
++imuPointerCur;
}
--imuPointerCur;
// 没有合规的imu数据
if (imuPointerCur <= 0)
return;
cloudInfo.imuAvailable = true;
}然后是odomDeskewInfo()，这个函数的作用是把初始时刻的位姿和起止时刻间的位姿变化量给到cloudInfo，你会发现起止时刻间的里程计信息他都没用。
void odomDeskewInfo()
{
cloudInfo.odomAvailable = false;
// 从imu里程计队列中删除当前激光帧0.01s前面时刻的imu数据
while (!odomQueue.empty())
{
if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
odomQueue.pop_front();
else
break;
}
if (odomQueue.empty())
return;
// 要求必须有当前激光帧时刻之前的imu里程计数据
if (odomQueue.front().header.stamp.toSec() > timeScanCur)
return;
// 提取当前激光帧起始时刻的imu里程计
nav_msgs::Odometry startOdomMsg;
for (int i = 0; i < (int)odomQueue.size(); ++i)
{
startOdomMsg = odomQueue[i];
if (ROS_TIME(&startOdomMsg) < timeScanCur)
continue;
else
break;
}
// 提取imu里程计姿态角
tf::Quaternion orientation;
tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);
double roll, pitch, yaw;
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
// 用当前激光帧起始时刻的imu里程计，初始化lidar位姿，后面用于mapOptmization
cloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x;
cloudInfo.initialGuessY = startOdomMsg.pose.pose.position.y;
cloudInfo.initialGuessZ = startOdomMsg.pose.pose.position.z;
cloudInfo.initialGuessRoll = roll;
cloudInfo.initialGuessPitch = pitch;
cloudInfo.initialGuessYaw = yaw;
cloudInfo.odomAvailable = true;
odomDeskewFlag = false;
// 如果当前激光帧结束时刻之后没有imu里程计数据，返回
if (odomQueue.back().header.stamp.toSec() < timeScanEnd)
return;
// 提取当前激光帧结束时刻的imu里程计
nav_msgs::Odometry endOdomMsg;
for (int i = 0; i < (int)odomQueue.size(); ++i)
{
endOdomMsg = odomQueue[i];
if (ROS_TIME(&endOdomMsg) < timeScanEnd)
continue;
else
break;
}
// 如果起止时刻对应imu里程计的方差不等，返回
if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0])))
return;
Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);
tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation);
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw);
// 起止时刻imu里程计的相对变换
Eigen::Affine3f transBt = transBegin.inverse() * transEnd;
// 相对变换，提取增量平移、旋转（欧拉角）
float rollIncre, pitchIncre, yawIncre;
pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);
odomDeskewFlag = true;
}你可以看到，关于该雷达帧初始时刻的rollpitchyaw，这里同时记录了IMU和里程计的数据。
2.3.3 lidar帧点云转到初始时刻的lidar坐标系下 projectPointCloud()
这个函数的作用是把当前帧的所有点云的序号找到，同时变换到帧初始时刻所在的坐标系下，这个函数才是去畸变。
void projectPointCloud()
{
int cloudSize = laserCloudIn->points.size();
// 遍历当前帧激光点云
for (int i = 0; i < cloudSize; ++i)
{
// pcl格式
PointType thisPoint;
thisPoint.x = laserCloudIn->points[i].x;
thisPoint.y = laserCloudIn->points[i].y;
thisPoint.z = laserCloudIn->points[i].z;
thisPoint.intensity = laserCloudIn->points[i].intensity;
// 距离检查
float range = pointDistance(thisPoint);
if (range < lidarMinRange || range > lidarMaxRange)
continue;
对于当前lidar帧范围内的每一个点云，首先是计算row方向上的index，
// 距离检查
float range = pointDistance(thisPoint);
if (range < lidarMinRange || range > lidarMaxRange)
continue;
// 扫描线检查
int rowIdn = laserCloudIn->points[i].ring;
if (rowIdn < 0 || rowIdn >= N_SCAN)
continue;
// 扫描线如果有降采样，跳过采样的扫描线这里要跳过
if (rowIdn % downsampleRate != 0)
continue;然后计算col方向上的index，
float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
// 水平扫描角度步长，例如一周扫描1800次，则两次扫描间隔角度0.2°
static float ang_res_x = 360.0/float(Horizon_SCAN);
int columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
if (columnIdn >= Horizon_SCAN)
columnIdn -= Horizon_SCAN;
if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
continue;
// 已经存过该点，不再处理
if (rangeMat.at(rowIdn, columnIdn) != FLT_MAX)
continue; 接下来是正菜了，根据当前点云的时间，把当前点云转换到该lidar帧初始时刻所在的坐标系下，
// 激光运动畸变校正
// 利用当前帧起止时刻之间的imu数据计算旋转增量，imu里程计数据计算平移增量，进而将每一时刻激光点位置变换到第一个激光点坐标系下，进行运动补偿
thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);我们看看这个函数里面是什么内容：
PointType deskewPoint(PointType *point, double relTime)
{
if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
return *point;
// relTime是当前激光点相对于激光帧起始时刻的时间，pointTime则是当前激光点的时间戳
double pointTime = timeScanCur + relTime;
// 在当前激光帧起止时间范围内，计算某一时刻的旋转（相对于起始时刻的旋转增量）
float rotXCur, rotYCur, rotZCur;
findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);
// 在当前激光帧起止时间范围内，计算某一时刻的平移（相对于起始时刻的平移增量）
float posXCur, posYCur, posZCur;
findPosition(relTime, &posXCur, &posYCur, &posZCur);这里面有findRotation()和findPosition()，作用是根据当前点的时间，起止时间内位姿变化量，插值计算获取当前点云相对于帧初始时刻的x,y,z,roll,pitch,yaw角的增量。这两个函数你可以看到，短时间内的旋转对位姿的影响远大于平移。
/**
* 在当前激光帧起止时间范围内，计算某一时刻的旋转（相对于起始时刻的旋转增量）
*/
void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
{
*rotXCur = 0; *rotYCur = 0; *rotZCur = 0;
// 查找当前时刻在imuTime下的索引
int imuPointerFront = 0;
while (imuPointerFront < imuPointerCur)
{
if (pointTime < imuTime[imuPointerFront])
break;
++imuPointerFront;
}
// 设为离当前时刻最近的旋转增量
if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0)
{
*rotXCur = imuRotX[imuPointerFront];
*rotYCur = imuRotY[imuPointerFront];
*rotZCur = imuRotZ[imuPointerFront];
} else {
// 前后时刻插值计算当前时刻的旋转增量
int imuPointerBack = imuPointerFront - 1;
double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
*rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;
*rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;
*rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;
}
} /**
* 在当前激光帧起止时间范围内，计算某一时刻的平移（相对于起始时刻的平移增量）
*/
void findPosition(double relTime, float *posXCur, float *posYCur, float *posZCur)
{
// 如果传感器移动速度较慢，例如人行走的速度，那么可以认为激光在一帧时间范围内，平移量小到可以忽略不计
*posXCur = 0; *posYCur = 0; *posZCur = 0;
// If the sensor moves relatively slow, like walking speed, positional deskew seems to have little benefits. Thus code below is commented.
// if (cloudInfo.odomAvailable == false || odomDeskewFlag == false)
// return;
// float ratio = relTime / (timeScanEnd - timeScanCur);
// *posXCur = ratio * odomIncreX;
// *posYCur = ratio * odomIncreY;
// *posZCur = ratio * odomIncreZ;
}再回到deskewPoint(),我们可以看到，所有点都会转换到第一个lidar点所在的坐标系下，为啥呢，因为第一个lidar点的时刻和第一个IMU时刻或第一个里程计时刻的时间不是完全重合的。
// 第一个点的位姿增量（0），求逆
if (firstPointFlag == true)
{
transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();
firstPointFlag = false;
}
// 当前时刻激光点与第一个激光点的位姿变换
Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);
Eigen::Affine3f transBt = transStartInverse * transFinal;
// 当前激光点在第一个激光点坐标系下的坐标
PointType newPoint;
newPoint.x = transBt(0,0) * point->x + transBt(0,1) * point->y + transBt(0,2) * point->z + transBt(0,3);
newPoint.y = transBt(1,0) * point->x + transBt(1,1) * point->y + transBt(1,2) * point->z + transBt(1,3);
newPoint.z = transBt(2,0) * point->x + transBt(2,1) * point->y + transBt(2,2) * point->z + transBt(2,3);
newPoint.intensity = point->intensity;
return newPoint;
}最后回到projectPointCloud()里，收尾工作：
// 矩阵存激光点的距离
rangeMat.at(rowIdn, columnIdn) = range;
// 转换成一维索引，存校正之后的激光点
int index = columnIdn + rowIdn * Horizon_SCAN;
fullCloud->points[index] = thisPoint; 2.3.4 cloudExtraction()
往cloudInfo里面加点，中间如果有不合格的点不要，一层层ring往里面放，可以通过startRingIndex和endRingIndex找到某一个序号为count的点在哪一个ring上。
因为容器是一维的，而数据是二维的，那么就需要某种方式找到一维序号和二维序号之间的关系。一维序号是count，通过startRingIndex[i]或endRingIndex[i]的i找到是哪个ring上的，然后再根据startRingIndex[i]或endRingIndex[i]里面的数和count进行对比从而知道这个点是当前ring上的哪个点。
/**
* 提取有效激光点，存extractedCloud
*/
void cloudExtraction()
{
// 有效激光点数量
int count = 0;
// 遍历所有激光点
for (int i = 0; i < N_SCAN; ++i)
{
// 记录每根扫描线起始第5个激光点在一维数组中的索引
cloudInfo.startRingIndex[i] = count - 1 + 5;
for (int j = 0; j < Horizon_SCAN; ++j)
{
// 有效激光点
if (rangeMat.at(i,j) != FLT_MAX)
{
// 记录激光点对应的Horizon_SCAN方向上的索引
cloudInfo.pointColInd[count] = j;
// 激光点距离
cloudInfo.pointRange[count] = rangeMat.at(i,j);
// 加入有效激光点
extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
++count;
}
}
// 记录每根扫描线倒数第5个激光点在一维数组中的索引
cloudInfo.endRingIndex[i] = count -1 - 5;
}
} 2.3.5 publishClouds()
void publishClouds()
{
cloudInfo.header = cloudHeader;
cloudInfo.cloud_deskewed = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
pubLaserCloudInfo.publish(cloudInfo);
}
};
这一部分就完结了，怎么样，过程还是很简单吧。