tf坐标变换
来源:互联网 发布:c语言产生随机数的函数 编辑:程序博客网 时间:2024/04/27 03:26
1.数据类型
2.原理
基本原理:tb的类里有个publisher,tl的类里有个subscriber,一个发布叫/tf的topic,一个订阅这个topic,传送的消息tf message里包含了parent frame id和child frame id的信息。
3.base_link, odom和map的关系
预备知识:
1.map是虚拟世界中的固定frame,与odom同为全局坐标系,原点为地图原点(地图原点在地图相应的yaml文件中有规定),odom的原点为开始计算位姿那个时刻的机器人的位置。
2.从W的原点看A所在的位置坐标 p,就是A->W转换矩阵(A的坐标变换到W坐标系、A->W的坐标变换、W坐标系下A的位置),称为WAT. Ap为A坐标系的点, 它在W上的坐标 Wp = WAT*Ap
3.已知tf转换:
map->base_link(或base_link->map,因为其坐标关系是时刻在被发布)
base_link->odom(其坐标关系是通过位姿估计出来的)
4.推算:
map->odom = map->base_link - base_link->odom
5.代码实现
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]); //amcl估计出的地图上的点
// 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)); //首先得到base_link->map坐标变换关系
tf::Stamped<tf::Pose> tmp_tf_stamped (tmp_tf.inverse(),
laser_scan->header.stamp,
base_frame_id_);//tmp_tf.inverse():map->base_link.
this->tf_->transformPose(odom_frame_id_,
tmp_tf_stamped,
odom_to_map);//base_link->odom。上面已经将map转换到base_link下,所以再通过base_link->odom也就得到map->odom坐标变换
}
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;
if (tf_broadcast_ == true)
{
// We want to send a transform that is good up until a
// tolerance time so that odom can be used
ros::Time transform_expiration = (laser_scan->header.stamp +
transform_tolerance_);
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),//变为odom->map
transform_expiration,
global_frame_id_, odom_frame_id_);
this->tfb_->sendTransform(tmp_tf_stamped);
sent_first_transform_ = true;
}
最终将odom->map,然后将tf发布出去
- tf坐标变换
- ROS之tf坐标变换
- ROS专题----tf和tf2坐标变换
- ROS之tf空间坐标变换浅析
- ROS的tf包中坐标变换的方法
- ROS的tf包中坐标变换的方法
- ROS之tf空间坐标变换浅析 (二)
- 坐标变换
- 坐标变换
- 坐标变换
- 坐标变换
- 坐标变换
- 坐标变换
- 坐标变换
- 坐标变换
- 坐标变换
- 坐标变换
- ROS-tf坐标转换
- 217. Contains Duplicate
- Qt在ubuntu无法输入中文的解决
- Andorid 文件浏览器简易实现
- 孰优孰劣——RelativeLayout 对比ConstraintLayout
- RabbitMQ 基础教程 Routing(消息路由)
- tf坐标变换
- 使用JavaScript时在变量前加2个感叹号意义
- 设计模式之适配器模式
- 牛客网编程题总结
- image的srcset属性
- Java 包装类
- 四大组件学习之路
- NYOJ 991 Registration system(map容器)
- C++的封装、继承、多态