VINS（七）estimator_node 数据对齐 imu预积分 vision

首先通过vins_estimator mode监听几个Topic（频率2000Hz），将imu数据，feature数据，raw_image数据（用于回环检测）通过各自的回调函数封装起来

ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, , imu_callback, ros::TransportHints().tcpNoDelay());

ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", , feature_callback);

ros::Subscriber sub_raw_image = n.subscribe(IMAGE_TOPIC, , raw_image_callback);

imu_buf.push(imu_msg);

feature_buf.push(feature_msg);

image_buf.push(make_pair(img_ptr->image, img_msg->header.stamp.toSec()));

然后开启处理measurement的线程

std::thread measurement_process{process};

process()函数中，首先将获取的传感器数据imu_buf feature_buf对齐，注意这里只保证了相邻的feature数据之间有完整的imu数据，并不能保证imu和feature数据的精确对齐

// multiple IMU measurements and only one vision(features) measurements

std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>>

getMeasurements()

{

    std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;

    while (true)

    {

        if (imu_buf.empty() || feature_buf.empty())

            return measurements;

        // synchronize, if strictly synchronize, should change to ">="

        // end up with : imu_buf.front()->header.stamp < feature_buf.front()->header.stamp

        // 1. should have overlap

        if (!(imu_buf.back()->header.stamp > feature_buf.front()->header.stamp))

        {

            ROS_WARN("wait for imu, only should happen at the beginning");

            sum_of_wait++;

            return measurements;

        }

        // 2. should have complete imu measurements between two feature_buf msg

        if (!(imu_buf.front()->header.stamp < feature_buf.front()->header.stamp))

        {

            ROS_WARN("throw img, only should happen at the beginning");

            feature_buf.pop();

            continue;

        }

        sensor_msgs::PointCloudConstPtr img_msg = feature_buf.front();

        feature_buf.pop();

        std::vector<sensor_msgs::ImuConstPtr> IMUs;

        while (imu_buf.front()->header.stamp <= img_msg->header.stamp)

        {

            IMUs.emplace_back(imu_buf.front());

            imu_buf.pop();

        }

        measurements.emplace_back(IMUs, img_msg);

    }

    return measurements;

}

接下来进入对measurements数据的处理：

处理imu数据的接口函数是processIMU()

处理vision数据的借口函数是processImage()

（一）IMU

1. 核心API：

midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,

                            linearized_ba, linearized_bg,

                            result_delta_p, result_delta_q, result_delta_v,

                            result_linearized_ba, result_linearized_bg, );

其中，0代表上次测量值，1代表当前测量值，delta_p，delta_q，delta_v代表相对预积分初始参考系的位移，旋转四元数，以及速度（例如，从k帧预积分到k+1帧，则参考系是k帧的imu坐标系）

对应实现的是公式：

相应的离散实现使用Euler，Mid-point，或者龙格库塔（RK4）数值积分方法。

Euler方法如下：

2. 求状态向量对bias的Jacobian，当bias变化较小时，使用Jacobian去更新状态；否则需要以当前imu为参考系，重新预积分，对应repropagation()。同时，需要计算error state model中误差传播方程的系数矩阵F和V：

    // pre-integration

    // time interval of two imu; last and current imu measurements; p,q,v relate to local frame; ba and bg; propagated p,q,v,ba,bg;

    // whether to update Jacobian and calculate F,V

    void midPointIntegration(double _dt,

                            const Eigen::Vector3d &_acc_0, const Eigen::Vector3d &_gyr_0,

                            const Eigen::Vector3d &_acc_1, const Eigen::Vector3d &_gyr_1,

                            const Eigen::Vector3d &delta_p, const Eigen::Quaterniond &delta_q, const Eigen::Vector3d &delta_v,

                            const Eigen::Vector3d &linearized_ba, const Eigen::Vector3d &linearized_bg,

                            Eigen::Vector3d &result_delta_p, Eigen::Quaterniond &result_delta_q, Eigen::Vector3d &result_delta_v,

                            Eigen::Vector3d &result_linearized_ba, Eigen::Vector3d &result_linearized_bg, bool update_jacobian)

    {

        //ROS_INFO("midpoint integration");

        // mid-point integration with bias = 0

        Vector3d un_acc_0 = delta_q * (_acc_0 - linearized_ba);

        Vector3d un_gyr = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;

        result_delta_q = delta_q * Quaterniond(, un_gyr() * _dt / , un_gyr() * _dt / , un_gyr() * _dt / );

        Vector3d un_acc_1 = result_delta_q * (_acc_1 - linearized_ba);

        Vector3d un_acc = 0.5 * (un_acc_0 + un_acc_1);

        result_delta_p = delta_p + delta_v * _dt + 0.5 * un_acc * _dt * _dt;

        result_delta_v = delta_v + un_acc * _dt;

        // ba and bg donot change

        result_linearized_ba = linearized_ba;

        result_linearized_bg = linearized_bg;

        // jacobian to bias, used when the bias changes slightly and no need of repropagation

        if(update_jacobian)

        {

            // same as un_gyr, gyrometer reference to the local frame bk

            Vector3d w_x = 0.5 * (_gyr_0 + _gyr_1) - linearized_bg;

            // last acceleration measurement

            Vector3d a_0_x = _acc_0 - linearized_ba;

            // current acceleration measurement

            Vector3d a_1_x = _acc_1 - linearized_ba;

            // used for cross-product

            // pay attention to derivation of matrix product

            Matrix3d R_w_x, R_a_0_x, R_a_1_x;

            R_w_x<<, -w_x(), w_x(),

                w_x(), , -w_x(),

                -w_x(), w_x(), ;

            R_a_0_x<<, -a_0_x(), a_0_x(),

                a_0_x(), , -a_0_x(),

                -a_0_x(), a_0_x(), ;

            R_a_1_x<<, -a_1_x(), a_1_x(),

                a_1_x(), , -a_1_x(),

                -a_1_x(), a_1_x(), ;

            // error state model

            // should use discrete format and mid-point approximation

            MatrixXd F = MatrixXd::Zero(, );

            F.block<, >(, ) = Matrix3d::Identity();

            F.block<, >(, ) = -0.25 * delta_q.toRotationMatrix() * R_a_0_x * _dt * _dt +

                                  -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt * _dt;

            F.block<, >(, ) = MatrixXd::Identity(,) * _dt;

            F.block<, >(, ) = -0.25 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt * _dt;

            F.block<, >(, ) = -0.25 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * _dt * -_dt;

            F.block<, >(, ) = Matrix3d::Identity() - R_w_x * _dt;

            F.block<, >(, ) = -1.0 * MatrixXd::Identity(,) * _dt;

            F.block<, >(, ) = -0.5 * delta_q.toRotationMatrix() * R_a_0_x * _dt +

                                  -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * (Matrix3d::Identity() - R_w_x * _dt) * _dt;

            F.block<, >(, ) = Matrix3d::Identity();

            F.block<, >(, ) = -0.5 * (delta_q.toRotationMatrix() + result_delta_q.toRotationMatrix()) * _dt;

            F.block<, >(, ) = -0.5 * result_delta_q.toRotationMatrix() * R_a_1_x * _dt * -_dt;

            F.block<, >(, ) = Matrix3d::Identity();

            F.block<, >(, ) = Matrix3d::Identity();

            MatrixXd V = MatrixXd::Zero(,);

            V.block<, >(, ) =  0.25 * delta_q.toRotationMatrix() * _dt * _dt;

            V.block<, >(, ) =  0.25 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * _dt * 0.5 * _dt;

            V.block<, >(, ) =  0.25 * result_delta_q.toRotationMatrix() * _dt * _dt;

            V.block<, >(, ) =  V.block<, >(, );

            V.block<, >(, ) =  0.5 * MatrixXd::Identity(,) * _dt;

            V.block<, >(, ) =  0.5 * MatrixXd::Identity(,) * _dt;

            V.block<, >(, ) =  0.5 * delta_q.toRotationMatrix() * _dt;

            V.block<, >(, ) =  0.5 * -result_delta_q.toRotationMatrix() * R_a_1_x  * _dt * 0.5 * _dt;

            V.block<, >(, ) =  0.5 * result_delta_q.toRotationMatrix() * _dt;

            V.block<, >(, ) =  V.block<, >(, );

            V.block<, >(, ) = MatrixXd::Identity(,) * _dt;

            V.block<, >(, ) = MatrixXd::Identity(,) * _dt;

            //step_jacobian = F;

            //step_V = V;

            jacobian = F * jacobian;

            covariance = F * covariance * F.transpose() + V * noise * V.transpose();

        }

    }

（二）Vision

首先判断该帧是否关键帧：

    if (f_manager.addFeatureCheckParallax(frame_count, image))

        marginalization_flag = MARGIN_OLD;

    else

        marginalization_flag = MARGIN_SECOND_NEW;

关键帧的判断依据是rotation-compensated过后的parallax足够大，并且tracking上的feature足够多；关键帧会保留在当前Sliding Window中，marginalize掉Sliding Window中最旧的状态，如果是非关键帧则优先marginalize掉。

1. 标定外参旋转矩阵

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

其中

pre_integrations[frame_count]->delta_q

是使用imu pre-integration获取的旋转矩阵，会和视觉跟踪求解fundamentalMatrix分解后获得的旋转矩阵构建约束方程，从而标定出外参旋转矩阵。

2. 线性初始化

    if (solver_flag == INITIAL)

    {

        if (frame_count == WINDOW_SIZE)

        {

            bool result = false;

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

            {

               result = initialStructure();

               initial_timestamp = header.stamp.toSec();

            }

            if(result)

            {

                solver_flag = NON_LINEAR;

                solveOdometry();

                slideWindow();

                f_manager.removeFailures();

                ROS_INFO("Initialization finish!");

                last_R = Rs[WINDOW_SIZE];

                last_P = Ps[WINDOW_SIZE];

                last_R0 = Rs[];

                last_P0 = Ps[];

            }

            else

                slideWindow();

        }

        else

            frame_count++;

    }

3. 非线性优化

    else

    {

        TicToc t_solve;

        solveOdometry();

        ROS_DEBUG("solver costs: %fms", t_solve.toc());

        if (failureDetection())

        {

            ROS_WARN("failure detection!");

            failure_occur = ;

            clearState();

            setParameter();

            ROS_WARN("system reboot!");

            return;

        }

        TicToc t_margin;

        slideWindow();

        f_manager.removeFailures();

        ROS_DEBUG("marginalization costs: %fms", t_margin.toc());

        // prepare output of VINS

        key_poses.clear();

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

            key_poses.push_back(Ps[i]);

        last_R = Rs[WINDOW_SIZE];

        last_P = Ps[WINDOW_SIZE];

        last_R0 = Rs[];

        last_P0 = Ps[];

    }

主要的初始化，非线性优化的api均在这里，因此放在后面去说明。

巴特西

VINS（七）estimator_node 数据对齐 imu预积分 vision

最新文章

热门文章