# ROS 主动蒙特卡罗粒子滤波定位算法 AMCL 解析-- map与odom坐标转换的方法

27 篇文章 23 订阅
1 篇文章 0 订阅

ROS AMCL 算法根据订阅到的地图数据配合激光扫描特征，使用粒子滤波获取最佳定位点，该点称为M(point on map), 它是相对于地图map上的坐标,也就是base_link相对map上的坐标。

      ROS_DEBUG("New pose: %6.3f %6.3f %6.3f",
hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
hyps[max_weight_hyp].pf_pose_mean.v[2]);

// subtracting base to odom from map to base and send map to odom instead
tf::Stamped<tf::Pose> odom_to_map;
try
{
tf::Transform tmp_tf(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]),
tf::Vector3(hyps[max_weight_hyp].pf_pose_mean.v[0],
hyps[max_weight_hyp].pf_pose_mean.v[1],
0.0));
tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
base_frame_id_);
this->tf_->transformPose(odom_frame_id_,
tmp_tf_stamped,
odom_to_map);
}
catch(tf::TransformException)
{
ROS_DEBUG("Failed to subtract base to odom transform");
return;
}

latest_tf_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
tf::Point(odom_to_map.getOrigin()));
latest_tf_valid_ = true;

{
// We want to send a transform that is good up until a
// tolerance time so that odom can be used
transform_tolerance_);
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
transform_expiration,
global_frame_id_, odom_frame_id_);
this->tfb_->sendTransform(tmp_tf_stamped);
sent_first_transform_ = true;
}

hyps[max_weight_hyp].pf_pose_mean.v[0], [1], [2] 就代表了Mp

• 14
点赞
• 88
收藏
觉得还不错? 一键收藏
• 10
评论
07-07 2579
06-26 1084
06-02 292
04-14 6925
08-30 2093
01-29 560
08-20 1987
01-27 1297
01-19 7437
11-05 629

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。