AMCL代码详解(二)位姿初始化
AMCL的初始位姿估计可以来自于两个地方:
1.用粒子滤波算法估计出的机器人位姿作为最新的位姿
在AmclNode::AmclNode() 中参数设置完后首先调用了这么一个函数:
updatePoseFromServer();
跳到该函数看一下:
void AmclNode::updatePoseFromServer()
{init_pose_[0] = 0.0;init_pose_[1] = 0.0;init_pose_[2] = 0.0;init_cov_[0] = 0.5 * 0.5;init_cov_[1] = 0.5 * 0.5;init_cov_[2] = (M_PI/12.0) * (M_PI/12.0);// Check for NAN on input from param server, #5239double tmp_pos;//设置初始位姿,该位姿来自于上一次退出时存储的位姿,如果没有参数则默认为0.//将initial_pose_x赋值给tmp_pos,没有时使用默认值init_pose_[0]。private_nh_.param("initial_pose_x", tmp_pos, init_pose_[0]);if(!std::isnan(tmp_pos))init_pose_[0] = tmp_pos;else ROS_WARN("ignoring NAN in initial pose X position");private_nh_.param("initial_pose_y", tmp_pos, init_pose_[1]);if(!std::isnan(tmp_pos))init_pose_[1] = tmp_pos;elseROS_WARN("ignoring NAN in initial pose Y position");private_nh_.param("initial_pose_a", tmp_pos, init_pose_[2]);if(!std::isnan(tmp_pos))init_pose_[2] = tmp_pos;elseROS_WARN("ignoring NAN in initial pose Yaw");private_nh_.param("initial_cov_xx", tmp_pos, init_cov_[0]);if(!std::isnan(tmp_pos))init_cov_[0] =tmp_pos;elseROS_WARN("ignoring NAN in initial covariance XX");private_nh_.param("initial_cov_yy", tmp_pos, init_cov_[1]);if(!std::isnan(tmp_pos))init_cov_[1] = tmp_pos;elseROS_WARN("ignoring NAN in initial covariance YY");private_nh_.param("initial_cov_aa", tmp_pos, init_cov_[2]);if(!std::isnan(tmp_pos))init_cov_[2] = tmp_pos;elseROS_WARN("ignoring NAN in initial covariance AA");
}
这个函数设置了一个位姿以及一个方差,位姿参数来自于:"initial_pose_x"以及"initial_pose_y"等,这个参数在另外一个函数:
//这里只是把最新的odom位姿转换成最新的地图位姿去存储,即初始位姿map_pose,我们也对last_published_pose取其协方差,
//作为初始位姿map_pose的协方差。记住last_published_pose很重要的哦!它一般来源于上一次粒子滤波算法估计出的机器人位置。
void AmclNode::savePoseToServer()
{// We need to apply the last transform to the latest odom pose to get// the latest map pose to store. We'll take the covariance from// last_published_pose.//这里的tf2有点不太好理解,似乎最后存储了一个世界坐标系下的坐标,但是它初始应该是从odom变换过去的还是怎么得到的???tf2::Transform odom_pose_tf2;tf2::convert(latest_odom_pose_.pose, odom_pose_tf2);tf2::Transform map_pose = latest_tf_.inverse() * odom_pose_tf2;//获取坐标的角度double yaw = tf2::getYaw(map_pose.getRotation());ROS_DEBUG("Saving pose to server. x: %.3f, y: %.3f", map_pose.getOrigin().x(), map_pose.getOrigin().y() );//获取坐标的xy值private_nh_.setParam("initial_pose_x", map_pose.getOrigin().x());private_nh_.setParam("initial_pose_y", map_pose.getOrigin().y());private_nh_.setParam("initial_pose_a", yaw);//获取协方差值?private_nh_.setParam("initial_cov_xx", last_published_pose.pose.covariance[6*0+0]);private_nh_.setParam("initial_cov_yy", last_published_pose.pose.covariance[6*1+1]);private_nh_.setParam("initial_cov_aa", last_published_pose.pose.covariance[6*5+5]);
}
这里主要用到了tf2的一些知识,将map_pose的位姿赋值给了上述几个参数。具体的tf2变换可以参考ROS官网对于tf2的描述。
这里其实赋值完后就没事了,但是在后面:
if(use_map_topic_) {map_sub_ = nh_.subscribe("map", 1, &AmclNode::mapReceived, this);ROS_INFO("Subscribed to map topic.");} else {//所以这里调用了map_server节点requestMap();//请求静态地图,调用map_server节点}
默认use_map_topic_=false所以调用了requestMap();函数。
AmclNode::requestMap()
{boost::recursive_mutex::scoped_lock ml(configuration_mutex_);// get map via RPC GetMap服务nav_msgs::GetMap::Request req;nav_msgs::GetMap::Response resp;ROS_INFO("Requesting the map...");while(!ros::service::call("static_map", req, resp)){ROS_WARN("Request for map failed; trying again...");ros::Duration d(0.5);d.sleep();}handleMapMessage( resp.map );
}
这里转到了:
AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg)
函数处理,注意到在这个函数中有一段代码:
updatePoseFromServer();pf_vector_t pf_init_pose_mean = pf_vector_zero();pf_init_pose_mean.v[0] = init_pose_[0];pf_init_pose_mean.v[1] = init_pose_[1];pf_init_pose_mean.v[2] = init_pose_[2];pf_matrix_t pf_init_pose_cov = pf_matrix_zero();pf_init_pose_cov.m[0][0] = init_cov_[0];pf_init_pose_cov.m[1][1] = init_cov_[1];pf_init_pose_cov.m[2][2] = init_cov_[2];pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);pf_init_ = false;
这里跟前面的位姿初始化关联起来了。这个位姿最后是赋值给了pf_init_pose_mean,这是init_pose_唯一被使用到的地方。pf_init_pose_mean的值再次被传参到:
pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
函数对粒子进行初始化。也就是说算法通过该方法初始化的粒子的位姿最初是来自于AmclNode::savePoseToServer函数的。
2.订阅话题获得初始位姿
除了从上述方法获得位姿外,算法还可以通过订阅话题的方式获得位姿。函数中订阅了一个位姿话题:
initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);
回调函数initialPoseReceived调用了另外一个函数handleInitialPoseMessage去处理了这个消息:
AmclNode::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg)
{handleInitialPoseMessage(*msg);
}
进入到handleInitialPoseMessage函数,可以看到其主要做了一件事情:
将位姿赋值给变量initial_pose_hyp_并调用applyInitialPose函数进行滤波器的初始化。具体内容包括了以下几个部分:
2.1 frame_id确认
对于传入的消息,首先确认其消息是否是对应frame_id。frame_id需要对应为“map“,否则跳出。
if(msg.header.frame_id == ""){// This should be removed at some pointROS_WARN("Received initial pose with empty frame_id. You should always supply a frame_id.");}// We only accept initial pose estimates in the global frame, #5148.else if(stripSlash(msg.header.frame_id) != global_frame_id_)//global_frame_id_是指map{ROS_WARN("Ignoring initial pose in frame \"%s\"; initial poses must be in the global frame, \"%s\"",stripSlash(msg.header.frame_id).c_str(),global_frame_id_.c_str());return;}
感觉这里if语句缺少一个return。
2.2 tf2变换
确认消息输入没有问题的情况下,调用tf2变换将位姿变换到指定坐标系下。
geometry_msgs::TransformStamped tx_odom;try{ros::Time now = ros::Time::now();// wait a little for the latest tf to become available//数据的坐标变换。获得两个坐标系之间转换的关系,包括旋转和平移。tx_odom = tf_->lookupTransform(base_frame_id_, msg.header.stamp,base_frame_id_, ros::Time::now(),//base_linkodom_frame_id_, ros::Duration(0.5));//odom}catch(tf2::TransformException e){// If we've never sent a transform, then this is normal, because the// global_frame_id_ frame doesn't exist. We only care about in-time// transformation for on-the-move pose-setting, so ignoring this// startup condition doesn't really cost us anything.if(sent_first_transform_)ROS_WARN("Failed to transform initial pose in time (%s)", e.what());tf2::convert(tf2::Transform::getIdentity(), tx_odom.transform);}tf2::Transform tx_odom_tf2;tf2::convert(tx_odom.transform, tx_odom_tf2);tf2::Transform pose_old, pose_new;tf2::convert(msg.pose.pose, pose_old);pose_new = pose_old * tx_odom_tf2;// Transform into the global frame//变换一个世界坐标ROS_INFO("Setting pose (%.6f): %.3f %.3f %.3f",ros::Time::now().toSec(),pose_new.getOrigin().x(),pose_new.getOrigin().y(),tf2::getYaw(pose_new.getRotation()));// Re-initialize the filterpf_vector_t pf_init_pose_mean = pf_vector_zero();pf_init_pose_mean.v[0] = pose_new.getOrigin().x();pf_init_pose_mean.v[1] = pose_new.getOrigin().y();pf_init_pose_mean.v[2] = tf2::getYaw(pose_new.getRotation());pf_matrix_t pf_init_pose_cov = pf_matrix_zero();// Copy in the covariance, converting from 6-D to 3-Dfor(int i=0; i<2; i++){for(int j=0; j<2; j++){pf_init_pose_cov.m[i][j] = msg.pose.covariance[6*i+j];}}pf_init_pose_cov.m[2][2] = msg.pose.covariance[6*5+5];delete initial_pose_hyp_;initial_pose_hyp_ = new amcl_hyp_t();initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;
这一部分感觉是位姿初始化中最难理解的地方,tf2的内容网上能查到的不太多,目前也是一知半解,后面再找个时间专门学习一下。
2.3 初始化粒子分布
调用applyInitialPose函数对粒子进行初始化,这里除了需要一个初始位姿还需要一个条件:地图存在。
AmclNode::applyInitialPose()
{boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);if( initial_pose_hyp_ != NULL && map_ != NULL ) {// 用高斯分布来初始化滤波器pf_init(pf_, initial_pose_hyp_->pf_pose_mean, initial_pose_hyp_->pf_pose_cov);pf_init_ = false;delete initial_pose_hyp_;initial_pose_hyp_ = NULL;}
}//经历如此种种之后,粒子滤波器得到初始化,也就是粒子得到了新的位姿。
上述的两个办法中粒子的初始化使用的都是高斯分布,其实粒子初始化模型有两种,另外一种只在特定条件下使用,后面再详细介绍。
两种方式的对比
这两种方式都用于初始化位姿,但是用处不同。很多时候我们算法中没有对应的话题时我们都会使用第一种方式进行初始化。
注意到第一种方式中的savePoseToServer,其实是用于位姿保存的,该函数会在每次程序结束时执行一次,以及每隔一定频率也会执行一次:
如果程序关闭而ROS master并没有关闭的话,该位姿会被记录下来,下次启动时还会调用该位姿。
例如下图中,机器人处于场景中某一个非原点的位置时,我们关闭amcl节点。如果我们保留ROS master然后重新打开amcl节点,会发现机器人的初始化位姿就是我们上一次节点关闭时的位姿:
而当我们关闭amcl节点时,同时关闭ROS master节点。然后重新打开ROS master以及amcl。会发现机器人的初始化位姿变成了原点而不是机器人真实存在的位姿:
所以第一种方式生效的条件一般是需要机器人启动时在地图原点或者机器人的ROS master节点不能关闭的情况。
而对于第二种情况,我们可以通过一个自己写的节点测试一下。这里简单写了一个读取仿真中odom数据然后以initialpose话题名称重新发布的节点,注意数据格式要跟amcl中的数据格式对应起来,要不然会报错。这里使用的数据格式是<geometry_msgs::PoseWithCovarianceStamped>格式。
这个是在上面一张图的基础上新增了一个initialpose后得到的结果,可以看到initialpose话题对于amcl节点来说其优先级高于第一种初始化方式。它修正了第一种位姿初始化后的错误的位姿估计。
另外,这里由于我的initialpose节点是一直发布的,所以最后得到的位姿始终只有一个,就是只显示一个箭头:
包括我连续运行:
而当我将该节点关闭之后,继续运行之后会回到最初的状态,也就是一堆离散的坐标点:
根据这张消息打印结果大致也能够看出,在程序开始时,算法调用第一种初始化位姿方式进行了一次位姿初始化,然后在订阅到initialpose后采用第二种方式再次进行了一次初始化。程序的主要执行顺序即根据打印消息的顺序执行。由于这里initialpose是一直在接收的,所以打印消息中出现了很多次。如果只执行一次的话后面的顺序跟前面第一张终端图中的消息顺序是一样的。
参考:
https://zhuanlan.zhihu.com/p/434271496
AMCL代码详解(二)位姿初始化相关推荐
- Pytorch|YOWO原理及代码详解(二)
Pytorch|YOWO原理及代码详解(二) 本博客上接,Pytorch|YOWO原理及代码详解(一),阅前可看. 1.正式训练 if opt.evaluate:logging('evaluating ...
- AMCL代码详解(七)amcl中的kd-Tree
1.kd-Tree基础 kd树(k-dimensional树的简称),是一种分割k维数据空间的数据结构,主要应用于多维空间关键数据的近邻查找(Nearest Neighbor)和近似最近邻查找(App ...
- AMCL代码详解(五)根据激光观测更新粒子权重
上一篇中讲述了里程计在amcl中的作用,算法根据里程计反馈对每个粒子的位姿进行更新.在粒子更新后,则需要根据激光的观测数据对粒子的权重进行更新,这里主要看一下激光电云在amcl中的作用: 1.激光回调 ...
- AMCL代码详解(三)创建粒子模型
AMCL中创建粒子模型的方式有两种: 1.高斯分布模型 第一种方式的模型主要使用了高斯模型去建立了一系列粒子点: // // 使用高斯模型(均值,协方差)初始化滤波器 void pf_init(pf_ ...
- AMCL代码详解(六)amcl中的重采样
1.重采样判断 上一章讲述了amcl中如何根据激光观测更新粒子权重,当粒子更新完后amcl会需要根据程序判断是否需要进行重采样.这个判断在粒子观测更新权重后进行判断,代码在amcl_node.cpp中 ...
- ELMo代码详解(二)
ELMo代码解读笔记2:模型代码 2.模型代码介绍 模型代码主要包括以下几个部分:1.构建word embedding; 2.构建word_char embedding的准备; 3.语言模型介绍( ...
- VINS技术路线与代码详解
VINS技术路线 写在前面:本文整和自己的思路,希望对学习VINS或者VIO的同学有所帮助,如果你觉得文章写的对你的理解有一点帮助,可以推荐给周围的小伙伴们,当然,如果你有任何问题想要交流,欢迎随时探 ...
- 天津理工大学《操作系统》实验二,存储器的分配与回收算法实现,代码详解,保姆式注释讲解
天津理工大学<操作系统>实验二,存储器的分配与回收算法实现,代码详解,保姆式注释讲解 实验内容 1. 本实验是模拟操作系统的主存分配,运用可变分区的存储管理算法设计主存分配和回收程序,并不 ...
- linux 进程间通信 dbus-glib【实例】详解二(下) 消息和消息总线(ListActivatableNames和服务器的自动启动)(附代码)
linux 进程间通信 dbus-glib[实例]详解一(附代码)(d-feet工具使用) linux 进程间通信 dbus-glib[实例]详解二(上) 消息和消息总线(附代码) linux 进程间 ...
- linux 进程间通信 dbus-glib【实例】详解二(上) 消息和消息总线(附代码)
linux 进程间通信 dbus-glib[实例]详解一(附代码)(d-feet工具使用) linux 进程间通信 dbus-glib[实例]详解二(上) 消息和消息总线(附代码) linux 进程间 ...
最新文章
- fastText实现文本分类
- qt5使用mysql
- Linux环境下虚拟环境virtualenv安装和使用(转)
- 科技创业公司的效率工具箱
- PECompact3.0
- c语言推箱子给上颜色,本人的C语言大作业——推箱子
- Angular全套知识讲解,错过必悔!
- 单词搜索—leetcode79
- 火山引擎端云一体化服务:打造面向体验的视频云
- 我来更新了,说说工作中的Java处理异常
- 删除了几个月的照片能找回么_手机删除的照片还能恢复?打开这里,一年前的也可以找回...
- codevs1958 刺激
- Java技术图谱!黑马java培训课程目录
- 什么是具身认知?反身理论?
- 代理商为何要卖增值业务?
- 计算 变异系数 标准差 标准偏差 相关系数 平滑指数 回归系数等C++ 模板类
- 用序列到序列和注意模型实现的翻译:Translation with a Sequence to Sequence Network and Attention
- 膨胀卷积的缺点_卷积、反卷积与膨胀卷积
- Spring MVC学习笔记
- 将linux系统安装到U盘