LVI-SAM imageProjection代码解析
2024-04-23 10:00:07
对于imageProjection这部分，主要功能就是去畸变，即deskew有关内容，下面进行分析。
头文件
#include "utility.h" // 和featureExtraction相同的头文件，这里不解释了
#include "lvi_sam/cloud_info.h"和featureExtraction相同的头文件，这里不做过多的介绍，可以去lvi_sam/cloud_info.h里面看看作者团队定义了一些啥变量。
结构体
// Velodyne
// 这里的Velodyne是激光雷达的一种，下面的Ouster也是激光雷达的一种，不过Ouster更适合在无人驾驶的汽车上面使用
// 下面是定义的结构体
struct PointXYZIRT
{PCL_ADD_POINT4D // 表示欧几里得xyz坐标 和 强度值的点结构PCL_ADD_INTENSITY; // 激光点的反射强度，也可以存点的索引，里面是一个float类型的变量 看到有人说这里的强度可以存放深度信息uint16_t ring; // 扫描的激光线float time; // 时间EIGEN_MAKE_ALIGNED_OPERATOR_NEW // 这个地方不太明白，也没加分号。懂了 这个是eigen的字节对齐的东西
} EIGEN_ALIGN16;//注册为PCL点云格式
// 一个套路的写法，把上面的结构体里面的变量搬下来
POINT_CLOUD_REGISTER_POINT_STRUCT(PointXYZIRT,(float, x, x)(float, y, y)(float, z, z)(float, intensity, intensity)(uint16_t, ring, ring)(float, time, time))// Ouster
// struct PointXYZIRT {
// 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(PointXYZIRT,
// (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)
// )结构体里面，就是一些点云的信息，雷达扫描的线之类的内容，没啥好说的。还有那个EIGEN_MAKE_ALIGNED_OPERATOR_NEW这个经过网上的资料查找，发现是和字节对齐的有关内容，这个就是计算机存贮的时候，有的变量字节长，有的短，使用这个就可以使得字节对齐，这样可以提高空间利用率（或者说内存利用率）。
类
const int queueLength = 500; // 队列的长度class ImageProjection : public ParamServer
{
private:// imu队列 、 odom队列互斥锁std::mutex imuLock;std::mutex odoLock;// std::mutex imuLock;// std::mutex odoLock;// 订阅原始激光点云ros::Subscriber subLaserCloud; // 点云信息ros::Publisher pubLaserCloud; //// 发布当前帧校正后点云，有效点云ros::Publisher pubExtractedCloud; // 点云信息ros::Publisher pubLaserCloudInfo; // 点云信息// imu数据队列（原始数据，转lidar系下）ros::Subscriber subImu; // 输入imu信息std::deque<sensor_msgs::Imu> imuQueue; // deque双端队列// imu里程计队列ros::Subscriber subOdom; // 输入odometry信息std::deque<nav_msgs::Odometry> odomQueue;// 激光点云数据队列std::deque<sensor_msgs::PointCloud2> cloudQueue;// 队列front帧，作为当前处理帧点云sensor_msgs::PointCloud2 currentCloudMsg;// 当前激光帧起止时刻间对应的imu数据，计算相对于起始时刻的旋转增量，以及时间戳;// 用于插值计算当前激光帧起止时间范围内，每一时刻的旋转姿态double *imuTime = new double[queueLength]; // queueLength = 500 上面赋值过了double *imuRotX = new double[queueLength];double *imuRotY = new double[queueLength];double *imuRotZ = new double[queueLength];int imuPointerCur;bool firstPointFlag;Eigen::Affine3f transStartInverse; // Affine3f是仿射变换用到的库// 当前帧原始激光点云pcl::PointCloud<PointXYZIRT>::Ptr laserCloudIn;// 当前帧运动畸变矫正之后的激光点云pcl::PointCloud<PointType>::Ptr fullCloud;// 从fullcloud中提取的有效点pcl::PointCloud<PointType>::Ptr extractedCloud;int deskewFlag; // deskew 去歪斜cv::Mat rangeMat; // cv::Mat 表示矩阵bool odomDeskewFlag;// 当前激光帧起止时刻对应imu里程计位姿变换，该变换对应的平移增量；用于插值计算当前激光帧起止时间范围内，每一时刻的位置float odomIncreX;float odomIncreY;float odomIncreZ;// 当前帧激光点云运动畸变矫正之后的数据，包括点云数据、初始位姿、姿态角等，发布给featureExtraction进行特征提取lvi_sam::cloud_info cloudInfo; // 这里的cloudInfo 是lvi_sam::自己定义的// 当前帧起始时刻double timeScanCur;// 下一帧的开始时刻double timeScanNext;// 当前帧header 包含时间戳信息std_msgs::Header cloudHeader; // 存疑：这个header里面到底存储了啥上面的话就是定义了一些变量。其中最开始的queueLength是个常量，赋值为500。然后下面就是锁、点云信息、队列。imuRotX这个的话就是在下面的imudeskew里面的一个预测值，就是用上一时刻的旋转，加上时间差乘角速度，得出来的数值。接下来的话，就是一些和下面函数有关内容的变量。没啥强调的。现在对于header的理解，它包含了时间戳stamp信息，这个在某些函数里面来进行判断用的。还有就是imu的时间跨度需要大于雷达的时间跨度，这个得在下面的代码中进行体会。
函数
ImageProjection() : deskewFlag(0) // 冒号后面是赋值的意思{//订阅原始imu数据subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay()); // 这个transporthints是确定传输的方式，后面跟着tcp传输//订阅imu里程计，由imuPreintegration积分计算得到的没时刻imu位姿subOdom = nh.subscribe<nav_msgs::Odometry>(PROJECT_NAME + "/vins/odometry/imu_propagate_ros", 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>(PROJECT_NAME + "/lidar/deskew/cloud_deskewed", 5);//发布激光点云信息pubLaserCloudInfo = nh.advertise<lvi_sam::cloud_info>(PROJECT_NAME + "/lidar/deskew/cloud_info", 5);//初始化，分配内存allocateMemory();//重置参数resetParameters();// pcl日志级别，只打ERROR日志pcl::console::setVerbosityLevel(pcl::console::L_ERROR);}构造函数，这里每个变量后面 赋值的时候 括号里面的参数如果是函数的话 就会进行运行，然后变量赋值完，在初始化，在重置参数，看别的博客说，每次接受激光帧的时候都会reset一下参数。最后是一个打印错误日志的内容这个可以暂时不用管。
void allocateMemory(){laserCloudIn.reset(new pcl::PointCloud<PointXYZIRT>()); // 重新reset一个，后面括号里面的和前面的类型对应上就okfullCloud.reset(new pcl::PointCloud<PointType>());extractedCloud.reset(new pcl::PointCloud<PointType>());fullCloud->points.resize(N_SCAN * Horizon_SCAN); // 这里重新设置大小，就是行列的范围cloudInfo.startRingIndex.assign(N_SCAN, 0); // assign 根据函数的意思，这里是给括号里面的变量重新赋值 cloudInfo.endRingIndex.assign(N_SCAN, 0);cloudInfo.pointColInd.assign(N_SCAN * Horizon_SCAN, 0);cloudInfo.pointRange.assign(N_SCAN * Horizon_SCAN, 0); // 也是重新赋值，这里应该是pointRange的大小，而不是里面的值resetParameters(); // 重新设置参数 看到网上的解析说雷达每帧数据都需要进行重置参数}这个函数就是分配内存的，reset、resize、assign一下里面的变量或者类的实体啥的，就ok，这个没啥好讲的，这个地方最后进行了一次参数的重置。
//重置参数，接受每帧lidar数据都要重置这些参数void resetParameters(){laserCloudIn->clear(); // 先清空原有的内容extractedCloud->clear();// reset range matrix for range image projectionrangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX)); // 新建一个矩阵imuPointerCur = 0; // 应该是imu的指针 我的理解firstPointFlag = true; // 第一个点odomDeskewFlag = false; // 里程计去歪斜，感觉这个就是去畸变的意思// 一个for循环，把下面的内容全部归0，这里的queueLength的长度为500上面赋值过了，下面如果再出现不再提起for (int i = 0; i < queueLength; ++i){imuTime[i] = 0;imuRotX[i] = 0;imuRotY[i] = 0;imuRotZ[i] = 0;}}就是重置参数，里面的内容还原成原始的状态，又clear，又重新赋值的，这个地方没啥好说的。
// 析构函数 一个空的函数体~ImageProjection() {}一个非常简单的析构函数，但是我记得好像不写的话，这个运行的时候也会自动搞成默认的析构函数。
然后接下来就是handler有关的函数了，可以叫句柄，也可以叫回调函数，这个再考证。
// 回调函数 通过阅读网上的解析，这里的回调函数 有handler和callback两种。应该是的，// 不过handler我们可以使用 Handler 发送并处理与一个线程关联的 Message 和 Runnable 。// 简单地接受消息，然后放入imu消息的队列中void imuHandler(const sensor_msgs::Imu::ConstPtr &imuMsg){// imu原始测量数据转换到lidar系，加速度、角速度、RPY// imuConverter（）的实现在utilitysensor_msgs::Imu thisImu = imuConverter(*imuMsg); // 这里的converter只有旋转没有平移，将imu数据转到lidar系中，就是乘一个矩阵，做一个线性变换// 上锁，添加数据的时候队列不可用。执行完函数的时候自动解锁std::lock_guard<std::mutex> lock1(imuLock); // 这里加入数据的时候，先上锁，然后再pushback// 一个疑问：共享的数据都有哪些 猜测：应该是pushback后面的数据进行上锁，这样在pushback的时候不会被修改imuQueue.push_back(thisImu);}imu的handler函数，这个就是先把imu的测量数据转换到lidar系，然后再将数据push到imuQueue里面，至于这个锁，我现在的理解的话应该是锁注thisImu的内容，不让它在push的时候被修改了。
// 订阅imu里程计，由imuPreintegration积分计算得到的每时刻imu位姿void odometryHandler(const nav_msgs::Odometry::ConstPtr &odometryMsg){std::lock_guard<std::mutex> lock2(odoLock);// std::lock_guard<std::mutex> lock2(odoLock);odomQueue.push_back(*odometryMsg);}很简单的一个里程计handler，不解释了
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg){// 添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性// 这个就是在函数运行之前先对自己的数据内容进行检查！if (!cachePointCloud(laserCloudMsg))return;// 当前帧起止时刻对应的imu数据、imu里程计数据处理if (!deskewInfo())return;// 当前帧激光点云运动畸变校正// 1.检查激光点距离，扫描线是否合规// 2.激光运动畸变校正，保存激光点projectPointCloud();//提取有效激光点，存extractedCloudcloudExtraction();//发布当前帧校正后点云，有效点publishClouds();//重置参数，接受每帧lidar数据都要重置这些参数resetParameters();//回调函数执行的这一套流程，是imageProjection的关键// 1.首先是接受数据，计算时间戳，检查数据// 2.然后与里程计 和 imu数据进行时间戳同步，并处理// 3.接着检查并校准数据// 4.提取信息// 5.发布校准后的数据，以及提取到的信息// 6.重置参数，等待下一次回调函数执行}这个函数可以说一下，先接受数据，cachepointcloud就是缓存下来进行时间戳的检查、数据的检查、点云密度、扫描线等内容的检查，可以看做是鲁棒性的体现。然后在进行deskewinfo，这个就是我的理解的话，计算了imu的旋转和平移，和畸变的话没啥关系。就是处理imu和imu里程计数据的。然后进行激光点云畸变校正、点云提取，这里Extraction的啥内容呢，就是下面有个矩阵，（i，j）被赋值了range，这个range可以理解成距离，也可以理解成深度。然后这些被赋值的点云会被push到一个容器里面，然后发布出来。就是publish的内容，最后在重新reset一下参数就可以了。这部分的话还是可以的，这个函数的精髓，就是去畸变的。
接下来对上面提到的函数进行解析。
// 添加一帧激光点云到队列，取出最早一帧作为当前帧，计算起止时间戳，检查数据有效性// 先判断队列缓存中的数据，再检查点云的密度，看有没有NaN的点// 再检查ring channel ，再检查时间戳，这里的时间戳和deskew关系十分密切 最后结束！bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg){// cache point cloud 缓存点云cloudQueue.push_back(*laserCloudMsg);//队列缓存中数据过少if (cloudQueue.size() <= 2)return false;else{// 取出激光点云队列中最早的一帧 取出以后队首被弹出来了currentCloudMsg = cloudQueue.front();cloudQueue.pop_front();// 当前帧头部 说明头部存储了时间戳信息cloudHeader = currentCloudMsg.header;// 当前帧起始时刻timeScanCur = cloudHeader.stamp.toSec();// 下一帧的开始时刻timeScanNext = cloudQueue.front().header.stamp.toSec();}// convert cloud// 转换成pcl点云格式pcl::fromROSMsg(currentCloudMsg, *laserCloudIn);// check dense flag// 存在无效点，Nan 或者Infif (laserCloudIn->is_dense == false){ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");// 点云不是密集格式，请先去除NaN点！ 检查它的格式的这个可以不用过多深究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// 检查时间戳，以及是否存在timeif (deskewFlag == 0){deskewFlag = -1;for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i){if (currentCloudMsg.fields[i].name == timeField){deskewFlag = 1;break;}}if (deskewFlag == -1)ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!");// 点云时间戳不可用，去偏功能禁用，系统会明显漂移！ }return true;}至于检查一下啥内容都在上面了，不做过多解释。
// 用于处理激光帧起止时刻对应的imu数据，imu里程计数据// 1.用于处理IMU数据// 2.用于处理IMU里程计数据bool deskewInfo(){std::lock_guard<std::mutex> lock1(imuLock); // 处理数据之前先上锁，避免影响处理的数据std::lock_guard<std::mutex> lock2(odoLock);// make sure IMU data available for the scan// 确保IMU数据的时间戳包含了整个lidar数据的时间戳，否则就不处理了// 这里的判断还没看明白 第二遍好好看一下// imu不能为空，并且imu队首的时间要比timeScanCur要提前，队尾的时间要比timeScanNext要晚// 可以说，imu的时间跨度更大一些if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanNext){ROS_DEBUG("Waiting for IMU data ...");return false;}// 下面就是分别对imu 和 odom进行去歪斜// 当前帧对应imu数据处理// 注：imu数据都已经转换到lidar系下了imuDeskewInfo();// 当前帧对应imu里程计处理// 注：imu数据都已经转换到lidar系下了odomDeskewInfo();return true;}这个是处理数据的，这个函数里面最主要的就是imudeskewinfo（）和odomdeskewinfo（）两个内容，作者又对着两个内容下面进行了详细的书写。
// 遍历当前激光帧起止时刻之间的imu数据，初始时刻对应imu的姿态角RPY设为当前帧的初始姿态角// 然后用角速度、时间积分，计算每一时刻相对于初始时刻的旋转量，初始时刻旋转设为0.// 物体姿态计算，是由这个函数来完成的void imuDeskewInfo(){cloudInfo.imuAvailable = false; // 先把available变量设置为false// 删除imu队列中，当前lidar数据的时间戳的0.01s前的数据// 进行一个while循环，把早于timeScanCur0.01的数据进行pop掉。// 这里用了pop_front 而不是pop 这就是它的双端队列进行pop的操作，还是有区别的while (!imuQueue.empty()){if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)imuQueue.pop_front();elsebreak;}// 如果imu缓存队列中没有数据，直接返回// 这相当于一个鲁棒性的判断if (imuQueue.empty())return;// 处理的imu的帧数，或者说游标// 这个就是记录下面数据的// 这个变量就想当个指针，来存放当前imu帧处理到第几个了imuPointerCur = 0;// 遍历当前lidar数据帧起止时刻（前后拓展0.01s）之间的imu数据for (int i = 0; i < (int)imuQueue.size(); ++i){sensor_msgs::Imu thisImuMsg = imuQueue[i]; // 提取imu信息double currentImuTime = thisImuMsg.header.stamp.toSec(); // 提取时间，toSec这个函数很人性化，转换成s，统一计算// get roll, pitch, and yaw estimation for this scan// 提取imu姿态角RPY，作为当前lidar帧初始姿态角if (currentImuTime <= timeScanCur)// 这个imuRPY to rosRPY就是一个数据的转换// 判断条件：当前imu的时间戳，小于雷达的时间。scan表示了雷达的扫描imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);// 超过当前lidar数据的时间戳结束时刻0.01s，结束if (currentImuTime > timeScanNext + 0.01)break;// 第一帧imu旋转角初始化// 所以imuPointerCur就相当于一个指针来用if (imuPointerCur == 0){imuRotX[0] = 0;imuRotY[0] = 0;imuRotZ[0] = 0;imuTime[0] = currentImuTime;++imuPointerCur; // 指针加1continue; // 结束本次大的循环，然后++i}// 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; // 最后让available的值为true}这个就是在计算rotx、y、z的，然后其中一些判断，可以自己读读代码没什么难度的。
// 遍历当前激光帧起止时刻之间的imu里程计数据，初始时刻对应imu里程计设为当前帧的初始位姿// 并用起始、终止时刻对应imu里程计，计算相对位姿变换，保存平移增量void odomDeskewInfo(){cloudInfo.odomAvailable = false;// 删除odom队列中，当前时间戳的0.01s前的数据// 和imu一样的判断while (!odomQueue.empty()){if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01)odomQueue.pop_front();elsebreak;}// 如果删除后为空，直接返回if (odomQueue.empty())return;// if (odomQueue.empty())// return;// 必须包含当前lidar数据的时间戳之前的odom数据// 这里我的理解是 imu的时间跨度 必须比 雷达的要大if (odomQueue.front().header.stamp.toSec() > timeScanCur)return;// get start odometry at the beinning of the scan// 提取当前lidar数据的时间戳// 下面这个for循环是找到timeScanCur最近的那个odom的信息nav_msgs::Odometry startOdomMsg;for (int i = 0; i < (int)odomQueue.size(); ++i){startOdomMsg = odomQueue[i];if (ROS_TIME(&startOdomMsg) < timeScanCur)continue;elsebreak;}// 提取imu里程计姿态角tf::Quaternion orientation; // 四元数tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation); // 朝向 RPY 一个四元数的转换，这个startOdomMsg是上面for循环得到的double roll, pitch, yaw;tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw); // 获取RPY// Initial guess used in mapOptimization// 用当前激光帧起始时刻的odom，初始化lidar位姿，后面用于mapOptimizationcloudInfo.odomX = startOdomMsg.pose.pose.position.x;cloudInfo.odomY = startOdomMsg.pose.pose.position.y;cloudInfo.odomZ = startOdomMsg.pose.pose.position.z;cloudInfo.odomRoll = roll;cloudInfo.odomPitch = pitch;cloudInfo.odomYaw = yaw;cloudInfo.odomResetId = (int)round(startOdomMsg.pose.covariance[0]);cloudInfo.odomAvailable = true;// get end odometry at the end of the scan// 获取最后的odomodomDeskewFlag = false;// 如果当前激光帧结束时刻之后没有odom数据，直接返回if (odomQueue.back().header.stamp.toSec() < timeScanNext)return;// 提取当前激光帧结束时刻的odomnav_msgs::Odometry endOdomMsg;for (int i = 0; i < (int)odomQueue.size(); ++i){endOdomMsg = odomQueue[i];if (ROS_TIME(&endOdomMsg) < timeScanNext)continue;elsebreak;}// 如果起止时刻对应odom的协方差不等，可能是里程计出现了问题，它们两个的相关性应该是一致的，所以直接返回// 这里存疑：covariance[0]表示的是啥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); // 先转换成四元数，然后得到RPYEigen::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;}这个是求odom的信息的，里面的话就是求了最初的，然后求了现在的，根据矩阵乘积，得到一个转换矩阵，这里的话在xyz的基础上加了RPY。这里的话，需要注意的是通过四元数来求得RPY，然后这个地方的话，可以理解是做了一个预测，因为这里计算的值会在下面的部分用到。
// 检查雷达数据并校正void projectPointCloud(){int cloudSize = (int)laserCloudIn->points.size(); // 获取激光点云的大小// range image projection// 遍历当前帧激光点云for (int i = 0; i < cloudSize; ++i){// pcl格式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;// 扫描线检查// 这个ring 存放的应该是扫描的条数int rowIdn = laserCloudIn->points[i].ring;if (rowIdn < 0 || rowIdn >= N_SCAN)continue;// 扫描线如果有降采样，则跳过// 看了一下 downsampleRate取值为1// 不太明白这里，感觉谁除以1都余0耶if (rowIdn % downsampleRate != 0)continue;float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; // 获取水平的角度范围// 水平扫描角度步长，例如一周扫描1800次，则两次扫描间隔角度0.2°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;// 计算到lidar的距离，具体实现在utility.hfloat range = pointDistance(thisPoint);// 所以这个range可以表示深度，也可以表示距离// float pointDistance(PointType p)// {// return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);// }// 在utility.h中找到的，就是一般的求距离的公式，平方和之后开根号// 如果距离小于一个阈值，则跳过该点，比如说扫到手持设备的人if (range < 1.0)continue;// 已经存过该点，不再处理if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)continue;// for the amsterdam dataset// 这个针对阿姆斯特丹数据集，有特别的判断条件，这个数据集没看到// if (range < 6.0 && rowIdn <= 7 && (columnIdn >= 1600 || columnIdn <= 200))// continue;// if (thisPoint.z < -2.0)// continue;// 矩阵存激光点的距离rangeMat.at<float>(rowIdn, columnIdn) = range;// 激光运动畸变校正// 利用当前帧起止时刻之间的imu数据计算旋转增量，imu里程计数据计算平移增量，// 进而将每一时刻激光点位置变换到第一个激光点坐标系下，进行运动补偿thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time); // Velodyne// thisPoint = deskewPoint(&thisPoint, (float)laserCloudIn->points[i].t / 1000000000.0); // Ouster// 这个地方出现了Velodyne 和 Ouster 说明lvi-sam还可以用来做Ouster雷达的 这个通过资料可知，这个雷达最常用在无人驾驶上面// 转换成一维索引，存校正之后的激光点// 这里可以看出index 是按照行号进行排的int index = columnIdn + rowIdn * Horizon_SCAN;fullCloud->points[index] = thisPoint;}}这个部分就是本代码里最重要的部分了，操作就是一开始经过一系列的判断，然后最后用deskewPoint函数更新thispoint的值，还有rangeMat.at<float>(rowIdn, columnIdn) = range;这个操作就是下面extract时候的条件，如果有值就push，没有值为FLT_MAX的时候就不会push进来。
// 用当前帧起止时刻之间的imu数据计算旋转增量，imu里程计数据计算平移增量// 进而将每一时刻激光点位置变换到第一个激光点坐标系下，进行运动补偿。PointType deskewPoint(PointType *point, double relTime){// 检查是否有时间戳信息，和是否有合规的imu数据if (deskewFlag == -1 || cloudInfo.imuAvailable == false)return *point;// relTime是当前激光点相对于激光帧起始时刻的时间，pointTime则是当前激光点的时间戳double pointTime = timeScanCur + relTime;// 在当前激光帧起止时间范围内，计算某一时刻的旋转（相对于起始时刻的旋转增量）float rotXCur, rotYCur, rotZCur;findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);// 在当前激光帧起止时间范围内，计算某一时刻的平移（相对于起始时刻的平移增量）float posXCur, posYCur, posZCur;findPosition(relTime, &posXCur, &posYCur, &posZCur);// 第一个点的位姿增量（theta），求逆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;// 在上面的代码片段中，实现了激光运动畸变校正的功能，通俗来讲，就是将在运动过程中的获取到// 基于运动时的位置的点图，转变成在最初始的位置的点图，其中需要用到imu信息进行转变。而用来// 计算这些转变时条用了Eigen和pcl库进行计算。// 不过作者也定义了两个简单的函数去计算旋转增量 和 平移增量 findRotation() 和 findPositon()}去畸变的操作，就是通过一系列判断，然后计算得到一个现在和一开始点的变换矩阵，然后用矩阵相乘，得到新的点的数值。按照别人博客的说法，就是将运动过程中获取到的基于运动时的位置的点图转变成最初始的位置的点图，就是把所有的点，以第一个点为基准，来放到同一个地方。
这里的话又多了findRotation（）和findPosition（）两个函数，下面进行解析。
// 在当前激光帧起止时间范围内，计算某一时刻的旋转（相对于起始时刻的旋转增量）void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur){*rotXCur = 0;*rotYCur = 0;*rotZCur = 0;// 查找当前时刻在imuTime下的索引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;// 3.23修改 下一内容 注释-->非注释if (cloudInfo.odomAvailable == false || odomDeskewFlag == false)return;float ratio = relTime / (timeScanNext - timeScanCur);*posXCur = ratio * odomIncreX;*posYCur = ratio * odomIncreY;*posZCur = ratio * odomIncreZ;}
就是一系列数值的计算，这里判断了imu时间和雷达扫描时间，如果大了咋样，小了咋样。这个的话可以具体到函数里面看一下。
// 提取有效激光点，存extractedCloud// 这里的有效点，初步理解 rangeMat.at<float>(i,j)被上面的函数中赋值的点称为有效点// 并且这里记录了雷达每根扫描线的第5个点 和 倒数第5个点 （有种算曲率的意思）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// 标记点的列索引，以便以后标记能对应上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;}}
这个就是提取，在上面已经介绍过了，就是那个矩阵=range的时候会被push进来。
而且这句：j + i * Horizon_SCAN，我自己理解，这个是点云的一维存储方式，按行来存储，就是一行一行来的。前面那篇文章的问题中的其中一个，想着应该是解决了。
void publishClouds(){cloudInfo.header = cloudHeader; // 现在知道header里面有时间戳cloudInfo.cloud_deskewed = publishCloud(&pubExtractedCloud, extractedCloud, cloudHeader.stamp, "base_link");pubLaserCloudInfo.publish(cloudInfo);}publish，这个没啥好说的，一个技巧吧。然后header里面有stamp，这个暂时就是知道这么多。
int main(int argc, char **argv)
{ros::init(argc, argv, "lidar");ImageProjection IP;ROS_INFO("\033[1;32m----> Lidar Cloud Deskew Started.\033[0m");// 激光雷达云校正开始 ros::MultiThreadedSpinner spinner(3); // 多线程spinner.spin(); // 这个地方就是来调用回调函数的return 0;
}最后主函数，先rosinit，再类的实体，就一些列handler里面的内容，计算啥的，最后这个函数的效果就是去畸变。可以搜一搜雷达运动畸变的内容，就是说构造的点云和实际的点云有些偏差，这个是由于雷达运动引起的。最简单的办法可以通过lcp方法来最小化点点之间的距离，找到最小值的那个，把点云恢复到实际位置上。然后多线程的spin，这个就是接受回调函数的，这个地方上篇文章写过。
到此，这个文件结束，雷达部分还有两个，争取下周分析完成。
问题
1、共享的值都有哪些，哪些值在push的时候需要lock
2、继续对代码理解，多读多写
float range = pointDistance(thisPoint);这个地方，range是距离的意思，这个range现在理解成距离吧！
LVI-SAM imageProjection代码解析相关推荐
- matrix_multiply代码解析
matrix_multiply代码解析 关于matrix_multiply 程序执行代码里两个矩阵的乘法,并将相乘结果打印在屏幕上. 示例的主要目的是展现怎么实现一个自定义CPU计算任务. 参考:ht ...
- CornerNet代码解析——损失函数
CornerNet代码解析--损失函数 文章目录 CornerNet代码解析--损失函数 前言 总体损失 1.Heatmap的损失 2.Embedding的损失 3.Offset的损失 前言 今天要解 ...
- 视觉SLAM开源算法ORB-SLAM3 原理与代码解析
来源:深蓝学院,文稿整理者:何常鑫,审核&修改:刘国庆 本文总结于上交感知与导航研究所科研助理--刘国庆关于[视觉SLAM开源算法ORB-SLAM3 原理与代码解析]的公开课. ORB-SLA ...
- java获取object属性值_java反射获取一个object属性值代码解析
有些时候你明明知道这个object里面是什么,但是因为种种原因,你不能将它转化成一个对象,只是想单纯地提取出这个object里的一些东西,这个时候就需要用反射了. 假如你这个类是这样的: privat ...
- python中的doc_基于Python获取docx/doc文件内容代码解析
这篇文章主要介绍了基于Python获取docx/doc文件内容代码解析,文中通过示例代码介绍的非常详细,对大家的学习或者工作具有一定的参考学习价值,需要的朋友可以参考下 整体思路: 下载文件并修改后缀 ...
- mongoose框架示例代码解析（一）
mongoose框架示例代码解析(一) 参考: Mongoose Networking Library Documentation(Server) Mongoose Networking Librar ...
- ViBe算法原理和代码解析
ViBe - a powerful technique for background detection and subtraction in video sequences 算法官网:http:// ...
- 【Android 逆向】使用 Python 代码解析 ELF 文件 ( PyCharm 中进行断点调试 | ELFFile 实例对象分析 )
文章目录 一.PyCharm 中进行断点调试 二.ELFFile 实例对象分析 一.PyCharm 中进行断点调试 在上一篇博客 [Android 逆向]使用 Python 代码解析 ELF 文件 ( ...
- 密码算法中iv值是什么_?标检测中的?极?值抑制算法(nms)：python代码解析
⾮极⼤值抑制(Non-Maximum Suppression)原理 ⾮极⼤值抑制,顾名思义,找出极⼤值,抑制⾮极⼤值.这种思路和算法在各个领域中应⽤⼴泛,⽐如边缘检测算法canny算⼦中就使⽤了该⽅法 ...
最新文章
- ATS中的RAM缓存简介
- 8 9区别 endnote7_SAT和ACT的区别，你知道吗？
- pytorch生成一个数组
- 1365. 有多少小于当前数字的数字（哈希表）
- 像@Transactional一样利用注解自定义aop切片
- 将VMware Workstation 12 Pro的虚拟网卡修改为自己希望的IP网段
- PythonGIS可视化—Matplot basemap工具箱
- 腾讯的敌人只有傲慢的自己
- 主流视频客户端核心代码的实现
- C 编译器、链接器、加载器详解
- 为Windows添加日志事件
- Tomcat 连接池的配置
- 红帽linux中文系统下载iso,红帽子9.0版下载-redhat linux 9.0 iso下载 简体中文正式版-IT猫扑网...
- FAT32文件系统的数据结构
- zabbix开启网页报警声音方法：网页也可以有报警声音（46）
- 世界杯花样营销：争夺32亿人眼球中看到三大趋势
- 安装Cloudera Manager-5.12.2 集成 CHD-5.12.2 问题总结
- 下载苹果官网视频的方式
- 支持DDR5，超频更简单，小雕够给力，技嘉B760M小雕WIFI主板上手
- 水果生鲜类抖音号被大量封号