【无标题】

ok，在上一张中，重点针对processIMU这个函数进行了讲解
实际上

            if(USE_IMU)
            {
                if(!initFirstPoseFlag)
                    initFirstIMUPose(accVector);///IMU重力对齐，与世界坐标系对齐
                for(size_t i = 0; i < accVector.size(); i++)
                {
                    ///实际上得到的时间戳的顺序是 t0<accVector[0]<accVector[end]<t1
                    double dt;
                    if(i == 0)
                        dt = accVector[i].first - prevTime;///第一个惯性数据与图像之间的时间差
                    else if (i == accVector.size() - 1)///最后个惯性数据与图像之间的时间差
                        dt = curTime - accVector[i - 1].first;
                    else
                        dt = accVector[i].first - accVector[i - 1].first;///中间的惯性数据之间的时间差
                    ///预积分首先看下该函数的入参
                    //
                    //    t : 为当前数据的时间戳
                    //    dt : 为相邻两个IMU数据之间的时间间隔
                    //    linear_acceleration : 加速度
                    //    angular_velocity : 角速度
                    processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);
                }
            }

实际上它处于while（1）的循环当中，对 for(size_t i = 0; i < accVector.size(); i++) 这个accvector里面的长度进行积分，所以得到的预计分实际上是针对两帧图像之间的时间，但是里面包含的惯性的数据仍然在作世界坐标系下的累积积分
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;
此时的 RS[j] 还表示的是上一个时刻的旋转，还没有更新，注意这里的 j 表示的图像帧的数量。
、、

今天

在这里插入代码片///2.1.7
void Estimator::processMeasurements()
{
    while (1)
    {
        //printf("process measurments\n");
        pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > feature;
        vector<pair<double, Eigen::Vector3d>> accVector, gyrVector;
        if(!featureBuf.empty())
        {
            feature = featureBuf.front();
            curTime = feature.first + td;
            while(1)
            {
                if ((!USE_IMU  || IMUAvailable(feature.first + td)))
                    break;
                else
                {
                    printf("wait for imu ... \n");
                    if (! MULTIPLE_THREAD)
                        return;
                    std::chrono::milliseconds dura(5);
                    std::this_thread::sleep_for(dura);///睡眠5ms
                }
            }
            mBuf.lock();
            if(USE_IMU)得到图像时间之间的IMU
                getIMUInterval(prevTime, curTime, accVector, gyrVector);
            ///prevTime为上一帧图像的时间，curTime为当前帧图像的时间，
            ///accVector与gyrVector为获取图像帧之间的IMU数据
            featureBuf.pop();///删掉上一帧图像的数据
            mBuf.unlock();

            if(USE_IMU)
            {
                if(!initFirstPoseFlag)
                    initFirstIMUPose(accVector);///IMU重力对齐，与世界坐标系对齐
                for(size_t i = 0; i < accVector.size(); i++)
                {
                    ///实际上得到的时间戳的顺序是 t0<accVector[0]<accVector[end]<t1
                    double dt;
                    if(i == 0)
                        dt = accVector[i].first - prevTime;///第一个惯性数据与图像之间的时间差
                    else if (i == accVector.size() - 1)///最后个惯性数据与图像之间的时间差
                        dt = curTime - accVector[i - 1].first;
                    else
                        dt = accVector[i].first - accVector[i - 1].first;///中间的惯性数据之间的时间差
                    ///预积分首先看下该函数的入参
                    //
                    //    t : 为当前数据的时间戳
                    //    dt : 为相邻两个IMU数据之间的时间间隔
                    //    linear_acceleration : 加速度
                    //    angular_velocity : 角速度
                    processIMU(accVector[i].first, dt, accVector[i].second, gyrVector[i].second);
                }
            }

            processImage(feature.second, feature.first);
            prevTime = curTime;

            printStatistics(*this, 0);

            std_msgs::Header header;
            header.frame_id = "world";
            header.stamp = ros::Time(feature.first);

            pubOdometry(*this, header);
            pubKeyPoses(*this, header);
            pubCameraPose(*this, header);
            pubPointCloud(*this, header);
            pubKeyframe(*this);
            pubTF(*this, header);
        }

        if (! MULTIPLE_THREAD)
            break;

        std::chrono::milliseconds dura(2);
        std::this_thread::sleep_for(dura);
    }
}

首先是processImage(feature.second, feature.first);

///2.1.12
void Estimator::processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const double 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;
        //printf("keyframe\n");
    }
    else
    {
        marginalization_flag = MARGIN_SECOND_NEW;
        //printf("non-keyframe\n");
    }

    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);
    imageframe.pre_integration = tmp_pre_integration;
    all_image_frame.insert(make_pair(header, 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<pair<Vector3d, Vector3d>> 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))
            {
                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)
    {
        // monocular + IMU initilization
        if (!(STEREO || DEPTH) && USE_IMU)
        {
            if (frame_count == WINDOW_SIZE)
            {
                bool result = false;
                if(ESTIMATE_EXTRINSIC != 2 && (header - initial_timestamp) > 0.1)
                {
                    result = initialStructure();
                    initial_timestamp = header;   
                }
                if(result)
                {
                    solver_flag = NON_LINEAR;
                    optimization();
                    slideWindow();
                    ROS_INFO("Initialization finish!");
                }
                else
                    slideWindow();
            }
        }

        // stereo + IMU initilization
        else if((STEREO || DEPTH) && USE_IMU)
        {
            f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
            // f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
            if (frame_count == WINDOW_SIZE)
            {
                map<double, ImageFrame>::iterator frame_it;
                int i = 0;
                for (frame_it = all_image_frame.begin(); frame_it != all_image_frame.end(); frame_it++)
                {
                    frame_it->second.R = Rs[i];
                    frame_it->second.T = Ps[i];
                    i++;
                }
                bool result = false;
                if(ESTIMATE_EXTRINSIC != 2 && (header - initial_timestamp) > 0.1)
                {
                    result = 1;//visualInitialAlignWithDepth();
                    initial_timestamp = header;
                }

                if(result)
                {

                    solveGyroscopeBias(all_image_frame,Bgs);
                    for (int i = 0; i <= WINDOW_SIZE; i++)
                    {
                        pre_integrations[i]->repropagate(Vector3d::Zero(), Bgs[i]);
                    }
                    solver_flag = NON_LINEAR;
                    optimization();
                    slideWindow();
                    ROS_INFO("Initialization finish!");
                }
                else
                    slideWindow();
            }
        }

        // stereo only initilization
        else if((STEREO || DEPTH) && !USE_IMU)
        {
            f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
            f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
            optimization();

            if(frame_count == WINDOW_SIZE)
            {
                solver_flag = NON_LINEAR;
                slideWindow();
                ROS_INFO("Initialization finish!");
            }
        }

        if(frame_count < WINDOW_SIZE)
        {
            frame_count++;
            int prev_frame = frame_count - 1;
            Ps[frame_count] = Ps[prev_frame];
            Vs[frame_count] = Vs[prev_frame];
            Rs[frame_count] = Rs[prev_frame];
            Bas[frame_count] = Bas[prev_frame];
            Bgs[frame_count] = Bgs[prev_frame];
        }

    }
    else
    {
        TicToc t_solve;
        if(!USE_IMU)
            f_manager.initFramePoseByPnP(frame_count, Ps, Rs, tic, ric);
        f_manager.triangulate(frame_count, Ps, Rs, tic, ric);
        optimization();
        set<int> removeIndex;
        outliersRejection(removeIndex);
        f_manager.removeOutlier(removeIndex);
        if (! MULTIPLE_THREAD)
        {
            featureTracker.removeOutliers(removeIndex);
            predictPtsInNextFrame();
        }
            
        ROS_DEBUG("solver costs: %fms", t_solve.toc());

        if (failureDetection())
        {
            ROS_WARN("failure detection!");
            failure_occur = 1;
            clearState();
            setParameter();
            ROS_WARN("system reboot!");
            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];
        updateLatestStates();
    }  
}

这个部分很长，首先看到的是这个函数，该函数在vins_estimator文件夹下的feature_manager.cpp文件中。其主要功能是根据视差判断其是否为关键帧。

///6.1.6
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;
    last_average_parallax = 0;
    new_feature_num = 0;
    long_track_num = 0;
    for (auto &id_pts : image)
    {
        FeaturePerFrame f_per_fra(id_pts.second[0].second, td);
        assert(id_pts.second[0].first == 0);
        if(id_pts.second.size() == 2)
        {
            f_per_fra.rightObservation(id_pts.second[1].second);
            assert(id_pts.second[1].first == 1);
        }

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

        if (it == feature.end())
        {
            feature.push_back(FeaturePerId(feature_id, frame_count));
            feature.back().feature_per_frame.push_back(f_per_fra);
            new_feature_num++;
        }
        else if (it->feature_id == feature_id)
        {
            it->feature_per_frame.push_back(f_per_fra);
            last_track_num++;
            if( it-> feature_per_frame.size() >= 4)
                long_track_num++;
        }
    }

    //if (frame_count < 2 || last_track_num < 20)
    //if (frame_count < 2 || last_track_num < 20 || new_feature_num > 0.5 * last_track_num)
    if (frame_count < 2 || last_track_num < 20 || long_track_num < 40 || new_feature_num > 0.5 * last_track_num)
        return true;

    for (auto &it_per_id : feature)
    {
        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)
        {
            parallax_sum += compensatedParallax2(it_per_id, frame_count);
            parallax_num++;
        }
    }

    if (parallax_num == 0)
    {
        return true;
    }
    else
    {
        ROS_DEBUG("parallax_sum: %lf, parallax_num: %d", parallax_sum, parallax_num);
        ROS_DEBUG("current parallax: %lf", parallax_sum / parallax_num * FOCAL_LENGTH);
        last_average_parallax = parallax_sum / parallax_num * FOCAL_LENGTH;
        return parallax_sum / parallax_num >= MIN_PARALLAX;
    }
}

当然第二行就调用了

///6.1.5
int FeatureManager::getFeatureCount()
{
    int cnt = 0;
    for (auto &it : feature)// 遍历feature
    {
        it.used_num = it.feature_per_frame.size(); // 所有特征点被观测到的帧数
        // 如果该特征点有4帧以上观测到了
        if (it.used_num >= 4)
        {
            cnt++;// 这个特征点是有效的
        }
    }
    return cnt;
}

实际上这里对有效特征点进行了计数，对所有特征点遍历，在不同观测帧上的观测的次数大于4，认为是有效帧。

接下来是对每一个特征点进行比较

/// _point 每帧的特征点[x,y,z,u,v,vx,vy]和td IMU和cam同步时间差
///const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image
    ///id_pts.second[0].second可以认为是image的map类型中的第id_pts个点的vector的第0个数的igen::Matrix<double, 7, 1>
        FeaturePerFrame f_per_fra(id_pts.second[0].second, td);

这个调用了

class FeaturePerFrame //每一个特征点的在一张图像中属性 point
{
public:
  FeaturePerFrame(const Eigen::Matrix<double, 7, 1> &_point, double td) //_point:[x,y,z,u,v,vx,vy]
  {
    point.x() = _point(0);
    point.y() = _point(1);
    point.z() = _point(2); //特征点归一化坐标、特征点的像素坐标、特征点的跟踪速度、时间延迟
    uv.x() = _point(3);
    uv.y() = _point(4);
    velocity.x() = _point(5);
    velocity.y() = _point(6);
    cur_td = td;  //td  IMU和cam同步时间差
  }
  double cur_td; //imu-camera的不同步时的相差时间
  Vector3d point; //特征点空间坐标
  Vector2d uv; //特征点映射到该帧上的图像坐标
  Vector2d velocity; //特征点的跟踪速度
  double z; // 特征点的深度
  bool is_used; // 是否被用了
  double parallax; // 视差 ??? not used
  MatrixXd A;//变换矩阵
  VectorXd b;
  double dep_gradient;  //not used ???
};

从英文上来看FeaturePerFrame ，每一帧的特征，= 每一个特征点的在一张图像中属性。
///id_pts.second[0].second可以认为是image的map类型中的第id_pts个点的vector的第0个数的Eigen::Matrix<double, 7, 1>