在看AMCL过程中,对坐标转移的关系不太清晰,整理下:
1、tf2::Transform map_pose = latest_tf_.inverse() * odom_pose_tf2;表示的是:
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::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() ); 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]); }
2、pose_new = pose_old * tx_odom_tf2;
void AmclNode::handleInitialPoseMessage(const geometry_msgs::PoseWithCovarianceStamped& msg) { boost::recursive_mutex::scoped_lock prl(configuration_mutex_); if(msg.header.frame_id == "") { // This should be removed at some point ROS_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_) { 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; } // In case the client sent us a pose estimate in the past, integrate the // intervening odometric change. 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(), odom_frame_id_, ros::Duration(0.5)); } 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 filter pf_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-D for(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; applyInitialPose(); }