1. 定义ImageProjection 对象IP
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 1, &ImageProjection::cloudHandler, this);
pubFullCloud = nh.advertise<sensor_msgs::PointCloud2> ("/full_cloud_projected", 1);
pubFullInfoCloud = nh.advertise<sensor_msgs::PointCloud2> ("/full_cloud_info", 1);
pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2> ("/ground_cloud", 1);
pubSegmentedCloud = nh.advertise<sensor_msgs::PointCloud2> ("/segmented_cloud", 1);
pubSegmentedCloudPure = nh.advertise<sensor_msgs::PointCloud2> ("/segmented_cloud_pure", 1);
pubSegmentedCloudInfo = nh.advertise<cloud_msgs::cloud_info> ("/segmented_cloud_info", 1);
pubOutlierCloud = nh.advertise<sensor_msgs::PointCloud2> ("/outlier_cloud", 1);
nanPoint.x = std::numeric_limits<float>::quiet_NaN();
nanPoint.y = std::numeric_limits<float>::quiet_NaN();
nanPoint.z = std::numeric_limits<float>::quiet_NaN();
nanPoint.intensity = -1;
//分配内存
allocateMemory();
//初始化参数
resetParameters();2. 订阅点云，进入回调函数cloudHandler()
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
// 1. Convert ros message to pcl point cloud
copyPointCloud(laserCloudMsg);
// 2. Start and end angle of a scan
findStartEndAngle();
// 3. Range image projection
projectPointCloud();
// 4. Mark ground points
groundRemoval();
// 5. Point cloud segmentation
cloudSegmentation();
// 6. Publish all clouds
publishCloud();
// 7. Reset parameters for next iteration
resetParameters();
}3. copyPointCloud函数，将点云转换为pcl类型（百度网盘中的驱动转换为pcl时带有ring和tm），
此时laserCloudIn为去除无效点之后的点云。
void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
cloudHeader = laserCloudMsg->header;
pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
// Remove Nan points
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
// have "ring" channel in the cloud
if (useCloudRing == true){
pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
if (laserCloudInRing->is_dense == false) {
ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
ros::shutdown();
}
}
}4. findStartEndAngle函数，计算一帧点云的起始角和终止角。
4.1 velodyne激光器的坐标系，以及线ID的排序
4.2. 代码中使用atan2(y,x),求的是扫描点相对于原点的角度（即扫描的起始角度，以x轴为起始轴，按顺时针进行扫描，而实际坐标系为右手定则，所以需要加上负号），所求解的startOrientation，endOrientation，orientationDiff并未使用
void findStartEndAngle(){
// start and end orientation of this cloud
segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
segMsg.endOrientation = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y, laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI;
if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI) {
segMsg.endOrientation -= 2 * M_PI;
} else if (segMsg.endOrientation - segMsg.startOrientation < M_PI)
segMsg.endOrientation += 2 * M_PI;
segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;
}5. projectPointCloud函数，将点云投影成图片的方式，线id作为该像素点的行，通过扫描的角分辨率去确定该像素点的列，
void projectPointCloud(){
// range image projection
float verticalAngle, horizonAngle, range;
size_t rowIdn, columnIdn, index, cloudSize;
PointType thisPoint;
cloudSize = laserCloudIn->points.size();
for (size_t i = 0; i < cloudSize; ++i){
thisPoint.x = laserCloudIn->points[i].x;
thisPoint.y = laserCloudIn->points[i].y;
thisPoint.z = laserCloudIn->points[i].z;
// find the row and column index in the iamge for this point
if (useCloudRing == true){
rowIdn = laserCloudInRing->points[i].ring;
}
else{
verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
}
if (rowIdn < 0 || rowIdn >= N_SCAN)
continue;
//horizonAngle的角度范围为[-180, 180]
horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
// horizonAngle-90 将角度范围变为[-270, 90], round之后将范围变为[3/4H, -1/4H]
// 再加上1/2H, 使columnIdn的变为[5/4H， 1/4H]
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;
range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
if (range < sensorMinimumRange)
continue;
//将点云的距离存放在rangeMat中
rangeMat.at<float>(rowIdn, columnIdn) = range;
//强度的整数部分存在线id，小数部分存放像素点的列
thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;
//像素点的索引号
index = columnIdn + rowIdn * Horizon_SCAN;
//fullCloud和fullInfoCloud两者存放点的区别在于强度的存储值，后者存放的是距离
fullCloud->points[index] = thisPoint;
fullInfoCloud->points[index] = thisPoint;
fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"
}
}6. groundRemoval 地面点的检测, 这个地方可以对比hdl_graph_slam和mulls中地面点清除
6.1. lego-loam地面点检测：计算上下两根线对应点的俯仰角atan2(z, sqrt(x^2+y^2)),如果角度 在10度以下，则认为两者为地面点
void groundRemoval(){
size_t lowerInd, upperInd;
float diffX, diffY, diffZ, angle;
// groundMat
// -1, no valid info to check if ground of not
// 0, initial value, after validation, means not ground
// 1, ground
for (size_t j = 0; j < Horizon_SCAN; ++j){
for (size_t i = 0; i < groundScanInd; ++i){
lowerInd = j + ( i )*Horizon_SCAN;
upperInd = j + (i+1)*Horizon_SCAN;
if (fullCloud->points[lowerInd].intensity == -1 ||
fullCloud->points[upperInd].intensity == -1){
// no info to check, invalid points
groundMat.at<int8_t>(i,j) = -1;
continue;
}
diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;
angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;
if (abs(angle - sensorMountAngle) <= 10){
groundMat.at<int8_t>(i,j) = 1;
groundMat.at<int8_t>(i+1,j) = 1;
}
}
}
// extract ground cloud (groundMat == 1)
// mark entry that doesn't need to label (ground and invalid point) for segmentation
// note that ground remove is from 0~N_SCAN-1, need rangeMat for mark label matrix for the 16th scan
for (size_t i = 0; i < N_SCAN; ++i){
for (size_t j = 0; j < Horizon_SCAN; ++j){
if (groundMat.at<int8_t>(i,j) == 1 || rangeMat.at<float>(i,j) == FLT_MAX){
labelMat.at<int>(i,j) = -1;
}
}
}
if (pubGroundCloud.getNumSubscribers() != 0){
for (size_t i = 0; i <= groundScanInd; ++i){
for (size_t j = 0; j < Horizon_SCAN; ++j){
if (groundMat.at<int8_t>(i,j) == 1)
groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
}
}
}
if(scan_num == 10)
{
for (size_t i = 0; i <= groundScanInd; ++i){
for (size_t j = 0; j < Horizon_SCAN; ++j){
if (groundMat.at<int8_t>(i,j) == 1)
outfile1 << fullCloud->points[j + i*Horizon_SCAN].x << " " << fullCloud->points[j + i*Horizon_SCAN].y << " " << fullCloud->points[j + i*Horizon_SCAN].z << endl;
}
}
}
}
地面点提取的效果图:
6.2. hdl_graph_slam地面点检测：根据高度参数将地面分割出来，然后使用法向量过滤，最 后通过采用Ransac平面约束来筛选
void groundRemoval_hdl()
{
Eigen::Matrix4f tilt_matrix = Eigen::Matrix4f::Identity();
tilt_matrix.topLeftCorner(3, 3) = Eigen::AngleAxisf(tilt_deg * M_PI / 180.0f,
Eigen::Vector3f::UnitY()).toRotationMatrix();
// filtering before RANSAC (height and normal filtering)
if(scan_num == 10)
{
pcl::io::savePLYFileASCII(testDirectory+"before.ply", *fullCloud);
}
pcl::PointCloud<PointType>::Ptr filtered(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*fullCloud, *filtered, tilt_matrix);
//false get the upward
filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height + height_clip_range), false);
//true get the down
filtered = plane_clip(filtered, Eigen::Vector4f(0.0f, 0.0f, 1.0f, sensor_height - height_clip_range), true);
if(scan_num == 10)
{
std::cout << "num:" << filtered->points.size() << endl;
pcl::io::savePLYFileASCII(testDirectory+"after.ply", *filtered);
}
if(use_normal_filtering) {
filtered = normal_filtering(filtered);
}
if(scan_num == 10)
{
pcl::io::savePLYFileASCII(testDirectory+"normal_filter.ply", *filtered);
}
pcl::SampleConsensusModelPlane<PointType>::Ptr model_p(new
pcl::SampleConsensusModelPlane<PointType>(filtered));
pcl::RandomSampleConsensus<PointType> ransac(model_p);
ransac.setDistanceThreshold(0.1);
ransac.computeModel();
pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
ransac.getInliers(inliers->indices);
// too few inliers
if(inliers->indices.size() < floor_pts_thresh) {
return ;
}
// verticality check of the detected floor's normal
Eigen::Vector4f reference = tilt_matrix.inverse() * Eigen::Vector4f::UnitZ();
Eigen::VectorXf coeffs;
ransac.getModelCoefficients(coeffs);
double dot = coeffs.head<3>().dot(reference.head<3>());
if(std::abs(dot) < std::cos(floor_normal_thresh * M_PI / 180.0)) {
// the normal is not vertical
return ;
}
// make the normal upward
if(coeffs.head<3>().dot(Eigen::Vector3f::UnitZ()) < 0.0f) {
coeffs *= -1.0f;
}
if(scan_num == 10) {
pcl::PointCloud<PointType>::Ptr inlier_cloud(new pcl::PointCloud<PointType>);
pcl::ExtractIndices<PointType> extract;
extract.setInputCloud(filtered);
extract.setIndices(inliers);
extract.filter(*inlier_cloud);
if(scan_num == 10)
{
pcl::io::savePLYFileASCII(testDirectory+"ransac_filter.ply", *inlier_cloud);
}
}
}地面点的提取效果为：
6.3 MULLS地面点检测：通过设置两个阈值来进行筛选
地面点的提取效果为(MULLS 里面有很多参数都可以调节)：
对比三种方法hdl和MULLS的方法要远比lego-loam的好，都采用了RANSAC进行约束，通过高度过滤进行地面提取。
7. cloudSegmentation点云分割，首先对每个点执行labelComponents函数，使用BFS来对点云进行分类，
BFS的讲解可以查看搜索思想——DFS & BFS（基础基础篇） - 知乎 (zhihu.com)，
该部分的论文为：Fast Range Image-Based Segmentation of Sparse 3D Laser Scans for Online Operation。
void labelComponents(int row, int col){
// use std::queue std::vector std::deque will slow the program down greatly
float d1, d2, alpha, angle;
int fromIndX, fromIndY, thisIndX, thisIndY;
bool lineCountFlag[N_SCAN] = {false};
queueIndX[0] = row;
queueIndY[0] = col;
int queueSize = 1;
int queueStartInd = 0;
int queueEndInd = 1;
allPushedIndX[0] = row;
allPushedIndY[0] = col;
int allPushedIndSize = 1;
while(queueSize > 0){
// Pop point
fromIndX = queueIndX[queueStartInd];
fromIndY = queueIndY[queueStartInd];
--queueSize;
++queueStartInd;
// Mark popped point
labelMat.at<int>(fromIndX, fromIndY) = labelCount;
//遍历该点周围的4个点，确保周围点的坐标在投影图像中，将投影图看成是圆柱型
for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){
// new index
thisIndX = fromIndX + (*iter).first;
thisIndY = fromIndY + (*iter).second;
// index should be within the boundary
if (thisIndX < 0 || thisIndX >= N_SCAN)
continue;
// at range image margin (left or right side)
if (thisIndY < 0)
thisIndY = Horizon_SCAN - 1;
if (thisIndY >= Horizon_SCAN)
thisIndY = 0;
//labelMat 不等于零的点为已经分类之后的点
if (labelMat.at<int>(thisIndX, thisIndY) != 0)
continue;
//β=arctan(d2*sinα/(d1 - d2cosα)) 其中α为两个点之间的夹角，对应垂直和水平方向的分辨率
d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY),
rangeMat.at<float>(thisIndX, thisIndY));
d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY),
rangeMat.at<float>(thisIndX, thisIndY));
if ((*iter).first == 0)
alpha = segmentAlphaX;
else
alpha = segmentAlphaY;
angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
//若计算的角度大于segmentTheta，则认为该点和待分配点是同一类点，并把该点的像素坐标保存，便于下次将该点作为基准点
//将该点的label值记为和待分配点一致
if (angle > segmentTheta){
queueIndX[queueEndInd] = thisIndX;
queueIndY[queueEndInd] = thisIndY;
++queueSize;
++queueEndInd;
labelMat.at<int>(thisIndX, thisIndY) = labelCount;
//将该点所在线序记为true
lineCountFlag[thisIndX] = true;
//将该点的像素值保存
allPushedIndX[allPushedIndSize] = thisIndX;
allPushedIndY[allPushedIndSize] = thisIndY;
++allPushedIndSize;
}
}
}
// check if this segment is valid
// 同类的点数大于30或者同类点小于30大于5，并且这些点在至少3根线，则认为这些点为同类点
//将feasibleSegment置为true，否则为false 并将label置为999999
bool feasibleSegment = false;
if (allPushedIndSize >= 30)
feasibleSegment = true;
else if (allPushedIndSize >= segmentValidPointNum){
int lineCount = 0;
for (size_t i = 0; i < N_SCAN; ++i)
if (lineCountFlag[i] == true)
++lineCount;
if (lineCount >= segmentValidLineNum)
feasibleSegment = true;
}
// segment is valid, mark these points
if (feasibleSegment == true){
++labelCount;
}else{ // segment is invalid, mark these points
for (size_t i = 0; i < allPushedIndSize; ++i){
labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
}
}
}cloudSegmentation，对分类后的点云进行处理，其中外点主要包括label值为999999并且该点的像素值u大于地面的线序值，自定义类型segMsg中，segmentedCloudGroundFlag为true则代表其为地面点 false为非地面点，segmentedCloudColInd为该像素点的v，segmentedCloudRange为该点的距离，segmentedCloud存放的是分类后的点云并舍弃了每条线前后的五个点，分类后的点存放在segmentedCloudPure中（不包含地面点）
void cloudSegmentation(){
// segmentation process
for (size_t i = 0; i < N_SCAN; ++i)
for (size_t j = 0; j < Horizon_SCAN; ++j)
if (labelMat.at<int>(i,j) == 0)
labelComponents(i, j);
int sizeOfSegCloud = 0;
// extract segmented cloud for lidar odometry
for (size_t i = 0; i < N_SCAN; ++i) {
segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5;
for (size_t j = 0; j < Horizon_SCAN; ++j) {
//如果该点有被分类，或者属于地面点
if (labelMat.at<int>(i,j) > 0 || groundMat.at<int8_t>(i,j) == 1){
// outliers that will not be used for optimization (always continue)
//999999代表的是无效点，outlierCloud不包含线id小于地面线的点，降采样，隔5个点采样一次
if (labelMat.at<int>(i,j) == 999999){
if (i > groundScanInd && j % 5 == 0){
outlierCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
continue;
}else{
continue;
}
}
// majority of ground points are skipped
if (groundMat.at<int8_t>(i,j) == 1){
if (j%5!=0 && j>5 && j<Horizon_SCAN-5)
continue;
}
// mark ground points so they will not be considered as edge features later
segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);
// mark the points' column index for marking occlusion later
segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
// save range info
segMsg.segmentedCloudRange[sizeOfSegCloud] = rangeMat.at<float>(i,j);
// save seg cloud
segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
// size of seg cloud
++sizeOfSegCloud;
}
}
segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;
}
// extract segmented cloud for visualization
//如果需要显示分割之后的点，则将其强度值信息赋值为类型号
if (pubSegmentedCloudPure.getNumSubscribers() != 0){
for (size_t i = 0; i < N_SCAN; ++i){
for (size_t j = 0; j < Horizon_SCAN; ++j){
if (labelMat.at<int>(i,j) > 0 && labelMat.at<int>(i,j) != 999999){
segmentedCloudPure->push_back(fullCloud->points[j + i*Horizon_SCAN]);
segmentedCloudPure->points.back().intensity = labelMat.at<int>(i,j);
}
}
}
}
}8. 发布对应的点：
pubFullCloud 为将点云投影为图像之后的所有点，其中强度信息保存的是uv值，u为整型部分，v为小数部分
pubFullInfoCloud 和pubFullCloud类似，只是其强度信息中保存的是点的距离 pubGroundCloud 为地面点
pubSegmentedCloud 分类后的点包括地面点
pubSegmentedCloudPure 分类后的点云 不包括地面点;
pubSegmentedCloudInfo 自定义类型的点云;
pubOutlierCloud 外点;
void publishCloud(){
// 1. Publish Seg Cloud Info
segMsg.header = cloudHeader;
pubSegmentedCloudInfo.publish(segMsg);
// 2. Publish clouds
sensor_msgs::PointCloud2 laserCloudTemp;
pcl::toROSMsg(*outlierCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubOutlierCloud.publish(laserCloudTemp);
// segmented cloud with ground
pcl::toROSMsg(*segmentedCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubSegmentedCloud.publish(laserCloudTemp);
// projected full cloud
if (pubFullCloud.getNumSubscribers() != 0){
pcl::toROSMsg(*fullCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubFullCloud.publish(laserCloudTemp);
}
// original dense ground cloud
if (pubGroundCloud.getNumSubscribers() != 0){
pcl::toROSMsg(*groundCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubGroundCloud.publish(laserCloudTemp);
}
// segmented cloud without ground
if (pubSegmentedCloudPure.getNumSubscribers() != 0){
pcl::toROSMsg(*segmentedCloudPure, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubSegmentedCloudPure.publish(laserCloudTemp);
}
// projected full cloud info
if (pubFullInfoCloud.getNumSubscribers() != 0){
pcl::toROSMsg(*fullInfoCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubFullInfoCloud.publish(laserCloudTemp);
}
}
1121
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
