内容列表
一、注解：
代码流程：订阅了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)
同时，将velodyne的三种多线激光进行 static 设定
/** Multi scan mapper for Velodyne VLP-16 according to data sheet. */ static inline MultiScanMapper Velodyne_VLP_16() { return MultiScanMapper(-15, 15, 16); }; /** Multi scan mapper for Velodyne HDL-32 according to data sheet. */ static inline MultiScanMapper Velodyne_HDL_32() { return MultiScanMapper(-30.67f, 10.67f, 32); }; /** Multi scan mapper for Velodyne HDL-64E according to data sheet. */ static inline MultiScanMapper Velodyne_HDL_64E() { return MultiScanMapper(-24.9f, 2, 64); };
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);
参数变量有：
float scanPeriod 激光每次扫描的时间int imuHistorySize IMU历史状态缓冲区的大小。int nFeatureRegions 用于在扫描中分布特征提取的（大小相等）区域的数量int curvatureRegion 用于计算点曲率的周围点数（点周围的+/-区域）int maxCornerSharp 每个要素区域的最大锐角点数。int maxCornerLessSharp 每个特征区域的不那么尖锐的角点的最大数量 10 * maxCornerSharp_int maxSurfaceFlat 每个要素区域的最大平面点数。float lessFlatFilterSize 用于缩小剩余的较小平坦表面点的体素尺寸。float surfaceCurvatureThreshold 低于/高于点的曲率阈值被认为是平坦/角点
2.3.2 子类ScanRegistration调用其函数setupROS
!ScanRegistration::setupROS(node, privateNode, config_out)
该函数里面第一步，解析参数
if (!parseParams(privateNode, config_out))
该函数是从launch 文件中读取参数 读取的参数为2.3.1中配置的参数
订阅IMU话题和发布话题 订阅IMU话题，发布点云等
imu topic : /imu/data
// subscribe to IMU topic _subImu = node.subscribe<sensor_msgs::Imu>("/imu/data", 50, &ScanRegistration::handleIMUMessage, this); // advertise scan registration topics _pubLaserCloud = node.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 2); _pubCornerPointsSharp = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 2); _pubCornerPointsLessSharp = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 2); _pubSurfPointsFlat = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 2); _pubSurfPointsLessFlat = node.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 2); _pubImuTrans = node.advertise<sensor_msgs::PointCloud2>("/imu_trans", 5);
2.3.3 激光参数确定
若有 "lidar" 这个参数，则代表velodyne，确定激光的类型VLP-16 HDL-32 HDL-64E，并且根据 static 设置的三种进行设置
同时设置激光 扫描时间 默认 0.1
否则，设置 最小角、最大角以及 激光层数
订阅激光 topic : multi_scan_points // subscribe to input cloud topic _subLaserCloud = node.subscribe<sensor_msgs::PointCloud2> ("/multi_scan_points", 2, &MultiScanRegistration::handleCloudMessage, this);
三、IMU回调函数
ScanRegistration::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;
四、点云数据 回调函数
MultiScanRegistration::handleCloudMessage
- 1.系统启动延迟计数器 int _systemDelay = 20;
- systemDelay 有延时作用，保证有imu数据后在调用laserCloudHandler
- 其实这样不太好，最好是等imu有数据时，在进行激光call
if (_systemDelay > 0) { --_systemDelay; return; }
2.将 激光数据 转化成 pcl 点云 描述
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);
在ROS中表示点云的数据结构有： sensor_msgs::PointCloud sensor_msgs::PointCloud2 于PCL在ros的数据的结构 pcl::PointCloud<T>。具体的介绍可查 看 wiki.ros.org/pcl/Overview
关于sensor_msgs::PointCloud2 和 pcl::PointCloud<T>之间的转换使用pcl::fromROSMsg 和 pcl::toROSMsg
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 adaptersinline 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<std::chrono::nanoseconds>(time_point.time_since_epoch()).count());}
4.1 激光运行的核心函数
- process(laserCloudIn, fromROSTime(laserCloudMsg->header.stamp));
- 1. 确定 激光的 开始以及结束 角度
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++) {
注意：
point.x = laserCloudIn[i].y;
point.y = laserCloudIn[i].z;
point.z = laserCloudIn[i].x;
首先跳过 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(激光在那条激光线上) ，若超出设定线则舍去-
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 放到 点云强度中： 正常ID + 比例
float relTime = config().scanPeriod * (ori - startOri) / (endOri - startOri);
point.intensity = scanID + relTime;
使用相应的IMU数据投射到扫描开始的点 projectPointToStartOfSweep
将点云根据scanID有序放到容器中 //for循环结束
4. 将新云处理为一组扫描线。 processScanlines
5.发布结果：发布完整的分辨率点云和特征点云auto sweepStartTime = toROSTime(sweepStart());// publish full resolution and feature point cloudspublishCloudMsg(_pubLaserCloud, laserCloud(), sweepStartTime, "/camera");publishCloudMsg(_pubCornerPointsSharp, cornerPointsSharp(), sweepStartTime, "/camera");publishCloudMsg(_pubCornerPointsLessSharp, cornerPointsLessSharp(), sweepStartTime, "/camera");publishCloudMsg(_pubSurfPointsFlat, surfacePointsFlat(), sweepStartTime, "/camera");publishCloudMsg(_pubSurfPointsLessFlat, surfacePointsLessFlat(), sweepStartTime, "/camera");// publish corresponding IMU transformation informationpublishCloudMsg(_pubImuTrans, imuTransform(), sweepStartTime, "/camera");
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_relfloat 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);
五、激光处理函数
processScanlines
void processScanlines(const Time& scanTime, std::vector<pcl::PointCloud<pcl::PointXYZI>> const& laserCloudScans);
1.内部缓冲区 基于当前激光时间重置设置IMU启动状态 reset(scanTime);
得到当前时刻imu的状态并清除缓存数据
void BasicScanRegistration::reset(const Time& scanTime)
{
_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<IndexRange>
typedef std::pair<size_t, size_t> IndexRange;
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
5.1. 点云中提取特征 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);
重置激光邻域选取 _scanNeighborPicked，获取激光一条线上的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]);
4.计算下一个点与当前点的距离 diffNext = （）
float diffNext = calcSquaredDiff(nextPoint, point);
5.如果该值大于0.1时，则可能有差异， 计算point和nextPoint到原点的距离值
if (diffNext > 0.1) {float depth1 = calcPointDistance(point);float depth2 = calcPointDistance(nextPoint);
2.根据 二者之间的距离差以及 权重 标记。 scanNeighborPicked表示一个点的周围不能再设置为特征点
- 距离权重的计算：红蓝线的比值，根据等腰三角形的性质，如果Xi与Xi+1的夹角小于阈值0.1即对应5.732°
2.
3. 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);
6. 针对论文a情接着直接计算前一个点与后一个点与当前的距离，满足条件也设置相邻。
float diffPrevious = calcSquaredDiff(point, previousPoint); float dis = calcSquaredPointDistance(point); if (diffNext > 0.0002 * dis && diffPrevious > 0.0002 * dis) { _scanNeighborPicked[i - startIdx] = 1; }
4.将扫描区域重新分配。即将每条线分成6段，计算每段点云的开头sp，结尾ep位置。
sp=s+(e-s)*j/6,ep=s+(e-s)/6+(e-s)*j/6-1
- *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<float> _regionCurvature; ///< point curvature buffer 点曲率缓冲区std::vector<PointLabel> _regionLabel; ///< point label buffer 点标签缓冲区std::vector<size_t> _regionSortIndices; ///< sorted region indices based on point curvature基于点曲率的分类区域索引std::vector<int> _scanNeighborPicked; ///< 如果已经选择了相邻点，则标记
计算点曲率，取i点周围的10个点，计算距离，求出曲率半径，曲率=1/曲率半径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]);
}
}
}- .提取角特征 + point_less_sharp
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<size_t> _regionSortIndices; 基于点曲率的分类区域索引
regionSize = ep - sp + 1;
scanIdx 为该线激光的第多少帧id
取出这部分当曲率大于阈值时： 在最大标记范围内 角点 否则为： 角落不太敏锐的点
将这些点设置为：标记为已选择
提取平面特征
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
- 如果曲率小于阈值且平面特征点小于预定值 则 选出，然后将平面特征点存好
- 将这些点设置为：标记为已选择
- 提取较少的平坦表面特征 剩下归平面特征点备选
-
for (int k = 0; k < regionSize; k++) {if (_regionLabel[k] <= SURFACE_LESS_FLAT) {surfPointsLessFlatScan->push_back(_laserCloud[sp + k]);}}
-
- 降采样
-
pcl::PointCloud<pcl::PointXYZI> surfPointsLessFlatScanDS;pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;downSizeFilter.setInputCloud(surfPointsLessFlatScan);downSizeFilter.setLeafSize(_config.lessFlatFilterSize, _config.lessFlatFilterSize, _config.lessFlatFilterSize);downSizeFilter.filter(surfPointsLessFlatScanDS);
5.2 两回调总结
评论（0）
您还未登录，请登录后发表或查看评论