LeGo-LOAM源码阅读笔记(五)---transformFusion.cpp
2020-08-17 16:31
3471 查看
文章目录
1.transformFusion节点框架
1.1 main函数
int main(int argc, char** argv) { ros::init(argc, argv, "lego_loam"); TransformFusion TFusion;//构造函数,重要的两个回调函数 ROS_INFO("\033[1;32m---->\033[0m Transform Fusion Started."); ros::spin(); return 0; }
1.2 transformFusion构造函数
//订阅话题: "/laser_odom_to_init"(nav_msgs::Odometry),数据处理函数laserOdometryHandler "/aft_mapped_to_init"(nav_msgs::Odometry),数据处理函数odomAftMappedHandler //发布话题,这些topic有: "/integrated_to_init"(nav_msgs::Odometry)
2.回调函数
2.1 odomAftMappedHandler
通过
odomAftMappedHandler函数获取精配准后的位姿作为transformAftMapped,而获取配准后的速度信息作为transformBefMapped准备下一次计算。
//通过odomAftMappedHandler函数获取精配准后的位姿作为transformAftMapped,而获取配准后的速度信息作为transformBefMapped准备下一次计算 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; } };
2.2 laserOdometryHandler
laserOdometryHandler是将粗配准的里程计信息与精配准的里程计信息融合计算,并在回调函数中便发送了最终外发的里程计话题。在该回调函数中的TF与里程计话题才是最终决定的。
//laserOdometryHandler是将粗配准的里程计信息与精配准的里程计信息融合计算,并在回调函数中便发送了最终外发的里程计话题 void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry) { currentHeader = laserOdometry->header; 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(); 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]; pubLaserOdometry2.publish(laserOdometry2); 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])); tfBroadcaster2.sendTransform(laserOdometryTrans2); }
3.总结
粗配准的里程计信息是
FeatureAssociation发出的,精配准的信息是mapOptimization发出的,均以200Hz的频率,当odomAftMappedHandler收到精配准信息后更新位姿,这个位姿将在laserOdometryHandler收到下一条粗配准信息后综合计算再发出。
相关文章推荐
- Caffe 源码阅读笔记 [基本模块] Caffe.cpp
- CCLabelIBMFont.cpp 源码阅读笔记
- JAVA 集合类(java.util)源码阅读笔记------Vector
- memcached源码阅读笔记(二)
- Linux TCP/IP协议栈源码阅读笔记
- curator-client源码阅读笔记
- winvnc源码阅读笔记(三)---------vncClient::SendUpdate线程
- caffe源码阅读(一)---examples/cpp_classification/classification.cpp
- PostgreSql源码阅读笔记4(参考husthxd在ITPUB的博客)
- Caffe 源码阅读笔记 [基本模块] Solver
- 源码阅读笔记 - 3 std::string 与 Short String Optimization
- Three.js源码阅读笔记(Object3D类)
- Nginx 源码阅读笔记7 worker 主循环
- Delta3D 源码阅读笔记
- Yii源码阅读笔记 - 日志组件
- CI框架源码阅读笔记5 基准测试 BenchMark.php
- CI框架源码阅读笔记6 扩展钩子 Hook.php
- Netty源码阅读笔记1:ChannelPipeline责任链模式
- Java Apache CollectionUtils 源码阅读笔记,注意3.2版本bug
- java.lang.String源码阅读笔记