VINS

http://blog.csdn.net/qq_31785865/article/details/75220304

https://www.cnblogs.com/shhu1993/archive/2017/06/03/6938715.html

http://blog.csdn.net/u012871872/article/details/78128087?locationNum=8&fps=1

http://blog.csdn.net/q597967420/article/details/76099409#/

VINS原理框架

1. Measurement Preprocessing(观测预处理):对图像提取 Harris 特征（feature）做特征跟踪（ feature tracking）,输出跟踪特征列表（ tracked feature list）, 对IMU做预积分,输出两帧图像间的IMU积分结果.

2. Initialization（初始化）: vision-only SfM用纯视觉估计相机运动和特征深度, 视觉得到一个相对运动, IMU预积分得到一个相对运动, 二者做对齐（visual-inertial alignment）, 从而标定出尺度, 重力加速地, 速度, 和bias.
3. Local BA: 后端融合优化, 状态向量包括: sliding window中的IMU states + 相机外参 + feature点的深度. loss包括: marginalization提供的prior + IMU residuals + visual residuals. 优化完之后, 得到各个时刻相机的位姿, 实现VIO.
4. Loop detection（环路检测） + Global Pose Graph Optimization（全局图优化）: VIO有累积误差, 加入闭环检测（fast特征提取+BRIEF描述子、利用BoW的闭环检测）, 然后图优化, 只优化4 个自由度, 因为scale, roll 和 pitch是可观的, 误差只会在(x,y,z)和yaw四个维度上累积.

算法的核心在于后端Visual Inertial融合优化部分--VIO--目前视觉和IMU融合分为松耦合和紧耦合, 松耦合的性能要逊色于紧耦合. 紧耦合的主流融合框架有基于滤波的方法(MSCKF)和基于优化的方法(VINS-Mono).

非线性优化

非线性优化是VINS中非常创新的一部分，也是整个代码中最为复杂的部分。

在全局非线性优化中，第一项为先验信息优化，第二项为IMU测量残差优化，第三项为camera测量残差优化，第四项为全局闭环残差优化

git地址：https://github.com/HKUST-Aerial-Robotics/VINS-Mono.git

系统启动命令：

$ roslaunch vins_estimator euroc.launch
$ roslaunch vins_estimator vins_rviz.launch 
$ rosbag play YOUR_PATH_TO_DATASET/MH_03_medium.bag 
$ roslaunch benchmark_publisher publish.launch  sequence_name:=MH_03_medium

代码目录

核心：feature_tracker、vins_estimator

node和topic的拓扑图：

rosbag将记录好的imu数据和单目相机获取的图像数据分别发布到/imu0和/cam0/image_raw话题；/feature_tracker节点通过订阅/cam0/image_raw话题获取图像数据，/vins_estimator节点通过订阅/imu0话题获取imu数据，同时/feature_tracker节点将提取出的图像特征发布到/feature_tracker/feature话题，由/vins_estimator订阅获取。

/feature_tracker节点中就一个主线程, 接收图像信息,调用img_callback()函数进行处理，负责视觉提取和跟踪./vins_estimator节点中开了3个子线程, 分别是:process,loop_detection和pose_graph，分别处理VIO后端、闭环检测、全局优化，是融合系统的主要部分。如果不需要做闭环检测, 可以把LOOP_CLOUSRE置0, 则后面两个线程不会创建.

代码框架：

Feature tracker

这部分代码在feature_tracker包下面，主要是接收图像topic,使用KLT光流算法跟踪特征点，同时保持每一帧图像有最少的(100-300)个特征点。

根据配置文件中的freq，确定每隔多久的时候，把检测到的特征点打包成/feature_tracker/featuretopic 发出去，如果没有达到发送的时间，这幅图像的feature就作为下一时刻的KLT追踪的特征点，因此不是每一副图像都要处理。为了保证好的前端，这里的freq配置文件至少设置10。

void img_callback(const sensor_msgs::ImageConstPtr &img_msg)
{
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
        ROS_DEBUG("processing camera %d", i);
        if (i != 1 || !STEREO_TRACK)
        //调用FeatureTracker的readImage
            trackerData[i].readImage(ptr->image.rowRange(ROW * i, ROW * (i + 1)));
    }

    for (unsigned int i = 0;; i++)
    {
        bool completed = false;
        for (int j = 0; j < NUM_OF_CAM; j++)
            if (j != 1 || !STEREO_TRACK)
            //更新feature的ID
                completed |= trackerData[j].updateID(i);
        if (!completed)
            break;
    }
    
    //发布特征点topic
    if (round(1.0 * pub_count / (img_msg->header.stamp.toSec() - first_image_time)) <= FREQ)
    {
        sensor_msgs::PointCloudPtr feature_points(new sensor_msgs::PointCloud);
        //特征点的id，图像的(u,v)坐标
        sensor_msgs::ChannelFloat32 id_of_point;
        sensor_msgs::ChannelFloat32 u_of_point;
        sensor_msgs::ChannelFloat32 v_of_point;

        pub_img.publish(feature_points);

    }

     if (SHOW_TRACK)
     {
        //根据特征点被追踪的次数，显示他的颜色，越红表示这个特征点看到的越久，一幅图像要是大部分特征点是蓝色，代表前端tracker效果很差
        double len = std::min(1.0, 1.0 * trackerData[i].track_cnt[j] / WINDOW_SIZE);
        cv::circle(tmp_img, trackerData[i].cur_pts[j], 2, cv::Scalar(255 * (1 - len), 0, 255 * len), 2);
     }


}

void FeatureTracker::readImage(const cv::Mat &_img)
{
    //直方图均匀化
    //if image is too dark or light, trun on equalize to find enough features
    if (EQUALIZE)
    {
        cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0, cv::Size(8, 8));
        TicToc t_c;
        clahe->apply(_img, img);
        ROS_DEBUG("CLAHE costs: %fms", t_c.toc());
    }

    if (cur_pts.size() > 0)
    {
        TicToc t_o;
        vector<uchar> status;
        vector<float> err;
        //根据上一时刻的cur_img,cur_pts,寻找当前时刻的forw_pts,
        cv::calcOpticalFlowPyrLK(cur_img, forw_img, cur_pts, forw_pts, status, err, cv::Size(21, 21), 3);
    }

    if (img_cnt == 0)
    {
        //根据fundamentalMatrix中的ransac去除一些outlier
        rejectWithF();
        //跟新特征点track的次数
        for (auto &n : track_cnt)
            n++;
        //为下面的goodFeaturesToTrack保证相邻的特征点之间要相隔30个像素,设置mask image
        setMask();

        int n_max_cnt = MAX_CNT - static_cast<int>(forw_pts.size());
        if (n_max_cnt > 0)
        {
            //保证每个image有足够的特征点，不够就新提取
            cv::goodFeaturesToTrack(forw_img, n_pts, MAX_CNT - forw_pts.size(), 0.1, MIN_DIST, mask);
        }


    }
}

Slide Window

主要是对imu的数据进行预积分，vision重投影误差的构造，loop-closure的检测，slide-window的维护，marginzation prior的维护。。。。

loop-closure的检测使用视觉词带，这里的特征是通过订阅IMAGE_TOPIC,传递到闭环检测部分，重新检测。

核心：最小二乘法方程构建

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

    if (frame_count != 0)
    {
        pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);
        //调用imu的预积分，propagation ,计算对应的雅可比矩阵
        //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);

        //提供imu计算的当前位置，速度，作为优化的初值
        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;
    }

}

void Estimator::processImage(const map<int, vector<pair<int, Vector3d>>> &image, const std_msgs::Header &header)
{
    //根据视差判断是不是关键帧，
    if (f_manager.addFeatureCheckParallax(frame_count, image))
        marginalization_flag = MARGIN_OLD;
    else
        marginalization_flag = MARGIN_SECOND_NEW;

    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]};

//参数要是设置imu-camera的外参数未知，也可以帮你求解的
    if(ESTIMATE_EXTRINSIC == 2)
    {
    }

//初始化的流程
    if (solver_flag == INITIAL)
    {
        if (frame_count == WINDOW_SIZE)
        {
            bool result = false;
            if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
            {
            //构造sfm，优化imu偏差，加速度g，尺度的确定
               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[0];
                last_P0 = Ps[0];
                
            }
            else
                slideWindow();
        }
    //先凑够window-size的数量的Frame
        else
            frame_count++;
    }
    else
    {
       
        solveOdometry();

//失败的检测
        if (failureDetection())
        {
            clearState();
            setParameter();
            return;
        }

        slideWindow();
        f_manager.removeFailures();
        // prepare output of VINS
        key_poses.clear();
        for (int i = 0; i <= WINDOW_SIZE; i++)
            key_poses.push_back(Ps[i]);

        last_R = Rs[WINDOW_SIZE];
        last_P = Ps[WINDOW_SIZE];
        last_R0 = Rs[0];
        last_P0 = Ps[0];
    }

}

代码参考资料：http://blog.csdn.net/u012871872/article/details/78128087?locationNum=8&fps=1

代码注释参考资料：https://github.com/castiel520/VINS-Mono

https://www.zhihu.com/question/64381223

非线性优化

Feature tracker

Slide Window

猜你喜欢