本人讲解关于slam一系列文章汇总链接:史上最全slam从零开始,针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解:https://blog.csdn.net/weixin_43013761/article/details/127350885
文
末
正
下
方
中
心
提
供
了
本
人
联
系
方
式
,
点
击
本
人
照
片
即
可
显
示
W
X
→
官
方
认
证
{\color{blue}{文末正下方中心}提供了本人 \color{red} 联系方式,\color{blue}点击本人照片即可显示WX→官方认证}
文末正下方中心提供了本人联系方式,点击本人照片即可显示WX→官方认证
一、前言
再前面的博客中,已经对 Odome、landmark、Imu、GPS 的预处理进行了讲解,有的朋友或许会觉得GPS的处理挺复杂的,那么再来看看稍微复杂一些的点云处理,如下:
SensorBridge::HandleLaserScanMessage()
SensorBridge::HandleMultiEchoLaserScanMessage()
SensorBridge::HandlePointCloud2Message()
其上函数都实现于 src/cartographer_ros/cartographer_ros/cartographer_ros/sensor_bridge.cc 文件中。另外,这里所谓的复杂,仅仅是代码逻辑上的复杂,而非算法,那么就开始讲解吧。
二、雷达重要指标
1.视野范围→数据的最大最小距离以及扫描的角度范围
激光雷达的视野范围决定了这款雷达的应用场景,墙距离20m的屋子里不能可能使用视野范围为4米的雷达,根本扫不到东西。
2.角度分辨率或点数→2个数据点间的角度
雷达的角度分辨率决定了激光雷达每帧数据点的个数。如果是0.25度的分辨率,360度视角的激光雷达,那其一帧最多有1441个点。每帧数据的点数越多,越能更好的表征环境的细节信息。当然,点数也不是越多越好,点数越多,计算成本也越高。
3.频率:发出数据的频率,如10HZ,就可以理解为1秒10帧数据
雷达的频率是一个十分重要的指标,雷达的频率越高,雷达2帧数据的间隔就越小。假设雷达的频率是20Hz,也就是50ms来一次数据。当我进行定位的时候,这50ms时间内的机器人的运动,只能通过估计来获得,所以雷达的频率越高,我们通过估计的距离也就越小,定位也就越准确。
4.强度:发出数据的能量强度,主要物体材质相关
激光雷达的激光点是有能量的,不同品牌激光点的能量也不同。当能量太小时,远距离情况下可能存在返回不了数据的情况。
5.精度:点的跳动程度(精度)等等
这是最重要的一个指标,如果一个激光雷达的数据跳动特别大,那这个雷达就没法用了。现在一般厂商的雷达的精度都是2%。也就是100m的情况下,点的跳动幅度为2cm。但是,实际感觉能达到这个精度的雷达不是很多。
雷
达
数
据
帧
概
念
:
\color{blue}雷达数据帧概念:
雷达数据帧概念:
明白了基本概念之后,这里再讲解以下雷达帧的概念,假设雷达为20HZ,那么他的帧率可以理解为20,也就是每间隔50ms发送一帧数据,一帧数据中包含了雷达视野范围的所有点云数据。如20HZ单线180度视角的雷达,说明其1秒内对180度视野范围扫描了20次(帧),每次的数据为180/0.25=720个点云数据。
三、雷达消息
讲解代码之前,先来看消息类型。
1、单线雷达
在终端执行指令:
rosmsg show LaserScan
std_msgs/Header header
uint32 seq
time stamp
string frame_id
float32 angle_min
float32 angle_max
float32 angle_increment
float32 time_increment
float32 scan_time
float32 range_min
float32 range_max
float32[] ranges
float32[] intensities
2、多回声雷达
在终端执行指令:
rosmsg show MultiEchoLaserScan
可以看到与 LaserScan 的结构基本一致的,不同点如下所示
sensor_msgs/LaserEcho[] ranges
float32[] echoes
sensor_msgs/LaserEcho[] intensities
float32[] echoes
普通的激光扫描消息(LaserScan)表述的是:每个激光脉冲的单个返回深度和强度值,如果返回了多个,通常只会选取其中强度最强的一个。
多回波传感器(MultiEchoLaserScan)不同的是,它能够为每个激光脉冲接收多个回波。例如,如果您扫描窗户,则通常会从玻璃以及玻璃后面的墙壁中接收到回波;如果在两个物体的边界处,则会收取到不同深度的回波。在创建地图和定位机器人的位置时,这些数据可以为您提供更多附加信息。因此,使用 MultiEchoLaserScan 的节点可以充分利用这种类型的传感器。
理解其为高级传感器,简单的说,每次激光打出,可以接受多个反馈信息,也就是多少深度值。
2、多线雷达
在终端执行指令:
rosmsg show PointCloud2
std_msgs/Header header
uint32 seq
time stamp
string frame_id
uint32 height
uint32 width
sensor_msgs/PointField[] fields
uint8 INT8=1
uint8 UINT8=2
uint8 INT16=3
uint8 UINT16=4
uint8 INT32=5
uint8 UINT32=6
uint8 FLOAT32=7
uint8 FLOAT64=8
string name
uint32 offset
uint8 datatype
uint32 count
bool is_bigendian
uint32 point_step
uint32 row_step
uint8[] data
bool is_dense
比如下面来看一帧多线点云的数据,再终端执行如下指令:
rostopic list
rostopic echo /rslidar_points > rslidar_points.txt
header:
seq: 7390
stamp:
secs: 1606808670
nsecs: 787610000
frame_id: "front_laser_link"
height: 16
width: 1032
fields:
-
name: "x"
offset: 0
datatype: 7
count: 1
-
name: "y"
offset: 4
datatype: 7
count: 1
-
name: "z"
offset: 8
datatype: 7
count: 1
-
name: "intensity"
offset: 16
datatype: 7
count: 1
is_bigendian: False
point_step: 32
row_step: 33024
data: [181, 159, 96, 64, 101, 243, 14, 191, 239, 125, 113, ......]
is_dense: False
根据上面的内容,可以知道每点云数据包含 x,y,z,intensity信息,其数据类型为 FLOAT32,也就是x,y,z,intensity各占用4个字节,共16个字节,共32(point_step)个beye数。使用小端存储,不含非法数据。height=16,可以知道其为16线雷达,每线(行) width=1032点云数据,共16x1032=16512个点云数据,共占用beyes数16512x32=528384。执行代码 msg->data.size() 可以直接获得共占用的beyes数。
四、函数重载
关于 HandleLaserScanMessage()、HandleMultiEchoLaserScanMessage()、HandlePointCloud2Message() 函数,注释如下:
void SensorBridge::HandleLaserScanMessage(
const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
carto::sensor::PointCloudWithIntensities point_cloud;
carto::common::Time time;
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}
void SensorBridge::HandleMultiEchoLaserScanMessage(
const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
carto::sensor::PointCloudWithIntensities point_cloud;
carto::common::Time time;
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}
void SensorBridge::HandlePointCloud2Message(
const std::string& sensor_id,
const sensor_msgs::PointCloud2::ConstPtr& msg) {
carto::sensor::PointCloudWithIntensities point_cloud;
carto::common::Time time;
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
HandleRangefinder(sensor_id, time, msg->header.frame_id, point_cloud.points);
}
可以看到其处理过程都是非常类似的,其核心都是调用 ToPointCloudWithIntensities() 函数把 msg 转换成 carto 算法需要的
carto::sensor::PointCloudWithIntensities 类型。但是这里注意,虽然其调用函数的的函数名都是 ToPointCloudWithIntensities(),但是其处理过程都是不一样的,因为其有三个重载函数,根据输入的数据类型,分别调用不同的函数,重载函数如下(src/cartographer_ros/cartographer_ros/cartographer_ros/msg_conversion.cc):
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) {
return LaserScanToPointCloudWithIntensities(msg);
}
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg) {
return LaserScanToPointCloudWithIntensities(msg);
}
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg){
......
......
}
这里需要注意,LaserScanToPointCloudWithIntensities()为模板函数(理解为高级重载),那么就先对齐进行讲解,然后再分析 ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) 吧
五、LaserScanToPointCloudWithIntensities()
该函数为模板函数,大家这里可能会觉得奇怪,上面代码再调用的时候,明明没有指定模板参数,却可以调用模板函数,这主要来自于编译器的自动模板参数推导。先来看一下代码注释,然后再对其中的重难点进行分析:
template <typename LaserMessageType>
std::tuple<PointCloudWithIntensities, ::cartographer::common::Time>
LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
CHECK_GE(msg.range_min, 0.f);
CHECK_GE(msg.range_max, msg.range_min);
if (msg.angle_increment > 0.f) {
CHECK_GT(msg.angle_max, msg.angle_min);
} else {
CHECK_GT(msg.angle_min, msg.angle_max);
}
PointCloudWithIntensities point_cloud;
float angle = msg.angle_min;
for (size_t i = 0; i < msg.ranges.size(); ++i) {
const auto& echoes = msg.ranges[i];
if (HasEcho(echoes)) {
const float first_echo = GetFirstEcho(echoes);
if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
const cartographer::sensor::TimedRangefinderPoint point{
rotation * (first_echo * Eigen::Vector3f::UnitX()),
i * msg.time_increment};
point_cloud.points.push_back(point);
if (msg.intensities.size() > 0) {
CHECK_EQ(msg.intensities.size(), msg.ranges.size());
const auto& echo_intensities = msg.intensities[i];
CHECK(HasEcho(echo_intensities));
point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));
} else {
point_cloud.intensities.push_back(0.f);
}
}
}
angle += msg.angle_increment;
}
相信大部分注释都是没有难度的,可能只有如下代码不太好理解而已:
const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
const cartographer::sensor::TimedRangefinderPoint point{
rotation * (first_echo * Eigen::Vector3f::UnitX()),
i * msg.time_increment};
首先需要了解到的是, 再ROS中,使用的通常都是右手坐标系,如下所示:
右
手
坐
标
系
:
\color{blue} 右手坐标系:
右手坐标系:也就是一般来说,雷达正前方为X+方向,左边为Y+方向,上面为Z+反向,逆时针旋转为正旋转,且通常以弧度为单位,范围在[-Π,Π]之间。
这里举一个例子,假设 first_echo=19.6152687,angle= -3.0089736,那么该点在雷达坐标系的占位置为
P
\mathbf P
P = (-19.4430275,-2.59373975, 0)。从俯视视角来看,那么如下图所示:
很明显的知道,
P
\mathbf P
P 其应该在的位置为第四象限(逆时针方向计算),可以简单通过
x
=
−
∣
P
∣
cos
(
Π
−
θ
)
x=-|\mathbf P|\cos(Π-\theta)
x=−∣P∣cos(Π−θ),
y
=
−
∣
P
∣
sin
(
Π
−
θ
)
y=-|\mathbf P|\sin(Π-\theta)
y=−∣P∣sin(Π−θ) 求得坐标,但是源码的实现是通过对
P
′
\mathbf P'
P′ 进行旋转,从而获得
P
\mathbf P
P 的坐标,大家可以使用勾股定理验证一下结果。
六、ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg)
最后,在来了解处理多线雷达 sensor_msgs::PointCloud2 类型数据的 ToPointCloudWithIntensities() 函数。
该函数根据 msg 中的字段,分成了四种情况进行处理,如下所示:
(
1
)
有
强
度
,
有
时
间
:
\color{blue} (1)有强度,有时间:
(1)有强度,有时间: 调用pcl中的fromROSMsg()接口,先把msg中的点云数据转换成 PointXYZIT(I表示强度,T表示有时间)类型全部存储于 pcl_point_cloud 变量中,然后对齐进行遍历:①利用点云的 x,y,z 加 time 构建 TimedRangefinderPoint 数据存放于 point_cloud.points 变量之中。②提取点云的强度信息存储于 point_cloud.intensities 变量之中。
(
2
)
有
强
度
,
无
时
间
:
\color{blue} (2)有强度,无时间:
(2)有强度,无时间: 调用pcl中的fromROSMsg()接口,先把msg中的点云数据转换成 PointXYZI(I表示强度)类型全部存储于 pcl_point_cloud 变量中,然后对其进行遍历:①利用点云的 x,y,z 加 time 构建 TimedRangefinderPoint 数据存放于 point_cloud.points 变量之中,其中 time 都设置为 0.f。②提取点云的强度信息存储于 point_cloud.intensities 变量之中。
(
3
)
无
强
度
,
有
时
间
:
\color{blue} (3)无强度,有时间:
(3)无强度,有时间: 调用pcl中的fromROSMsg()接口,先把msg中的点云数据转换成 PointXYZT(T表示时间)类型全部存储于 pcl_point_cloud 变量中,然后对其进行遍历:①利用点云的 x,y,z 加 time 构建 TimedRangefinderPoint 数据存放于 point_cloud.points 变量之中。②点云强度设置为 1.0f 存储于 point_cloud.intensities 变量之中。
(
4
)
无
强
度
,
无
时
间
:
\color{blue} (4)无强度,无时间:
(4)无强度,无时间: 调用pcl中的fromROSMsg()接口,先把msg中的点云数据转换成 PointXYZ类型全部存储于 pcl_point_cloud 变量中,然后对其进行遍历:时间设置为0.f,强度设置为 1.0f。
总
结
:
\color{blue} 总结:
总结:总的来说,没有时间,则时间设置为0.f,如果没有强度,则强度设置为1.0f,xyz保持不变,存储于 point_cloud 变量中。
另外,点云第一个数据时间为0,即最后一个数据的时间戳可以认为是点云帧的持续时间,这里把点云最后一个数据的时间作为整个点云的时间戳,所以其他的点时间戳应该是目前的时间戳,加上点云帧的持续时间。然后再验证一下点云时间是否正确,正常情况下每个点的时间应该都是小于最后一个数据点的时间(帧持续时间)。代码注释如下:
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
PointCloudWithIntensities point_cloud;
if (PointCloud2HasField(msg, "intensity")) {
if (PointCloud2HasField(msg, "time")) {
pcl::PointCloud<PointXYZIT> pcl_point_cloud;
pcl::fromROSMsg(msg, pcl_point_cloud);
point_cloud.points.reserve(pcl_point_cloud.size());
point_cloud.intensities.reserve(pcl_point_cloud.size());
for (const auto& point : pcl_point_cloud) {
point_cloud.points.push_back(
{Eigen::Vector3f{point.x, point.y, point.z}, point.time});
point_cloud.intensities.push_back(point.intensity);
}
}
else {
pcl::PointCloud<pcl::PointXYZI> pcl_point_cloud;
pcl::fromROSMsg(msg, pcl_point_cloud);
point_cloud.points.reserve(pcl_point_cloud.size());
point_cloud.intensities.reserve(pcl_point_cloud.size());
for (const auto& point : pcl_point_cloud) {
point_cloud.points.push_back(
{Eigen::Vector3f{point.x, point.y, point.z}, 0.f});
point_cloud.intensities.push_back(point.intensity);
}
}
}
else {
if (PointCloud2HasField(msg, "time")) {
pcl::PointCloud<PointXYZT> pcl_point_cloud;
pcl::fromROSMsg(msg, pcl_point_cloud);
point_cloud.points.reserve(pcl_point_cloud.size());
point_cloud.intensities.reserve(pcl_point_cloud.size());
for (const auto& point : pcl_point_cloud) {
point_cloud.points.push_back(
{Eigen::Vector3f{point.x, point.y, point.z}, point.time});
point_cloud.intensities.push_back(1.0f);
}
}
else {
pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
pcl::fromROSMsg(msg, pcl_point_cloud);
point_cloud.points.reserve(pcl_point_cloud.size());
point_cloud.intensities.reserve(pcl_point_cloud.size());
for (const auto& point : pcl_point_cloud) {
point_cloud.points.push_back(
{Eigen::Vector3f{point.x, point.y, point.z}, 0.f});
point_cloud.intensities.push_back(1.0f);
}
}
}
::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
if (!point_cloud.points.empty()) {
const double duration = point_cloud.points.back().time;
timestamp += cartographer::common::FromSeconds(duration);
for (auto& point : point_cloud.points) {
point.time -= duration;
CHECK_LE(point.time, 0.f)
<< "Encountered a point with a larger stamp than "
"the last point in the cloud.";
}
}
return std::make_tuple(point_cloud, timestamp);
}
七、HandleLaserScan
通过该篇博客,已经对雷达预处理过程进行了详细的分析,也就是如下三个函数:
SensorBridge::HandleLaserScanMessage()
SensorBridge::HandleMultiEchoLaserScanMessage()
SensorBridge::HandlePointCloud2Message()
他们有一个共同点,那就是调用:
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
函数,把点云数据都转换成 cartographer::sensor::PointCloudWithIntensities 数据,以及一个时间戳的格式。共同表示为 std::tie(point_cloud, time),但是接下来有一小部分没有讲解,如下所示
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
HandleRangefinder(sensor_id, time, msg->header.frame_id, point_cloud.points);
该函数就比较简单了,就是把雷达点云数据转换到 tracking_frame=“img” 坐标系下。先看HandleLaserScan() 函数:
void SensorBridge::HandleLaserScan(
const std::string& sensor_id, const carto::common::Time time,
const std::string& frame_id,
const carto::sensor::PointCloudWithIntensities& points) {
if (points.points.empty()) {
return;
}
CHECK_LE(points.points.back().time, 0.f);
for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
const size_t start_index =
points.points.size() * i / num_subdivisions_per_laser_scan_;
const size_t end_index =
points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
carto::sensor::TimedPointCloud subdivision(
points.points.begin() + start_index, points.points.begin() + end_index);
if (start_index == end_index) {
continue;
}
const double time_to_subdivision_end = subdivision.back().time;
const carto::common::Time subdivision_time =
time + carto::common::FromSeconds(time_to_subdivision_end);
auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
if (it != sensor_to_previous_subdivision_time_.end() &&
it->second >= subdivision_time) {
LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
<< sensor_id << " because previous subdivision time "
<< it->second << " is not before current subdivision time "
<< subdivision_time;
continue;
}
sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
for (auto& point : subdivision) {
point.time -= time_to_subdivision_end;
}
CHECK_EQ(subdivision.back().time, 0.f);
HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
}
}
其原理函数比较简单的,即使根据 .lua 文件中 num_subdivisions_per_laser_scan 参数,把一帧点云分成几段数据,同时更新每段点数据的时间戳。然后调用 HandleRangefinder() 函数。
八、结语
对于 HandleRangefinder() 函数,其把基于雷达的传感器的点云数据坐标系变换到 tracking_frame=“imu” 坐标系下,代码如下
void SensorBridge::HandleRangefinder(
const std::string& sensor_id, const carto::common::Time time,
const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
if (!ranges.empty()) {
CHECK_LE(ranges.back().time, 0.f);
}
const auto sensor_to_tracking =
tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
if (sensor_to_tracking != nullptr) {
trajectory_builder_->AddSensorData(
sensor_id, carto::sensor::TimedPointCloudData{
time,
sensor_to_tracking->translation().cast<float>(),
carto::sensor::TransformTimedPointCloud(
ranges, sensor_to_tracking->cast<float>())} );
}
}
那么到这里位置,可以说对于雷达点云数据预处理的预处理都讲解完成了。大家再回过头来看 sensor_bridge.h 这个文件,可以发现其包含的函数于变量都讲解完毕了,
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)