LOAM_velodyne 1.特征点的提取 点云/IMU数据处理
Table of Contents
一、注解：
二、代码流程：
2.1 主函数：main
2.2 MultiScanRegistration类的构造
2.3 类对象multiScan调用setup函数
2.3.1 参数配置 RegistrationParams
2.3.2 子类ScanRegistration调用其函数setupROS
2.3.3 激光参数确定
2.3.IMU回调函数
2.4.点云数据 回调函数
2.4.1 激光运行的核心函数
2.4.2 projectPointToStartOfSweep 函数
2.4.3 激光处理函数
2.3. 点云中提取特征 extractFeatures
2.4 总结数据传输：
一、注解：
代码流程：订阅了2个节点和发布了6个节点。通过回调函数的处理，将处理后的点云重新发出去。
功能：对点云和IMU数据进行预处理，用于特征点的配准。
具体实现：一次扫描的点通过曲率值来分类，特征点曲率大于阈值的为边缘点；特征点曲率小于阈值的为平面点。为了使特征点均匀的分布在环境中，将一次扫描划分为4个独立的子区域。每个子区域最多提供2个边缘点和4个平面点。此外，将不稳定的特征点（瑕点）排除。
二、代码流程：
2.1 主函数：main
/** Main node entry point. */
int main(int argc, char **argv)
{
ros::init(argc, argv, "scanRegistration");
ros::NodeHandle node;
ros::NodeHandle privateNode("~");
loam::MultiScanRegistration multiScan;
if (multiScan.setup(node, privateNode)) {
// initialization successful
ros::spin();
}
return 0;
}2.2 MultiScanRegistration类的构造
loam::MultiScanRegistration multiScan 对象
MultiScanRegistration类 继承了ScanRegistration类，ScanRegistration类集成了BasicScanRegistration。
二者均为默认构造函数，不用管。
MultiScanRegistration(const MultiScanMapper& scanMapper = MultiScanMapper());其中MultiScanMapper用于设置：激光垂直下限、激光垂直上限以及是多少线的激光，这儿可以不用管，因在初始化时MultiScanRegistration时可重新设置该参数。
MultiScanMapper(const float& lowerBound = -15,
const float& upperBound = 15,
const uint16_t& nScanRings = 16);参数：
- 默认设定了激光的扫描角度
- 第一扫描环的垂直角度 _lowerBound
- 最后一个扫描环的垂直角度 _upperBound
- 线性差值因子 _factor = (nScanRings - 1) / (upperBound - lowerBound)
2.3 类对象multiScan调用setup函数
multiScan.setup(node, privateNode)
bool MultiScanRegistration::setup(ros::NodeHandle& node, ros::NodeHandle& privateNode)
{
RegistrationParams config;
if (!setupROS(node, privateNode, config))
return false;
configure(config);
return true;
}2.3.1 参数配置 RegistrationParams
RegistrationParams(const float& scanPeriod_ = 0.1,
const int& imuHistorySize_ = 200,
const int& nFeatureRegions_ = 6,
const int& curvatureRegion_ = 5, //曲率
const int& maxCornerSharp_ = 2,
const int& maxSurfaceFlat_ = 4,
const float& lessFlatFilterSize_ = 0.2,
const float& surfaceCurvatureThreshold_ = 0.1);scanPeriod 激光每次扫描的时间
imuHistorySize IMU历史状态缓冲区的大小。
nFeatureRegions 用于在扫描中分布特征提取的（大小相等）区域的数量
curvatureRegion 用于计算点曲率的周围点数（点周围的+/-区域）
maxCornerSharp 每个要素区域的最大锐角点数。
maxCornerLessSharp 每个要素区域的最小锐角点数的最大数量 10 * maxCornerSharp_
maxSurfaceFlat 每个要素区域的最大平面点数。
lessFlatFilterSize 用于缩小剩余的较小平坦表面点的体素尺寸。
surfaceCurvatureThreshold 低于/高于点的曲率阈值被认为是平坦/角点
2.3.2 子类ScanRegistration调用其函数setupROS
!ScanRegistration::setupROS(node, privateNode, config_out)1>该函数里面第一步，解析参数
if (!parseParams(privateNode, config_out))该函数是从launch 文件中读取参数读取的参数为2.3.1中配置的参数
2>订阅IMU话题和发布话题 订阅IMU话题，发布点云等
// subscribe to IMU topic
_subImu = node.subscribe("/imu/data", 50, &ScanRegistration::handleIMUMessage, this);
// advertise scan registration topics
_pubLaserCloud = node.advertise("/velodyne_cloud_2", 2);
_pubCornerPointsSharp = node.advertise("/laser_cloud_sharp", 2);
_pubCornerPointsLessSharp = node.advertise("/laser_cloud_less_sharp", 2);
_pubSurfPointsFlat = node.advertise("/laser_cloud_flat", 2);
_pubSurfPointsLessFlat = node.advertise("/laser_cloud_less_flat", 2);
_pubImuTrans = node.advertise("/imu_trans", 5); 2.3.3 激光参数确定
1>确定激光的类型VLP-16 HDL-32 HDL-64E，并且设定了 垂直视场角和线数
2>订阅点云数据
// subscribe to input cloud topic
_subLaserCloud = node.subscribe
("/multi_scan_points", 2, &MultiScanRegistration::handleCloudMessage, this); 2.3.IMU回调函数
handleIMUMessage
1.将imu的方向 四元素转化成 rpy角
tf::quaternionMsgToTF(imuIn->orientation, orientation);
double roll, pitch, yaw;
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);2.将x y z 的加速度去重力，减去各自方向的重力加速度
Vector3 acc;
acc.x() = float(imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81);
acc.y() = float(imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81);
acc.z() = float(imuIn->linear_acceleration.x + sin(pitch) * 9.81);3.跟新IMU数据 updateIMUData(acc, newState);随时间累积IMU位置和速度
- 根据roll，pitch，yaw计算 出 世界坐标系下的加速度。
- 取到buff中上一帧IMU数据
- 推算出当前imu的最新状态，位姿+速度
- 放到 _imuHistory 队列中。
// accumulate IMU position and velocity over time
rotateZXY(acc, newState.roll, newState.pitch, newState.yaw);
const IMUState& prevState = _imuHistory.last();
float timeDiff = toSec(newState.stamp - prevState.stamp);
newState.position = prevState.position
+ (prevState.velocity * timeDiff)
+ (0.5 * acc * timeDiff * timeDiff);
newState.velocity = prevState.velocity
+ acc * timeDiff;2.4.点云数据 回调函数
handleCloudMessage1.系统启动延迟计数器 int _systemDelay = 20;
systemDelay 有延时作用，保证有imu数据后在调用laserCloudHandler
if (_systemDelay > 0)
{
--_systemDelay;
return;
}2.将 激光数据 转化成 pcl 点云 描述
pcl::PointCloud laserCloudIn;
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn); 在ROS中表示点云的数据结构有： sensor_msgs::PointCloud sensor_msgs::PointCloud2
关于PCL在ros的数据的结构 pcl::PointCloud
关于sensor_msgs::PointCloud2 和 pcl::PointCloud
sensor_msgs::PointCloud 和 sensor_msgs::PointCloud2之间的转换
使用sensor_msgs::convertPointCloud2ToPointCloud 和sensor_msgs::convertPointCloudToPointCloud2.
3. 开始核心函数：
process(laserCloudIn, fromROSTime(laserCloudMsg->header.stamp));时间采用系统时间代替ros时间：Time = std::chrono::system_clock::time_point;
二者之间的相互转换
// ROS time adapters
inline Time fromROSTime(ros::Time const& rosTime)
{
auto epoch = std::chrono::system_clock::time_point();
auto since_epoch = std::chrono::seconds(rosTime.sec) + std::chrono::nanoseconds(rosTime.nsec);
return epoch + since_epoch;
}
inline ros::Time toROSTime(Time const& time_point)
{
return ros::Time().fromNSec(std::chrono::duration_cast(time_point.time_since_epoch()).count());
}
2.4.1 激光运行的核心函数
process1. 确定扫描的开始和结束方向
float startOri = -std::atan2(laserCloudIn[0].y, laserCloudIn[0].x);
float endOri = -std::atan2(laserCloudIn[cloudSize - 1].y,
laserCloudIn[cloudSize - 1].x) + 2 * float(M_PI);
if (endOri - startOri > 3 * M_PI) {
endOri -= 2 * M_PI;
} else if (endOri - startOri < M_PI) {
endOri += 2 * M_PI;
}2.清除所有扫描线点--清除以前数据，使得容器中时刻保持最新数据
bool halfPassed = false;
pcl::PointXYZI point;
_laserCloudScans.resize(_scanMapper.getNumberOfScanRings());
// clear all scanline points
std::for_each(_laserCloudScans.begin(), _laserCloudScans.end(), [](auto&&v) {v.clear(); }); 3.从输入点云中提取有效点，就是遍历点云数据
for (int i = 0; i < cloudSize; i++) {- 首先剔除 NaN和INF值点 和 零点
if (!pcl_isfinite(point.x) ||
!pcl_isfinite(point.y) ||
!pcl_isfinite(point.z)) {
continue;
}
if (point.x * point.x + point.y * point.y + point.z * point.z < 0.0001) {
continue;
}- 然后计算垂直角度 angle确定ID scanID为计算激光在那条激光线上（16线）
float angle = std::atan(point.y / std::sqrt(point.x * point.x + point.z * point.z));
int scanID = _scanMapper.getRingForAngle(angle);
if (scanID >= _scanMapper.getNumberOfScanRings() || scanID < 0 ){
continue;
}- 计算水平点角度，可以确定是否遍历过了一半点
float ori = -std::atan2(point.x, point.z);
if (!halfPassed) {
if (ori < startOri - M_PI / 2) {
ori += 2 * M_PI;
} else if (ori > startOri + M_PI * 3 / 2) {
ori -= 2 * M_PI;
}
if (ori - startOri > M_PI) {
halfPassed = true;
}
} else {
ori += 2 * M_PI;
if (ori < endOri - M_PI * 3 / 2) {
ori += 2 * M_PI;
} else if (ori > endOri + M_PI / 2) {
ori -= 2 * M_PI;
}
}
- 根据点方向计算相对扫描时间 进而确定独特的id放到强度中： 扫描一次时间*比例 加上scanID
float relTime = config().scanPeriod * (ori - startOri) / (endOri - startOri);
point.intensity = scanID + relTime;- 使用相应的IMU数据投射到扫描开始的点 projectPointToStartOfSweep
projectPointToStartOfSweep(point, relTime);- 将点云根据scanID有序放到容器中 //for循环结束
_laserCloudScans[scanID].push_back(point); std::vector
4. 将新云处理为一组扫描线。 processScanlines
processScanlines(scanTime, _laserCloudScans);5.发布结果：发布完整的分辨率点云和特征点云
2.4.2 projectPointToStartOfSweep 函数
使用imu数据时才进行，否则直接略过
1.设置IMU转换根据时间 计算 IMU位姿的偏移
- 插入IMU状态，根据该时间 内插的IMU状态对应于当前处理的激光扫描点的时间
interpolateIMUStateFor（const float &relTime, IMUState &outputState)
- 首先循环等待，直到imu时间超过该点时间，确定 _imuIdx
- 计算出该时间占两帧imu数据的比例。
- 进行线性差值，输出当前时间IMUState 如何差值呢？
- 根据两帧imu之间的比例，计算出 roll,pitch,yaw和 vel,position
double timeDiff = toSec(_scanTime - _imuHistory[_imuIdx].stamp) + relTime;
while (_imuIdx < _imuHistory.size() - 1 && timeDiff > 0) {
_imuIdx++;
timeDiff = toSec(_scanTime - _imuHistory[_imuIdx].stamp) + relTime;
}
if (_imuIdx == 0 || timeDiff > 0) {
outputState = _imuHistory[_imuIdx];
} else {
float ratio = -timeDiff / toSec(_imuHistory[_imuIdx].stamp - _imuHistory[_imuIdx - 1].stamp);
IMUState::interpolate(_imuHistory[_imuIdx], _imuHistory[_imuIdx - 1], ratio, outputState);
}- 计算IMU的累计偏移
//最近激光的时间 - 开始扫描的时间 + delta_rel
float relSweepTime = toSec(_scanTime - _sweepStart) + relTime;
//累积IMU位置和插值IMU位置之间的位置偏移
_imuPositionShift = _imuCur.position - _imuStart.position - _imuStart.velocity * relSweepTime;2.将该点转化到 startIMU上
- 旋转点到全局坐标系下 根据imu提供的旋转角roll,pitch,yaw
rotateZXY(point, _imuCur.roll, _imuCur.pitch, _imuCur.yaw);- 在该坐标系下补偿imu的偏移
point.x += _imuPositionShift.x();
point.y += _imuPositionShift.y();
point.z += _imuPositionShift.z();- 将该点反转换到实际坐标系下
rotateYXZ(point, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);2.4.3 激光处理函数
processScanlines
void processScanlines(const Time& scanTime, std::vector> const& laserCloudScans);
1. 重置内部缓冲区并根据当前扫描时间设置IMU启动状态
得到当前时刻imu的状态并清除缓存数据
_scanTime = scanTime;
// re-initialize IMU start index and state
_imuIdx = 0;
if (hasIMUData()) {
interpolateIMUStateFor(0, _imuStart);
}
// clear internal cloud buffers at the beginning of a sweep
if (true/*newSweep*/) {
_sweepStart = scanTime;
// clear cloud buffers
_laserCloud.clear();
_cornerPointsSharp.clear();
_cornerPointsLessSharp.clear();
_surfacePointsFlat.clear();
_surfacePointsLessFlat.clear();
// clear scan indices vector
_scanIndices.clear();
}
2.构建排序的全分辨率云
将多线点云搞成一组点云，并通过_scanIndices 记录每束激光在该组点云的下的开始和结尾index
size_t cloudSize = 0;
for (int i = 0; i < laserCloudScans.size(); i++) {
_laserCloud += laserCloudScans[i];
IndexRange range(cloudSize, 0);
cloudSize += laserCloudScans[i].size();
range.second = cloudSize > 0 ? cloudSize - 1 : 0;
_scanIndices.push_back(range);
}- _scanIndices std::vector
- typedef std::pair
IndexRange 第一个参数为：每一线激光的开头的index ，第二个参数为：每一线激光结束的index
3.提取特征 extractFeatures
4.跟新IMU状态 updateIMUTransform
imu位姿偏移 imuShiftFromStart
- imuTrans[0] x y z = imuStart pitch yaw roll
- imuTrans[1] x y z = imuCur pitch yaw roll
- imuTrans[2] x y z = imuShiftFromStart pitch yaw roll
- imuTrans[3] x y z = imuVelocityFromStart pitch yaw roll
2.3. 点云中提取特征 extractFeatures
1. 遍历每一束激光 --从单个扫描中提取特征
size_t nScans = _scanIndices.size(); //指有多少束激光
for (size_t i = beginIdx; i < nScans; i++) {2.找出这帧数据在激光点云坐标的起始，并且跳过“计算曲率不够的点"
[start+config.curvatureRegion,end -config.curvatureRegion]
size_t scanStartIdx = _scanIndices[i].first;
size_t scanEndIdx = _scanIndices[i].second;
// skip empty scans
if (scanEndIdx <= scanStartIdx + 2 * _config.curvatureRegion) {
continue;
}3.重新设定scan 的 buffers setScanBuffersFor函数
遍历所有点（除去前五个和后六个），判断该点及其周边点是否可以作为特征点位：当某点及其后点间的距离平方大于某阈值a（说明这两点有一定距离），且两向量夹角小于某阈值b时（夹角小就可能存在遮挡），将其一侧的临近6个点设为不可标记为特征点的点；若某点到其前后两点的距离均大于c倍的该点深度，则该点判定为不可标记特征点的点（入射角越小，点间距越大，即激光发射方向与投射到的平面越近似水平）。
setScanBuffersFor(scanStartIdx, scanEndIdx);- 获取激光一条线上的scan的数量
size_t scanSize = endIdx - startIdx + 1;
_scanNeighborPicked.assign(scanSize, 0);- 将不可靠的点标记为已挑选，遍历可能成为特征的点的下标 [start+config.curvatureRegion,end -config.curvatureRegion]
for (size_t i = startIdx + _config.curvatureRegion; i < endIdx - _config.curvatureRegion; i++) {
_config.curvatureRegion 为：用于计算点曲率的周围点数（点周围的+/-区域）
- 取出连续的3个点：前一个点，当前点，后一个点
const pcl::PointXYZI& previousPoint = (_laserCloud[i - 1]);
const pcl::PointXYZI& point = (_laserCloud[i]);
const pcl::PointXYZI& nextPoint = (_laserCloud[i + 1]);- 计算下一个点与当前点的距离 （
）
float diffNext = calcSquaredDiff(nextPoint, point);- 如果该值大于0.1时，计算point和nextPoint到原点的距离值，用于计算
if (diffNext > 0.1) {
float depth1 = calcPointDistance(point);
float depth2 = calcPointDistance(nextPoint);- 计算权重距离 ，进而确定激光点的相邻点。
if (depth1 > depth2) {
float weighted_distance = std::sqrt(calcSquaredDiff(nextPoint, point, depth2 / depth1)) / depth2;
if (weighted_distance < 0.1) {
std::fill_n(&_scanNeighborPicked[i - startIdx - _config.curvatureRegion], _config.curvatureRegion + 1, 1);
continue;
}
} else {
float weighted_distance = std::sqrt(calcSquaredDiff(point, nextPoint, depth1 / depth2)) / depth1;
if (weighted_distance < 0.1) {
std::fill_n(&_scanNeighborPicked[i - startIdx + 1], _config.curvatureRegion + 1, 1);
}
}- fill_n 将之间的数填充 本文中填充的值为1
- 接着直接计算前一个点与后一个点与当前的距离，满足条件也设置相邻。
float diffPrevious = calcSquaredDiff(point, previousPoint);
float dis = calcSquaredPointDistance(point);
if (diffNext > 0.0002 * dis && diffPrevious > 0.0002 * dis) {
_scanNeighborPicked[i - startIdx] = 1;
}4.从相同大小的扫描区域提取特征
- *nFeatureRegions 6 用于在扫描中分布特征提取的（大小相等）区域的数量。
- /curvatureRegion 5 用于计算点曲率的周围点数（点周围的+/-区域）。
- 将每个线等分为六段，分别进行处理（sp、ep分别为各段的起始和终止位置）
for (int j = 0; j < _config.nFeatureRegions; j++) {
size_t sp = ((scanStartIdx + _config.curvatureRegion) * (_config.nFeatureRegions - j)
+ (scanEndIdx - _config.curvatureRegion) * j) / _config.nFeatureRegions;
size_t ep = ((scanStartIdx + _config.curvatureRegion) * (_config.nFeatureRegions - 1 - j)
+ (scanEndIdx - _config.curvatureRegion) * (j + 1)) / _config.nFeatureRegions - 1;- 跳过空白区域 跳过不符合条件的点
if (ep <= sp) {
continue;
}5.为求特征区域设置区域缓冲区 setRegionBuffersFor(sp, ep);
- 获取其buffers 的尺寸
size_t regionSize = endIdx - startIdx + 1;
_regionCurvature.resize(regionSize);
_regionSortIndices.resize(regionSize);
_regionLabel.assign(regionSize, SURFACE_LESS_FLAT);- std::vector
_regionCurvature; ///< point curvature buffer 点曲率缓冲区 - std::vector
_regionLabel; ///< point label buffer 点标签缓冲区 - std::vector
_regionSortIndices; ///< sorted region indices based on point curvature基于点曲率的分类区域索引 - std::vector
_scanNeighborPicked; ///< 如果已经选择了相邻点，则标记
- 计算点曲率并重置排序指数
float pointWeight = -2 * _config.curvatureRegion;
for (size_t i = startIdx, regionIdx = 0; i <= endIdx; i++, regionIdx++) {
float diffX = pointWeight * _laserCloud[i].x;
float diffY = pointWeight * _laserCloud[i].y;
float diffZ = pointWeight * _laserCloud[i].z;
for (int j = 1; j <= _config.curvatureRegion; j++) {
diffX += _laserCloud[i + j].x + _laserCloud[i - j].x;
diffY += _laserCloud[i + j].y + _laserCloud[i - j].y;
diffZ += _laserCloud[i + j].z + _laserCloud[i - j].z;
}
_regionCurvature[regionIdx] = diffX * diffX + diffY * diffY + diffZ * diffZ;
_regionSortIndices[regionIdx] = i;
}- 排序点曲率
for (size_t i = 1; i < regionSize; i++) {
for (size_t j = i; j >= 1; j--) {
if (_regionCurvature[_regionSortIndices[j] - startIdx] < _regionCurvature[_regionSortIndices[j - 1] - startIdx]) {
std::swap(_regionSortIndices[j], _regionSortIndices[j - 1]);
}
}
}6.提取角落特征
for (size_t k = regionSize; k > 0 && largestPickedNum < _config.maxCornerLessSharp;) {
size_t idx = _regionSortIndices[--k];
size_t scanIdx = idx - scanStartIdx;
size_t regionIdx = idx - sp;
if (_scanNeighborPicked[scanIdx] == 0 &&
_regionCurvature[regionIdx] > _config.surfaceCurvatureThreshold) {
largestPickedNum++;
if (largestPickedNum <= _config.maxCornerSharp) {
_regionLabel[regionIdx] = CORNER_SHARP;
_cornerPointsSharp.push_back(_laserCloud[idx]);
} else {
_regionLabel[regionIdx] = CORNER_LESS_SHARP;
}
_cornerPointsLessSharp.push_back(_laserCloud[idx]);
markAsPicked(idx, scanIdx);
}
}
- int maxCornerLessSharp 2 / **每个特征区域的最小锐角点数的最大数量。*/
- std::vector
_regionSortIndices; 基于点曲率的分类区域索引 - regionSize = ep - sp + 1;
- scanIdx 为该线激光的第多少帧id
- 取出这部分当曲率大于阈值时： 在最大标记范围内 角点 否则为： 角落不太敏锐的点
- 将这些点设置为：标记为已选择
7.提取平面特征
for (int k = 0; k < regionSize && smallestPickedNum < _config.maxSurfaceFlat; k++) {
size_t idx = _regionSortIndices[k];
size_t scanIdx = idx - scanStartIdx;
size_t regionIdx = idx - sp;
if (_scanNeighborPicked[scanIdx] == 0 &&
_regionCurvature[regionIdx] < _config.surfaceCurvatureThreshold) {
smallestPickedNum++;
_regionLabel[regionIdx] = SURFACE_FLAT;
_surfacePointsFlat.push_back(_laserCloud[idx]);
markAsPicked(idx, scanIdx);
}
}- 跟提取角点一样，首先找到曲率最小的idx，进而找到 scanIdx 为该线激光的第多少帧id
- 如果曲率小于阈值且平面特征点小于预定值 则 选出，然后将平面特征点存好
- 将这些点设置为：标记为已选择
8.提取较少的平坦表面特征
for (int k = 0; k < regionSize; k++) {
if (_regionLabel[k] <= SURFACE_LESS_FLAT) {
surfPointsLessFlatScan->push_back(_laserCloud[sp + k]);
}
}9.降采样
pcl::PointCloud surfPointsLessFlatScanDS;
pcl::VoxelGrid downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(_config.lessFlatFilterSize, _config.lessFlatFilterSize, _config.lessFlatFilterSize);
downSizeFilter.filter(surfPointsLessFlatScanDS); 2.4 总结数据传输：