imageProjection
总体叙述
主要功能为：
- 利用当前激光帧起止时刻间的imu数据计算旋转增量，IMU里程计数据（来自ImuPreintegration）计算平移增量，
- 对该帧激光每一时刻的激光点进行运动畸变校正（利用相对于激光帧起始时刻的位姿增量，
- 变换当前激光点到起始时刻激光点的坐标系下，实现校正）；
- 最后用IMU数据的姿态角（
RPY，roll、pitch、yaw）、IMU里程计数据的的位姿，对当前帧激光位姿进行粗略初始化。
struct VelodynePointXYZIRT
{
PCL_ADD_POINT4D // 位置
PCL_ADD_INTENSITY; //激光点云强度，也可存点的索引
uint16_t ring; //扫描线
float time; //时间戳，记录相对于当前帧第一个激光点的时差，第一个点time=0
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
类成员变量
- 互斥锁：
std::mutex imuLock、odoLock - 订阅原始点云
subLaserCloud、pubLaserCloud(未用) - 发布当前激光帧运动畸变校正后的点云
pubExtractedCloud、pubLaserCloudInfo - imu数据队列（原始数据，转lidar系下）
subImu、imuQueue - 激光点云数据队列
cloudQueue，currentCloudMsg(PointCloud2) - 当前激光帧起止时刻间对应的imu数据，计算相对于起始时刻的旋转增量，以及时时间戳；
double *imuTime,*imuRotX,*imuRotY,*imuRotZ- 目的：用于插值计算当前激光帧起止时间范围内，每一时刻的旋
- 当前帧起始时间+当前帧起始对应imu位姿变化
imuPointerCur - 当前帧原始激光点云 + 当期帧运动畸变校正之后的激光点云
- 当前激光帧起止时刻对应imu里程计位姿变换，该变换对应的平移增量；
用于插值计算当前激光帧起止时间范围内，每一时刻的位置
odomIncreX odomIncreY odomIncreZ - 当前帧激光点云运动畸变校正之后的数据，包括点云数据、初始位姿、姿态角等
lio_sam::cloud_info cloudInfo
构造
// 订阅原始imu数据
subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
// 订阅imu里程计，由imuPreintegration积分计算得到的每时刻imu位姿
subOdom = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
// 订阅原始lidar数据
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());
// 发布当前激光帧运动畸变校正后的点云，有效点
pubExtractedCloud = nh.advertise<sensor_msgs::PointCloud2> ("lio_sam/deskew/cloud_deskewed", 1);
// 发布当前激光帧运动畸变校正后的点云信息
pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/deskew/cloud_info", 1);
allocateMemory(); // 初始化
resetParameters(); // 重置参数
pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
imu/odometryHandler
imu回调
- 将imu原始测量数据转换到lidar系，加速度，角速度，PRY
- 将数据放入队列
odom回调
- 直接放入odom_deque
cloudHandler 点云回调
订阅原始lidar数据
* 1、添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性
* 2、当前帧起止时刻对应的imu数据、imu里程计数据处理
* imu数据：
* 1) 遍历当前激光帧起止时刻之间的imu数据，初始时刻对应imu的姿态角RPY设为当前帧的初始姿态角
* 2) 用角速度、时间积分，计算每一时刻相对于初始时刻的旋转量，初始时刻旋转设为0
* imu里程计数据：
* 1) 遍历当前激光帧起止时刻之间的imu里程计数据，初始时刻对应imu里程计设为当前帧的初始位姿
* 2) 用起始、终止时刻对应imu里程计，计算相对位姿变换，保存平移增量
* 3、当前帧激光点云运动畸变校正
* 1) 检查激光点距离、扫描线是否合规
* 2) 激光运动畸变校正，保存激光点
* 4、提取有效激光点，存extractedCloud
* 5、发布当前帧校正后点云，有效点
* 6、重置参数，接收每帧lidar数据都要重置这些参数
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
// 添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性
if (!cachePointCloud(laserCloudMsg))
return;
// 当前帧起止时刻对应的imu数据、imu里程计数据处理
if (!deskewInfo())
return;
// 当前帧激光点云运动畸变校正
projectPointCloud();
// 提取有效激光点，存extractedCloud
cloudExtraction();
// 发布当前帧校正后点云，有效点
publishClouds();
// 重置参数，接收每帧lidar数据都要重置这些参数
resetParameters();
}
cachePointCloud
添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性
- 缓存三帧，计算第一帧，检测数据的有效性+检测有
time+ring通道
// 添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性
bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
// cache point cloud
cloudQueue.push_back(*laserCloudMsg);
// 延迟了两帧？？
if (cloudQueue.size() <= 2)
return false;
// convert cloud // 取出激光点云队列中最早的一帧
currentCloudMsg = std::move(cloudQueue.front());
cloudQueue.pop_front();
if (sensor == SensorType::VELODYNE)
{
// 转换成pcl点云格式
pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);
}
else if (sensor == SensorType::OUSTER)
{
// Convert to Velodyne format // 转换成Velodyne格式
pcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn);
laserCloudIn->points.resize(tmpOusterCloudIn->size());
laserCloudIn->is_dense = tmpOusterCloudIn->is_dense;
for (size_t i = 0; i < tmpOusterCloudIn->size(); i++)
{
auto &src = tmpOusterCloudIn->points[i];
auto &dst = laserCloudIn->points[i];
dst.x = src.x;
dst.y = src.y;
dst.z = src.z;
dst.intensity = src.intensity;
dst.ring = src.ring;
// 主要是时间戳
dst.time = src.t * 1e-9f;
}
}
else
{
ROS_ERROR_STREAM("Unknown sensor type: " << int(sensor));
ros::shutdown();
}
// get timestamp
cloudHeader = currentCloudMsg.header;
timeScanCur = cloudHeader.stamp.toSec();
// 当前帧结束时刻，注：点云中激光点的time记录相对于当前帧第一个激光点的时差，第一个点time=0
timeScanEnd = timeScanCur + laserCloudIn->points.back().time;
// check dense flag // 存在无效点，Nan或者Inf
if (laserCloudIn->is_dense == false)
{
ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
ros::shutdown();
}
// check ring channel // 检查是否存在ring通道，注意static只检查一次
static int ringFlag = 0;
if (ringFlag == 0)
{
ringFlag = -1;
for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i)
{
if (currentCloudMsg.fields[i].name == "ring")
{
ringFlag = 1;
break;
}
}
if (ringFlag == -1)
{
ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!");
ros::shutdown();
}
}
// check point time // 检查是否存在time通道
if (deskewFlag == 0)
{
deskewFlag = -1;
for (auto &field : currentCloudMsg.fields)
{
if (field.name == "time" || field.name == "t")
{
deskewFlag = 1;
break;
}
}
if (deskewFlag == -1)
ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!");
}
return true;
}
deskewInfo
处理当前激光帧起止时刻对应的IMU数据、IMU里程计数据
- 要求imu数据包含激光数据，否则不往下处理了
if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd) { ROS_DEBUG("Waiting for IMU data ..."); return false; }
2、 当前帧对应imu数据处理
- 遍历当前激光帧起止时刻之间的imu数据，初始时刻对应imu的姿态角RPY设为当前帧的初始姿态角
- 用角速度、时间积分，计算每一时刻相对于初始时刻的旋转量，初始时刻旋转设为0
- 注：imu数据都已经转换到lidar系下了
void imuDeskewInfo() { cloudInfo.imuAvailable = false; // 从imu队列中删除当前激光帧0.01s前面时刻的imu数据 while (!imuQueue.empty()) { if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01) imuQueue.pop_front(); else break; } if (imuQueue.empty()) return; imuPointerCur = 0; // 遍历当前激光帧起止时刻（前后扩展0.01s）之间的imu数据 for (int i = 0; i < (int)imuQueue.size(); ++i) { sensor_msgs::Imu thisImuMsg = imuQueue[i]; double currentImuTime = thisImuMsg.header.stamp.toSec(); // get roll, pitch, and yaw estimation for this scan // 提取imu姿态角RPY，作为当前lidar帧初始姿态角 if (currentImuTime <= timeScanCur) imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit); // 超过当前激光帧结束时刻0.01s，结束 if (currentImuTime > timeScanEnd + 0.01) break; // 第一帧imu旋转角初始化 if (imuPointerCur == 0){ imuRotX[0] = 0; imuRotY[0] = 0; imuRotZ[0] = 0; imuTime[0] = currentImuTime; ++imuPointerCur; continue; } // get angular velocity // 提取imu角速度 double angular_x, angular_y, angular_z; imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z); // integrate rotation // imu帧间时差 double timeDiff = currentImuTime - imuTime[imuPointerCur-1]; // 当前时刻旋转角 = 前一时刻旋转角 + 角速度 * 时差 imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff; imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff; imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff; imuTime[imuPointerCur] = currentImuTime; ++imuPointerCur; } --imuPointerCur; // 没有合规的imu数据 if (imuPointerCur <= 0) return; cloudInfo.imuAvailable = true; }
3、当前帧对应imu里程计处理
- 遍历当前激光帧起止时刻之间的imu里程计数据，初始时刻对应imu里程计设为当前帧的初始位姿
- 用起始、终止时刻对应imu里程计，计算相对位姿变换，保存平移增量
- 注：imu数据都已经转换到lidar系下了
void odomDeskewInfo() { cloudInfo.odomAvailable = false; // 从imu里程计队列中删除当前激光帧0.01s前面时刻的imu数据 while (!odomQueue.empty()) { if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01) odomQueue.pop_front(); else break; } if (odomQueue.empty()) return; // 要求必须有当前激光帧时刻之前的imu里程计数据 if (odomQueue.front().header.stamp.toSec() > timeScanCur) return; // get start odometry at the beinning of the scan // 提取当前激光帧起始时刻的imu里程计 nav_msgs::Odometry startOdomMsg; for (int i = 0; i < (int)odomQueue.size(); ++i) { startOdomMsg = odomQueue[i]; if (ROS_TIME(&startOdomMsg) < timeScanCur) continue; else break; } // 提取imu里程计姿态角 tf::Quaternion orientation; tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation); double roll, pitch, yaw; tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); // Initial guess used in mapOptimization // 用当前激光帧起始时刻的imu里程计，初始化lidar位姿，后面用于mapOptmization cloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x; cloudInfo.initialGuessY = startOdomMsg.pose.pose.position.y; cloudInfo.initialGuessZ = startOdomMsg.pose.pose.position.z; cloudInfo.initialGuessRoll = roll; cloudInfo.initialGuessPitch = pitch; cloudInfo.initialGuessYaw = yaw; cloudInfo.odomAvailable = true; // get end odometry at the end of the scan odomDeskewFlag = false; // 如果当前激光帧结束时刻之后没有imu里程计数据，返回 if (odomQueue.back().header.stamp.toSec() < timeScanEnd) return; // 提取当前激光帧结束时刻的imu里程计 nav_msgs::Odometry endOdomMsg; for (int i = 0; i < (int)odomQueue.size(); ++i) { endOdomMsg = odomQueue[i]; if (ROS_TIME(&endOdomMsg) < timeScanEnd) continue; else break; } // 如果起止时刻对应imu里程计的方差不等，返回 if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0]))) return; Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw); tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation); tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw); // 起止时刻imu里程计的相对变换 Eigen::Affine3f transBt = transBegin.inverse() * transEnd; // 相对变换，提取增量平移、旋转（欧拉角） float rollIncre, pitchIncre, yawIncre; pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre); odomDeskewFlag = true; }
projectPointCloud
- 当前帧激光点云运动畸变校正
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;
// 距离超出阈值时，舍弃
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;
// 计算水平角度，直接转为角度了 horizonAngle = 90 -angle
float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
static float ang_res_x = 360.0/float(Horizon_SCAN); // 角分辨率
// 计算图像的列，[-180,180]
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;
if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
continue;
// 运动去畸变
thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);
// rangeMat 存放的长度
rangeMat.at<float>(rowIdn, columnIdn) = range;
// fullCloud 存放去畸变后的点云
int index = columnIdn + rowIdn * Horizon_SCAN;
fullCloud->points[index] = thisPoint;
}
}
deskewPoint
- input: 点云去畸变，当前点+距离起点的时间戳
- output: 去畸变后的点云
- function:
- 利用当前帧起止时刻之间的imu数据计算旋转增量，imu里程计数据计算平移增量，
- 进而将每一时刻激光点位置变换到第一个激光点坐标系下，进行运动补偿
- Code:
PointType deskewPoint(PointType *point, double relTime) { if (deskewFlag == -1 || cloudInfo.imuAvailable == false) return *point; // relTime是当前激光点相对于激光帧起始时刻的时间，pointTime则是当前激光点的时间戳 double pointTime = timeScanCur + relTime; float rotXCur, rotYCur, rotZCur; // 在当前激光帧起止时间范围内，计算某一时刻的旋转（相对于起始时刻的旋转增量） // 由imu队列计算得到 findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur); float posXCur, posYCur, posZCur; // 在当前激光帧起止时间范围内，计算某一时刻的平移（相对于起始时刻的平移增量） // 由里程计队列得到 findPosition(relTime, &posXCur, &posYCur, &posZCur); // 第一个点的位姿增量（0），求逆 if (firstPointFlag == true) { transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse(); firstPointFlag = false; } // transform points to start // 当前时刻激光点与第一个激光点的位姿变换 Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur); Eigen::Affine3f transBt = transStartInverse * transFinal; PointType newPoint; // 当前激光点在第一个激光点坐标系下的坐标 newPoint.x = transBt(0,0) * point->x + transBt(0,1) * point->y + transBt(0,2) * point->z + transBt(0,3); newPoint.y = transBt(1,0) * point->x + transBt(1,1) * point->y + transBt(1,2) * point->z + transBt(1,3); newPoint.z = transBt(2,0) * point->x + transBt(2,1) * point->y + transBt(2,2) * point->z + transBt(2,3); newPoint.intensity = point->intensity; return newPoint; }
cloudExtraction
- 提取有效激光点，把长度不符合去掉，且只保留有效点
- 记录了每条线 起始和终止5个点的索引
cloudInfo.startRingIndex/endRingIndex
void cloudExtraction()
{
// 有效激光点数量
int count = 0;
// extract segmented cloud for lidar odometry
for (int i = 0; i < N_SCAN; ++i)
{
// 记录每根扫描线起始第5个激光点在一维数组中的索引
cloudInfo.startRingIndex[i] = count - 1 + 5;
for (int j = 0; j < Horizon_SCAN; ++j)
{
// 有效激光点，
if (rangeMat.at<float>(i,j) != FLT_MAX)
{
// mark the points' column index for marking occlusion later
// 记录激光点对应的Horizon_SCAN方向上的索引
cloudInfo.pointColInd[count] = j;
// save range info// 激光点距离
cloudInfo.pointRange[count] = rangeMat.at<float>(i,j);
// save extracted cloud // 加入有效激光点
extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
// size of extracted cloud
++count;
}
}
// 记录每根扫描线倒数第5个激光点在一维数组中的索引
cloudInfo.endRingIndex[i] = count -1 - 5;
}
}
2415
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
