VINS-Mono 代码解析二、初始化 第2部分

书接上文!

(4)对img信息进行处理(  process()  核心!)

            map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> image;
            
            //(2)这个数据结构定义完之后,接下来就是往这个容器中放数据。
            //遍历img_msg里面的每一个特征点的归一化坐标
            for (unsigned int i = 0; i < img_msg->points.size(); i++)
            {
                //把img的信息提取出来放在image容器里去,通过这里,可以理解img信息里面装的都是些什么
                int v = img_msg->channels[0].values[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->channels[1].values[i];
                double p_v = img_msg->channels[2].values[i];
                double velocity_x = img_msg->channels[3].values[i];
                double velocity_y = img_msg->channels[4].values[i];
                ROS_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);
            }
            
            //(3) 处理图像processImage() (核心!)
            //这一部分的内容非常多,将一层层剥开讲解!这里实现了视觉与IMU的初始化以及非线性优化的紧耦合。
            estimator.processImage(image, img_msg->header);

相关概念:

新的数据结构 : map int, vector  <  pair <  int, Eigen::Matrix<double, 7, 1>   >  >  > image

             1、虽然它叫image,但是这个容器里面存放的信息是每一个特征点的

             2、索引值是feature_id;

             3、value值是一个vector,如果系统是多目的,那么同一个特征点在不同摄像头下会有不同的观测信息,那么这个vector,就是存储着某个特征点在所有摄像头上的信息。对于VINS-mono来说,value它不是vector,仅仅是一个pair,其实是可以的。

             4、接下来看这个vector里面的每一pair。int对应的是camera_id,告诉我们这些数据是当前特征点在哪个摄像头上获得的

             5、Matrix<double, 7, 1>是一个7维向量,依次存放着当前 feature_id 的特征点在 camera_id 的相机中的归一化坐标,像素坐标 和 像素运动速度,这些信息都是在feature_tracker_node.cpp中获得的;

分析:把每张图像中 所有特征点 信息取出来放在 image容器里去,然后进行  estimator.processImage(image, img_msg->header)

因此,最重要的是分析 processImage()  这个程序!其如下所示:

子程序  processImage()

void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const std_msgs::Header &header)
{
    ROS_DEBUG("new image coming ------------------------------------------");
    ROS_DEBUG("Adding feature points %lu", image.size());
    
//【1】通过检测两帧之间的视差决定是否作为关键帧(经过旋转补偿),同时添加之前检测到的特征点到feature容器中,计算每一个点跟踪的次数,以及它的视差, 并以此来选择边缘化的方式,image中存放的是归一化平面上的点,这里就是关键帧的选择。
    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;

    //【2】将图像数据、时间、临时预积分值存储到图像帧类中,ImageFrame这个类的定义在initial_alignment.h中
    //数据结构: ImageFrame imageframe,imageframe是ImageFrame的一个实例,定义在initial/initial_alignment.h里
    ImageFrame imageframe(image, header.stamp.toSec());
    imageframe.pre_integration = tmp_pre_integration;
    
    //可以在此处new一个pre_integration然后深度赋值给tmp_pre_integration,减少计算量
    //very important!!这里不能使用pre_integrations[frame_count]来进行赋值,因为pre_integrations[frame_count]可能是多帧的预积分!
    //imageframe.pre_integration = new IntegrationBase(*(pre_integrations[frame_count]));
    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]};

    //【3】如果需要标定外参,则标定
    if(ESTIMATE_EXTRINSIC == 2)//如果没有外参则进行标定
    {
        ROS_INFO("calibrating extrinsic param, rotation movement is needed");
        if (frame_count != 0)
        {
            //得到两帧之间归一化特征点
            vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
            Matrix3d calib_ric;//标定从camera到IMU之间的外参数
            if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
            {
                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;// 完成外参标定
            }
        }
    }

    //【4】进行初始化,一般初始化只进行一次;
    if (solver_flag == INITIAL)
    {
        //frame_count是滑动窗口中图像帧的数量,一开始初始化为0,滑动窗口总帧数WINDOW_SIZE=10
        if (frame_count == WINDOW_SIZE)
        {
            bool result = false;
            //确保有足够的frame参与初始化,有外参,且当前帧时间戳大于初始化时间戳+0.1秒
            if( ESTIMATE_EXTRINSIC != 2 && (header.stamp.toSec() - initial_timestamp) > 0.1)
            {
                // 4.1 视觉惯性联合初始化-包括纯视觉SfM,视觉和IMU的松耦合
               result = initialStructure();
                // 4.2 更新初始化时间戳-
               initial_timestamp = header.stamp.toSec();
            }
            if(result)//初始化成功则进行一次非线性优化
            {
                solver_flag = NON_LINEAR;// 先进行一次滑动窗口非线性优化,得到当前帧与第一帧的位姿
                solveOdometry();         // 执行非线性优化具体函数solveOdometry()
                slideWindow();           // 滑动窗
                
            // 4.5 剔除feature中估计失败的点(solve_flag == 2)0 haven't solve yet; 1 solve succ;
                f_manager.removeFailures();
                ROS_INFO("Initialization finish!");
                
                // 4.6 初始化窗口中PVQ
                last_R = Rs[WINDOW_SIZE];//得到当前帧与第一帧的位姿
                last_P = Ps[WINDOW_SIZE];
                last_R0 = Rs[0];
                last_P0 = Ps[0];
                
            }
            else
                slideWindow();// 初始化失败则直接滑动窗口
                //因为只有在solveOdometry后才确定特征点的solve_flag(是否成功解算得到depth值)
        }
        else
            frame_count++;// 图像帧数量+1,直到满足窗口数量
    }
    else//【5】紧耦合的非线性优化,这是一个重要模块。一般非线性优化是稳定状态下VINS系统一直要循环的部分
    {
        TicToc t_solve;
        solveOdometry();// 5.1 非线性化求解里程计
        ROS_DEBUG("solver costs: %fms", t_solve.toc());

         //5.2 故障检测与恢复,一旦检测到故障,系统将切换回初始化阶段
        if (failureDetection())
        {
            ROS_WARN("failure detection!");
            failure_occur = 1;//失败标志位=1
            clearState();     // 清空状态
            setParameter();   // 重设参数
            ROS_WARN("system reboot!");
            return;
        }

        TicToc t_margin;
        slideWindow();             //5.3 滑动窗口
        f_manager.removeFailures();//5.4 去除估计失败的点并发布关键点位置/
        ROS_DEBUG("marginalization costs: %fms", t_margin.toc());
        
        // 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];  //用于failure dectection
        last_P = Ps[WINDOW_SIZE];
        last_R0 = Rs[0];
        last_P0 = Ps[0];
        
        //而optimization()恰好在solveOdometry()里,所以可以发现,代码是先优化,后marg
    }
}

注释:

先看函数名,它传入的参数分别是 当前帧上的所有特征点  和  当前帧的时间戳
1、addFeatureCheckParallax()添加之前检测到的特征点到feature容器list中,计算每一个点跟踪的次数,以及它的视差并通过检测两帧之间的视差决定是否作为关键帧;
2、判断并进行外参标定;
3、进行视觉惯性联合初始化或基于滑动窗口非线性优化的紧耦合VIO;

  • frame_count:窗口内帧的个数
  • image       :       某帧所有特征点的[camera_id,[x,y,z,u,v,vx,vy]]构成的map,索引为feature_id
  • td               :       相机和IMU同步校准得到的时间差
  • header     :      某帧图像的头信息

这里出现了一个新的数据结构  f_manager 和一个新的函数  addFeatureCheckParallax()---定义在  feature_manager.cpp
数据结构: f_manager
f_manager是FeatureManager的一个对象。它定义在utility/feature_manager.h里。
f_manager可以看作为一个存放着滑窗内所有特征点信息的容器,其中最关键的部分是list<FeaturePerId> feature。其中每一个特征点,可以看作是一个FeaturePerId的对象,它存放着一个特征点在滑窗中的所有信息,其中最关键的部分是vector<FeaturePerFrame> feature_per_frame。其中一个特征点在一个帧中的信息,可以看作是一个FeaturePerFrame的对象,它存放着一个特征点在滑窗里一个帧里面的信息,包括归一化坐标,像素坐标,像素速度等。
 

下面对 processImage()  进行一步一步的分析和解读,这一部分会和理论 密切,自己也会推导一遍,解我心头之恨!

【1】通过检测两帧之间的视差决定是否作为关键帧(经过旋转补偿)

 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;

子程序  f_manager.addFeatureCheckParallax(frame_count, image, td)

bool FeatureManager::addFeatureCheckParallax(int frame_count, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, double td)
{
    ROS_DEBUG("input feature: %d", (int)image.size());// 特征点数量
    ROS_DEBUG("num of feature: %d", getFeatureCount());// 能够作为特征点的数量
    double parallax_sum = 0;// 所有特征点视差总和
    int parallax_num = 0;   //平行特征点数
    last_track_num = 0;     //统计在滑窗中的特征点有多少个在当前帧中继续被追踪到了
    
    // 1. 把image map中的所有特征点放入feature list容器中
    // 遍历特征点,看该特征点是否在特征点的列表中,如果没在,则将<FeatureID,Start_frame>存入到Feature列表中;否则统计数目
    for (auto &id_pts : image) //遍历当前帧的每一个特征点
    {
        //把当前特征点封装成一个FeaturePerFrame对象
        //point 每帧的特征点[x,y,z,u,v,vx,vy]和td IMU和cam同步时间差
        FeaturePerFrame f_per_fra(id_pts.second[0].second, td);
        
         //获取当前帧的feature_id
        int feature_id = id_pts.first;
        
        //在滑窗的所有特征点中,看看能不能找到当前这个特征点
        auto it = find_if(feature.begin(), feature.end(), [feature_id](const FeaturePerId &it)
                          {
            return it.feature_id == feature_id;
                          });

        //如果这个特征点是一个新的特征(在特征点库里没有找到),那么就把它加入到滑窗的特征点库里
        // 并在feature管理器的list容器最后添加:FeaturePerId、FeaturePerFrame
        if (it == feature.end())
        {
            // (特征点ID,首次观测到特征点的图像帧ID)
            feature.push_back(FeaturePerId(feature_id, frame_count));
            feature.back().feature_per_frame.push_back(f_per_fra);
        }
        
        //如果这个特征在滑窗中已经被观测到过,那么就补充上这个特征点在当前帧的数据,并且把共视点统计数+1
        else if (it->feature_id == feature_id)
        {
            it->feature_per_frame.push_back(f_per_fra);
            last_track_num++;// 此帧有多少相同特征点被跟踪
        }
    }

    //如果总共2帧,或者说共视点<20,那么说明次新帧是关键帧,marg_old
    if (frame_count < 2 || last_track_num < 20)
        return true;

    // 遍历滑窗中的每一个特征点,计算每个特征在次新帧和次次新帧中的视差
    for (auto &it_per_id : feature)
    {
         //如果当前特征点在当前帧-2以前出现过而且至少在当前帧-1还在,那么他就是平行特征点
        if (it_per_id.start_frame <= frame_count - 2 &&
            it_per_id.start_frame + int(it_per_id.feature_per_frame.size()) - 1 >= frame_count - 1)
        {
            // 总视差:该特征点在两帧的归一化平面上的坐标点的距离ans
            parallax_sum += compensatedParallax2(it_per_id, frame_count);
            parallax_num++; //平行特征点数
        }
    }

    //判断标准1:平行特征点数为0
    if (parallax_num == 0)
    {
        return true;
    }
    else//判断标准2:平均平行度小于threshold
    {
        ROS_DEBUG("parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num);
        ROS_DEBUG("current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH);
        
        //平均视差大于阈值的是关键帧
        return parallax_sum / parallax_num >= MIN_PARALLAX;
    }
    
    /*这一部分代码完全在秦通大神的IV.A部分体现了。秦神在这部分里写了2个判断关键帧的判断指标,第一个是“the average parallax apart from the previous keyframe”,对应着代码中parallax_num和parallax_sum / parallax_num;第二个是“If the number of tracked features goes below a certain threshold, we treat this frame as a new keyframe”,对应着代码里的last_track_num。注意,这部分里还有一个函数是compensatedParallax2(),用来计算当前特征点的视差*/
}

功能:

通过检测两帧之间的视差决定是否作为关键帧(经过旋转补偿),同时添加之前检测到的特征点到feature容器中,计算每一个点跟踪的次数,以及它的视差, 并以此来选择边缘化的方式,image中存放的是归一化平面上的点,这里就是关键帧的选择。输入的是特征点,但是会把能观测到这个特征点的所有帧也都放进去,第一个索引是特征点ID,第二个索引是观测到该特征点的相机帧 ID。

介绍:FeatureManager 中几个变量:

为什么要检查视差?
VINS中为了控制优化计算量,只对当前帧之前某一部分帧进行优化,而不是全部历史帧,局部优化帧数量的大小就是窗口大小。为了维持窗口大小,需要去除旧帧添加新帧,也就是边缘化Marginalization。到底是删去最旧的帧(MARGIN_OLD)还是删去刚刚进来窗口倒数第二帧(MARGIN_SECOND_NEW),就需要对当前帧与之前帧进行视差比较,如果是当前帧变化很小,就会删去倒数第二帧,如果变化很大,就删去最旧的帧,通过检测两帧之间的视差以及特征点数量决定 次新帧是否作为关键帧。

关键帧选取:

  • 当前帧相对最近的关键帧的特征平均视差大于一个阈值就为关键帧(因为视差可以根据平移和旋转共同得到,而纯旋转则导致不能三角化成功,所以这一步需要IMU预积分进行补偿)
  • 当前帧跟踪到的特征点数量小于阈值视为关键帧;
     

可能才开始看这些话有些不明白,把整个系统理解之后才逐渐明白的!下面我用白话文说一下吧!我们优化方法可以是EKF,也可以是全局优化,但是他们都有自己的优缺点呀!

EKF时间快,但是精度相对全局优化低;全局优化精度高,但运行时间慢呀;那怎么办?当然是找一个取中的方案了,就是所谓的滑动窗口呗,优化一定数量的关键帧,既可以满足实时性,又提高了精度;既然是滑动窗口,还要保持窗口内帧的数量固定,那你当然要一边进来帧,一边出去帧吧,这个很容易理解!而这时候问题就来了,我们应该让哪些帧出去,需要有个标准呀,这个标准就是这段代码做的事情。假如,滑动窗口进来了新的一帧图像,经过与前一关键帧比较,你发现特征点有甚多一样的,说明了什么呀!当然是你这个相机相对前一帧运动的很小了,也就是说获取新的外界信息很小,那这个帧就没有必要要了吧,自己感受一下这个意思吧!反之,我们就把最新帧设置成关键帧,进行优化呀,保存呀,用于回环检测呀....

【2】数据的赋值

 
    ImageFrame imageframe(image, header.stamp.toSec());
    imageframe.pre_integration = tmp_pre_integration;
    
    //可以在此处new一个pre_integration然后深度赋值给tmp_pre_integration,减少计算量
    //very important!!这里不能使用pre_integrations[frame_count]来进行赋值,因为pre_integrations[frame_count]可能是多帧的预积分!
    //imageframe.pre_integration = new IntegrationBase(*(pre_integrations[frame_count]));
    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]};

功能:

图像数据、时间、临时预积分值 存储到图像帧类中,   ImageFrame  这个类的定义在initial_alignment.h中;数据结构: ImageFrame imageframe,imageframe是ImageFrame的一个实例,定义在initial/initial_alignment.h里

【3】如果需要标定外参,则标定

if(ESTIMATE_EXTRINSIC == 2)//如果没有外参则进行标定
    {
        ROS_INFO("calibrating extrinsic param, rotation movement is needed");
        if (frame_count != 0)
        {
            //得到两帧之间归一化特征点
            vector<pair<Vector3d, Vector3d>> corres = f_manager.getCorresponding(frame_count - 1, frame_count);
            Matrix3d calib_ric;//标定从camera到IMU之间的外参数
            if (initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric))
            {
                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;// 完成外参标定
            }
        }
    }

1、子程序  f_manager.getCorresponding(frame_count - 1, frame_count)

vector<pair<Vector3d, Vector3d>> FeatureManager::getCorresponding(int frame_count_l, int frame_count_r)
{
    vector<pair<Vector3d, Vector3d>> corres;
    for (auto &it : feature)// 遍历滑动窗口中的所有特征点=feature ;
    {
        if (it.start_frame <= frame_count_l && it.endFrame() >= frame_count_r)
        {
            // 如果左右两帧在 该特征点开始帧和结尾帧中间,则左右帧一定观测到了这个特征点; 
            Vector3d a = Vector3d::Zero(), b = Vector3d::Zero();
            int idx_l = frame_count_l - it.start_frame;// 当前帧-第一次观测到特征点的帧数
            int idx_r = frame_count_r - it.start_frame;

            a = it.feature_per_frame[idx_l].point;
            b = it.feature_per_frame[idx_r].point;
            
            corres.push_back(make_pair(a, b));
        }
    }
    return corres;
}

目的:求出前后两帧中对应的特征点,返回到 corres里面;

2、子程序  initial_ex_rotation.CalibrationExRotation(corres, pre_integrations[frame_count]->delta_q, calib_ric)

输入参数为:vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, 匹配的特征点 和 IMU预积分得的旋转矩阵Q;

理论部分可以参考下面:

VINS 细节系列 - IMU 相机 外参在线标定_努力努力努力-CSDN博客

/*
1、输入参数为:vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, 匹配的特征点 和 IMU预积分得的旋转矩阵Q
2、输出参数:Matrix3d &calib_ric_result  标定的外参数

*/
bool InitialEXRotation::CalibrationExRotation(vector<pair<Vector3d, Vector3d>> corres, Quaterniond delta_q_imu, Matrix3d &calib_ric_result)
{
    frame_count++;
    Rc.push_back(solveRelativeR(corres));// 相机帧之间旋转矩阵
    Rimu.push_back(delta_q_imu.toRotationMatrix());//IMU之间旋转矩阵,由IMU预积分得到
    Rc_g.push_back(ric.inverse() * delta_q_imu * ric);//每帧IMU相对于起始帧IMU的R,初始化为IMU预积分

    // 2.SVD分解,Ax=0,对A填充
    Eigen::MatrixXd A(frame_count * 4, 4);// 多帧组成A,一对点组成的A是4*4的。
    A.setZero();
    int sum_ok = 0;
    for (int i = 1; i <= frame_count; i++)
    {
        // 2.1求解核函数
        Quaterniond r1(Rc[i]);
        Quaterniond r2(Rc_g[i]);

        double angular_distance = 180 / M_PI * r1.angularDistance(r2);
        ROS_DEBUG(
            "%d %f", i, angular_distance);

        //huber核函数做加权
        double huber = angular_distance > 5.0 ? 5.0 / angular_distance : 1.0;
        
        ++sum_ok;
        // 2.2 分别为L和R赋值,L R 分别为左乘和右乘矩阵,都是4*4矩阵
        //R_bk+1^bk * R_c^b = R_c^b * R_ck+1^ck
        //[Q1(q_bk+1^bk) - Q2(q_ck+1^ck)] * q_c^b = 0
 
        Matrix4d L, R;

        double w = Quaterniond(Rc[i]).w();// 相机间旋转矩阵
        Vector3d q = Quaterniond(Rc[i]).vec();
        L.block<3, 3>(0, 0) = w * Matrix3d::Identity() + Utility::skewSymmetric(q);
        L.block<3, 1>(0, 3) = q;
        L.block<1, 3>(3, 0) = -q.transpose();
        L(3, 3) = w;

        Quaterniond R_ij(Rimu[i]);// IMU间旋转矩阵
        w = R_ij.w();
        q = R_ij.vec();
        R.block<3, 3>(0, 0) = w * Matrix3d::Identity() - Utility::skewSymmetric(q);
        R.block<3, 1>(0, 3) = q;
        R.block<1, 3>(3, 0) = -q.transpose();
        R(3, 3) = w;

        A.block<4, 4>((i - 1) * 4, 0) = huber * (L - R);
    }

    // 2.3 svd分解中最小奇异值对应的右奇异向量作为旋转四元数
    JacobiSVD<MatrixXd> svd(A, ComputeFullU | ComputeFullV);
    
    //这里的四元数存储的顺序是[x,y,z,w]',即[qv qw]'
    Matrix<double, 4, 1> x = svd.matrixV().col(3);// 右奇异向量作为旋转四元数
    Quaterniond estimated_R(x);
    ric = estimated_R.toRotationMatrix().inverse();// 求逆
    //cout << svd.singularValues().transpose() << endl;
    //cout << ric << endl;
    Vector3d ric_cov;
    ric_cov = svd.singularValues().tail<3>();
    
    //3.至少迭代计算了WINDOW_SIZE次,且R的奇异值大于0.25才认为标定成功
    if (frame_count >= WINDOW_SIZE && ric_cov(1) > 0.25)
    {
        calib_ric_result = ric;
        return true;
    }
    else
        return false;
}

什么是内参?什么是外参?可以看一下我的博客!  针孔模型相机 坐标系之间的变换 详解_努力努力努力-CSDN博客

外参是相机与IMU的转换矩阵,实际情况相机和IMU是放在载体上面的,只是位置不一样而以,我们在跑公共数据集的时候,会直接加载别人给的外参,但是如果我们想跑自己的数据就必须估计出我们相机与IMU之间的外参;外参的估计是很重要的,这里涉及到后面的联合初始化问题,后面我们推导的时候再说吧!

这里强调一下,由于触发延时、传输延时的存在,传感器的采样时间和时间戳的时间不匹配,如下图所示,从而导致相机和IMU之间存在时间差td。IMU与相机的之间的时间差td也是外参,t_imu  =  t_cam  +  td;   将相机的时间戳平移td后,相机与IMU之间实现了同步(相机硬件同步的基础上再次实现的时间同步)。

下面我们重点推导一下外参的理论部分,在与我们的代码结合分析:

利用旋转约束估计外参数旋转 q_{bc}:   

上式可写成:

 疑问:为什么公式(7)采用SVD求解,最小奇异值对应的奇异向量就是方程的解?

答:请看这个连接!     https://blog.csdn.net/hltt3838/article/details/108834711

然后你把这些东西看懂,在去看上面的程序,就会很轻松,顶多是C++不理解,而不是概念的问题,反正我是这样的;还有为什么要加核函数?网上都有解释的!

这一部分我们就讲到这吧,因为后面的更多的概念、公式需要解释和推导!

  • 5
    点赞
  • 20
    收藏
    觉得还不错? 一键收藏
  • 打赏
    打赏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

他人是一面镜子,保持谦虚的态度

你的鼓励是我最大的动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值