疑问:frame_count指的是哪些帧的数量 all_image_frame存储的是哪些帧
process() (estimator_node.cpp)
1.获取在时间上“对齐”的IMU和图像数据的组合
measurements = getMeasurements()
2.遍历measurements,其实就是遍历获取每一个img_msg和其对应的imu_msg对数据进行处理
3.对每一个IMU值进行预积分
estimator.processIMU()
4.设置重定位帧
5图像特征处理,包括初始化和非线性优化
processImage() (estimator.cpp)
{
1.通过检测两帧之间的视差决定次新帧是否作为关键帧
2.如果没有外参则进行标定
if(ESTIMATE_EXTRINSIC == 2)
3.如果需要初始化则进行初始化
if (solver_flag == INITIAL)
{
result = initialStructure(); //视觉惯性联合初始化
1.1.check imu observibility 通过计算标准差来进行判断IMU是否有充分运动以初始化。
1.2.将f_manager.feature中的feature存储到sfm_f中
1.3.位姿求解:这里计算的不是相邻两帧间的位姿变换,而是滑动窗口中的从第一帧开始到滑动窗口中第一个满足与当前帧的平均视差足够大的帧的相对位姿变换。
1.4.三角化特征点,对滑窗每一帧求解sfm问题
GlobalSFM::construct() //纯视觉sfm
首先定义第 l 帧为原点帧,之后求解滑动窗口中所有图像帧与第 l 帧的旋转矩阵与平移矩阵,之后对进行三角化,最后进行非线性优化。
1、先三角化第l帧(参考帧)与第frame_num-1帧(当前帧)的路标点
2、pnp求解从第l+1开始的每一帧到第l帧的变换矩阵R_initial, P_initial,保存在Pose中
3、对第l帧与第l+1到frame_num -2的每一帧再进行三角化
4、PNP求解从第l-1到第0帧的每一帧与第l帧之间的变换矩阵,并进行三角化
5、三角化其他未恢复的特征点,至此得到了滑动窗口中所有图像帧的位姿以及特征点的3d坐标
6、使用cares进行全局BA优化
1.5.对于所有的图像帧,包括不在滑动窗口中的,提供初始的RT估计,然后solvePnP进行优化
1.6.视觉惯性联合初始化,其作用是将纯视觉估计出的 sfm 和imu的轨迹对齐。
Estimator::visualInitialAlign();
if(result)//如果初始化成功
solveOdometry();//三角化求解所有特征点的深度,并进行非线性优化
optimization(); //基于滑动窗口紧耦合的非线性优化,残差项的构造和求解,使用ceres进行非线性优化,比较复杂的一个函数。
silideWindow(); //滑动窗口边缘化
}
4.如果不需要初始化则直接进行非线性优化。
solveOdometry();//三角化求解所有特征点的深度,并进行非线性优化
}
6.给Rviz发送里程计信息:odometry、path、relo_path这几个topic。
vin_mono中的变量说明。
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;//measurements存储的是时间对齐的imu和img数据,measurements中的元素是一个个pair
measurements = getMeasurements()//由此函数获得
img_msg = measurement.second // pair.second是时间对齐的一个img的数据。
imu_msg : measurement.first // pair.first是时间对齐的若干个imu数据。
img_msg->points //存储的是img数据中的特征点信息。
int v = img_msg->channels[0].values[i] + 0.5;// v表示的是什么 应该是图像上特征的索引
int feature_id = v / NUM_OF_CAM; // 特征id
int camera_id = v % NUM_OF_CAM; //图像id const int NUM_OF_CAM = 1;
Eigen::Matrix<double, 7, 1> xyz_uv_velocity;
//存放的分别是img_msg中第i个点的x,y,z坐标,这个是归一化后的坐标值;p_u,p_v是特征点在图像上的像素坐标;velocity_x, velocity_y为像素点在x,y方向上的速度。
xyz_uv_velocity << x, y, z, p_u, p_v, velocity_x, velocity_y;
image[feature_id].emplace_back(camera_id, xyz_uv_velocity);//该image向量即为estimator.processImage(image, img_msg->header)的输入参数。
vins_mono中的数据接口
注意:process函数处理的不是原始的image图像,而是再图像上提取的特征点。
getMeasurements() // 返回收到的Imu 和 image 消息, 在该函数中已经将imu和image(feature)对齐,这里的对齐指的并不是在时间戳上的对齐,而是依据时间戳的前后关系,将相应的image(feature)和imu放到一个pair中实现对齐。
std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>,sensor_msgs::PointCloudConstPtr>> measurements;// 该函数返回的数据类型,pair中是若干imu数据和一个图像帧(图像帧对应的点云)数据。
void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)// imu 的回调函数,将收到的 imu 数据保存到 imu_buf中
void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg)
//后段优化的流程, optimization()
void Estimator::solveOdometry()//三角化求解所有特征点的深度,并进行非线性优化
optimization(); //基于滑动窗口紧耦合的非线性优化,残差项的构造和求解