vins中imu融合_VINS代码解读

VINS_estimator

摘抄

我们初始化的原因是单目惯性紧耦合系统是一个非线性程度很高的系统，首先单目是无法获得空间中的绝对尺度，而IMU又必然存在偏置，在后面进行求解的时候还需要用到重力加速度(包括大小和方向)，对于速度比较敏感的条件下，比如说无人机，又要精确的速度信息，因此，如何有效的在紧耦合系统处理之前计算出这些量，对整个紧耦合系统的鲁棒性有着重大的意义(其实这里就可以理解成相机标定一样，没有正确的标定好相机的内参，相机在进行定位的时候必然不准，而且很有可能会挂掉)。所以初始化要做的事其实说起来很简单，就是计算出绝对尺度s、陀螺仪偏置bg、加速度偏置ba、重力加速度G和每个IMU时刻的速度v，VINS中重点说明了加速度计偏置值一般都会和重力加速度耦合到一起(也就是被重力加速度给吸收掉)，重力加速度的量级要远大于其加速度偏置，而且在初始化时间内加速度计偏置比较小，很难真正的计算得到，因此忽略加速度计偏置的影响，在初始化中不再计算。初始化的作用是不言而喻的，直接影响整个紧耦合系统的鲁棒性以及定位精度，并且初始化一般都需要一个比较漫长的时间，VINS大概需要十秒左右，ORB_SLAM2结合IMU的时间设定在15秒完成初始化。

VINS-MONO中,为了处理一些悬停的case,引入了一个two-way marginalization, 简单来说就是:如果倒数第二帧是关键帧, 则将最旧的pose移出sliding window, 将最旧帧相关联的视觉和惯性数据边缘化掉，也就是MARGIN_OLD，作为一部分先验值,如果倒数第二帧不是关键帧, 则将倒数第二帧pose移出sliding window,将倒数第二帧的视觉观测值直接舍弃，保留相关联的IMU数据， 也就是MARGIN_NEW。选取关键帧的策略是视差足够大，在悬停等运动较小的情况下, 会频繁的MARGIN_NEW, 这样也就保留了那些比较旧但是视差比较大的pose. 这种情况如果一直MARGIN_OLD的话, 视觉约束不够强, 状态估计会受IMU积分误差影响, 具有较大的累积误差。

系统的初始化主要包括三个环节：求取相机与IMU之间的相对旋转、相机初始化(局部滑窗内的SFM，包括没有尺度的BA)、IMU与视觉的对齐(IMU预积分中的 α等和相机的translation)。

[image(estimator.cpp)]

某一帧图像得到的某个特征点，某一帧图像得到的特征点，>

f_manager

滑窗的特征点管理器

[keyframe_database(estimator_node.cpp)]

关键帧集合，每次在闭环检测线程中增加新元素，每个关键帧在滑窗内的导数第二个位置时被[添加]。

[keyframe_buf(estimator_node.cpp)]

为闭环检测提供临时的Keyframe存储,[只包含滑窗内的倒数第二Keyframe]，当其包含的某帧进入闭环检测环节会被[弹出]。

[retrive_data_vector]

存储闭环检测结果，包块闭环帧和匹配帧的位姿关系、匹配帧的ID、闭环帧的位姿、闭环帧中良好的匹配点以及匹配正良好匹配点的ID。

Estimator::processIMU IMU预积分

process()这个函数主要完成了以下几件事：

将测量值的IMU部分进行预积分处理；

processImage，将测量值的特征点部分进行处理并进行imu融合，在processImage函数中主要完成以下两项工作：

滑窗 slideWindow()；

滑窗内位姿优化：位于solveOdometry 中的 optimization()函数中，为VINS中IMU与视觉融合的重点部分。；

topic

ground_truth_path /benchmark_publisher/path

tracked image /feature_tracker/feature_img 当前图像

raw image /cam0/image_raw 无显示

path /vins_estimator/path

camera_visual /vins_estimator/camera_pose_visual

current_point /vins_estimator/point_cloud

pose_graph_path /pose_graph/pose_graph_path

base_path /pose_graph/base_path

loop_visual /pose_graph/pose_graph

camera_visual /pose_graph/camera_pose_visual

loop_match_image /pose_graph/match_image 与之前的图像匹配上

Marker /pose_graph/key_odometrys

Sequence1 /pose_graph/path_1

~2

~3

~4

~5

no_loop_path /pose_graph/no_loop_path

include "estimator.h"

Estimator::Estimator(): f_manager{Rs}

{

ROS_INFO("init begins");

clearState();

}

void Estimator::setParameter()

{

for (int i = 0; i < NUM_OF_CAM; i++)

{

tic[i] = TIC[i];

ric[i] = RIC[i];

}

f_manager.setRic(ric);

ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();

ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();

td = TD;

}

void Estimator::clearState()

{

for (int i = 0; i < WINDOW_SIZE + 1; i++)

{

Rs[i].setIdentity();

Ps[i].setZero();

Vs[i].setZero();

Bas[i].setZero();

Bgs[i].setZero();

dt_buf[i].clear();

linear_acceleration_buf[i].clear();

angular_velocity_buf[i].clear();

if (pre_integrations[i] != nullptr)

delete pre_integrations[i];

pre_integrations[i] = nullptr;

}

for (int i = 0; i < NUM_OF_CAM; i++)

{

tic[i] = Vector3d::Zero();

ric[i] = Matrix3d::Identity();

}

for (auto &it : all_image_frame)

{

if (it.second.pre_integration != nullptr)

{

delete it.second.pre_integration;

it.second.pre_integration = nullptr;

}

}

solver_flag = INITIAL;

first_imu = false,

sum_of_back = 0;

sum_of_front = 0;

frame_count = 0;

solver_flag = INITIAL;

initial_timestamp = 0;

all_image_frame.clear();

td = TD;

if (tmp_pre_integration != nullptr)

delete tmp_pre_integration;

if (last_marginalization_info != nullptr)

delete last_marginalization_info;

tmp_pre_integration = nullptr;

last_marginalization_info = nullptr;

last_marginalization_parameter_blocks.clear();

f_manager.clearState();

failure_occur = 0;

relocalization_info = 0;

drift_correct_r = Matrix3d::Identity();

drift_correct_t = Vector3d::Zero();

}

//处理imu数据的接口函数,其中，0代表上次测量值，1代表当前测量值，

//delta_p，delta_q，delta_v代表相对预积分初始参考系的位移，旋转四元数，以及速度(例如，从k帧预积分到k+1帧，则参考系是k帧的imu坐标系)

void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity)

{

if (!first_imu)

{

first_imu = true;

acc_0 = linear_acceleration;

gyr_0 = angular_velocity;

}

if (!pre_integrations[frame_count])

{

pre_integrations[frame_count] = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};

}

if (frame_count != 0)

{

pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);

//if(solver_flag != NON_LINEAR)

tmp_pre_integration->push_back(dt, linear_acceleration, angular_velocity);

dt_buf[frame_count].push_back(dt);

linear_acceleration_buf[frame_count].push_back(linear_acceleration);

angular_velocity_buf[frame_count].push_back(angular_velocity);

int j = frame_count;

Vector3d un_acc_0 = Rs[j] * (acc_0 - Bas[j]) - g;

Vector3d un_gyr = 0.5 * (gyr_0 + angular_velocity) - Bgs[j];

Rs[j] *= Utility::deltaQ(un_gyr * dt).toRotationMatrix();

Vector3d un_acc_1 = Rs[j] * (linear_acceleration - Bas[j]) - g;

Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);

Ps[j] += dt * Vs[j] + 0.5 * dt * dt * un_acc;

Vs[j] += dt * un_acc;

}

acc_0 = linear_acceleration;

gyr_0 = angular_velocity;

}

void Estimator::processImage(const map>>> &image, const std_msgs::Header &header)

{

ROS_DEBUG("new image coming ------------------------------------------");

ROS_DEBUG("Adding feature points %lu", image.size());

if (f_manager.addFeatureCheckParallax(frame_count, image, td))//判断该帧是否关键帧

marginalization_flag = MARGIN_OLD;

else

marginalization_flag = MARGIN_SECOND_NEW;

ROS_DEBUG("this frame is--------------------%s", marginalization_flag ? "reject" : "accept");

ROS_DEBUG("%s", marginalization_flag ? "Non-keyframe" : "Keyframe");

ROS_DEBUG("Solving %d", frame_count);

ROS_DEBUG("number of feature: %d", f_manager.getFeatureCount());

Headers[frame_count] = header;

ImageFrame imageframe(image, header.stamp.toSec());

imageframe.pre_integration = tmp_pre_integration;

all_image_frame.insert(make_pair(header.stamp.toSec(), imageframe));

tmp_pre_integration = new IntegrationBase{acc_0, gyr_0, Bas[frame_count], Bgs[frame_count]};

if(ESTIMATE_EXTRINSIC == 2)

{

ROS_INFO("calibrating extrinsic param, rotation movement is needed");

if (frame_count != 0)

{

vector> corres = f_manager.getCorresponding(frame_count - 1, frame_count);

Matrix3d calib_ric;

if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))

//使用imu pre-integration获取的旋转矩阵

//会和视觉跟踪求解fundamentalMatrix分解后获得的旋转矩阵构建约束方程，从而标定出外参旋转矩阵

{

ROS_WARN("initial extrinsic rotation calib success");

ROS_WARN_STREAM("initial extrinsic rotation: " << endl << calib_ric);

ric[0] = calib_ric;

RIC[0] = calib_ric;

ESTIMATE_EXTRINSIC = 1;

}

}

}

//线性初始化

if (solver_flag == INITIAL)

{

if (frame_count == WINDOW_SIZE)

{

bool result = false;

if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)

{

result = initialStructure();

initial_timestamp = header.stamp.toSec();

}<