赞
踩
头文件里主要是放了一些通用的参数定义,比方说:
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文件涉及到话题发布的地方,都会调用它。
其他的函数都是一些数据转换函数,没什么可说的。
- #pragma once
- #ifndef _UTILITY_LIDAR_ODOMETRY_H_
- #define _UTILITY_LIDAR_ODOMETRY_H_
-
- #include <ros/ros.h>
-
- #include <std_msgs/Header.h>
- #include <std_msgs/Float64MultiArray.h>
- #include <sensor_msgs/Imu.h>
- #include <sensor_msgs/PointCloud2.h>
- #include <sensor_msgs/NavSatFix.h>
- #include <nav_msgs/Odometry.h>
- #include <nav_msgs/Path.h>
- #include <visualization_msgs/Marker.h>
- #include <visualization_msgs/MarkerArray.h>
-
- #include <opencv2/opencv.hpp>
-
- #include <pcl/point_cloud.h>
- #include <pcl/point_types.h>
- #include <pcl/search/impl/search.hpp>
- #include <pcl/range_image/range_image.h>
- #include <pcl/kdtree/kdtree_flann.h>
- #include <pcl/common/common.h>
- #include <pcl/common/transforms.h>
- #include <pcl/registration/icp.h>
- #include <pcl/io/pcd_io.h>
- #include <pcl/filters/filter.h>
- #include <pcl/filters/voxel_grid.h>
- #include <pcl/filters/crop_box.h>
- #include <pcl_conversions/pcl_conversions.h>
-
- #include <tf/LinearMath/Quaternion.h>
- #include <tf/transform_listener.h>
- #include <tf/transform_datatypes.h>
- #include <tf/transform_broadcaster.h>
-
- #include <vector>
- #include <cmath>
- #include <algorithm>
- #include <queue>
- #include <deque>
- #include <iostream>
- #include <fstream>
- #include <ctime>
- #include <cfloat>
- #include <iterator>
- #include <sstream>
- #include <string>
- #include <limits>
- #include <iomanip>
- #include <array>
- #include <thread>
- #include <mutex>
-
- using namespace std;
-
- typedef pcl::PointXYZI PointType;
-
- // 传感器型号
- enum class SensorType { VELODYNE, OUSTER };
-
- class ParamServer
- {
- public:
-
- ros::NodeHandle nh;
-
- std::string robot_id;
-
- // 话题
- string pointCloudTopic; // points_raw 原始点云数据
- string imuTopic; // imu_raw 对应park数据集,imu_correct对应outdoor数据集,都是原始imu数据,不同的坐标系表示
- string odomTopic; // odometry/imu,imu里程计,imu积分计算得到
- string gpsTopic; // odometry/gps,gps里程计
-
- // 坐标系
- string lidarFrame; // 激光坐标系
- string baselinkFrame; // 载体坐标系
- string odometryFrame; // 里程计坐标系
- string mapFrame; // 世界坐标系
-
- // GPS参数
- bool useImuHeadingInitialization; //
- bool useGpsElevation;
- float gpsCovThreshold;
- float poseCovThreshold;
-
- // 保存PCD
- bool savePCD; // 是否保存地图
- string savePCDDirectory; // 保存路径
-
- // 激光传感器参数
- SensorType sensor; // 传感器型号
- int N_SCAN; // 扫描线数,例如16、64
- int Horizon_SCAN; // 扫描一周计数,例如每隔0.2°扫描一次,一周360°可以扫描1800次
- int downsampleRate; // 扫描线降采样,跳过一些扫描线
- float lidarMinRange; // 最小范围
- float lidarMaxRange; // 最大范围
-
- // IMU参数
- float imuAccNoise; // 加速度噪声标准差
- float imuGyrNoise; // 角速度噪声标准差
- float imuAccBiasN; //
- float imuGyrBiasN;
- float imuGravity; // 重力加速度
- float imuRPYWeight;
- vector<double> extRotV;
- vector<double> extRPYV;
- vector<double> extTransV;
- Eigen::Matrix3d extRot; // xyz坐标系旋转
- Eigen::Matrix3d extRPY; // RPY欧拉角的变换关系
- Eigen::Vector3d extTrans; // xyz坐标系平移
- Eigen::Quaterniond extQRPY;
-
- // LOAM
- float edgeThreshold;
- float surfThreshold;
- int edgeFeatureMinValidNum;
- int surfFeatureMinValidNum;
-
- // voxel filter paprams
- float odometrySurfLeafSize;
- float mappingCornerLeafSize;
- float mappingSurfLeafSize ;
-
- float z_tollerance;
- float rotation_tollerance;
-
- // CPU Params
- int numberOfCores;
- double mappingProcessInterval;
-
- // Surrounding map
- float surroundingkeyframeAddingDistThreshold;
- float surroundingkeyframeAddingAngleThreshold;
- float surroundingKeyframeDensity;
- float surroundingKeyframeSearchRadius;
-
- // Loop closure
- bool loopClosureEnableFlag;
- float loopClosureFrequency;
- int surroundingKeyframeSize;
- float historyKeyframeSearchRadius;
- float historyKeyframeSearchTimeDiff;
- int historyKeyframeSearchNum;
- float historyKeyframeFitnessScore;
-
- // global map visualization radius
- float globalMapVisualizationSearchRadius;
- float globalMapVisualizationPoseDensity;
- float globalMapVisualizationLeafSize;
-
- ParamServer()
- {
- nh.param<std::string>("/robot_id", robot_id, "roboat");
-
- // 从param server中读取key为"lio_sam/pointCloudTopic"对应的参数,存pointCloudTopic,第三个参数是默认值
- // launch文件中定义<rosparam file="$(find lio_sam)/config/params.yaml" command="load" />,从yaml文件加载参数
- nh.param<std::string>("lio_sam/pointCloudTopic", pointCloudTopic, "points_raw");
- nh.param<std::string>("lio_sam/imuTopic", imuTopic, "imu_correct");
- nh.param<std::string>("lio_sam/odomTopic", odomTopic, "odometry/imu");
- nh.param<std::string>("lio_sam/gpsTopic", gpsTopic, "odometry/gps");
-
- nh.param<std::string>("lio_sam/lidarFrame", lidarFrame, "base_link");
- nh.param<std::string>("lio_sam/baselinkFrame", baselinkFrame, "base_link");
- nh.param<std::string>("lio_sam/odometryFrame", odometryFrame, "odom");
- nh.param<std::string>("lio_sam/mapFrame", mapFrame, "map");
-
- nh.param<bool>("lio_sam/useImuHeadingInitialization", useImuHeadingInitialization, false);
- nh.param<bool>("lio_sam/useGpsElevation", useGpsElevation, false);
- nh.param<float>("lio_sam/gpsCovThreshold", gpsCovThreshold, 2.0);
- nh.param<float>("lio_sam/poseCovThreshold", poseCovThreshold, 25.0);
-
- nh.param<bool>("lio_sam/savePCD", savePCD, false);
- nh.param<std::string>("lio_sam/savePCDDirectory", savePCDDirectory, "/Downloads/LOAM/");
-
- std::string sensorStr;
- nh.param<std::string>("lio_sam/sensor", sensorStr, "");
- if (sensorStr == "velodyne")
- {
- sensor = SensorType::VELODYNE;
- }
- else if (sensorStr == "ouster")
- {
- sensor = SensorType::OUSTER;
- }
- else
- {
- ROS_ERROR_STREAM(
- "Invalid sensor type (must be either 'velodyne' or 'ouster'): " << sensorStr);
- ros::shutdown();
- }
-
- nh.param<int>("lio_sam/N_SCAN", N_SCAN, 16);
- nh.param<int>("lio_sam/Horizon_SCAN", Horizon_SCAN, 1800);
- nh.param<int>("lio_sam/downsampleRate", downsampleRate, 1);
- nh.param<float>("lio_sam/lidarMinRange", lidarMinRange, 1.0);
- nh.param<float>("lio_sam/lidarMaxRange", lidarMaxRange, 1000.0);
-
- nh.param<float>("lio_sam/imuAccNoise", imuAccNoise, 0.01);
- nh.param<float>("lio_sam/imuGyrNoise", imuGyrNoise, 0.001);
- nh.param<float>("lio_sam/imuAccBiasN", imuAccBiasN, 0.0002);
- nh.param<float>("lio_sam/imuGyrBiasN", imuGyrBiasN, 0.00003);
- nh.param<float>("lio_sam/imuGravity", imuGravity, 9.80511);
- nh.param<float>("lio_sam/imuRPYWeight", imuRPYWeight, 0.01);
- nh.param<vector<double>>("lio_sam/extrinsicRot", extRotV, vector<double>());
- nh.param<vector<double>>("lio_sam/extrinsicRPY", extRPYV, vector<double>());
- nh.param<vector<double>>("lio_sam/extrinsicTrans", extTransV, vector<double>());
- extRot = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRotV.data(), 3, 3);
- extRPY = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extRPYV.data(), 3, 3);
- extTrans = Eigen::Map<const Eigen::Matrix<double, -1, -1, Eigen::RowMajor>>(extTransV.data(), 3, 1);
- extQRPY = Eigen::Quaterniond(extRPY);
-
- nh.param<float>("lio_sam/edgeThreshold", edgeThreshold, 0.1);
- nh.param<float>("lio_sam/surfThreshold", surfThreshold, 0.1);
- nh.param<int>("lio_sam/edgeFeatureMinValidNum", edgeFeatureMinValidNum, 10);
- nh.param<int>("lio_sam/surfFeatureMinValidNum", surfFeatureMinValidNum, 100);
-
- nh.param<float>("lio_sam/odometrySurfLeafSize", odometrySurfLeafSize, 0.2);
- nh.param<float>("lio_sam/mappingCornerLeafSize", mappingCornerLeafSize, 0.2);
- nh.param<float>("lio_sam/mappingSurfLeafSize", mappingSurfLeafSize, 0.2);
-
- nh.param<float>("lio_sam/z_tollerance", z_tollerance, FLT_MAX);
- nh.param<float>("lio_sam/rotation_tollerance", rotation_tollerance, FLT_MAX);
-
- nh.param<int>("lio_sam/numberOfCores", numberOfCores, 2);
- nh.param<double>("lio_sam/mappingProcessInterval", mappingProcessInterval, 0.15);
-
- nh.param<float>("lio_sam/surroundingkeyframeAddingDistThreshold", surroundingkeyframeAddingDistThreshold, 1.0);
- nh.param<float>("lio_sam/surroundingkeyframeAddingAngleThreshold", surroundingkeyframeAddingAngleThreshold, 0.2);
- nh.param<float>("lio_sam/surroundingKeyframeDensity", surroundingKeyframeDensity, 1.0);
- nh.param<float>("lio_sam/surroundingKeyframeSearchRadius", surroundingKeyframeSearchRadius, 50.0);
-
- nh.param<bool>("lio_sam/loopClosureEnableFlag", loopClosureEnableFlag, false);
- nh.param<float>("lio_sam/loopClosureFrequency", loopClosureFrequency, 1.0);
- nh.param<int>("lio_sam/surroundingKeyframeSize", surroundingKeyframeSize, 50);
- nh.param<float>("lio_sam/historyKeyframeSearchRadius", historyKeyframeSearchRadius, 10.0);
- nh.param<float>("lio_sam/historyKeyframeSearchTimeDiff", historyKeyframeSearchTimeDiff, 30.0);
- nh.param<int>("lio_sam/historyKeyframeSearchNum", historyKeyframeSearchNum, 25);
- nh.param<float>("lio_sam/historyKeyframeFitnessScore", historyKeyframeFitnessScore, 0.3);
-
- nh.param<float>("lio_sam/globalMapVisualizationSearchRadius", globalMapVisualizationSearchRadius, 1e3);
- nh.param<float>("lio_sam/globalMapVisualizationPoseDensity", globalMapVisualizationPoseDensity, 10.0);
- nh.param<float>("lio_sam/globalMapVisualizationLeafSize", globalMapVisualizationLeafSize, 1.0);
-
- usleep(100);
- }
-
- /**
- * imu原始测量数据转换到lidar系,加速度、角速度、RPY
- */
- sensor_msgs::Imu imuConverter(const sensor_msgs::Imu& imu_in)
- {
- sensor_msgs::Imu imu_out = imu_in;
- // 加速度,只跟xyz坐标系的旋转有关系
- Eigen::Vector3d acc(imu_in.linear_acceleration.x, imu_in.linear_acceleration.y, imu_in.linear_acceleration.z);
- acc = extRot * acc;
- imu_out.linear_acceleration.x = acc.x();
- imu_out.linear_acceleration.y = acc.y();
- imu_out.linear_acceleration.z = acc.z();
- // 角速度,只跟xyz坐标系的旋转有关系
- Eigen::Vector3d gyr(imu_in.angular_velocity.x, imu_in.angular_velocity.y, imu_in.angular_velocity.z);
- gyr = extRot * gyr;
- imu_out.angular_velocity.x = gyr.x();
- imu_out.angular_velocity.y = gyr.y();
- imu_out.angular_velocity.z = gyr.z();
- // RPY
- Eigen::Quaterniond q_from(imu_in.orientation.w, imu_in.orientation.x, imu_in.orientation.y, imu_in.orientation.z);
- // 为什么是右乘,可以动手画一下看看
- Eigen::Quaterniond q_final = q_from * extQRPY;
- imu_out.orientation.x = q_final.x();
- imu_out.orientation.y = q_final.y();
- imu_out.orientation.z = q_final.z();
- imu_out.orientation.w = q_final.w();
-
- 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)
- {
- ROS_ERROR("Invalid quaternion, please use a 9-axis IMU!");
- ros::shutdown();
- }
-
- return imu_out;
- }
- };
-
-
- /**
- * 发布thisCloud,返回thisCloud对应msg格式
- */
- sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, pcl::PointCloud<PointType>::Ptr thisCloud, ros::Time thisStamp, std::string thisFrame)
- {
- sensor_msgs::PointCloud2 tempCloud;
- pcl::toROSMsg(*thisCloud, tempCloud);
- tempCloud.header.stamp = thisStamp;
- tempCloud.header.frame_id = thisFrame;
- if (thisPub->getNumSubscribers() != 0)
- thisPub->publish(tempCloud);
- return tempCloud;
- }
-
- /**
- * msg时间戳
- */
- template<typename T>
- double ROS_TIME(T msg)
- {
- return msg->header.stamp.toSec();
- }
-
- /**
- * 提取imu角速度
- */
- template<typename T>
- void imuAngular2rosAngular(sensor_msgs::Imu *thisImuMsg, T *angular_x, T *angular_y, T *angular_z)
- {
- *angular_x = thisImuMsg->angular_velocity.x;
- *angular_y = thisImuMsg->angular_velocity.y;
- *angular_z = thisImuMsg->angular_velocity.z;
- }
-
- /**
- * 提取imu加速度
- */
- template<typename T>
- void imuAccel2rosAccel(sensor_msgs::Imu *thisImuMsg, T *acc_x, T *acc_y, T *acc_z)
- {
- *acc_x = thisImuMsg->linear_acceleration.x;
- *acc_y = thisImuMsg->linear_acceleration.y;
- *acc_z = thisImuMsg->linear_acceleration.z;
- }
-
- /**
- * 提取imu姿态角RPY
- */
- template<typename T>
- void imuRPY2rosRPY(sensor_msgs::Imu *thisImuMsg, T *rosRoll, T *rosPitch, T *rosYaw)
- {
- double imuRoll, imuPitch, imuYaw;
- tf::Quaternion orientation;
- tf::quaternionMsgToTF(thisImuMsg->orientation, orientation);
- tf::Matrix3x3(orientation).getRPY(imuRoll, imuPitch, imuYaw);
-
- *rosRoll = imuRoll;
- *rosPitch = imuPitch;
- *rosYaw = imuYaw;
- }
-
- /**
- * 点到坐标系原点距离
- */
- float pointDistance(PointType p)
- {
- return sqrt(p.x*p.x + p.y*p.y + p.z*p.z);
- }
-
- /**
- * 两点之间距离
- */
- float pointDistance(PointType p1, PointType p2)
- {
- 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));
- }
-
- #endif
Copyright © 2003-2013 www.wpsshop.cn 版权所有,并保留所有权利。