LOAM_velodyne 特徵點的提取 點雲/IMU資料處理
註解：
程式碼流程：訂閱了2個節點和釋出了6個節點。通過回撥函式的處理，將處理後的點雲重新發出去。
功能：對點雲和IMU資料進行預處理，用於特徵點的配準。
具體實現：一次掃描的點通過曲率值來分類，特徵點曲率大於閾值的為邊緣點；特徵點曲率小於閾值的為平面點。為了使特徵點均勻的分佈在環境中，將一次掃描劃分為4個獨立的子區域。每個子區域最多提供2個邊緣點和4個平面點。此外，將不穩定的特徵點（瑕點）排除。
程式碼流程：
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; }
1.構造 loam::MultiScanRegistration multiScan 物件
MultiScanRegistration(const MultiScanMapper& scanMapper = MultiScanMapper());MultiScanMapper(const float& lowerBound = -15, const float& upperBound = 15, const uint16_t& nScanRings = 16);
預設設定了鐳射的掃描角度
第一掃描環的垂直角度 _lowerBound
最後一個掃描環的垂直角度 _upperBound
線性差值因子 _factor = (nScanRings - 1) / (upperBound - lowerBound)
2.setup函式的呼叫
2.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; }
*1 呼叫ScanRegistration 中的 setupROS
if (!ScanRegistration::setupROS(node, privateNode, config_out))
return false;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)!setupROS(node, privateNode, config)
!ScanRegistration::setupROS(node, privateNode, config_out)該函式裡面第一步，解析引數
if (!parseParams(privateNode, config_out))該函式是從launch 檔案中讀取引數
3)訂閱IMU話題和釋出話題
// 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繼續執行
確定鐳射的型別VLP-16 HDL-32 HDL-64E，並且設定了 垂直視場角和線數
訂閱點雲資料
// subscribe to input cloud topic
_subLaserCloud = node.subscribe<sensor_msgs::PointCloud2>
("/multi_scan_points", 2, &MultiScanRegistration::handleCloudMessage, this);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位置和速度
// 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;4.點雲資料 回撥函式
1.系統啟動延遲計數器 int _systemDelay = 20;
systemDelay 有延時作用，保證有imu資料後在呼叫laserCloudHandler
if (_systemDelay > 0)
{
--_systemDelay;
return;
}2.將 鐳射資料 轉化成 pcl 點雲 描述
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);在ROS中點雲的資料型別
在ROS中表示點雲的資料結構有： sensor_msgs::PointCloud sensor_msgs::PointCloud2 pcl::PointCloud<T>
關於PCL在ros的資料的結構，具體的介紹可查 看 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.確定掃描開始和結束方向
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;
}4.clear all scanline points 清除所有掃描線點
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(); }); 5.從輸入雲中提取有效點，就是遍歷點雲資料
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;
}然後計算垂直角度 和 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);根據點方向計算相對掃描時間 掃描一次時間*比例 加上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<pcl::PointCloud<pcl::PointXYZI> > _laserCloudScans;
將新雲處理為一組掃描線。 processScanlines
processScanlines(scanTime, _laserCloudScans);4、 projectPointToStartOfSweep 函式
1.設定IMU轉換根據時間 計算 IMU位姿的偏移
插入IMU狀態根據該時間 內插的IMU狀態對應於當前處理的鐳射掃描點的時間
interpolateIMUStateFor(relTime, _imuCur);計算IMU的偏移
float relSweepTime = toSec(_scanTime - _sweepStart) + relTime;
_imuPositionShift = _imuCur.position - _imuStart.position - _imuStart.velocity * relSweepTime;2.將該點轉化到 startIMU上
旋轉點到全域性IMU系統下
rotateZXY(point, _imuCur.roll, _imuCur.pitch, _imuCur.yaw);新增全域性IMU位姿的偏移
point.x += _imuPositionShift.x();
point.y += _imuPositionShift.y();
point.z += _imuPositionShift.z();相對於啟動IMU狀態，將點旋轉回本地IMU系統 rotate point back to local IMU system relative to the start IMU state
rotateYXZ(point, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);5、processScanlines 函式
void processScanlines(const Time& scanTime, std::vector<pcl::PointCloud<pcl::PointXYZI>> const& laserCloudScans);
1. 重置內部緩衝區並根據當前掃描時間設定IMU啟動狀態
reset(scanTime); 初始化 _scanTime imuIdx =0,同時初始化IMU插入的位姿 在掃描開始時清除內部雲緩衝區
interpolateIMUStateFor(0, _imuStart);2.構建排序的全解析度雲
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
extractFeatures();4.跟新IMU轉化
imuTrans[0] x y z = imuStart pitch yaw roll
imuTrans[1] x y z = imuCur pitch yaw roll
imu位姿偏移 imuShiftFromStart
imuTrans[2] x y z = imuShiftFromStart pitch yaw roll
imuTrans[3] x y z = imuVelocityFromStart pitch yaw roll
6、extractFeatures 函式
1.從單個掃描中提取特徵
size_t nScans = _scanIndices.size();
for (size_t i = beginIdx; i < nScans; i++) {_scanIndices std::vector<IndexRange> typedef std::pair<size_t, size_t> IndexRange;
2.找出這幀資料在鐳射點雲座標的起始，並且跳過空的 scan 資料
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函式
setScanBuffersFor(scanStartIdx, scanEndIdx);遍歷所有點（除去前五個和後六個），判斷該點及其周邊點是否可以作為特徵點位：當某點及其後點間的距離平方大於某閾值a（說明這兩點有一定距離），且兩向量夾角小於某閾值b時（夾角小就可能存在遮擋），將其一側的臨近6個點設為不可標記為特徵點的點；若某點到其前後兩點的距離均大於c倍的該點深度，則該點判定為不可標記特徵點的點（入射角越小，點間距越大，即鐳射發射方向與投射到的平面越近似水平）。
獲取鐳射一條線上的scan的數量
size_t scanSize = endIdx - startIdx + 1;
_scanNeighborPicked.assign(scanSize, 0);將不可靠的點標記為已挑選
for (size_t i = startIdx + _config.curvatureRegion; i < endIdx - _config.curvatureRegion; i++) {
_config.curvatureRegion 為：用於計算點曲率的周圍點數（點周圍的+/-區域）
得到前一個點，當前點，後一個點
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<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; ///< flag if neighboring point was already picked如果已經選擇了相鄰點，則標記
計算點曲率並重置排序指數
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<size_t> _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<pcl::PointXYZI> surfPointsLessFlatScanDS;
pcl::VoxelGrid<pcl::PointXYZI> downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(_config.lessFlatFilterSize, _config.lessFlatFilterSize, _config.lessFlatFilterSize);
downSizeFilter.filter(surfPointsLessFlatScanDS);7、總結資料傳輸：