多传感器融合SLAM --- 12.LIO-SAM图像投影代码分析 imageProjection.cpp
发布时间
阅读量：
阅读量
目录
0 流程图
1 代码解析
1.1 构造函数解析
1.2 IMU回调函数解析
1.3 后端里程计回调函数
1.4 原始点云回调函数
1.4.1 缓存点云信息函数cachePointCloud
1.4.2 获取运动补偿所需的信息 deskewInfo
1.4.3 把点云投影到一个矩阵上 保存每个点的信息 projectPointCloud
1.4.4 剔除点的信息 cloudExtraction
1.4.5 发布点云信息 publishClouds
1.4.6 系统置位 resetParameters
0 流程图
1 代码解析
1.1 构造函数解析
> 1. ImageProjection():
>
> 2. deskewFlag(0)
>
> 3. {
>
> 4. // 订阅IMU数据、后端里程计数据、原始点云数据
>
> 5. subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 2000, &ImageProjection::imuHandler, this, ros::TransportHints().tcpNoDelay());
>
> 6. subOdom = nh.subscribe<nav_msgs::Odometry>(odomTopic+"_incremental", 2000, &ImageProjection::odometryHandler, this, ros::TransportHints().tcpNoDelay());
>
> 7. subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 5, &ImageProjection::cloudHandler, this, ros::TransportHints().tcpNoDelay());
>
> 8.
>
> 9. // 发布运动补偿后的点云以及点云信息
>
> 10. pubExtractedCloud = nh.advertise<sensor_msgs::PointCloud2> ("lio_sam/deskew/cloud_deskewed", 1);
>
> 11. pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/deskew/cloud_info", 1);
>
> 12.
>
> 13. allocateMemory();
>
> 14. resetParameters();
>
> 15.
>
> 16. pcl::console::setVerbosityLevel(pcl::console::L_ERROR);
>
> 17. }
>
>
>
>
> 代码解读在该函数体内，代码主要订阅了三个关键信息源：来自惯性测量单元的原始数据、来自后端的里程计读数以及原始三维点云数据。
> 1. IN utility.h
>
> 2. nh.param<std::string>("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");
>
> 3. nh.param<std::string>("lio_sam/imuTopic", imuTopic, "imu_correct");
>
>
>
>
> 代码解读
发布了两则相关信息：针对单帧点云中的各点进行补偿处理后生成的数据内容以及完整的原始数据。
> 1. pubExtractedCloud = nh.advertise<sensor_msgs::PointCloud2> ("lio_sam/deskew/cloud_deskewed", 1);
>
> 2. pubLaserCloudInfo = nh.advertise<lio_sam::cloud_info> ("lio_sam/deskew/cloud_info", 1);
>
>
>
>
> 代码解读1.2 IMU回调函数解析
> 1. // IMU订阅信息回调函数
>
> 2. void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
>
> 3. {
>
> 4. // 将IMU坐标系转换为前左上坐标系
>
> 5. sensor_msgs::Imu thisImu = imuConverter(*imuMsg);
>
> 6.
>
> 7. std::lock_guard<std::mutex> lock1(imuLock);
>
> 8. imuQueue.push_back(thisImu);
>
> 9.
>
> 10. // debug IMU data
>
> 11. // cout << std::setprecision(6);
>
> 12. // cout << "IMU acc: " << endl;
>
> 13. // cout << "x: " << thisImu.linear_acceleration.x <<
>
> 14. // ", y: " << thisImu.linear_acceleration.y <<
>
> 15. // ", z: " << thisImu.linear_acceleration.z << endl;
>
> 16. // cout << "IMU gyro: " << endl;
>
> 17. // cout << "x: " << thisImu.angular_velocity.x <<
>
> 18. // ", y: " << thisImu.angular_velocity.y <<
>
> 19. // ", z: " << thisImu.angular_velocity.z << endl;
>
> 20. // double imuRoll, imuPitch, imuYaw;
>
> 21. // tf::Quaternion orientation;
>
> 22. // tf::quaternionMsgToTF(thisImu.orientation, orientation);
>
> 23. // tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
>
> 24. // cout << "IMU roll pitch yaw: " << endl;
>
> 25. // cout << "roll: " << imuRoll << ", pitch: " << imuPitch << ", yaw: " << imuYaw << endl << endl;
>
> 26. }
>
>
>
>
> 代码解读这里很简单的将IMU信息放入了队列imuQueue中。
1.3 后端里程计回调函数
> 1. void odometryHandler(const nav_msgs::Odometry::ConstPtr& odometryMsg)
>
> 2. {
>
> 3. std::lock_guard<std::mutex> lock2(odoLock);
>
> 4. odomQueue.push_back(*odometryMsg);
>
> 5. }
>
>
>
>
> 代码解读这里很简单的将IMU信息放入了队列odomQueue中。
1.4 原始点云回调函数
输入的是驱动所发出来的激光雷达的点云消息，主要分为如下几步：
> 1. 点云回调函数 输入的是驱动所发出来的激光雷达的点云消息
>
> 2. void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
>
> 3. {
>
> 4. 把点云消息缓存下
>
> 5. if (!cachePointCloud(laserCloudMsg))
>
> 6. return;
>
> 7.
>
> 8. 获取运动补偿所需的信息
>
> 9. if (!deskewInfo())
>
> 10. return;
>
> 11.
>
> 12. 把点云投影到一个矩阵上 保存每个点的信息
>
> 13. projectPointCloud();
>
> 14.
>
> 15. 剔除有效点的信息
>
> 16. cloudExtraction();
>
> 17.
>
> 18. 发布点云信息
>
> 19. publishClouds();
>
> 20.
>
> 21. 系统复位
>
> 22. resetParameters();
>
> 23. }
>
>
>
>
> 代码解读
我们逐一详解各个模块。
1.4.1 缓存点云信息函数cachePointCloud
> 1. bool cachePointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
>
> 2. {
>
> 3. // cache point cloud
>
> 4. // 把点云放到队列cloudQueue中，要保证其中有两帧以上的点云
>
> 5. cloudQueue.push_back(*laserCloudMsg);
>
> 6. if (cloudQueue.size() <= 2)
>
> 7. return false;
>
> 8.
>
> 9. // 拿出最前面的点云
>
> 10. currentCloudMsg = std::move(cloudQueue.front());
>
> 11. cloudQueue.pop_front();
>
> 12.
>
> 13.
>
> 14. if (sensor == SensorType::VELODYNE || sensor == SensorType::LIVOX)
>
> 15. {
>
> 16. pcl::moveFromROSMsg(currentCloudMsg, *laserCloudIn);
>
> 17. }
>
> 18. else if (sensor == SensorType::OUSTER)
>
> 19. {
>
> 20. // Convert to Velodyne format
>
> 21. pcl::moveFromROSMsg(currentCloudMsg, *tmpOusterCloudIn);
>
> 22. laserCloudIn->points.resize(tmpOusterCloudIn->size());
>
> 23. laserCloudIn->is_dense = tmpOusterCloudIn->is_dense;
>
> 24. for (size_t i = 0; i < tmpOusterCloudIn->size(); i++)
>
> 25. {
>
> 26. auto &src = tmpOusterCloudIn->points[i];
>
> 27. auto &dst = laserCloudIn->points[i];
>
> 28. dst.x = src.x;
>
> 29. dst.y = src.y;
>
> 30. dst.z = src.z;
>
> 31. dst.intensity = src.intensity;
>
> 32. dst.ring = src.ring;
>
> 33. dst.time = src.t * 1e-9f;
>
> 34. }
>
> 35. }
>
> 36. else
>
> 37. {
>
> 38. ROS_ERROR_STREAM("Unknown sensor type: " << int(sensor));
>
> 39. ros::shutdown();
>
> 40. }
>
> 41.
>
> 42. // get timestamp
>
> 43. // 计算点云相关信息 存储最早的点云时间
>
> 44. cloudHeader = currentCloudMsg.header;
>
> 45. timeScanCur = cloudHeader.stamp.toSec();
>
> 46. timeScanEnd = timeScanCur + laserCloudIn->points.back().time;
>
> 47.
>
> 48. // check dense flag
>
> 49. // 点云是否是有序的
>
> 50. if (laserCloudIn->is_dense == false)
>
> 51. {
>
> 52. ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
>
> 53. ros::shutdown();
>
> 54. }
>
> 55.
>
> 56. // check ring channel
>
> 57. // 查看驱动里是否把每个点属于哪一根扫描SCAN这个消息
>
> 58. static int ringFlag = 0;
>
> 59. if (ringFlag == 0)
>
> 60. {
>
> 61. ringFlag = -1;
>
> 62. for (int i = 0; i < (int)currentCloudMsg.fields.size(); ++i)
>
> 63. {
>
> 64. if (currentCloudMsg.fields[i].name == "ring")
>
> 65. {
>
> 66. ringFlag = 1;
>
> 67. break;
>
> 68. }
>
> 69. }
>
> 70. if (ringFlag == -1)
>
> 71. {
>
> 72. // 如果没有这个信息就需要像LOAM LEGO——LOAM一样手动计算scan-id
>
> 73. ROS_ERROR("Point cloud ring channel not available, please configure your point cloud data!");
>
> 74. ros::shutdown();
>
> 75. }
>
> 76. }
>
> 77.
>
> 78. // 检查是否有时间戳信息
>
> 79. // check point time
>
> 80. if (deskewFlag == 0)
>
> 81. {
>
> 82. deskewFlag = -1;
>
> 83. for (auto &field : currentCloudMsg.fields)
>
> 84. {
>
> 85. if (field.name == "time" || field.name == "t")
>
> 86. {
>
> 87. deskewFlag = 1;
>
> 88. break;
>
> 89. }
>
> 90. }
>
> 91. if (deskewFlag == -1)
>
> 92. ROS_WARN("Point cloud timestamp not available, deskew function disabled, system will drift significantly!");
>
> 93. }
>
> 94.
>
> 95. return true;
>
> 96. }
>
>
>
>
> 代码解读这里主要是对点云进行了一个简单的拆包：
把点云放到队列cloudQueue 中，要保证其中有两帧以上的点云。取出最前面的点云currentCloudMsg转换成PCL格式存储在laserCloudIn 中，计算点云的信息cloudHeader ，将点云中点的的最先时间存入timeScanCur 中，点云中点的最后的时间存入timeScanEnd 中。
查询驱动中是否有SCAN信息：currentCloudMsg.fields[i].name == "ring"
查询驱动中是否有时间戳信息：currentCloudMsg.fields[i].name == "time"
如果没有直接报错。这一部分的流程图如下：
1.4.2 获取运动补偿所需的信息 deskewInfo
这里主要是获取原始IMU补偿信息 和后端ODOM补偿信息 。
> 1. bool deskewInfo()
>
> 2. {
>
> 3. std::lock_guard<std::mutex> lock1(imuLock);
>
> 4. std::lock_guard<std::mutex> lock2(odoLock);
>
> 5.
>
> 6. // make sure IMU data available for the scan
>
> 7. // 确保IMU的数据覆盖这一帧的点云
>
> 8. if (imuQueue.empty() || imuQueue.front().header.stamp.toSec() > timeScanCur || imuQueue.back().header.stamp.toSec() < timeScanEnd)
>
> 9. {
>
> 10. ROS_DEBUG("Waiting for IMU data ...");
>
> 11. return false;
>
> 12. }
>
> 13.
>
> 14. // 准备IMU补偿信息
>
> 15.
>
> 16. // IMU
>
> 17. imuDeskewInfo();
>
> 18.
>
> 19. // ODOM
>
> 20. odomDeskewInfo();
>
> 21.
>
> 22. return true;
>
> 23. }
>
>
>
>
> 代码解读对于原始IMU补偿信息来说：在初始化阶段将cloudInfo.imuAvailable标记为不可用并清理掉较早采集的IMU数据；通过变量imuPointerCur来管理可参与补偿的数据（初始值设为零）。当接收到第一份点云数据时（包含初始IMU信息），将imuRotX[0]、imuRotY[0]、imuRotZ[0]置零后立即增加imuPointerCur指针；而对于后续数据，则计算累积角速度：将当前时刻的imuRotX值更新为前一时刻值加上angular_x乘以时间差值Δt，并相应地增加imuPointerCur指针；最后将cloudInfo.imuAvailable设为可用状态。
注意：此处提到的imuRot表示累计积累的IMU信息序列；即通过将初始点云位姿与第i时刻对应的**imuRot[i]**相乘（即应用第i时刻至当前的时间段内的累计旋转），即可获得当前时刻的整体位姿。
> 1. void imuDeskewInfo()
>
> 2. {
>
> 3. cloudInfo.imuAvailable = false;
>
> 4.
>
> 5. while (!imuQueue.empty())
>
> 6. {
>
> 7. // 扔掉过早的IMU信息
>
> 8. if (imuQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
>
> 9. imuQueue.pop_front();
>
> 10. else
>
> 11. break;
>
> 12. }
>
> 13.
>
> 14. if (imuQueue.empty())
>
> 15. return;
>
> 16.
>
> 17. imuPointerCur = 0;
>
> 18.
>
> 19. // 9轴IMU，除了加速度计和陀螺仪之外还有姿态的信息
>
> 20. for (int i = 0; i < (int)imuQueue.size(); ++i)
>
> 21. {
>
> 22. sensor_msgs::Imu thisImuMsg = imuQueue[i];
>
> 23. double currentImuTime = thisImuMsg.header.stamp.toSec();
>
> 24.
>
> 25. // get roll, pitch, and yaw estimation for this scan
>
> 26. // 当前IMU时间戳 < 帧的时间戳
>
> 27. if (currentImuTime <= timeScanCur)
>
> 28. imuRPY2rosRPY(&thisImuMsg, &cloudInfo.imuRollInit, &cloudInfo.imuPitchInit, &cloudInfo.imuYawInit);
>
> 29.
>
> 30. if (currentImuTime > timeScanEnd + 0.01)
>
> 31. break;
>
> 32.
>
> 33. // 起始帧
>
> 34. if (imuPointerCur == 0){
>
> 35. imuRotX[0] = 0;
>
> 36. imuRotY[0] = 0;
>
> 37. imuRotZ[0] = 0;
>
> 38. // 时间戳
>
> 39. imuTime[0] = currentImuTime;
>
> 40. ++imuPointerCur;
>
> 41. continue;
>
> 42. }
>
> 43.
>
> 44. // get angular velocity
>
> 45. double angular_x, angular_y, angular_z;
>
> 46. // 取出IMU的角速度
>
> 47. imuAngular2rosAngular(&thisImuMsg, &angular_x, &angular_y, &angular_z);
>
> 48.
>
> 49. // 当前IMU和上一帧IMU的 delta t
>
> 50. double timeDiff = currentImuTime - imuTime[imuPointerCur-1];
>
> 51. // 计算积分
>
> 52. imuRotX[imuPointerCur] = imuRotX[imuPointerCur-1] + angular_x * timeDiff;
>
> 53. imuRotY[imuPointerCur] = imuRotY[imuPointerCur-1] + angular_y * timeDiff;
>
> 54. imuRotZ[imuPointerCur] = imuRotZ[imuPointerCur-1] + angular_z * timeDiff;
>
> 55. imuTime[imuPointerCur] = currentImuTime;
>
> 56. ++imuPointerCur;
>
> 57. }
>
> 58.
>
> 59. --imuPointerCur;
>
> 60.
>
> 61. // 没有办法对IMU进行运动补偿
>
> 62. if (imuPointerCur <= 0)
>
> 63. return;
>
> 64.
>
> 65. // 可以使用IMU进行运动补偿
>
> 66. cloudInfo.imuAvailable = true;
>
> 67. }
>
>
>
>
> 代码解读IMU补偿的流程图如下：
> 1. void odomDeskewInfo()
>
> 2. {
>
> 3. cloudInfo.odomAvailable = false;
>
> 4.
>
> 5. // 扔掉过早的数据
>
> 6. while (!odomQueue.empty())
>
> 7. {
>
> 8. if (odomQueue.front().header.stamp.toSec() < timeScanCur - 0.01)
>
> 9. odomQueue.pop_front();
>
> 10. else
>
> 11. break;
>
> 12. }
>
> 13.
>
> 14. if (odomQueue.empty())
>
> 15. return;
>
> 16.
>
> 17. // 点云时间 *************
>
> 18. // 里程计时间 ********
>
> 19. if (odomQueue.front().header.stamp.toSec() > timeScanCur)
>
> 20. return;
>
> 21.
>
> 22. // get start odometry at the beinning of the scan
>
> 23. nav_msgs::Odometry startOdomMsg;
>
> 24.
>
> 25. // 找到对应的最早的点云时间的odom数据
>
> 26. for (int i = 0; i < (int)odomQueue.size(); ++i)
>
> 27. {
>
> 28. startOdomMsg = odomQueue[i];
>
> 29.
>
> 30. if (ROS_TIME(&startOdomMsg) < timeScanCur)
>
> 31. continue;
>
> 32. else
>
> 33. break;
>
> 34. }
>
> 35.
>
> 36. // 将ROS信息格式中的姿态转换为tf的格式
>
> 37. tf::Quaternion orientation;
>
> 38. tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);
>
> 39.
>
> 40. double roll, pitch, yaw;
>
> 41. // 将四元数转换成欧拉角
>
> 42. tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
>
> 43.
>
> 44. // Initial guess used in mapOptimization
>
> 45. // 记录点云起始时刻对应的odom姿态
>
> 46. cloudInfo.initialGuessX = startOdomMsg.pose.pose.position.x;
>
> 47. cloudInfo.initialGuessY = startOdomMsg.pose.pose.position.y;
>
> 48. cloudInfo.initialGuessZ = startOdomMsg.pose.pose.position.z;
>
> 49. cloudInfo.initialGuessRoll = roll;
>
> 50. cloudInfo.initialGuessPitch = pitch;
>
> 51. cloudInfo.initialGuessYaw = yaw;
>
> 52.
>
> 53. // odom提供了这一帧点云的初始位姿
>
> 54. cloudInfo.odomAvailable = true;
>
> 55.
>
> 56. // get end odometry at the end of the scan
>
> 57. odomDeskewFlag = false;
>
> 58.
>
> 59. // 这里发现没有覆盖到最后的点云 那就不能用odom数据来做运动补偿
>
> 60. if (odomQueue.back().header.stamp.toSec() < timeScanEnd)
>
> 61. return;
>
> 62.
>
> 63. nav_msgs::Odometry endOdomMsg;
>
> 64.
>
> 65. // 找到点云最晚时间对应的odom数据
>
> 66. for (int i = 0; i < (int)odomQueue.size(); ++i)
>
> 67. {
>
> 68. endOdomMsg = odomQueue[i];
>
> 69.
>
> 70. if (ROS_TIME(&endOdomMsg) < timeScanEnd)
>
> 71. continue;
>
> 72. else
>
> 73. break;
>
> 74. }
>
> 75.
>
> 76. // 代表这个odom退化了 置信度不高了
>
> 77. if (int(round(startOdomMsg.pose.covariance[0])) != int(round(endOdomMsg.pose.covariance[0])))
>
> 78. return;
>
> 79.
>
> 80. // 起始位姿和结束位姿都转成Affine3f这个数据结构
>
> 81. Eigen::Affine3f transBegin = pcl::getTransformation(startOdomMsg.pose.pose.position.x, startOdomMsg.pose.pose.position.y, startOdomMsg.pose.pose.position.z, roll, pitch, yaw);
>
> 82.
>
> 83. tf::quaternionMsgToTF(endOdomMsg.pose.pose.orientation, orientation);
>
> 84. tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);
>
> 85. Eigen::Affine3f transEnd = pcl::getTransformation(endOdomMsg.pose.pose.position.x, endOdomMsg.pose.pose.position.y, endOdomMsg.pose.pose.position.z, roll, pitch, yaw);
>
> 86.
>
> 87. // 计算起始位姿和结束位姿之间的delta pose
>
> 88. Eigen::Affine3f transBt = transBegin.inverse() * transEnd;
>
> 89.
>
> 90. float rollIncre, pitchIncre, yawIncre;
>
> 91. // 将这个增量转成xyz和rpy的格式
>
> 92. pcl::getTranslationAndEulerAngles(transBt, odomIncreX, odomIncreY, odomIncreZ, rollIncre, pitchIncre, yawIncre);
>
> 93.
>
> 94. // 表示可以用odom做运动补偿
>
> 95. odomDeskewFlag = true;
>
> 96. }
>
>
>
>
> 代码解读流程图略
1.4.3 把点云投影到一个矩阵上 保存每个点的信息 projectPointCloud
> 1. void projectPointCloud()
>
> 2. {
>
> 3. int cloudSize = laserCloudIn->points.size();
>
> 4. // 遍历整个点云
>
> 5. for (int i = 0; i < cloudSize; ++i)
>
> 6. {
>
> 7. // 取出原始点云对应的某个点
>
> 8. PointType thisPoint;
>
> 9. thisPoint.x = laserCloudIn->points[i].x;
>
> 10. thisPoint.y = laserCloudIn->points[i].y;
>
> 11. thisPoint.z = laserCloudIn->points[i].z;
>
> 12. thisPoint.intensity = laserCloudIn->points[i].intensity;
>
> 13.
>
> 14. // 计算这个点距离LIDAR中心的距离
>
> 15. float range = pointDistance(thisPoint);
>
> 16.
>
> 17. // 距离太小或太大被认为是异常点
>
> 18. if (range < lidarMinRange || range > lidarMaxRange)
>
> 19. continue;
>
> 20.
>
> 21. // 取出对应点在第几根线上
>
> 22. int rowIdn = laserCloudIn->points[i].ring;
>
> 23. if (rowIdn < 0 || rowIdn >= N_SCAN)
>
> 24. continue;
>
> 25.
>
> 26. // 如果需要降采样 就根据scan_id适当跳过
>
> 27. if (rowIdn % downsampleRate != 0)
>
> 28. continue;
>
> 29.
>
> 30. int columnIdn = -1;
>
> 31. if (sensor == SensorType::VELODYNE || sensor == SensorType::OUSTER)
>
> 32. {
>
> 33. // 计算水平角
>
> 34. float horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
>
> 35. static float ang_res_x = 360.0/float(Horizon_SCAN);
>
> 36. // 计算水平线束ID 转换到x负方向e为起始 顺时针为正方向 范围[0,H]
>
> 37. columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
>
> 38. if (columnIdn >= Horizon_SCAN)
>
> 39. columnIdn -= Horizon_SCAN;
>
> 40. }
>
> 41. else if (sensor == SensorType::LIVOX)
>
> 42. {
>
> 43. columnIdn = columnIdnCountVec[rowIdn];
>
> 44. columnIdnCountVec[rowIdn] += 1;
>
> 45. }
>
> 46.
>
> 47. // 对水平ID进行检查
>
> 48. if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
>
> 49. continue;
>
> 50.
>
> 51. // 如果这个位置有填充了跳过
>
> 52. if (rangeMat.at<float>(rowIdn, columnIdn) != FLT_MAX)
>
> 53. continue;
>
> 54.
>
> 55. // 对点做运动补偿
>
> 56. thisPoint = deskewPoint(&thisPoint, laserCloudIn->points[i].time);
>
> 57.
>
> 58. // 将这个点的距离数据保存到这个range矩阵中
>
> 59. rangeMat.at<float>(rowIdn, columnIdn) = range;
>
> 60.
>
> 61. // 算出这个点的索引
>
> 62. int index = columnIdn + rowIdn * Horizon_SCAN;
>
> 63.
>
> 64. // 保存这个点的坐标
>
> 65. fullCloud->points[index] = thisPoint;
>
> 66. }
>
> 67. }
>
>
>
>
> 代码解读我们先来看rangeMat的组织形式：
我们看看如何做运动补偿的：
> 1. PointType deskewPoint(PointType *point, double relTime)
>
> 2. {
>
> 3. if (deskewFlag == -1 || cloudInfo.imuAvailable == false)
>
> 4. return *point;
>
> 5.
>
> 6. // relTime是相对时间，加上起始时间就是绝对时间
>
> 7. double pointTime = timeScanCur + relTime;
>
> 8.
>
> 9. float rotXCur, rotYCur, rotZCur;
>
> 10. // 计算当前点相对起始点的相对旋转
>
> 11. findRotation(pointTime, &rotXCur, &rotYCur, &rotZCur);
>
> 12.
>
> 13. float posXCur, posYCur, posZCur;
>
> 14. // 这里没有做平移补偿
>
> 15. findPosition(relTime, &posXCur, &posYCur, &posZCur);
>
> 16.
>
> 17. if (firstPointFlag == true)
>
> 18. {
>
> 19. // 计算第一个点的相对位姿
>
> 20. transStartInverse = (pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur)).inverse();
>
> 21. firstPointFlag = false;
>
> 22. }
>
> 23.
>
> 24. // transform points to start
>
> 25. // 计算和第一个点的相对位姿
>
> 26. Eigen::Affine3f transFinal = pcl::getTransformation(posXCur, posYCur, posZCur, rotXCur, rotYCur, rotZCur);
>
> 27. Eigen::Affine3f transBt = transStartInverse * transFinal;
>
> 28.
>
> 29. // R * p + t 把点补偿到第一个点的相对位姿
>
> 30. PointType newPoint;
>
> 31. newPoint.x = transBt(0,0) * point->x + transBt(0,1) * point->y + transBt(0,2) * point->z + transBt(0,3);
>
> 32. newPoint.y = transBt(1,0) * point->x + transBt(1,1) * point->y + transBt(1,2) * point->z + transBt(1,3);
>
> 33. newPoint.z = transBt(2,0) * point->x + transBt(2,1) * point->y + transBt(2,2) * point->z + transBt(2,3);
>
> 34. newPoint.intensity = point->intensity;
>
> 35.
>
> 36. return newPoint;
>
> 37. }
>
>
>
>
> 代码解读> 1. void findRotation(double pointTime, float *rotXCur, float *rotYCur, float *rotZCur)
>
> 2. {
>
> 3. *rotXCur = 0; *rotYCur = 0; *rotZCur = 0;
>
> 4.
>
> 5. int imuPointerFront = 0;
>
> 6. // imuPointerCur是imu补偿的总大小 这里用的是一种朴素的确保不越界的方法
>
> 7. while (imuPointerFront < imuPointerCur)
>
> 8. {
>
> 9. if (pointTime < imuTime[imuPointerFront])
>
> 10. break;
>
> 11. ++imuPointerFront;
>
> 12. }
>
> 13.
>
> 14. // 如果时间戳不在两个IMU的旋转时间 则直接赋值了
>
> 15. if (pointTime > imuTime[imuPointerFront] || imuPointerFront == 0)
>
> 16. {
>
> 17. *rotXCur = imuRotX[imuPointerFront];
>
> 18. *rotYCur = imuRotY[imuPointerFront];
>
> 19. *rotZCur = imuRotZ[imuPointerFront];
>
> 20. } else {
>
> 21. // 做一个线性插值
>
> 22. int imuPointerBack = imuPointerFront - 1;
>
> 23. double ratioFront = (pointTime - imuTime[imuPointerBack]) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
>
> 24. double ratioBack = (imuTime[imuPointerFront] - pointTime) / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
>
> 25. *rotXCur = imuRotX[imuPointerFront] * ratioFront + imuRotX[imuPointerBack] * ratioBack;
>
> 26. *rotYCur = imuRotY[imuPointerFront] * ratioFront + imuRotY[imuPointerBack] * ratioBack;
>
> 27. *rotZCur = imuRotZ[imuPointerFront] * ratioFront + imuRotZ[imuPointerBack] * ratioBack;
>
> 28. }
>
> 29. }
>
>
>
>
> 代码解读线性插值的原理如下：
将当前帧中的点云数据延伸至下一帧末尾位置：
其中，
- transStartInverse参数记录了当前点云帧的第一个点相对于该时刻的空间位姿信息；
- transFinal参数则表示从上一帧开始至当前时刻的空间旋转与平移变换关系；
- transBt参数包含了从起始点至当前目标点的空间位姿变化；
此外， - fullCloud变量存储了将所有投影到当前帧中的三维坐标系转换为全局坐标系后的结果；
1.4.4 剔除点的信息 cloudExtraction
> 1. void cloudExtraction()
>
> 2. {
>
> 3. int count = 0;
>
> 4. // extract segmented cloud for lidar odometry
>
> 5. // 遍历每一根SCAN
>
> 6. for (int i = 0; i < N_SCAN; ++i)
>
> 7. {
>
> 8. // 这个scan可以计算曲率的起始点（计算曲率需要左右各五个点）
>
> 9. cloudInfo.startRingIndex[i] = count - 1 + 5;
>
> 10.
>
> 11. for (int j = 0; j < Horizon_SCAN; ++j)
>
> 12. {
>
> 13. // 如果这是个有用的点
>
> 14. if (rangeMat.at<float>(i,j) != FLT_MAX)
>
> 15. {
>
> 16. // mark the points' column index for marking occlusion later
>
> 17. // 这个点对应哪一根垂直线
>
> 18. cloudInfo.pointColInd[count] = j;
>
> 19. // save range info
>
> 20. // 距离信息
>
> 21. cloudInfo.pointRange[count] = rangeMat.at<float>(i,j);
>
> 22. // save extracted cloud
>
> 23. // 它的3D点信息
>
> 24. extractedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
>
> 25. // size of extracted cloud
>
> 26. ++count;
>
> 27. }
>
> 28. }
>
> 29. cloudInfo.endRingIndex[i] = count -1 - 5;
>
> 30. }
>
> 31. }
>
>
>
>
> 代码解读维护了几个云数据项：
- cloudDataItem.startRingIndex[i]: 存储了大小等于NSCAN=16的信息。
- cloudDataItem.pointColInd[count]: 存储了大小等于NSCAN*HorizonSCAN的信息。
- pointRange[count]: 存储了大小等于NSCAN*HorizonSCAN的信息。
- From the extracted cloud, push_back points from fullCloud->points[j + i*HorizonSCAN].
- cloudDataItem.endRingIndex[i]: 存储了大小等于NSCAN=16（等于16）的信息。
1.4.5 发布点云信息 publishClouds
> 1. void publishClouds()
>
> 2. {
>
> 3. cloudInfo.header = cloudHeader;
>
> 4. cloudInfo.cloud_deskewed = publishCloud(pubExtractedCloud, extractedCloud, cloudHeader.stamp, lidarFrame);
>
> 5. pubLaserCloudInfo.publish(cloudInfo);
>
> 6. }
>
>
>
>
> 代码解读发布站点位移补偿至初始站点的信息为extractedCloud。
发布站群信息为cloudInfo。
发布站点位移补偿至初始站点的信息为extractedCloud。
发布站群信息为cloudInfo。
1.4.6 系统置位 resetParameters
> 1. void resetParameters()
>
> 2. {
>
> 3. laserCloudIn->clear();
>
> 4. extractedCloud->clear();
>
> 5. // reset range matrix for range image projection
>
> 6. rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
>
> 7.
>
> 8. imuPointerCur = 0;
>
> 9. firstPointFlag = true;
>
> 10. odomDeskewFlag = false;
>
> 11.
>
> 12. for (int i = 0; i < queueLength; ++i)
>
> 13. {
>
> 14. imuTime[i] = 0;
>
> 15. imuRotX[i] = 0;
>
> 16. imuRotY[i] = 0;
>
> 17. imuRotZ[i] = 0;
>
> 18. }
>
> 19.
>
> 20. columnIdnCountVec.assign(N_SCAN, 0);
>
> 21. }
>
>
>
>
> 代码解读全部评论 (0)
还没有任何评论哟~