imageProjection.cpp关于制作RangeMat过程代码逻辑处理 #13
Description
/**
* @brief
* 3、执行点云去畸变
* 1. 检查激光点距离、扫描线是否合规
* 2. 单点去畸变
* 3. 保存激光点
*/
void projectPointCloud()
{
int cloudSize = laserCloudIn->points.size();
// range image projection
for (int i = 0; i < cloudSize; ++i)
{
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;
// range从点云的x、y、z算出来，而没有使用topic信息中的range
// range映射成2D矩阵时
// 行索引rowInd等于该点的ring
// 列索引根据角度和分辨率计算得到
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;
// 计算该点对应的列索引
/**
ID参考图： rangeMat参考的图片
^ y
|
|
id = 360 |
---------------------------------> x id = 180
id = 0 |
|
|
|
*/
float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
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;
// 将该点的range记录到rangeMat
// 用来后续进行特征提取
if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
continue;
rangeMat.at<float>(rowIdn, columnIdn) = range;
// 单点进行去畸变
thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);
// 将去完畸变的点云存储在中间变量 会进行顺序存储
int index = columnIdn + rowIdn * Horizon_SCAN;
fullCloud->points[index] = thisPoint;
}
}
作者我在学习您代码过程中发现：
在计算完雷达点的行列索引后，先将range直接给了rangeMat，这里还有当前帧去畸变的逻辑。
是不是应该先去完畸变在将结果给rangeMat。