VINS中特征点管理

VINS中的特征点

a.特征提取中的类

struct IMG_MSG
{
  double header;//时间戳
  vector<Vector3d> points;//相机坐标系下的坐标
  vector<int> id_of_point;//特征点的id
  vector<float> u_of_point;//像素坐标系下坐标
  vector<float> v_of_point;//像素坐标系下坐标
  vector<float> velocity_x_of_point;//像素坐标系下点的速度
  vector<float> velocity_y_of_point;//像素坐标下点的速度
}

将特征提取的点送到特征点管理中（结构体IMG_MSG to Matrix<double, 7, 1>）

    shared_ptr<IMU_MSG> imu_msg(new IMU_MSG());
    map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
            for (unsigned int i = 0; i < img_msg->points.size(); i++) 
            {
                int v = img_msg->id_of_point[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->u_of_point[i];
                double p_v = img_msg->v_of_point[i];
                double velocity_x = img_msg->velocity_x_of_point[i];
                double velocity_y = img_msg->velocity_y_of_point[i];
                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);
            }

//关键帧和该帧的imu预积分
class 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;
}

map<double, ImageFrame> all_image_frame; //关键帧时间戳和关键帧信息

将图像提取的特征点放到特征点管理中的类

FeaturePerFrame f_per_fra(id_pts.second[0].second, td)//second[0]表示第一个相机

这里调用了FeaturePerFrame 构造函数（起到桥梁作用）

  FeaturePerFrame(const Eigen::Matrix<double, 7, 1> &_point, double td)
  {
    point.x() = _point(0);
    point.y() = _point(1);
    point.z() = _point(2);
    uv.x() = _point(3);
    uv.y() = _point(4);
    velocity.x() = _point(5);
    velocity.y() = _point(6);
    cur_td = td;
  }

1.特征点管理中的类

//特征点的基本信息
class FeaturePerFrame
{
   Vector3d point;//特征点相机坐标系下坐标
   Vector2d uv;//特征点像素坐标
   Vector3d velocity;//特征点速度
   double z;
   bool is_used;
}

class FeaturePerId
{
   const int feature_id;//特征点id
   int start_frame;//特征点开始的帧
   vector<FeaturePerFrame> feature_per_frame;//FeaturePerId中有多个FeaturePerFrame
   int used_num;
   bool is_outlier;
   double estimated_depth;
   int solve_flag;
}

class FeatureManager
{
   ...
   list<FeaturePerId> feature;//FeatureManager中有多个FeaturePerId
   int last_track_num;

   const Matrix3d *Rs;
   Matrix3d ric[NUM_OF_CAM];fetur
}

2.SFMFeature类

struct SFMFeature
{
  bool state;//SFM特征点状态:是否三角化
  int id;//特征点id
  vector<pair<int, Vector2d>> observation;//特征点id,特征点在归一化平面坐标
  double position[3];//特征点所在的路标点的3D坐标
  double depth;//特征点的深度
}