VINS-Mono程式碼解讀——各種資料結構 sensor_msgs
前言
在看VINS-Mono的程式碼時，覺得非常有必要整理總結一下其中不同的資料結構，尤其是各種sensor_msgs的格式與具體含義。慢慢補充吧！
標準msg結構
1、std_msgs/Header
from file：std_msgs/Header.msg
一般在Image/PointCloud/IMU等各種感測器資料結構中都會出現的頭資訊
Definition：
uint32 seq # sequence ID: consecutively increasing ID time stamp #時間戳 stamp.sec: seconds since epoch /stamp.nsec: nanoseconds since stamp_secs string frame_id #座標系ID
2、sensor_msgs::ImageConstPtr
from file：sensor_msgs/Image.msg
在feature_trackers_node.cpp中回撥函式img_callback的輸入，表示一幅影象
Definition：
std_msgs/Header header #頭資訊 uint32 height # image height, number of rows uint32 width # image width, number of columns string encoding # Encoding of pixels -- channel meaning, ordering, size # taken from the list of strings in include/sensor_msgs/image_encodings.h uint8 is_bigendian #大端big endian(從低地址到高地址的順序存放資料的高位位元組到低位位元組)和小端little endian uint32 step # Full row length in bytes uint8[] data # actual matrix data, size is (step * rows)
3、sensor_msgs::PointCloudPtr feature_points
from file：sensor_msgs/PointCloud.msg
在feature_trackers.cpp中img_callback()中建立feature_points並封裝，在main()中釋出為話題“/feature_tracker/feature”，包含一幀影象中特徵點資訊
sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
pub_img.publish(feature_points) Definition：
std_msgs/Header header #頭資訊
#feature_points->header = img_msg->header;
#feature_points->header.frame_id = "world";
geometry_msgs/Point32[] points #3D點(x,y,z)
sensor_msgs/ChannelFloat32[] channels #[特徵點的ID,畫素座標u,v,速度vx,vy]
#feature_points->channels.push_back(id_of_point);
#feature_points->channels.push_back(u_of_point);
#feature_points->channels.push_back(v_of_point);
#feature_points->channels.push_back(velocity_x_of_point);
#feature_points->channels.push_back(velocity_y_of_point);
#如img_msg->channels[0].values[i]表示第i個特徵點的ID值
sensor_msgs::PointCloud msg_match_points
sensor_msgs::PointCloudConstPtr &points_msg跟它一樣
這個東西資料格式和之前的feature_points一樣，但是channels不一樣。
在keyframe.cpp中的findConnection()中建立並封裝成msg_match_points，
在pose_graph_node.cpp的main()中釋出話題“/pose_graph/match_points”
主要包含重定位的兩幀間匹配點和匹配關係（變換矩陣）
sensor_msgs::PointCloud msg_match_points;
pub_match_points.publish(msg_match_points);
pub_match_points = n.advertise<sensor_msgs::PointCloud>("match_points", 100);
在estimator_node.cpp的main()中該話題被訂閱，回撥函式為relocalization_callback()
ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);
void relocalization_callback(const sensor_msgs::PointCloudConstPtr &points_msg)
Definition：
std_msgs/Header header #頭資訊
#msg_match_points.header.stamp = ros::Time(time_stamp);
geometry_msgs/Point32[] points #3D點(x,y,z)
sensor_msgs/ChannelFloat32[] channels #[重定位幀的平移向量T的x,y,z，旋轉四元數w,x,y,z和索引值]
#t_q_index.values.push_back(T.x());
#t_q_index.values.push_back(T.y());
#t_q_index.values.push_back(T.z());
#t_q_index.values.push_back(Q.w());
#t_q_index.values.push_back(Q.x());
#t_q_index.values.push_back(Q.y());
#t_q_index.values.push_back(Q.z());
#t_q_index.values.push_back(index);
#msg_match_points.channels.push_back(t_q_index);
4、sensor_msgs::ImuConstPtr
from file：sensor_msgs/Imu.msg
IMU資訊的標準資料結構
Header header #頭資訊
geometry_msgs/Quaternion orientation #四元數[x,y,z,w]
float64[9] orientation_covariance # Row major about x, y, z axes
geometry_msgs/Vector3 angular_velocity #角速度[x,y,z]軸
float64[9] angular_velocity_covariance # 對應協方差矩陣，Row major(行主序) about x, y, z axes
geometry_msgs/Vector3 linear_acceleration #線性加速度[x,y,z]
float64[9] linear_acceleration_covariance # 對應協方差矩陣 Row major x, y z
程式碼中的組合結構
1、measurements
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
estimator_node.cpp中getMeasurements()函式將對imu和影象資料進行初步對齊得到的資料結構，確保影象關聯著對應時間戳內的所有IMU資料
sensor_msgs::PointCloudConstPtr 表示某一幀影象的feature_points
std::vector<sensor_msgs::ImuConstPtr> 表示當前幀和上一幀時間間隔中的所有IMU資料
將兩者組合成一個數據結構，並構建元素為這種結構的vector進行儲存
2、map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image
在estimator.cpp中的process()中被建立，在Estimator::processImage()中被呼叫
作用是建立每個特徵點(camera_id,[x,y,z,u,v,vx,vy])構成的map，索引為feature_id
for (unsigned int i = 0; i < img_msg->points.size(); i++)
{
int v = img_msg->channels[0].values[i] + 0.5;
int feature_id = v / NUM_OF_CAM;
int camera_id = v % NUM_OF_CAM;
double x = img_msg->points[i].x;
double y = img_msg->points[i].y;
double z = img_msg->points[i].z;
double p_u = img_msg->channels[1].values[i];
double p_v = img_msg->channels[2].values[i];
double velocity_x = img_msg->channels[3].values[i];
double velocity_y = img_msg->channels[4].values[i];
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;
image[feature_id].emplace_back(camera_id, xyz_uv_velocity);
}