imageProjection.cpp
#include "utility.h"
#include "lio_sam/cloud_info.h"
struct VelodynePointXYZIRT
{
PCL_ADD_POINT4D
PCL_ADD_INTENSITY;
uint16_t ring;
float time;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (VelodynePointXYZIRT,
(float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
(uint16_t, ring, ring) (float, time, time)
)
struct OusterPointXYZIRT {
PCL_ADD_POINT4D;
float intensity;
uint32_t t;
uint16_t reflectivity;
uint8_t ring;
uint16_t noise;
uint32_t range;
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT(OusterPointXYZIRT,
(float, x, x) (float, y, y) (float, z, z) (float, intensity, intensity)
(uint32_t, t, t) (uint16_t, reflectivity, reflectivity)
(uint8_t, ring, ring) (uint16_t, noise, noise) (uint32_t, range, range)
)
// Use the Velodyne point format as a common representation
using PointXYZIRT = VelodynePointXYZIRT;
//循环IMU长度,IMU频率设为2000Hz
const int queueLength = 2000;
//引用参数
//利用继承参数类对象,派生出新的类
class ImageProjection : public ParamServer
{
private:
std::mutex imuLock;
std::mutex odoLock;//互斥
//订阅和发布话题
ros::Subscriber subLaserCloud;
ros::Publisher pubLaserCloud;
ros::Publisher pubExtractedCloud;
ros::Publisher pubLaserCloudInfo;
//同时创建信息队列
ros::Subscriber subImu;
std::deque<sensor_msgs::Imu> imuQueue;
ros::Subscriber subOdom;
std::deque<nav_msgs::Odometry> odomQueue;
std::deque<sensor_msgs::PointCloud2> cloudQueue;
sensor_msgs::PointCloud2 currentCloudMsg;
//创建IMU数据空间
double *imuTime = new double[queueLength];
double *imuRotX = new double[queueLength];
double *imuRotY = new double[queueLength];
double *imuRotZ = new double[queueLength];
int imuPointerCur;//当前IMU点
bool firstPointFlag;//第一帧数据的标致位
Eigen::Affine3f transStartInverse;
//创建点云
pcl::PointCloud<PointXYZIRT>::Ptr laserCloudIn;
pcl::PointCloud<OusterPointXYZIRT>::Ptr tmpOusterCloudIn;
pcl::PointCloud<PointType>::Ptr fullCloud;
pcl::PointCloud<PointType>::Ptr extractedCloud;
int deskewFlag;
// cv::Mat统一了矩阵和图像这两个概念。矩阵变换函数
// 事实上,矩阵和图像其实是一样的。
// 由于cv::Mat是c++的类,所以也具备了相关的一些特征。
// 例如,内存的释放。在C++中,一个对象超出其使用范围后,会自动调用析构函数进行销毁。
// 而在c中,如果给CvMat类型的变量使用函数cvCreateImage 等函数分配了内存空间,
// 那么必须调用相应的函数进行释放,而不会自动销毁。如果没有相应的释放,则会造成内存泄漏。
cv::Mat rangeMat;
bool odomDeskewFlag;
float odomIncreX;
float odomIncreY;
float odomIncreZ;
//这个对象包括 IMU是否可用、odom是否可用、lidar的位姿、scan中各点对应线号、各点与光心距离信息
lio_sam::cloud_info cloudInfo;
double timeScanCur;
double timeScanEnd;
std_msgs::Header cloudHeader;
public:
ImageProjection():
deskewFlag(0)
{
subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &ImageProjection::imuHandler,
this, ros::TransportHints().tcpNoDelay());
subOdom = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000,
&ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
//其他函数都在这个回调函数进行
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数据清零
void allocateMemory()
{
laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>());
tmpOusterCloudIn.reset(new pcl::PointCloud<OusterPointXYZIRT>());
fullCloud.reset(new pcl::PointCloud<PointType>());
extractedCloud.reset(new pcl::PointCloud<PointType>());
fullCloud->points.resize(N_SCAN*Horizon_SCAN);
cloudInfo.startRingIndex.assign(N_SCAN, 0);
cloudInfo.endRingIndex.assign(N_SCAN, 0);
cloudInfo.pointColInd.assign(N_SCAN*Horizon_SCAN, 0);//全部点云与光心距离assign分配内存
cloudInfo.pointRange.assign(N_SCAN*Horizon_SCAN, 0);//点与光心距离
resetParameters();
}
//
void resetParameters()
{
laserCloudIn->clear();
extractedCloud->clear();
// reset range matrix for range image projection
// 5
rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
imuPointerCur = 0;
firstPointFlag = true;
odomDeskewFlag = false;
for (int i = 0; i < queueLength; ++i)
{
imuTime[i] = 0;
imuRotX[i] = 0;
imuRotY[i] = 0;
imuRotZ[i] = 0;
}
}
~ImageProjection(){}
//仅接收放队列
void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
{
//对IMU数据矫正
sensor_msgs::Imu thisImu = imuConverter(*imuMsg);
std::lock_guard<std::mutex> lock1(imuLock);
imuQueue.push_back(thisImu);
// debug IMU data
// cout << std::setprecision(6);
// cout << "IMU acc: " << endl;
// cout << "x: " << thisImu.linear_acceleration.x <<
// ", y: " << thisImu.linear_acceleration.y <<
// ", z: " << thisImu.linear_acceleration.z << endl;
// cout << "IMU gyro: " << endl;
// cout << "x: " << thisImu.angular_velocity.x <<
// ", y: " << thisImu.angular_velocity.y <<
// ", z: " << thisImu.angular_velocity.z << endl;
// double imuRoll, imuPitch, imuYaw;
// tf::Quaternion orientation;
// tf::quaternionMsgToTF(thisImu.orientation, orientation);
// tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
// cout << "IMU roll pitch yaw: " << endl;
// cout << "roll: " << imuRoll << ", pitch: " << imuPitch << ", yaw: " << imuYaw << endl << endl;
}
void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg)
{
std::lock_guard<std::mutex> lock2(odoLock);
odomQueue.push_back(*odometryMsg);
}
//进行调用其他函数、深度图生成、去畸变、深度图中提取点云、发布和重置参数
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
if (!cachePointCloud(laserCloudMsg))
return;
//处理IMU和里程计,同步scan数据
if (!deskewInfo())
return;
//点云投影 ,需要对点云去畸变
projectPointCloud();
cloudExtraction();
publishClouds();
resetParameters();
}
//清除临时点云,检测点云帧里面是否由ring和time通道
//按照顺序展开分析,点云存储到队列
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)//传感器类型是否等于0
{
//ros格式转化为pcl格式
pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);
}
else if (sensor == SensorType::OUSTER)//传感器类型是否等于1
{
// Convert to Velodyne format将最早的点云转给tmpOusterCloudIn指针
pcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn);
laserCloudIn->points.resize(tmpOusterCloudIn->size());//根据最早的点云大小设置空间大小
//is_dense :指定points中的数据是否全部是有限的,此时为true;当数据集中包含有Inf/NaN等值时,此时为false。
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();
timeScanEnd = timeScanCur + laserCloudIn->points.back().time;
// check dense flag
//is_dense :指定points中的数据是否全部是有限的,此时为true;当数据集中包含有Inf/NaN等值时,此时为false。
if (laserCloudIn->is_dense == false)
{
ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
ros::shutdown();
}
// check ring channel
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
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;
}
//检测队列数量
bool deskewInfo()
{
std::lock_guard<std::mutex> lock1(imuLock);
std::lock_guard<std::mutex> lock2(odoLock);
// make sure IMU data available for the scan
// 第一帧点云时间
if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd)
{
ROS_DEBUG("Waiting for IMU data ...");
return false;
}
imuDeskewInfo();
odomDeskewInfo();
return true;
}
//计算imu转角
void imuDeskewInfo()
{
cloudInfo.imuAvailable = false;
//确保激光数据比IMU数据早
while (!imuQueue.empty())
{ //如果IMU时间比激光雷达时间早,就丢弃IMU靠前数据
if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
imuQueue.pop_front();
else
break;
}
if (imuQueue.empty())
return;
imuPointerCur = 0;
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
if (currentImuTime <= timeScanCur)
//将thisImuMsg 转到 ros姿态:&cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit
imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);
//IMU的因子时间需要在激光雷达时间段内
if (currentImuTime > timeScanEnd + 0.01)
break;
if (imuPointerCur == 0){
imuRotX[0] = 0;
imuRotY[0] = 0;
imuRotZ[0] = 0;
imuTime[0] = currentImuTime;
++imuPointerCur;
continue;
}
// get angular velocity
double angular_x, angular_y, angular_z;
//(1234) 1转2+3+4 ros形式角速度
imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);
// integrate rotation,旋转角度积分 (角度 时间)
double timeDiff = currentImuTime - imuTime[imuPointerCur-1];//IMU数据通过imuPointerCur进行序号标签
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;
if (imuPointerCur <= 0)
return;
cloudInfo.imuAvailable = true;
}
//读取里程计数据,并根据协防差判断是否相信
void odomDeskewInfo()
{
cloudInfo.odomAvailable = false;
//确保所有里程计因子在激光时间段内
while (!odomQueue.empty())
{
if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
odomQueue.pop_front();
else
break;
}
if (odomQueue.empty())
return;
if (odomQueue.front().header.stamp.toSec() > timeScanCur)
return;
// get start odometry at the beinning of the scan
nav_msgs::Odometry startOdomMsg;
for (int i = 0; i < (int)odomQueue.size(); ++i)
{
startOdomMsg = odomQueue[i];
if (ROS_TIME(&startOdomMsg) < timeScanCur)
continue;
else
break;
}
tf::Quaternion orientation;
//里程计四元素 转 TF信息类型
tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);
double roll, pitch, yaw;
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
// Initial guess used in mapOptimization
//里程计值作为估计值
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;
//使得里程计信息在激光雷达信息最后?
if (odomQueue.back().header.stamp.toSec() < timeScanEnd)
return;
nav_msgs::Odometry endOdomMsg;
for (int i = 0; i < (int)odomQueue.size(); ++i)
{
endOdomMsg = odomQueue[i];
if (ROS_TIME(&endOdomMsg) < timeScanEnd)
continue;
else
break;
}
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);
//求出头点云和末尾点云的相对位置
Eigen::Affine3f transBt = transBegin.inverse() * transEnd;
float rollIncre, pitchIncre, yawIncre;
//矩阵转姿态
pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);
odomDeskewFlag = true;
}
//对应时间的IMU查找,需要整数
void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
{
*rotXCur = 0; *rotYCur = 0; *rotZCur = 0;
int imuPointerFront = 0;
//直到找到临近的整数值
while (imuPointerFront < imuPointerCur)
{
if (pointTime < imuTime[imuPointerFront])
break;
++imuPointerFront;
}
if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0)
{
*rotXCur = imuRotX[imuPointerFront];
*rotYCur = imuRotY[imuPointerFront];
*rotZCur = imuRotZ[imuPointerFront];
} else {
//插值
int imuPointerBack = imuPointerFront - 1;
double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
//在整数前后进行按照百分比求和
*rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;
*rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;
*rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;
}
}
//高速时按位移百分比平移点云
//通过里程计信息计算位移变化量
void findPosition(double relTime, float *posXCur, float *posYCur, float *posZCur)
{
*posXCur = 0; *posYCur = 0; *posZCur = 0;
// If the sensor moves relatively slow, like walking speed, positional deskew seems to have little benefits. Thus code below is commented.
// if (cloudInfo.odomAvailable == false || odomDeskewFlag == false)
// return;
// float ratio = relTime / (timeScanEnd - timeScanCur);
// *posXCur = ratio * odomIncreX;
// *posYCur = ratio * odomIncreY;
// *posZCur = ratio * odomIncreZ;
}
//通过时间变化点找出该时间点的其他因子位姿信息,对点云进行去畸变
PointType deskewPoint(PointType *point, double relTime)
{
if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
return *point;
double pointTime = timeScanCur + relTime;
float rotXCur, rotYCur, rotZCur;
//查找对应是是时间的姿态,过程用到插值器,用IMU求旋转
findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);
//用里程计求位移变化,不过这里没有用
float posXCur, posYCur, posZCur;
findPosition(relTime, &posXCur, &posYCur, &posZCur);
//获得第一帧的逆变换矩阵
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;
//加入IMU因子的变换矩阵???
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;
}
//将点云地图投影成深度图
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;
//仰角
float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
//分辨率
static float ang_res_x = 360.0/float(Horizon_SCAN);//Horizon_SCAN线数1800 ,求每两条线的角度间隔
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.at<float>(rowIdn, columnIdn) = range;
int index = columnIdn + rowIdn * Horizon_SCAN;
fullCloud->points[index] = thisPoint;
}
}
//去边缘和过远的点
void cloudExtraction()
{
int count = 0;
// extract segmented cloud for lidar odometry
//每一行增加一圈线数1800,和栅格地图布局类似
for (int i = 0; i < N_SCAN; ++i)//16线lidar
{
cloudInfo.startRingIndex[i] = count - 1 + 5;//圈数从4开始到20
for (int j = 0; j < Horizon_SCAN; ++j)//读取每一圈Horizon_SCAN=1800
{
if (rangeMat.at<float>(i,j) != FLT_MAX)//异常值去除
{
// mark the points' column index for marking occlusion later
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;
}
}
cloudInfo.endRingIndex[i] = count -1 - 5;
}
}
void publishClouds()
{
cloudInfo.header = cloudHeader;
//pcl信息转ros信息
cloudInfo.cloud_deskewed = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
pubLaserCloudInfo.publish(cloudInfo);
}
};
int main(int argc, char** argv)
{
ros::init(argc, argv, "lio_sam");
ImageProjection IP;//程序进口
ROS_INFO("\033[1;32m----> Image Projection Started.\033[0m");
ros::MultiThreadedSpinner spinner(3);
spinner.spin();
return 0;
}
参考博文:
https://bbs.csdn.net/topics/398260808
https://zhuanlan.zhihu.com/p/182408931
https://zhuanlan.zhihu.com/p/57351961
https://zhuanlan.zhihu.com/p/111388877