智能机器人（75）：ROS的TF-transform（03）
PART I – 几个坐标系
*1. map
*2. odom
*3. base_link
PART II – 几个变换
0. base_link–>-base_laser
1. odom–>base_link
2. map–>base_link
*3. map–>odom
PART III – map -> odom的代码
PART IV – map-> odom的作用
PART I – 几个坐标系
关于map，odom, base_link, base_laser，fixed_frame,target_frame的关系。
这些frames具体表述于 http://www.ros.org/reps/rep-0105.html ：
earth -> map -> odom -> base_link,
the map frame is the parent of odom, and odom is the parent of base_link.
1. map：代表了一个固定不变的世界。定位了机器人的绝对位姿
2. odom：代表了一个固定不变的世界。定位了机器人的相对位姿。
这个是里程计所测绘到的世界（odom is the world as measured by odometry）。用于定位机器人的相对位姿。
如果里程计/激光雷达等均是完美无缺的，那么odom frame无差别的等同于map frame。也就是map->odom的变换为0。
3. base_link： is rigidly attached to the robot base
*1. map
map是虚拟世界中的固定frame, 一般与odom（或者odom_combined）相连。 它的Z轴指向正上方，也就是天空。map与odom同为全局坐标系，原点为地图原点（地图原点在地图相应的yaml文件中有规定）。
The map frame is useful as a long-term global reference, but discrete jumps in position estimators make it a poor reference frame for local sensing and acting.
In a typical setup, a localization component constantly recomputes the robot pose in the map frame based on sensor observations, therefore eliminating drift, but causing discrete jumps when new sensor information arrives.
*2. odom
在一个典型设置中，odom 坐标系是基于测距源来计算的，如车轮里程计odom，视觉里程计vo或惯性测量单元imu。
In a typical setup the odom frame is computed based on an odometry source, such as wheel odometry, visual odometry or an inertial measurement unit.
odom是一个很好的固定参照frame。机器人的姿态相对odom而言是随时间是经常可变，所以在长远中它不是一个好的全局参照系。但在某一时间点而言它相对机器人的位置是不变的。
However, the pose of a robot in the odom frame is guaranteed to be continuous, meaning that the pose of a mobile platform in the odom frame always evolves in a smooth way, without discrete jumps.
Odom frame is called so because it is the frame where the odometry is used to update the pose of the robot.
The odom frame represents the starting point of the robot, and， the transform from odom to base_link represents the current position of the robot as measured by odometry.
The odometry measurements are a measure of how far the robot has traveled with respect to the /odom frame. The point of odometry messages is to give a measure of the distance the robot has traveled.
In this frame, there is no correction or jumps in the robot’s pose, it is continually updated based on the data coming from the motor’s encoders (sometimes fused with accelerometer and gyro).
*3. base_link
base_link参照系紧紧粘在移动机器人基座上的任何一个位置和角度。
一般位于tf-tree的最根部，物理语义原点一般为表示机器人中心，为相对机器人的本体的坐标系。其它所有的 frame都是相对于base_link定义并粘在一起的。
它们一起相对于大地图map移动，让机器人移动就是向tf发布geometry_msgs::TransformStamped 消息通知 base_link相对于map的tf转换关系。机器人监听到这个变换后移动。
PART II – 几个变换
0. base_link–>-base_laser，提供激光先对于机器人的位置信息，可以直接提供，或者通过urdf模型中提供。
1. odom–>base_link，是里程计节点，机器人在odom中的相对位姿。
2. map–>base_link，通常是在amcl的定位软件模块中，机器人相对于map的绝对位姿。
*3. map–>odom，由于上面1和2的发布导致base_link具有两个parent， 所以通常是这种形势发布。
坐标系变化重要的是，每个帧都只有一个父帧，有时候两个坐标A/B都和C有关联的话，就是A和B关联，B再和C关联，而不是A/B都和C关联。
具体到这里的三个坐标帧map/odom/base_link，其实map和odom都应该和base_link关联，但为了遵守“每个帧只能有一个父帧”的原则，所以只能有一个节点：或者由里程计发布odom->base_link的坐标关系，或者由激光雷达发布map->base_link，但只能有一个，不能同时； 但是却又要体现三个坐标的关系，所以是发布的A->B->C，即map –> odom –> base_link。
就是说：
1.的odom->base_link的坐标关系，由里程计节点计算，并且发布，
2.的map->base_link的坐标关系，由定位节点计算，但并不发布；而是利用odom->base_link的坐标关系，
计算出3.的map->odom，并且发布。
说明：
应用中也并不总是正规的1+3这种形式发布map->odom->base_link， 偶尔的也可以用1或者2这种的简单形式。
例如只有里程计没有激光雷达也能跑，只发布1. odom–>base_link
例如只有激光雷达没有里程计也能跑，只发布2. map–>base_link
或者既有激光雷达也有里程计，也可以仅仅发布1. odom–>base_link或者2. map–>base_link。
*0. base_link–>base_laser
It’s normally static, published by: static_tf published in one of launch files, or
robot_state_publisher from urdf model， such as turtlebot_description.
*1. odom->base_link
odom->base_link的转换是由里程计源计算和发布的。The link between odom and base_link is job of localization node.
It’s computed and broadcast by the odometry sources, or, published by robot_pose_ekf.
So, it may be robot_pose_ekf, or it will be controller’s node directly: just set /mobile_base_controller/enable_odom_tf to true.
*2. map–>base_link
It’s computed by a localization component.
However, the localization component does not broadcast map->base_link.
Instead, it get map->base_link, and with information of odom -> base_link, through this way, get map–>odom.
**3. map–>odom
Although someone would say that both map and odom should be attached to base_link, yes, map->base_link and odom->base_link are both correct.
From map->base_link subtracte base_link->odom will get map->odom.
Dehaoz Tips:
If we have a “perfect” odometry sensor and a “perfect” laser, the map–>odom tf will always be 0.
5. So, the tree looks like this:
But, this is not allowed , because each frame can only have one parent.
PART III – map -> odom的代码
（根据map和base_link以及odom和base_link的关系计算map和odom的坐标关系的代码）
ROS的AMCL算法，主要是：
根据订阅到的地图数据，配合激光扫描特征，使用粒子滤波器，获取最佳定位点Mp (point on map), 也就是base_link在map中的坐标。（map->base_link）
然后根据里程计得到的相对位姿信息 odom->base_link，原点是机器人启动时刻的位置
从而得到map->odom。
代码分析：
$ vi https://github.com/ros-planning/navigation/blob/kinetic-devel/amcl/src/amcl_node.cpp
at lien 1299:
AmclNode::AmclNode()
{
… …
private_nh_.param(“odom_frame_id”, odom_frame_id_, std::string(“odom”));
private_nh_.param(“base_frame_id”, base_frame_id_, std::string(“base_link”));
private_nh_.param(“global_frame_id”, global_frame_id_, std::string(“map”));
… …
odom_frame_id_ = config.odom_frame_id;
base_frame_id_ = config.base_frame_id;
global_frame_id_ = config.global_frame_id;
… …
updatePoseFromServer();
… …
void AmclNode::laserReceived(const sensor_msgs::LaserScanConstPtr& laser_scan)
{
… …
// Where was the robot when this scan was taken?
pf_vector_t pose;
if(!getOdomPose(latest_odom_pose_, pose.v[0], pose.v[1], pose.v[2],
laser_scan->header.stamp, base_frame_id_))
{
ROS_ERROR(“Couldn’t determine robot’s pose associated with laser scan”);
return;
}
… …
geometry_msgs::PoseWithCovarianceStamped p;
// Fill in the header
p.header.frame_id = global_frame_id_;
p.header.stamp = laser_scan->header.stamp;
// Copy in the pose
p.pose.pose.position.x = hyps[max_weight_hyp].pf_pose_mean.v[0];
p.pose.pose.position.y = hyps[max_weight_hyp].pf_pose_mean.v[1];
tf::quaternionTFToMsg(tf::createQuaternionFromYaw(hyps[max_weight_hyp].pf_pose_mean.v[2]), p.pose.pose.orientation);
// hyps[max_weight_hyp].pf_pose_mean.v[0], [1], [2] 代表了粒子滤波器的最佳定位点Mp，
// 也就是base_link在map中的坐标（map->base_link）
… …
pose_pub_.publish(p);
last_published_pose = p; // the pose of base_link among 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]); // //amcl估计出的map上的点
// subtracting base_link->odom FROM map->base_link, we get map->odom !
// 这个odom->map是需要求解的关键，
// 即odom坐标系到map坐标系的变换，或者说map在odom的位姿坐标。
tf::Stamped
try
{
// 根据假设的颗粒点在map的坐标
// 得到 tmp_tf = map->base_link的坐标变换，即base_link在map的位姿坐标，或者从map原点看base_link的位置。
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));
// 翻转得到tmp_tf_stamped = base_link->map
// 即获得map在base_kink坐标系中的坐标
tf::Stamped
laser_scan->header.stamp,
base_frame_id_);
// transformPose (“frame_c”, b->a, ??), ?? ==>> c->a。
// 这个函数就是由a在b中的位姿，得到a在c中的位姿。
// 这里根据tmp_tf_stamped = base_link->map
// 得到odom_to_map = odom->map，也就是odom坐标系到map坐标系的变换，或者说map在odom的位姿坐标，或者说从odom原点看map原点的位置。
this->tf_->transformPose(odom_frame_id_,
tmp_tf_stamped,
odom_to_map);
}
// subtracting base_link/odom from map/base_link and get map/odom , send map to odom instead
catch(tf::TransformException)
{
ROS_DEBUG(“Failed to subtract base to odom transform”);
return;
}
// latest_tf_ = odom->map
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 until a tolerance time so that odom can be used
ros::Time transform_expiration = (laser_scan->header.stamp + transform_tolerance_);
// 翻转为 map->odom，即odom在map中的位姿坐标，或者说从map原点看odom原点的位置。
tf::StampedTransform tmp_tf_stamped(latest_tf_.inverse(),
transform_expiration,
global_frame_id_, odom_frame_id_);
// 发布 map->odom
this->tfb_->sendTransform(tmp_tf_stamped);
sent_first_transform_ = true;
}
… …
odom->base_link是码盘提供的原始数据
map->base_link是激光帧间匹配计算得到的
ODOM_map = MAP_basekink * ODOM_bae_link，
odom坐标到map坐标的变换矩阵 = map坐标到baselink坐标的变换矩阵 * odom坐标到base_link坐标的变换矩阵
map在odom坐标中的位姿 = baselink在map坐标中的位姿 * map在odom坐标中位姿。
REF:
http://wiki.ros.org/amcl
https://github.com/ros-planning/navigation/tree/kinetic-devel/amcl#amcl_node.cpp
https://answers.ros.org/question/227425/why-gmapping-package-requires-the-base_link-odom-tf/
http://blog.csdn.net/crazyquhezheng/article/details/49154805
PART III – Hi58的总结
ROS的tf坐标系，最终都要归结为三个标准框架，可以解决许多常见的机器人问题：
1. 全局不准确，但局部光滑框架（odom）
2. 全局准确，但局部不连续的帧（map）
3. 机器人自身框架（base_link）
The odom pose is the /base_link frame pose relative to /odom frame.
IMU and encoders provide the odom pose which drifts over time and thus only right for a short time.
odom frame is a pseudo-global reference frame in which a dead reckoning position and orientation estimate is represented.
The absolute/global pose is the /base_link frame pose relative to /map frame.
GPS, amcl and indoor position system provide the absolute/global pose.
got odom pose means you got the tf odom->base_link,
got absolute/global pose means you got map->base_link,
To correct the robot location, you have to provide the tf map->odom,
which can be calculated from odom->base_link – map->base_link.
例如在 hector_slam_launch:
*1. publishes the map->odom transform:
*2 do not require the use of a odom frame (for example because your platform does not provide any usable odometry) ，
you can directly publish a transformation from map to base_link:
*3 it can also be used without broadcasting any tf transformation.
This can be useful if another node is responsible for publishing the map->odom or map->base_frame transform.
PART IV – map-> odom的作用
In order to run robot localization and model the drifts of the robot while it is navigating, a transform between the frame map and the frame odom was introduced.
At the beginning map and odom frame are overlapping.
While the robot navigates and localizes (using amcl for example) it will correct its drift but it will not do this correction by changing its pose in the odom frame (this pose published under the topic /odom).
It will update the transform between map and odom frames such that when you chain the transforms to get the pose of the robot in the map frame, you will get the corrected pose (thus the transform between map and odom frames represents the offset in the robot’s pose that is given in the odom frame).
Do not confuse the frame odom with the robot’s pose published under the topic /odom.
The frame odom is just a coordinate system where the robot has to be located. This location of the robot would then be the coordinates of base_link in the odom frame. These coordinates are published in the /odom topic.
So when you turn on the robot, base_link and odom are overlapping as the robot’s pose is zero at the beginning.
But once the robot’s wheels start rotating, base_link will get away from odom.
So base_link is a mobile frame and odom a fixed one.
1.
In the beginning, we assume that the origin of the world coordinate system and the origin of odometry are the same.
https://qiita-image-store.s3.amazonaws.com/0/110138/0e9bfbe3-8061-405c-c26b-191f2a87b0c4.png
2.
Then tobot move:
https://qiita-image-store.s3.amazonaws.com/0/110138/f35f03da-a3b5-f959-363d-c9e1a26f4aed.png
3.
However, in fact, due to accumulation of various disturbances, the origin of the world coordinate system and odometry will deviate.
https://qiita-image-store.s3.amazonaws.com/0/110138/40a3eada-b7ed-a304-3f74-3e7cdeaea151.png
Therefore, by matching the sensor information and the map information, we come to the idea of correcting the deviation between the origin of the world coordinate system and the origin of odometry. The package for that will be amcl .
rris
rris
I love everything you post. You have done great job