ROS-3DSLAM（5）：雷达部分交流分析A

最新推荐文章于 2022-12-03 13:21:09 发布

VIP文章 Lukeeeeeee！

最新推荐文章于 2022-12-03 13:21:09 发布

阅读量319

点赞数

分类专栏： ros 文章标签： 3d

本文链接：https://blog.csdn.net/m0_49088083/article/details/121047129

版权

2021@SDUSC
2021年10月26日星期二——2021年10月30日星期六

一、计划简介：

通过前几次的学习，我掌握了lvi-sam包的信息流、结构、功能，还着重分析了特征提取featureExtracted的代码。

这一周，我决定在分析图因子优化包之前，先来学习一下组内其他同学的研究成果，以方便接下来的进一步学习研究，更有效地上手后续地代码。同时，也可以借鉴其他同学们阅读代码的思路和方法，提高自己的知识技能。

我这一次的学习方式和上一次的信息流分析类似，也是借助博客注解加源代码的形式进行，以辅助自己快速上手理解。

二、阅读分析：

因为小组内的其他同学已经将整个包里的代码都解析了，我在这里如果再解析一遍的话也没有什么意义，倒不如把自己的遇到的问题和理解写在这里，这样才更有意义。

这周一的小组讨论中，我发现我对于信息流的部分的理解和同学的理解也存在一定的偏差，这一周也要再好好再处理一下有关于信息流的问题。

1、unity包：头文件，定义

这一个头文件主要功能为：引入头文件、定义类和方法。

引用的比较重要头文件为点云库PCL:

点云库（PCL）是一个独立的、大规模的、开放的项目，用于2D/3D图像和点云处理。PCL是根据BSD许可条款发布的，因此可免费用于商业和研究用途

而定义中比较重要的的类为paramSever，方法为imuConverter，此外，一些工具方法也应该掌握含义，以阅读其他的代码。

//工具函数代码：不难看出，这些工具函数的主要功能为imu信息和点云信息之间的转化以及获取ros时间、计算点云上信息的距离等等
template<typename T>
sensor_msgs::PointCloud2 publishCloud(ros::Publisher *thisPub, T 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;
}

template<typename T>
double ROS_TIME(T msg)
{
   
    return msg->header.stamp.toSec();
}


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;
}


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;
}


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));
}

2、imageProjection 生成图节点

这个节点是用来对输入imu信息以及里程计信息做分析，生成相应的深度图的。

开始就是定义结构、初始化、订阅消息等。

重点是cloudHandler。

如同我分析的特征提取功能一样，这里的cloudhandler也有六个重要的函数，实现了他的功能。

分别是：

cachePointCloud
deskewInfo
projectPointCloud
cloudExtraction
publishClouds
resetParameters

问题：

时间戳：timestamp
数学概念: 四元数；欧拉角roll，pitch，yaw；轴向角；旋转矩阵
deskewInfo() 函数
这里最重要的功能是激光运动畸变矫正，实现这个功能的函数是deskewPoint()函数：

原理可参照ly同学的第七篇博客，里面的介绍解决了我的困惑

tf结构：相当于坐标系记录点，这里定义的变量都是tf包内的。

值得注意的是，这个quataernion正是所谓的“四元数”。

 // 提取imu里程计姿态角
tf::Quaternion orientation;
tf::quaternionMsgToTF(startOdomMsg.pose.pose.orientation, orientation);
double roll, pitch, yaw;
tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

仿射变换 ?

总结：

这个文件下的功能简单地来说就是对于imu数据、里程计数据的处理，得到符合要求的数据，然后将得到的这些纠偏数据交给激光帧点云进行纠偏，得到最后正确的结果。

在这些函数当中，最重要的是deskewInfo() 函数，他完成了本文件的大部分主要功能。

3、imuPreintegration imu预积分节点

这个节点是用来完成imu预积分的。

虽然我之前已经初步了解了imu的知识，但是小组内同学的博客在这方面又加深了我对于这一概念的理解，让我意识到自己在这方面还有许多知识要补充。

这个代码中比较重要的是两个回调函数：imuHandler和odometryHandler。

而且这两者都同第三方库gtsam有着极深的联系。

问题：

gtsam库：GTSAM 4.0 is a BSD-licensed C++ library that implements sensor fusion for robotics and computer vision applications, including SLAM (Simultaneous Localization and Mapping), VO (Visual Odometry), and SFM (Structure from Motion). It uses factor graphs and Bayes networks as the underlying computing paradigm rather than sparse matrices to optimize for the most probable configuration or an optimal plan. Coupled with a capable sensor front-end (not provided here), GTSAM powers many impressive autonomous systems, in both academia and industry.

这是一个比较重要的库，在这一篇代码中占据了相当的分量。
imu预积分：

imu采样频率高->数据量大->减少处理的数据->每隔一段时间提取一个数据

但是，这样做有一个问题：

但是这样在做后端优化的过程中，当进行迭代求解计算来更新和调整PVQ的值时，一旦（比如第 1 秒）的PVQ进行了调整，每一个中间过程以及后面所有的轨迹都要重新再积分算一遍，如果是100Hz，两秒之间有100个采集数据，就要计算100次积分。

而我们进行预积分的目的就是简化这一过程，将这一过程中的100次积分变成1次。

此外，还需要解决简化过程而造成的不准确问题——预积分误差。

tf包下的：

// map -> odom
tf::Transform map_to_odom;
tf::TransformBroadcaster tfMap2Odom;
// odom -> base_link
tf::TransformBroadcaster tfOdom2BaseLink;

可以理解：机器人的里程计的信息 = 当前地图中的机器人的的位置减去地图中机器人的起点位置。

iSAM2

imuHandler:

imuHandler的功能并不复杂，就是接受从imu得到原始数据进行处理，thisImu。

然后利用时间戳信息在imu预积分器中加入该帧。

然后利用上一帧中的激光里程计时刻对应的状态和偏差，加入当前帧的预测，得到当前时刻的状态。

核心功能实现在中间部分的gtsam。

void imuHandler(const sensor_msgs::Imu::ConstPtr& imuMsg)
    {
     
        sensor_msgs::Imu thisImu = imuConverter(*imuMsg);
        // publish static tf
        tfMap2Odom.sendTransform(tf::StampedTransform(map_to_odom, thisImu.header.stamp, "map", "odom"));

        imuQueOpt.push_back(thisImu);
        imuQueImu.push_back(thisImu);

        if

最低0.47元/天解锁文章

Lukeeeeeee！

关注

0
点赞
踩
2

收藏

觉得还不错? 一键收藏
0
评论
ROS-3DSLAM（5）：雷达部分交流分析A

2021@SDUSC2021年10月26日星期二——2021年10月30日星期六一、计划简介：通过前几次的学习，我掌握了lvi-sam包的信息流、结构、功能，还着重分析了特征提取featureExtracted的代码。这一周，我决定在分析图因子优化包之前，先来学习一下组内其他同学的研究成果，以方便接下来的进一步学习研究，更有效地上手后续地代码。同时，也可以借鉴其他同学们阅读代码的思路和方法，提高自己的知识技能。我这一次的学习方式和上一次的信息流分析类似，也是借助博客注解加源代码的
复制链接

扫一扫