LeGo-LOAM激光雷达定位算法源码阅读(四)
文章目录
- 1.transforFusion节点框架
- 1.1main
- 1.2回调函数
- 2.odomAftMappedHandler
- 3.odomAftMappedHandler
- 3.总结
无人驾驶算法学习（九）：LeGo-LOAM激光雷达定位算法
LeGo-LOAM激光雷达定位算法源码阅读(一)
LeGo-LOAM激光雷达定位算法源码阅读(二)
LeGo-LOAM激光雷达定位算法源码阅读(三)
1.transforFusion节点框架
1.1main
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(){
pubLaserOdometry2 = nh.advertise ("/integrated_to_init", 5);////综合后发送的里程计信息
subLaserOdometry = nh.subscribe("/laser_odom_to_init", 5, &TransformFusion::laserOdometryHandler, this);//回调函数1,订阅特征匹配时粗配准的里程计信息
subOdomAftMapped = nh.subscribe("/aft_mapped_to_init", 5, &TransformFusion::odomAftMappedHandler, this);//回调函数2,订阅建图精配准之后的里程计信息
laserOdometry2.header.frame_id = "/camera_init";
laserOdometry2.child_frame_id = "/camera";
laserOdometryTrans2.frame_id_ = "/camera_init";
laserOdometryTrans2.child_frame_id_ = "/camera";
map_2_camera_init_Trans.frame_id_ = "/map";
map_2_camera_init_Trans.child_frame_id_ = "/camera_init";
camera_2_base_link_Trans.frame_id_ = "/camera";
camera_2_base_link_Trans.child_frame_id_ = "/base_link";
for (int i = 0; i < 6; ++i)
{
transformSum[i] = 0;
transformIncre[i] = 0;
transformMapped[i] = 0;
transformBefMapped[i] = 0;
transformAftMapped[i] = 0;
}
}
2.odomAftMappedHandler
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr& laserOdometry)
{
//laserOdometryHandler是将粗配准的里程计信息与精配准的里程计信息融合计算，并在回调函数中便发送了最终外发的里程计话题
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.odomAftMappedHandler
void odomAftMappedHandler(const nav_msgs::Odometry::ConstPtr& odomAftMapped)
{
//通过odomAftMappedHandler函数获取精配准后的位姿作为transformAftMapped，而获取配准后的速度信息作为transformBefMapped准备下一次计算
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;
}
3.总结
featureAssociation发出粗配准的里程计信息(/laser_odom_to_init)，mapOptimization发出精配准的信息(/aft_mapped_to_init)，均以200Hz的频率，当odomAftMappedHandler收到精配准信息后更新位姿，这个位姿将在laserOdometryHandler收到下一条粗配准信息后综合计算再发出,图示如下:
