loam源码解析8 ：transformMaintenance
2021/6/20 11:52:04
本文主要是介绍loam源码解析8 ：transformMaintenance，对大家解决编程问题具有一定的参考价值，需要的程序猿们随着小编来一起学习吧！
transformMaintenance.cpp解析
- 一、概述
- 二、变量说明
- 三、信息接受处理
- 1. transformAssociateToMap位姿融合
- 2. laserOdometryHandler里程计信息处理
- 3. odomAftMappedHandler地图信息处理
- 三、信息发送处理
- 四、结语
loam源码地址： https://github.com/cuitaixiang/LOAM_NOTED.
论文学习： LOAM: Lidar Odometry and Mapping in Real-time 论文阅读.
loam源码解析汇总：
loam源码解析1 ： scanRegistration（一）.
loam源码解析2 ： scanRegistration（二）.
loam源码解析3 ： laserOdometry（一）.
loam源码解析4 ： laserOdometry（二）.
loam源码解析5 ： laserOdometry（三）.
loam源码解析6 ： laserMapping（一）.
loam源码解析7 ： laserMapping（二）.
loam源码解析8 ： transformMaintenance.
一、概述
至此，我们已经完成了前三个部分的代码分析，进入到了最后一个部分transformMaintenance，这一部分也非常简单没有什么算法和细节难点，主要是接受信息和发布信息，废话少说，直接看代码。
二、变量说明
laserOdometry中传来的里程计信息（高频）：
//odometry计算的转移矩阵(实时高频量)
float transformSum[6] = {0};
//平移增量
float transformIncre[6] = {0};
laserMapping中传来的里程计信息（低频），优化后的里程计信息，地图：
//经过mapping矫正过后的最终的世界坐标系下的位姿
float transformMapped[6] = {0};
//mapping传递过来的优化前的位姿
float transformBefMapped[6] = {0};
//mapping传递过来的优化后的位姿
float transformAftMapped[6] = {0};
定义ROS和TF的发布：
ros::Publisher *pubLaserOdometry2Pointer = NULL; tf::TransformBroadcaster *tfBroadcaster2Pointer = NULL; nav_msgs::Odometry laserOdometry2; tf::StampedTransform laserOdometryTrans2;
三、信息接受处理
int main(int argc, char** argv)
{
ros::init(argc, argv, "transformMaintenance");
ros::NodeHandle nh;
ros::Subscriber subLaserOdometry = nh.subscribe<nav_msgs::Odometry>
("/laser_odom_to_init", 5, laserOdometryHandler);
ros::Subscriber subOdomAftMapped = nh.subscribe<nav_msgs::Odometry>
("/aft_mapped_to_init", 5, odomAftMappedHandler);
1. transformAssociateToMap位姿融合
这一部分与loam源码解析6 ： laserMapping（一）中的transformAssociateToMap（）函数几乎一模一样，所实现的功能是odometry的运动估计和mapping矫正量融合之后得到的最终的位姿transformMapped，唯一的区别就是在laserMapping中得到的是初始估计的
T
ˉ
k
W
(
t
k
+
1
)
\bar T^W_{k}(t_{k+1})
TˉkW(tk+1),而在这里得到的是laserMapping迭代优化后的
T
k
W
(
t
k
+
1
)
T^W_{k}(t_{k+1})
TkW(tk+1)，所以不再赘述。
//odometry的运动估计和mapping矫正量融合之后得到的最终的位姿transformMapped
void transformAssociateToMap()
{
//平移后绕y轴旋转（-transformSum[1]）
float x1 = cos(transformSum[1]) * (transformBefMapped[3] - transformSum[3])
- sin(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
float y1 = transformBefMapped[4] - transformSum[4];
float z1 = sin(transformSum[1]) * (transformBefMapped[3] - transformSum[3])
+ cos(transformSum[1]) * (transformBefMapped[5] - transformSum[5]);
//绕x轴旋转（-transformSum[0]）
float x2 = x1;
float y2 = cos(transformSum[0]) * y1 + sin(transformSum[0]) * z1;
float z2 = -sin(transformSum[0]) * y1 + cos(transformSum[0]) * z1;
//绕z轴旋转（-transformSum[2]）
transformIncre[3] = cos(transformSum[2]) * x2 + sin(transformSum[2]) * y2;
transformIncre[4] = -sin(transformSum[2]) * x2 + cos(transformSum[2]) * y2;
transformIncre[5] = z2;
float sbcx = sin(transformSum[0]);
float cbcx = cos(transformSum[0]);
float sbcy = sin(transformSum[1]);
float cbcy = cos(transformSum[1]);
float sbcz = sin(transformSum[2]);
float cbcz = cos(transformSum[2]);
float sblx = sin(transformBefMapped[0]);
float cblx = cos(transformBefMapped[0]);
float sbly = sin(transformBefMapped[1]);
float cbly = cos(transformBefMapped[1]);
float sblz = sin(transformBefMapped[2]);
float cblz = cos(transformBefMapped[2]);
float salx = sin(transformAftMapped[0]);
float calx = cos(transformAftMapped[0]);
float saly = sin(transformAftMapped[1]);
float caly = cos(transformAftMapped[1]);
float salz = sin(transformAftMapped[2]);
float calz = cos(transformAftMapped[2]);
float srx = -sbcx*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz)
- cbcx*sbcy*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
- cbcx*cbcy*(calx*salz*(cblz*sbly - cbly*sblx*sblz)
- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx);
transformMapped[0] = -asin(srx);
float srycrx = sbcx*(cblx*cblz*(caly*salz - calz*salx*saly)
- cblx*sblz*(caly*calz + salx*saly*salz) + calx*saly*sblx)
- cbcx*cbcy*((caly*calz + salx*saly*salz)*(cblz*sbly - cbly*sblx*sblz)
+ (caly*salz - calz*salx*saly)*(sbly*sblz + cbly*cblz*sblx) - calx*cblx*cbly*saly)
+ cbcx*sbcy*((caly*calz + salx*saly*salz)*(cbly*cblz + sblx*sbly*sblz)
+ (caly*salz - calz*salx*saly)*(cbly*sblz - cblz*sblx*sbly) + calx*cblx*saly*sbly);
float crycrx = sbcx*(cblx*sblz*(calz*saly - caly*salx*salz)
- cblx*cblz*(saly*salz + caly*calz*salx) + calx*caly*sblx)
+ cbcx*cbcy*((saly*salz + caly*calz*salx)*(sbly*sblz + cbly*cblz*sblx)
+ (calz*saly - caly*salx*salz)*(cblz*sbly - cbly*sblx*sblz) + calx*caly*cblx*cbly)
- cbcx*sbcy*((saly*salz + caly*calz*salx)*(cbly*sblz - cblz*sblx*sbly)
+ (calz*saly - caly*salx*salz)*(cbly*cblz + sblx*sbly*sblz) - calx*caly*cblx*sbly);
transformMapped[1] = atan2(srycrx / cos(transformMapped[0]),
crycrx / cos(transformMapped[0]));
float srzcrx = (cbcz*sbcy - cbcy*sbcx*sbcz)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)
- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)
- (cbcy*cbcz + sbcx*sbcy*sbcz)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
+ cbcx*sbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);
float crzcrx = (cbcy*sbcz - cbcz*sbcx*sbcy)*(calx*calz*(cbly*sblz - cblz*sblx*sbly)
- calx*salz*(cbly*cblz + sblx*sbly*sblz) + cblx*salx*sbly)
- (sbcy*sbcz + cbcy*cbcz*sbcx)*(calx*salz*(cblz*sbly - cbly*sblx*sblz)
- calx*calz*(sbly*sblz + cbly*cblz*sblx) + cblx*cbly*salx)
+ cbcx*cbcz*(salx*sblx + calx*cblx*salz*sblz + calx*calz*cblx*cblz);
transformMapped[2] = atan2(srzcrx / cos(transformMapped[0]),
crzcrx / cos(transformMapped[0]));
x1 = cos(transformMapped[2]) * transformIncre[3] - sin(transformMapped[2]) * transformIncre[4];
y1 = sin(transformMapped[2]) * transformIncre[3] + cos(transformMapped[2]) * transformIncre[4];
z1 = transformIncre[5];
x2 = x1;
y2 = cos(transformMapped[0]) * y1 - sin(transformMapped[0]) * z1;
z2 = sin(transformMapped[0]) * y1 + cos(transformMapped[0]) * z1;
transformMapped[3] = transformAftMapped[3]
- (cos(transformMapped[1]) * x2 + sin(transformMapped[1]) * z2);
transformMapped[4] = transformAftMapped[4] - y2;
transformMapped[5] = transformAftMapped[5]
- (-sin(transformMapped[1]) * x2 + cos(transformMapped[1]) * z2);
}
2. laserOdometryHandler里程计信息处理
将接受到的里程计信息姿态分解成旋转和平移：
//接收laserOdometry的信息
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{
double roll, pitch, yaw;
geometry_msgs::Quaternion geoQuat = laserOdometry->pose.pose.orientation;
tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
//得到旋转平移矩阵
transformSum[0] = -pitch;
transformSum[1] = -yaw;
transformSum[2] = roll;
transformSum[3] = laserOdometry->pose.pose.position.x;
transformSum[4] = laserOdometry->pose.pose.position.y;
transformSum[5] = laserOdometry->pose.pose.position.z;
将姿态从当前帧坐标系转换到地图坐标系：
transformAssociateToMap();
将转换到地图坐标系后的当前位姿ROS发布：
geoQuat = tf::createQuaternionMsgFromRollPitchYaw
(transformMapped[2], -transformMapped[0], -transformMapped[1]);
laserOdometry2.header.stamp = laserOdometry->header.stamp;
laserOdometry2.pose.pose.orientation.x = -geoQuat.y;
laserOdometry2.pose.pose.orientation.y = -geoQuat.z;
laserOdometry2.pose.pose.orientation.z = geoQuat.x;
laserOdometry2.pose.pose.orientation.w = geoQuat.w;
laserOdometry2.pose.pose.position.x = transformMapped[3];
laserOdometry2.pose.pose.position.y = transformMapped[4];
laserOdometry2.pose.pose.position.z = transformMapped[5];
pubLaserOdometry2Pointer->publish(laserOdometry2);
将转换到地图坐标系后的当前位姿TF发布：
//发送旋转平移量 laserOdometryTrans2.stamp_ = laserOdometry->header.stamp; laserOdometryTrans2.setRotation(tf::Quaternion(-geoQuat.y, -geoQuat.z, geoQuat.x, geoQuat.w)); laserOdometryTrans2.setOrigin(tf::Vector3(transformMapped[3], transformMapped[4], transformMapped[5])); tfBroadcaster2Pointer->sendTransform(laserOdometryTrans2); }
3. odomAftMappedHandler地图信息处理
接受来自laserMapping中优化后的位姿
T
ˉ
k
W
(
t
k
+
1
)
\bar T^W_{k}(t_{k+1})
TˉkW(tk+1)：
//接收laserMapping的转换信息
void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped)
{
double roll, pitch, yaw;
geometry_msgs::Quaternion geoQuat = odomAftMapped->pose.pose.orientation;
tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw);
transformAftMapped[0] = -pitch;
transformAftMapped[1] = -yaw;
transformAftMapped[2] = roll;
transformAftMapped[3] = odomAftMapped->pose.pose.position.x;
transformAftMapped[4] = odomAftMapped->pose.pose.position.y;
transformAftMapped[5] = odomAftMapped->pose.pose.position.z;
transformBefMapped[0] = odomAftMapped->twist.twist.angular.x;
transformBefMapped[1] = odomAftMapped->twist.twist.angular.y;
transformBefMapped[2] = odomAftMapped->twist.twist.angular.z;
transformBefMapped[3] = odomAftMapped->twist.twist.linear.x;
transformBefMapped[4] = odomAftMapped->twist.twist.linear.y;
transformBefMapped[5] = odomAftMapped->twist.twist.linear.z;
}
三、信息发送处理
主要是定义了两个发布的实例：
ros::Publisher pubLaserOdometry2 = nh.advertise<nav_msgs::Odometry> ("/integrated_to_init", 5);
pubLaserOdometry2Pointer = &pubLaserOdometry2;
laserOdometry2.header.frame_id = "/camera_init";
laserOdometry2.child_frame_id = "/camera";
tf::TransformBroadcaster tfBroadcaster2;
tfBroadcaster2Pointer = &tfBroadcaster2;
laserOdometryTrans2.frame_id_ = "/camera_init";
laserOdometryTrans2.child_frame_id_ = "/camera";
ros::spin();
return 0;
}
四、结语
终于，我写到了这里，如果你也看到了这里，那么恭喜你，也恭喜我。loam的学习也算告一段落了，我把这个学习过程记录在博客的原因就是方便查看，希望能够经常翻翻，但我也希望能够不用再翻拉～～～怕什么真理无穷，进一寸有一寸的欢喜，LeGO-LOAM，go！
这篇关于loam源码解析8 ：transformMaintenance的文章就介绍到这儿，希望我们推荐的文章对大家有所帮助，也希望大家多多支持为之网！
- 2024-09-28pyqt 怎么打包整个项目-icode9专业技术文章分享
- 2024-09-28laravel Commands 创建带有参数的 Artisan 命令的步骤和示例-icode9专业技术文章分享
- 2024-09-28antd怎么实现渲染tiff图片-icode9专业技术文章分享
- 2024-09-28英文半角中划线和中文全角的中划线有什么区别-icode9专业技术文章分享
- 2024-09-28nvm npm 和node 他们之间有什么关系-icode9专业技术文章分享
- 2024-09-28Node Version Manager (nvm)使用教程-icode9专业技术文章分享
- 2024-09-28nvm命令太慢,是什么原因-icode9专业技术文章分享
- 2024-09-28Kotlin 如何增加、删除和修改 MutableStateFlow 中的值。-icode9专业技术文章分享
- 2024-09-28Kotlin的stateFlow.update 写法介绍-icode9专业技术文章分享
- 2024-09-28kotlin 怎么获取当前时间格式-icode9专业技术文章分享
