当前位置:   article > 正文

LIO-SAM源码解析(七):utility.h_liosam源码注释

liosam源码注释

1. 代码功能

头文件里主要是放了一些通用的参数定义,比方说:

nh.param<std::string>("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");

这个意思就是我launch文件里面有这个"lio_sam/pointCloudTopic"(前面这个)的参数值的赋值,那么就赋值给pointCloudTopic(后面这个),后面的"/points_raw"就会忽略。那假如launch文件里面没有这个"lio_sam/pointCloudTopic"的定义,则就用"points_raw"。我们打开run.launch文件,可以看到是有加载参数的:

    <rosparam file="$(find lio_sam)/config/params.yaml" command="load" />

那么相关的参数就放在params.yaml文件中。

关于这个params.yaml,里面的东西乍一看很多,其实就几个比较重要:

第一:IMU Settings 来记录IMU的bias和噪声。当然IMU其实是加速度计和陀螺仪一共三个轴,这里却不分轴,用了一个平均数。如果没有转台之类的设备,就跑艾伦方差也可以标出各个轴的bias。作者的处理是:在imu预积分部分,三个轴都用了同样的bias方差(也就是写在配置文件里的这个),当然你要是有能力标的更准,那不妨这里把各个轴的bias都写进去,然后在imupreintegration.cpp文件里面改一改。

第二,IMU和雷达之间的外参。我不知道为什么作者TiXiaoshan很骚包的在这里写了一个Extrinsics (lidar -> IMU),其实不能这样写,应该写成IMU->Lidar。因为它其实是T_{LB},也就是说在IMU坐标系下的一个点,左乘T_{LB},就可以变换到lidar坐标系下。 而且作者用的IMU也不是正常IMU,我推测他用的应该是左手系IMU。对于正常人使用的话,就普通的IMU就行,假如你就是那个机器人,x射向正前方,y射向左边,z射向头顶。雷达和IMU都是这样摆放,所以extrinsicRot和extrinsicRPY全部弄成单位矩阵就行。差的不太远的话,extrinsicTrans弄成[0,0,0]就行。

不过有精力的话还是标定了一下,就用浙大在2020开源的标定工具,lidar_IMU_calib ,(这个博客讲怎么配置的,我用的是这个),我个人拿尺子量出来大致比一下,我觉得他们估的还挺准。顺便提一嘴,ETH还有一个标定雷达和IMU的,那个准确来说标的是雷达和里程计的,原理也比较简单,反正雷达运动算一个轨迹,IMU也来一个轨迹,两边用外参联系起来,构成一个环,来求解外参。但是这种是有问题的,因为IMU估计的轨迹本身也就不准,这样估出来的外参也就不对,所以人家是标雷达和里程计的,不能单估雷达和IMU。浙大的这个lidar_IMU_calib的文章我大致看了看,大致是用样条插值,相机去对齐IMU,通过这种方式来初始化,之后构建surfelsMap,迭代优化来精修。(细节以我的性格正常我会展开说的,但是我不喜欢他们这篇论文所以不讲,主要是因为他们在论文里表示坐标系变换,R_{LI}非要写成^L_I R,像这种反人类的写法我们应该坚决抵制 ^ _ ^)

第三,z_tollerance,可以给z一个比较大的限制,如果用的是无人车,那就不可能在z轴变化过大,这里就可以限制它,防止它飘走。

第四,voxel filter paprams,这个是一些体素滤波的下采样参数,注意室内和室外是有区别的。

现在回到这个头文件里,可以注意两个内容:

第一,imuConverter函数,这个函数之后会被频繁调用。它主要的作用,是把IMU的信息,从IMU坐标系,转换到雷达坐标系。(注意,这个函数只旋转,没有平移,和真正的雷达坐标系之间还是差了一个平移的。至于为什么没有平移,先提前剧透一下,在imuPreintegration.cpp文件中,还有两个imu2Lidar,lidar2imu变量,这俩变量只有平移,没有旋转。

事实上,作者后续是把imu数据先用imuConverter旋转到雷达系下(但其实还差了个平移)。然后他把雷达数据又根据lidar2Imu反向平移了一下,和转换以后差了个平移的imu数据在“中间系”对齐,之后算完又从中间系通过imu2Lidar挪回了雷达系进行publish。

第二,publishCloud函数,这个函数传入句柄,然后发布形参里的内容,在cpp文件涉及到话题发布的地方,都会调用它。

其他的函数都是一些数据转换函数,没什么可说的。

2. 代码

  1. #pragma once
  2. #ifndef _UTILITY_LIDAR_ODOMETRY_H_
  3. #define _UTILITY_LIDAR_ODOMETRY_H_
  4. #include <ros/ros.h>
  5. #include <std_msgs/Header.h>
  6. #include <std_msgs/Float64MultiArray.h>
  7. #include <sensor_msgs/Imu.h>
  8. #include <sensor_msgs/PointCloud2.h>
  9. #include <sensor_msgs/NavSatFix.h>
  10. #include <nav_msgs/Odometry.h>
  11. #include <nav_msgs/Path.h>
  12. #include <visualization_msgs/Marker.h>
  13. #include <visualization_msgs/MarkerArray.h>
  14. #include <opencv2/opencv.hpp>
  15. #include <pcl/point_cloud.h>
  16. #include <pcl/point_types.h>
  17. #include <pcl/search/impl/search.hpp>
  18. #include <pcl/range_image/range_image.h>
  19. #include <pcl/kdtree/kdtree_flann.h>
  20. #include <pcl/common/common.h>
  21. #include <pcl/common/transforms.h>
  22. #include <pcl/registration/icp.h>
  23. #include <pcl/io/pcd_io.h>
  24. #include <pcl/filters/filter.h>
  25. #include <pcl/filters/voxel_grid.h>
  26. #include <pcl/filters/crop_box.h>
  27. #include <pcl_conversions/pcl_conversions.h>
  28. #include <tf/LinearMath/Quaternion.h>
  29. #include <tf/transform_listener.h>
  30. #include <tf/transform_datatypes.h>
  31. #include <tf/transform_broadcaster.h>
  32. #include <vector>
  33. #include <cmath>
  34. #include <algorithm>
  35. #include <queue>
  36. #include <deque>
  37. #include <iostream>
  38. #include <fstream>
  39. #include <ctime>
  40. #include <cfloat>
  41. #include <iterator>
  42. #include <sstream>
  43. #include <string>
  44. #include <limits>
  45. #include <iomanip>
  46. #include <array>
  47. #include <thread>
  48. #include <mutex>
  49. using namespace std;
  50. typedef pcl::PointXYZI PointType;
  51. // 传感器型号
  52. enum class SensorType { VELODYNE, OUSTER };
  53. class ParamServer
  54. {
  55. public:
  56. ros::NodeHandle nh;
  57. std::string robot_id;
  58. // 话题
  59. string pointCloudTopic; // points_raw 原始点云数据
  60. string imuTopic; // imu_raw 对应park数据集,imu_correct对应outdoor数据集,都是原始imu数据,不同的坐标系表示
  61. string odomTopic; // odometry/imu,imu里程计,imu积分计算得到
  62. string gpsTopic; // odometry/gps,gps里程计
  63. // 坐标系
  64. string lidarFrame; // 激光坐标系
  65. string baselinkFrame; // 载体坐标系
  66. string odometryFrame; // 里程计坐标系
  67. string mapFrame; // 世界坐标系
  68. // GPS参数
  69. bool useImuHeadingInitialization; //
  70. bool useGpsElevation;
  71. float gpsCovThreshold;
  72. float poseCovThreshold;
  73. // 保存PCD
  74. bool savePCD; // 是否保存地图
  75. string savePCDDirectory; // 保存路径
  76. // 激光传感器参数
  77. SensorType sensor; // 传感器型号
  78. int N_SCAN; // 扫描线数,例如16、64
  79. int Horizon_SCAN; // 扫描一周计数,例如每隔0.2°扫描一次,一周360°可以扫描1800次
  80. int downsampleRate; // 扫描线降采样,跳过一些扫描线
  81. float lidarMinRange; // 最小范围
  82. float lidarMaxRange; // 最大范围
  83. // IMU参数
  84. float imuAccNoise; // 加速度噪声标准差
  85. float imuGyrNoise; // 角速度噪声标准差
  86. float imuAccBiasN; //
  87. float imuGyrBiasN;
  88. float imuGravity; // 重力加速度
  89. float imuRPYWeight;
  90. vector<double> extRotV;
  91. vector<double> extRPYV;
  92. vector<double> extTransV;
  93. Eigen::Matrix3d extRot; // xyz坐标系旋转
  94. Eigen::Matrix3d extRPY; // RPY欧拉角的变换关系
  95. Eigen::Vector3d extTrans; // xyz坐标系平移
  96. Eigen::Quaterniond extQRPY;
  97. // LOAM
  98. float edgeThreshold;
  99. float surfThreshold;
  100. int edgeFeatureMinValidNum;
  101. int surfFeatureMinValidNum;
  102. // voxel filter paprams
  103. float odometrySurfLeafSize;
  104. float mappingCornerLeafSize;
  105. float mappingSurfLeafSize ;
  106. float z_tollerance;
  107. float rotation_tollerance;
  108. // CPU Params
  109. int numberOfCores;
  110. double mappingProcessInterval;
  111. // Surrounding map
  112. float surroundingkeyframeAddingDistThreshold;
  113. float surroundingkeyframeAddingAngleThreshold;
  114. float surroundingKeyframeDensity;
  115. float surroundingKeyframeSearchRadius;
  116. // Loop closure
  117. bool loopClosureEnableFlag;
  118. float loopClosureFrequency;
  119. int surroundingKeyframeSize;
  120. float historyKeyframeSearchRadius;
  121. float historyKeyframeSearchTimeDiff;
  122. int historyKeyframeSearchNum;
  123. float historyKeyframeFitnessScore;
  124. // global map visualization radius
  125. float globalMapVisualizationSearchRadius;
  126. float globalMapVisualizationPoseDensity;
  127. float globalMapVisualizationLeafSize;
  128. ParamServer()
  129. {
  130. nh.param<std::string>("/robot_id", robot_id, "roboat");
  131. // 从param server中读取key为"lio_sam/pointCloudTopic"对应的参数,存pointCloudTopic,第三个参数是默认值
  132. // launch文件中定义<rosparam file="$(find lio_sam)/config/params.yaml" command="load" />,从yaml文件加载参数
  133. nh.param<std::string>("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");
  134. nh.param<std::string>("lio_sam/imuTopic", imuTopic, "imu_correct");
  135. nh.param<std::string>("lio_sam/odomTopic", odomTopic, "odometry/imu");
  136. nh.param<std::string>("lio_sam/gpsTopic", gpsTopic, "odometry/gps");
  137. nh.param<std::string>("lio_sam/lidarFrame", lidarFrame, "base_link");
  138. nh.param<std::string>("lio_sam/baselinkFrame", baselinkFrame, "base_link");
  139. nh.param<std::string>("lio_sam/odometryFrame", odometryFrame, "odom");
  140. nh.param<std::string>("lio_sam/mapFrame", mapFrame, "map");
  141. nh.param<bool>("lio_sam/useImuHeadingInitialization", useImuHeadingInitialization, false);
  142. nh.param<bool>("lio_sam/useGpsElevation", useGpsElevation, false);
  143. nh.param<float>("lio_sam/gpsCovThreshold", gpsCovThreshold, 2.0);
  144. nh.param<float>("lio_sam/poseCovThreshold", poseCovThreshold, 25.0);
  145. nh.param<bool>("lio_sam/savePCD", savePCD, false);
  146. nh.param<std::string>("lio_sam/savePCDDirectory", savePCDDirectory, "/Downloads/LOAM/");
  147. std::string sensorStr;
  148. nh.param<std::string>("lio_sam/sensor", sensorStr, "");
  149. if (sensorStr == "velodyne")
  150. {
  151. sensor = SensorType::VELODYNE;
  152. }
  153. else if (sensorStr == "ouster")
  154. {
  155. sensor = SensorType::OUSTER;
  156. }
  157. else
  158. {
  159. ROS_ERROR_STREAM(
  160. "Invalid sensor type (must be either 'velodyne' or 'ouster'): " << sensorStr);
  161. ros::shutdown();
  162. }
  163. nh.param<int>("lio_sam/N_SCAN", N_SCAN, 16);
  164. nh.param<int>("lio_sam/Horizon_SCAN", Horizon_SCAN, 1800);
  165. nh.param<int>("lio_sam/downsampleRate", downsampleRate, 1);
  166. nh.param<float>("lio_sam/lidarMinRange", lidarMinRange, 1.0);
  167. nh.param<float>("lio_sam/lidarMaxRange", lidarMaxRange, 1000.0);
  168. nh.param<float>("lio_sam/imuAccNoise", imuAccNoise, 0.01);
  169. nh.param<float>("lio_sam/imuGyrNoise", imuGyrNoise, 0.001);
  170. nh.param<float>("lio_sam/imuAccBiasN", imuAccBiasN, 0.0002);
  171. nh.param<float>("lio_sam/imuGyrBiasN", imuGyrBiasN, 0.00003);
  172. nh.param<float>("lio_sam/imuGravity", imuGravity, 9.80511);
  173. nh.param<float>("lio_sam/imuRPYWeight", imuRPYWeight, 0.01);
  174. nh.param<vector<double>>("lio_sam/extrinsicRot", extRotV, vector<double>());
  175. nh.param<vector<double>>("lio_sam/extrinsicRPY", extRPYV, vector<double>());
  176. nh.param<vector<double>>("lio_sam/extrinsicTrans", extTransV, vector<double>());
  177. extRot = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRotV.data(), 3, 3);
  178. extRPY = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRPYV.data(), 3, 3);
  179. extTrans = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extTransV.data(), 3, 1);
  180. extQRPY = Eigen::Quaterniond(extRPY);
  181. nh.param<float>("lio_sam/edgeThreshold", edgeThreshold, 0.1);
  182. nh.param<float>("lio_sam/surfThreshold", surfThreshold, 0.1);
  183. nh.param<int>("lio_sam/edgeFeatureMinValidNum", edgeFeatureMinValidNum, 10);
  184. nh.param<int>("lio_sam/surfFeatureMinValidNum", surfFeatureMinValidNum, 100);
  185. nh.param<float>("lio_sam/odometrySurfLeafSize", odometrySurfLeafSize, 0.2);
  186. nh.param<float>("lio_sam/mappingCornerLeafSize", mappingCornerLeafSize, 0.2);
  187. nh.param<float>("lio_sam/mappingSurfLeafSize", mappingSurfLeafSize, 0.2);
  188. nh.param<float>("lio_sam/z_tollerance", z_tollerance, FLT_MAX);
  189. nh.param<float>("lio_sam/rotation_tollerance", rotation_tollerance, FLT_MAX);
  190. nh.param<int>("lio_sam/numberOfCores", numberOfCores, 2);
  191. nh.param<double>("lio_sam/mappingProcessInterval", mappingProcessInterval, 0.15);
  192. nh.param<float>("lio_sam/surroundingkeyframeAddingDistThreshold", surroundingkeyframeAddingDistThreshold, 1.0);
  193. nh.param<float>("lio_sam/surroundingkeyframeAddingAngleThreshold", surroundingkeyframeAddingAngleThreshold, 0.2);
  194. nh.param<float>("lio_sam/surroundingKeyframeDensity", surroundingKeyframeDensity, 1.0);
  195. nh.param<float>("lio_sam/surroundingKeyframeSearchRadius", surroundingKeyframeSearchRadius, 50.0);
  196. nh.param<bool>("lio_sam/loopClosureEnableFlag", loopClosureEnableFlag, false);
  197. nh.param<float>("lio_sam/loopClosureFrequency", loopClosureFrequency, 1.0);
  198. nh.param<int>("lio_sam/surroundingKeyframeSize", surroundingKeyframeSize, 50);
  199. nh.param<float>("lio_sam/historyKeyframeSearchRadius", historyKeyframeSearchRadius, 10.0);
  200. nh.param<float>("lio_sam/historyKeyframeSearchTimeDiff", historyKeyframeSearchTimeDiff, 30.0);
  201. nh.param<int>("lio_sam/historyKeyframeSearchNum", historyKeyframeSearchNum, 25);
  202. nh.param<float>("lio_sam/historyKeyframeFitnessScore", historyKeyframeFitnessScore, 0.3);
  203. nh.param<float>("lio_sam/globalMapVisualizationSearchRadius", globalMapVisualizationSearchRadius, 1e3);
  204. nh.param<float>("lio_sam/globalMapVisualizationPoseDensity", globalMapVisualizationPoseDensity, 10.0);
  205. nh.param<float>("lio_sam/globalMapVisualizationLeafSize", globalMapVisualizationLeafSize, 1.0);
  206. usleep(100);
  207. }
  208. /**
  209. * imu原始测量数据转换到lidar系,加速度、角速度、RPY
  210. */
  211. sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
  212. {
  213. sensor_msgs::Imu imu_out = imu_in;
  214. // 加速度,只跟xyz坐标系的旋转有关系
  215. Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
  216. acc = extRot * acc;
  217. imu_out.linear_acceleration.x = acc.x();
  218. imu_out.linear_acceleration.y = acc.y();
  219. imu_out.linear_acceleration.z = acc.z();
  220. // 角速度,只跟xyz坐标系的旋转有关系
  221. Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
  222. gyr = extRot * gyr;
  223. imu_out.angular_velocity.x = gyr.x();
  224. imu_out.angular_velocity.y = gyr.y();
  225. imu_out.angular_velocity.z = gyr.z();
  226. // RPY
  227. Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
  228. // 为什么是右乘,可以动手画一下看看
  229. Eigen::Quaterniond q_final = q_from * extQRPY;
  230. imu_out.orientation.x = q_final.x();
  231. imu_out.orientation.y = q_final.y();
  232. imu_out.orientation.z = q_final.z();
  233. imu_out.orientation.w = q_final.w();
  234. if (sqrt(q_final.x()*q_final.x() + q_final.y()*q_final.y() + q_final.z()*q_final.z() + q_final.w()*q_final.w()) < 0.1)
  235. {
  236. ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");
  237. ros::shutdown();
  238. }
  239. return imu_out;
  240. }
  241. };
  242. /**
  243. * 发布thisCloud,返回thisCloud对应msg格式
  244. */
  245. sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud<PointType>::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame)
  246. {
  247. sensor_msgs::PointCloud2 tempCloud;
  248. pcl::toROSMsg(*thisCloud, tempCloud);
  249. tempCloud.header.stamp = thisStamp;
  250. tempCloud.header.frame_id = thisFrame;
  251. if (thisPub->getNumSubscribers() != 0)
  252. thisPub->publish(tempCloud);
  253. return tempCloud;
  254. }
  255. /**
  256. * msg时间戳
  257. */
  258. template<typename T>
  259. double ROS_TIME(T msg)
  260. {
  261. return msg->header.stamp.toSec();
  262. }
  263. /**
  264. * 提取imu角速度
  265. */
  266. template<typename T>
  267. void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z)
  268. {
  269. *angular_x = thisImuMsg->angular_velocity.x;
  270. *angular_y = thisImuMsg->angular_velocity.y;
  271. *angular_z = thisImuMsg->angular_velocity.z;
  272. }
  273. /**
  274. * 提取imu加速度
  275. */
  276. template<typename T>
  277. void imuAccel2rosAccel(sensor_msgs::Imu *thisImuMsg, T *acc_x, T *acc_y, T *acc_z)
  278. {
  279. *acc_x = thisImuMsg->linear_acceleration.x;
  280. *acc_y = thisImuMsg->linear_acceleration.y;
  281. *acc_z = thisImuMsg->linear_acceleration.z;
  282. }
  283. /**
  284. * 提取imu姿态角RPY
  285. */
  286. template<typename T>
  287. void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw)
  288. {
  289. double imuRoll, imuPitch, imuYaw;
  290. tf::Quaternion orientation;
  291. tf::quaternionMsgToTF(thisImuMsg->orientation, orientation);
  292. tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
  293. *rosRoll = imuRoll;
  294. *rosPitch = imuPitch;
  295. *rosYaw = imuYaw;
  296. }
  297. /**
  298. * 点到坐标系原点距离
  299. */
  300. float pointDistance(PointType p)
  301. {
  302. return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
  303. }
  304. /**
  305. * 两点之间距离
  306. */
  307. float pointDistance(PointType p1, PointType p2)
  308. {
  309. return sqrt((p1.x-p2.x)*(p1.x-p2.x) + (p1.y-p2.y)*(p1.y-p2.y) + (p1.z-p2.z)*(p1.z-p2.z));
  310. }
  311. #endif

参考文献

SLAM学习笔记(二十)LIO-SAM流程及代码详解(最全)_zkk9527的博客-CSDN博客_lio-sam 

声明:本文内容由网友自发贡献,不代表【wpsshop博客】立场,版权归原作者所有,本站不承担相应法律责任。如您发现有侵权的内容,请联系我们。转载请注明出处:https://www.wpsshop.cn/w/从前慢现在也慢/article/detail/116656?site
推荐阅读
相关标签
  

闽ICP备14008679号