VINS-Monoコード解読――各種データ構造sensor_msgs
16873 ワード
前言
VINS-MOONのコードを見ていると、その中の異なるデータ構造を整理してまとめる必要があると思います.msgsのフォーマットと具体的な意味.ゆっくり補充しましょう
標準msg構造
1、std_msgs/Header
from file:std_msgs/Header.msgは、一般的にImage/PointCloud/IMUなどの様々なセンサーデータ構造の中に現れるヘッダ情報です.
Definition:
from file:sensor_msgs/Image.msg在feature_trocers.node.cppの中のコールバック関数calbackの入力は、一枚の画像を表します.
Definition:
from file:sensor_msgs/PointCloud.msg在feature_trocers.cppは中calback()でfeature_を作成します.pointsをパッケージ化して、mainで話題の「/feature/feature」として発表しました.フレーム画像の特徴点情報を含みます.
センサーmsgs::PointCloudContstPtr&points_msgはそれと同じです
これはデータフォーマットと以前のフィーチャーチャーです.pointsは同じですが、チャンネルが違います.keyframe.cppのfindConnection()にmsg_matchpoints、poseにいますgraphnode.cppのmain()で発表された話題「/poselph/matchuploents」には主に重位置決めの2フレーム間整合点と整合関係(変換行列)が含まれています.
from file:sensor_msgs/Imu.msg IMU情報の標準データ構造
1、meass rements
2、map>>イメージ
estimart.cppにおけるprocess()において構築され、Estmator:processImage()において呼び出される役割は、それぞれの特徴点(camera uuid、[x,y,z,u,v,vx,vy])からなるmapを構築し、索引はfeature uである.id
estimart.hにおいて、class Estimartとしての属性キーは画像フレームのタイムスタンプであり、値は画像フレームタイプの画像フレーム類は画像フレームの特徴点とタイムスタンプで構成されています.また、ビット姿勢Rtが保存されています.プレ積分対象pre_uintegrationは、キーフレームかどうか.
VINS-MOONのコードを見ていると、その中の異なるデータ構造を整理してまとめる必要があると思います.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::ImageConstPtrfrom file:sensor_msgs/Image.msg在feature_trocers.node.cppの中のコールバック関数calbackの入力は、一枚の画像を表します.
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_pointsfrom file:sensor_msgs/PointCloud.msg在feature_trocers.cppは中calback()でfeature_を作成します.pointsをパッケージ化して、mainで話題の「/feature/feature」として発表しました.フレーム画像の特徴点情報を含みます.
sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
pub_img.publish(feature_points);
pub_img = n.advertise<sensor_msgs::PointCloud>("feature", 1000);
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
センサーmsgs::PointCloud msg_matchpointsセンサーmsgs::PointCloudContstPtr&points_msgはそれと同じです
これはデータフォーマットと以前のフィーチャーチャーです.pointsは同じですが、チャンネルが違います.keyframe.cppのfindConnection()にmsg_matchpoints、poseにいますgraphnode.cppのmain()で発表された話題「/poselph/matchuploents」には主に重位置決めの2フレーム間整合点と整合関係(変換行列)が含まれています.
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です.calback()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::ImunstPtrfrom 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、meass rements
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
estimatornode.cppにおけるgetMeass rements()関数は、imuと画像データを初期的に整列したデータ構造であり、画像が対応するタイムスタンプ内のすべてのIMUデータsensor_に関連していることを確認する.msgs::PointCloudContstPtrは、あるフレームの画像のfeature_を表します.points std::vector<:imuconstptr>は、現在のフレームと前のフレームの時間間隔のすべてのIMUデータを一つのデータ構造に結合し、このような構造のvectorのための要素を構築して格納することを意味します.2、map>>イメージ
estimart.cppにおけるprocess()において構築され、Estmator:processImage()において呼び出される役割は、それぞれの特徴点(camera uuid、[x,y,z,u,v,vx,vy])からなるmapを構築し、索引はfeature uである.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);
}
3、map all_イメージ.イメージframeestimart.hにおいて、class Estimartとしての属性キーは画像フレームのタイムスタンプであり、値は画像フレームタイプの画像フレーム類は画像フレームの特徴点とタイムスタンプで構成されています.また、ビット姿勢Rtが保存されています.プレ積分対象pre_uintegrationは、キーフレームかどうか.
class ImageFrame
{
public:
ImageFrame(){};
ImageFrame(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>>& _points, double _t):t{_t},is_key_frame{false}
{
points = _points;
};
map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>> > > points;
double t;
Matrix3d R;
Vector3d T;
IntegrationBase *pre_integration;
bool is_key_frame;
};