TF
TF 功能包可以让用户随时间跟踪多个坐标系
- 查看指定两个坐标系的变换关系
tf_monitor <source_frame> <target_frame>
tf_echo <source_frame> <target_frame>- 发布两个坐标系之间的静态关系(两个坐标系不发生相对位置的变化)
static_transform_publisher
x y z yaw pitch roll (qx qy qz qw) frame_id child_frame_id period_in_ms
rosrun tf view_frame 生成PDF版的整个TF树
静态TF坐标变换
<node pkg="tf" type="static_transform_publisher" name="link1_broadcaster" args="0 0 0 0 0 0 1 map odom 20" />
Axes
rosrun tf view_frames 可以打印系统中当前frame的连接关系。
tf_echo 查看两个坐标系的静态关系
path 轨迹可视化
就是将追踪对象的位置随着时间的变化给可视化出来
主要用 geometry_msgs/PoseStamped数组, 里面包含多个pose点
poses[i].stamp poses[i].pose.position
话题类型:nav_msgs/Path
消息类型:std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/PoseStamped[] poses
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
Odometry
刚体位姿的可视化 下一帧相对于当前帧的旋转和平移
话题类型:nav_msgs/Odometry
消息类型:pose & twiststd_msgs/Header header
uint32 seq
time stamp
string frame_id
string child_frame_id
geometry_msgs/PoseWithCovariance pose
geometry_msgs/Pose pose
geometry_msgs/Point position
float64 x
float64 y
float64 z
geometry_msgs/Quaternion orientation
float64 x
float64 y
float64 z
float64 w
float64[36] covariance
geometry_msgs/TwistWithCovariance twist
geometry_msgs/Twist twist
geometry_msgs/Vector3 linear
float64 x
float64 y
float64 z
geometry_msgs/Vector3 angular
float64 x
float64 y
float64 z
float64[36] covariance
PointCloud
sensor_msgs::PointCloud数据结构
sensor_msgs::PointCloud
std_msgs/Header header
uint32 seq
time stamp
string frame_id
geometry_msgs/Point32[] points 一帧点云所包含的所有点
float32 x
float32 y
float32 z
sensor_msgs/ChannelFloat32[] channels
string name
float32[] values
channels[] 数组中所包含的信息
channels[0].values[i] point_id
channels[1].values[i] camera_id
channels[2].values[i] p_u 点所对应的像素坐标
channels[3].values[i] p_v
channels[4].values[i] velocity_x 点所对应的速度
channels[5].values[i] velocity_y
channels[6].values[i] gx 点所对应的重力加速度
channels[7].values[i] gy
channels[8].values[i] gz
sensor_msgs::PointCloud使用示例
#include <map>
#include <Eigen/Core>
#include <Eigen/Geometry>
//订阅特征点
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
{
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> featureFrame;
//读取pointCloud message
for (unsigned int i = 0; i < feature_msg->points.size(); i++)
{
int feature_id = feature_msg->channels[0].values[i];
int camera_id = feature_msg->channels[1].values[i];
double x = feature_msg->points[i].x;
double y = feature_msg->points[i].y;
double z = feature_msg->points[i].z;
double p_u = feature_msg->channels[2].values[i];
double p_v = feature_msg->channels[3].values[i];
double velocity_x = feature_msg->channels[4].values[i];
double velocity_y = feature_msg->channels[5].values[i];
if(feature_msg->channels.size() > 5)
{
double gx = feature_msg->channels[6].values[i];
double gy = feature_msg->channels[7].values[i];
double gz = feature_msg->channels[8].values[i];
pts_gt[feature_id] = Eigen::Vector3d(gx, gy, gz);
//printf("receive pts gt %d %f %f %f\n", feature_id, gx, gy, gz);
}
ROS_ASSERT(z == 1);
Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
//压入 map vector
featureFrame[feature_id].emplace_back(camera_id, xyz_uv_velocity);
}
double t = feature_msg->header.stamp.toSec();
estimator.inputFeature(t, featureFrame);
return;
}