个人理解,欢迎指正
map:
地图坐标系,为固定坐标系,理论上应与odom
坐标系重合;
odom:
里程计坐标系;
base_link:
机器人本体坐标系;
laser_base:
激光传感器的坐标系,与base_link
之间的tf
恒定。
机器人开始运动时map
与odom
重合,但随着时间推移两者的tf
不为0。
tf
树中只有一种遍历关系--从父节点到子节点,如下图所示。
SLAM中的定位就是为了计算tf: map->odom
。
Karto SLAM中的代码,根据上述的遍历关系,是先计算base_link->odom
,再计算odom->map
再用.inverse()
计算map->odom
。如以下代码。
// Compute the map->odom transform
tf::Stamped<tf::Pose> odom_to_map;
m_.lock();
tf::Stamped<tf::Pose> odom_pose = odom_pose_;//base_link->odom
m_.unlock();
tf_.transformPose(odom_frame_, tf::Stamped<tf::Pose>(tf::Transform(tf::createQuaternionFromRPY(0, 0, corrected_pose.GetHeading()),
tf::Vector3(corrected_pose.GetX(), corrected_pose.GetY(), 0.0)).inverse(), odom_pose.stamp_, base_frame_), odom_to_map);
map_to_odom_mutex_.lock();
map_to_odom_ = tf::Transform(tf::Quaternion(odom_to_map.getRotation()),
tf::Point(odom_to_map.getOrigin()))
.inverse();
map_to_odom_mutex_.unlock();