ORB-SLAM3 IMU(李群)+Frame+KeyFrame+MapPoint

xiaoma_bk

已于 2022-03-25 18:42:02 修改

阅读量8.3k

点赞数 3

分类专栏： # ORB_SLAM3 文章标签：计算机视觉

于 2021-11-09 10:49:51 首次发布

本文链接：https://blog.csdn.net/xiaoma_bk/article/details/121158471

版权

ORB_SLAM3 专栏收录该内容

8 篇文章 9 订阅

订阅专栏

文章目录

IMU
Frame
KeyFrame
MapPoint

IMU

李群李代数

角度归一化/反对称

角度归一化

const float eps = 1e-4;

// 角度归一化
cv::Mat NormalizeRotation(const cv::Mat &R)
{
    cv::Mat U,w,Vt;
    cv::SVDecomp(R,w,U,Vt,cv::SVD::FULL_UV);
    return U*Vt;
}

计算反对称矩阵

cv::Mat Skew(const cv::Mat &v)
{
    const float x = v.at<float>(0);
    const float y = v.at<float>(1);
    const float z = v.at<float>(2);
    return (cv::Mat_<float>(3,3) << 0, -z, y,
            z, 0, -x,
            -y,  x, 0);
}

S03指数映射

由 $\phi$ 是三维向量，定义其模长 $\theta$ ，方向 ${a}$ ，有：
${exp(\phi) = cos(\theta)I+sin(θ)a^{\wedge}+(1-cos(θ))a^{\wedge}a^{\wedge}}$
// 由于θ较小，
${exp(\phi) = I+(W/θ)*sin(θ)+(W/θ)^2*(1-cos(θ)) }$
// 若θ<1e-4时，sin(θ)=θ
${exp(\phi) = I+W+(W/θ)^2* 2*(θ/2)^2=I+W+0.5W^2}$

代码实现：

cv::Mat ExpSO3(const float &x, const float &y, const float &z)
{
    cv::Mat I = cv::Mat::eye(3,3,CV_32F);
    const float d2 = x*x+y*y+z*z;
    const float d = sqrt(d2);
    cv::Mat W = (cv::Mat_<float>(3,3) << 0, -z, y,
                 z, 0, -x,
                 -y,  x, 0);
    if(d<eps)
        return (I + W + 0.5f*W*W);
    else
        return (I + W*sin(d)/d + W*W*(1.0f-cos(d))/d2);
}

// 跟上试一样
Eigen::Matrix<double,3,3> ExpSO3(const double &x, const double &y, const double &z)
{
    Eigen::Matrix<double,3,3> I = Eigen::MatrixXd::Identity(3,3);
    const double d2 = x*x+y*y+z*z;
    const double d = sqrt(d2);
    Eigen::Matrix<double,3,3> W;
    W(0,0) = 0;
    W(0,1) = -z;
    W(0,2) = y;
    W(1,0) = z;
    W(1,1) = 0;
    W(1,2) = -x;
    W(2,0) = -y;
    W(2,1) = x;
    W(2,2) = 0;

    if(d<eps)
        return (I + W + 0.5*W*W);
    else
        return (I + W*sin(d)/d + W*W*(1.0-cos(d))/d2);
}

cv::Mat ExpSO3(const cv::Mat &v)
{
    return ExpSO3(v.at<float>(0),v.at<float>(1),v.at<float>(2));
}

S03对数

对数： ${\theta = arccos\frac{tr(R)-1}{2}}$

代码实现：

cv::Mat LogSO3(const cv::Mat &R)
{
    const float tr = R.at<float>(0,0)+R.at<float>(1,1)+R.at<float>(2,2);
    cv::Mat w = (cv::Mat_<float>(3,1) <<(R.at<float>(2,1)-R.at<float>(1,2))/2,
                                        (R.at<float>(0,2)-R.at<float>(2,0))/2,
                                        (R.at<float>(1,0)-R.at<float>(0,1))/2);
    const float costheta = (tr-1.0f)*0.5f;
    if(costheta>1 || costheta<-1)
        return w;
    const float theta = acos(costheta);
    const float s = sin(theta);
    if(fabs(s)<eps)
        return w;
    else
        return theta*w/s;
}

右乘jacobian/取逆

求解jacobian

在 $\theta$ 较小时， ${a^{\wedge}a^{\wedge} = aa^T-I}$ ， $1-\frac{sin(\theta)}{\theta} =0$
${(1-\frac{sin(\theta)}{\theta})aa^T= = (1-\frac{sin(\theta)}{\theta})a^{\wedge}a^{\wedge} }$
左乘jacobian： ${J_L = \frac{sin(\theta)}{\theta} I+ (1-\frac{sin(\theta)}{\theta})aa^T+ \frac{1-cos(\theta)}{\theta}a^{\wedge}}$
右乘： ${J_R(\phi)=J_L(-\phi) = \frac{sin(\theta)}{\theta} I+ (1-\frac{sin(\theta)}{\theta})aa^T- \frac{1-cos(\theta)}{\theta}a^{\wedge}}$

jacobian取逆

左乘： ${J_L^{-1} = \frac{\theta}{2} cot \frac{\theta}{2} I+ (1-\frac{\theta}{2} cot \frac{\theta}{2}) aa^T - \frac{\theta}{2}a^{\wedge}}$
右乘： ${J_R^{-1} = \frac{\theta}{2} cot \frac{\theta}{2} I+ (1-\frac{\theta}{2} cot \frac{\theta}{2}) aa^T+ \frac{\theta}{2}a^{\wedge}}$

代码实现：

cv::Mat RightJacobianSO3(const float &x, const float &y, const float &z)
{
    cv::Mat I = cv::Mat::eye(3,3,CV_32F);
    const float d2 = x*x+y*y+z*z;
    const float d = sqrt(d2);
    cv::Mat W = (cv::Mat_<float>(3,3) << 0, -z, y,
                 z, 0, -x,
                 -y,  x, 0);
    if(d<eps)
    {
        return cv::Mat::eye(3,3,CV_32F);
    }
    else
    {
        return I - W*(1.0f-cos(d))/d2 + W*W*(d-sin(d))/(d2*d);
    }
}

cv::Mat RightJacobianSO3(const cv::Mat &v)
{
    return RightJacobianSO3(v.at<float>(0),v.at<float>(1),v.at<float>(2));
}

cv::Mat InverseRightJacobianSO3(const float &x, const float &y, const float &z)
{
    cv::Mat I = cv::Mat::eye(3,3,CV_32F);
    const float d2 = x*x+y*y+z*z;
    const float d = sqrt(d2);
    cv::Mat W = (cv::Mat_<float>(3,3) << 0, -z, y,
                 z, 0, -x,
                 -y,  x, 0);
    if(d<eps)
    {
        return cv::Mat::eye(3,3,CV_32F);
    }
    else
    {
        return I + W/2 + W*W*(1.0f/d2 - (1.0f+cos(d))/(2.0f*d*sin(d)));
    }
}

cv::Mat InverseRightJacobianSO3(const cv::Mat &v)
{
    return InverseRightJacobianSO3(v.at<float>(0),v.at<float>(1),v.at<float>(2));
}

ImuTypes

IMU测量数据

线加速度+角速度+时间

const float GRAVITY_VALUE=9.81;

//IMU measurement (gyro, accelerometer and timestamp)
class Point
{
public:
	// 7个参数分布传
    Point(const float &acc_x, const float &acc_y, const float &acc_z,
             const float &ang_vel_x, const float &ang_vel_y, const float &ang_vel_z,
             const double &timestamp): a(acc_x,acc_y,acc_z), w(ang_vel_x,ang_vel_y,ang_vel_z), t(timestamp){}
    // 直接赋值传
    Point(const cv::Point3f Acc, const cv::Point3f Gyro, const double &timestamp):
        a(Acc.x,Acc.y,Acc.z), w(Gyro.x,Gyro.y,Gyro.z), t(timestamp){}

public:  // 公有变量
    cv::Point3f a;
    cv::Point3f w;
    double t;
};

IMU bias估计

角速度和角加速度估计

class Bias
{
public:
    Bias():bax(0),bay(0),baz(0),bwx(0),bwy(0),bwz(0){}
    Bias(const float &b_acc_x, const float &b_acc_y, const float &b_acc_z,
            const float &b_ang_vel_x, const float &b_ang_vel_y, const float &b_ang_vel_z):
            bax(b_acc_x), bay(b_acc_y), baz(b_acc_z), bwx(b_ang_vel_x), bwy(b_ang_vel_y), bwz(b_ang_vel_z){}
    void CopyFrom(Bias &b);
    friend std::ostream& operator<< (std::ostream &out, const Bias &b);

public:
    float bax, bay, baz;
    float bwx, bwy, bwz;
};

IMU 参数

imu到相机+相机到imu+协方差+游走协方差

class Calib
{
public:
    Calib(const cv::Mat &Tbc_, const float &ng, const float &na, const float &ngw, const float &naw)
    {
        Set(Tbc_,ng,na,ngw,naw);
    }
    Calib(const Calib &calib);
    Calib(){}

    void Set(const cv::Mat &Tbc_, const float &ng, const float &na, const float &ngw, const float &naw);

public:
    cv::Mat Tcb;
    cv::Mat Tbc;
    cv::Mat Cov, CovWalk;
};

角速度计算旋转量

float deltaT; // 积分时间
cv::Mat deltaR; // 指数映射
cv::Mat rightJ; // 右乘 jacobian

class IntegratedRotation
{
public:
    IntegratedRotation(){}
    IntegratedRotation(const cv::Point3f &angVel, const Bias &imuBias, const float &time)：deltaT(time) {
	// 角度变化量
    const float x = (angVel.x-imuBias.bwx)*time;
    const float y = (angVel.y-imuBias.bwy)*time;
    const float z = (angVel.z-imuBias.bwz)*time;

    cv::Mat I = cv::Mat::eye(3,3,CV_32F);
	
	// 模长 \theta
    const float d2 = x*x+y*y+z*z;
    const float d = sqrt(d2);
	// 反对称矩阵
    cv::Mat W = (cv::Mat_<float>(3,3) << 0, -z, y,
                 z, 0, -x,
                 -y,  x, 0);
    if(d<eps)
    {
        deltaR = I + W;
        rightJ = cv::Mat::eye(3,3,CV_32F);
    }
    else
    {	// 跟上述右乘一样
        deltaR = I + W*sin(d)/d + W*W*(1.0f-cos(d))/d2;
        rightJ = I - W*(1.0f-cos(d))/d2 + W*W*(d-sin(d))/(d2*d);
    }
}

public:
    float deltaT; //integration time
    cv::Mat deltaR; //integrated rotation
    cv::Mat rightJ; // right jacobian
};

Imu测量的预积分

大部分内容是这里面的。
IMU Preintegration on Manifold for Efficient Visual-Inertial Maximum-a-
Posteriori Estimation

//Preintegration of Imu Measurements
class Preintegrated
{
public:
    Preintegrated(const Bias &b_, const Calib &calib) {
    	// 赋值协方差+游走协方差，并初始化数据
	    Nga = calib.Cov.clone();
	    NgaWalk = calib.CovWalk.clone();
	    Initialize(b_);
	}
    Preintegrated(Preintegrated* pImuPre); //里面所有参数都一一赋值
    Preintegrated() {}
    ~Preintegrated() {}
    void CopyFrom(Preintegrated* pImuPre);
    // 初始化所有变量
    void Initialize(const Bias &b_) {
		 dR = cv::Mat::eye(3,3,CV_32F);
	    dV = cv::Mat::zeros(3,1,CV_32F);
	    dP = cv::Mat::zeros(3,1,CV_32F);
	    JRg = cv::Mat::zeros(3,3,CV_32F);
	    JVg = cv::Mat::zeros(3,3,CV_32F);
	    JVa = cv::Mat::zeros(3,3,CV_32F);
	    JPg = cv::Mat::zeros(3,3,CV_32F);
	    JPa = cv::Mat::zeros(3,3,CV_32F);
	    C = cv::Mat::zeros(15,15,CV_32F);
	    Info=cv::Mat();
	    db = cv::Mat::zeros(6,1,CV_32F);
	    b=b_;
	    bu=b_;
	    avgA = cv::Mat::zeros(3,1,CV_32F);
	    avgW = cv::Mat::zeros(3,1,CV_32F);
	    dT=0.0f;
	    mvMeasurements.clear();
	}
    void IntegrateNewMeasurement(const cv::Point3f &acceleration, const cv::Point3f &angVel, const float &dt) {
    mvMeasurements.push_back(integrable(acceleration,angVel,dt));

    // 位置首先更新，因为它取决于先前计算的速度和旋转。
    // 第二更新速度，因为它取决于先前计算的旋转。
    // 旋转是最后更新的

    //Matrices to compute covariance
    cv::Mat A = cv::Mat::eye(9,9,CV_32F);
    cv::Mat B = cv::Mat::zeros(9,6,CV_32F);
	
	// 取出加速度和角加速度，其中二者都去了 零偏
    cv::Mat acc = (cv::Mat_<float>(3,1) << acceleration.x-b.bax,acceleration.y-b.bay, acceleration.z-b.baz);
    cv::Mat accW = (cv::Mat_<float>(3,1) << angVel.x-b.bwx, angVel.y-b.bwy, angVel.z-b.bwz);
	// 计算平均线速度 和 角速度
    avgA = (dT*avgA + dR*acc*dt)/(dT+dt);
    avgW = (dT*avgW + accW*dt)/(dT+dt);

    // 更新增量位置 dP 和速度 dV（依赖于未更新的增量旋转）
    dP = dP + dV*dt + 0.5f*dR*acc*dt*dt;
    dV = dV + dR*acc*dt;

    // 计算矩阵 A 和 B 的速度和位置部分（依赖于未更新的增量旋转）
    cv::Mat Wacc = (cv::Mat_<float>(3,3) << 0, -acc.at<float>(2), acc.at<float>(1),
                                    acc.at<float>(2), 0, -acc.at<float>(0),
                                   -acc.at<float>(1), acc.at<float>(0), 0);
    A.rowRange(3,6).colRange(0,3) = -dR*dt*Wacc;
    A.rowRange(6,9).colRange(0,3) = -0.5f*dR*dt*dt*Wacc;
    A.rowRange(6,9).colRange(3,6) = cv::Mat::eye(3,3,CV_32F)*dt;
    B.rowRange(3,6).colRange(3,6) = dR*dt;
    B.rowRange(6,9).colRange(3,6) = 0.5f*dR*dt*dt;

    // Update position and velocity jacobians wrt bias correction
    JPa = JPa + JVa*dt -0.5f*dR*dt*dt;
    JPg = JPg + JVg*dt -0.5f*dR*dt*dt*Wacc*JRg;
    JVa = JVa - dR*dt;
    JVg = JVg - dR*dt*Wacc*JRg;

    // Update delta rotation
    IntegratedRotation dRi(angVel,b,dt);
    dR = NormalizeRotation(dR*dRi.deltaR);

    // Compute rotation parts of matrices A and B
    A.rowRange(0,3).colRange(0,3) = dRi.deltaR.t();
    B.rowRange(0,3).colRange(0,3) = dRi.rightJ*dt;

    // Update covariance
    C.rowRange(0,9).colRange(0,9) = A*C.rowRange(0,9).colRange(0,9)*A.t() + B*Nga*B.t();
    C.rowRange(9,15).colRange(9,15) = C.rowRange(9,15).colRange(9,15) + NgaWalk;

    // Update rotation jacobian wrt bias correction
    JRg = dRi.deltaR.t()*JRg - dRi.rightJ*dt;

    // Total integrated time
    dT += dt;
}
    void Reintegrate();
    void MergePrevious(Preintegrated* pPrev);
    void SetNewBias(const Bias &bu_);
    IMU::Bias GetDeltaBias(const Bias &b_);
    cv::Mat GetDeltaRotation(const Bias &b_);
    cv::Mat GetDeltaVelocity(const Bias &b_);
    cv::Mat GetDeltaPosition(const Bias &b_);
    cv::Mat GetUpdatedDeltaRotation();
    cv::Mat GetUpdatedDeltaVelocity();
    cv::Mat GetUpdatedDeltaPosition();
    cv::Mat GetOriginalDeltaRotation();
    cv::Mat GetOriginalDeltaVelocity();
    cv::Mat GetOriginalDeltaPosition();
    Eigen::Matrix<double,15,15> GetInformationMatrix();
    cv::Mat GetDeltaBias();
    Bias GetOriginalBias();
    Bias GetUpdatedBias();

public:
    float dT;
    cv::Mat C;
    cv::Mat Info;
    cv::Mat Nga, NgaWalk;

    // Values for the original bias (when integration was computed)
    Bias b;
    cv::Mat dR, dV, dP;
    cv::Mat JRg, JVg, JVa, JPg, JPa;
    cv::Mat avgA;
    cv::Mat avgW;


private:
    // Updated bias
    Bias bu;
    // Dif between original and updated bias
    // This is used to compute the updated values of the preintegration
    cv::Mat db;

    struct integrable
    {
        integrable(const cv::Point3f &a_, const cv::Point3f &w_ , const float &t_):a(a_),w(w_),t(t_){}
        cv::Point3f a;
        cv::Point3f w;
        float t;
    };

    std::vector<integrable> mvMeasurements;

    std::mutex mMutex;
};

// Lie Algebra Functions
cv::Mat ExpSO3(const float &x, const float &y, const float &z);
Eigen::Matrix<double,3,3> ExpSO3(const double &x, const double &y, const double &z);
cv::Mat ExpSO3(const cv::Mat &v);
cv::Mat LogSO3(const cv::Mat &R);
cv::Mat RightJacobianSO3(const float &x, const float &y, const float &z);
cv::Mat RightJacobianSO3(const cv::Mat &v);
cv::Mat InverseRightJacobianSO3(const float &x, const float &y, const float &z);
cv::Mat InverseRightJacobianSO3(const cv::Mat &v);
cv::Mat Skew(const cv::Mat &v);
cv::Mat NormalizeRotation(const cv::Mat &R);

}

Frame

类成员说明

#define FRAME_GRID_ROWS 48
#define FRAME_GRID_COLS 64

// imu的位姿约束
ConstraintPoseImu* mpcpi;

// 相机在世界坐标系的关系
cv::Mat mRwc;
cv::Mat mOw;

// 用于重定位的词典
ORBVocabulary* mpORBvocabulary;

// Feature extractor. The right is used only in the stereo case.
// 提取特征，right只有在双目相机中使用
ORBextractor* mpORBextractorLeft, *mpORBextractorRight;

// Frame timestamp. 当前帧的时间
double mTimeStamp;

// Calibration matrix and OpenCV distortion parameters.
// 相机外参+内参+去畸变参数
cv::Mat mK;
static float fx;
static float fy;
static float cx;
static float cy;
static float invfx;
static float invfy;
cv::Mat mDistCoef;

// Stereo baseline multiplied by fx.
float mbf;
// Stereo baseline in meters.
float mb;

// 阈值近/远点。 从 1 个视图插入闭合点。 从 2 个视图插入远点，就像在单眼情况下一样。
float mThDepth;

// Number of KeyPoints. KeyPoints数量
int N;

// Vector of keypoints (original for visualization) and undistorted (actually used by the system).
// In the stereo case, mvKeysUn is redundant as images must be rectified.
// In the RGB-D case, RGB images can be distorted.
// mvKeys:原始左图像提取出的特征点（未校正）
// mvKeysRight:原始右图像提取出的特征点（未校正）
// mvKeysUn:校正mvKeys后的特征点，对于双目摄像头，一般得到的图像都是校正好的，再校正一次有点多余
std::vector<cv::KeyPoint> mvKeys, mvKeysRight;
std::vector<cv::KeyPoint> mvKeysUn;

// Corresponding stereo coordinate and depth for each keypoint.
// 每个关键点对应的立体坐标和深度。
std::vector<MapPoint*> mvpMapPoints;
// "Monocular" keypoints have a negative value.
// 单目摄像头，这两个容器中存的都是-1
std::vector<float> mvuRight;
std::vector<float> mvDepth;

// Bag of Words Vector structures. 词袋矢量结构。
DBoW2::BowVector mBowVec;
DBoW2::FeatureVector mFeatVec;

// ORB descriptor, each row associated to a keypoint.
// 左目摄像头和右目摄像头特征点对应的描述子
cv::Mat mDescriptors, mDescriptorsRight;

// MapPoints associated to keypoints, NULL pointer if no association.
// Flag to identify outlier associations. 观测不到Map中的3D点
std::vector<bool> mvbOutlier;
int mnCloseMPs;

// Keypoints are assigned to cells in a grid to reduce matching complexity when projecting MapPoints.
// 将关键点分配给网格中的单元格以降低投影 MapPoints 时的匹配复杂性。
static float mfGridElementWidthInv; // 64/图像宽度
static float mfGridElementHeightInv;  // 48/图像宽度
// 每个格子分配的特征点数，将图像分成格子，保证提取的特征点比较均匀
std::vector<std::size_t> mGrid[FRAME_GRID_COLS][FRAME_GRID_ROWS];


// Camera pose.
cv::Mat mTcw; ///< 相机姿态 世界坐标系到相机坐标坐标系的变换矩阵

// IMU linear velocity
cv::Mat mVw; // imu线速度

// 预测的位姿+速度，imu的预测bias
cv::Mat mPredRwb, mPredtwb, mPredVwb;
IMU::Bias mPredBias;

// IMU bias
IMU::Bias mImuBias;

// Imu calibration
IMU::Calib mImuCalib;

// Imu preintegration from last keyframe
IMU::Preintegrated* mpImuPreintegrated;
KeyFrame* mpLastKeyFrame;

// Pointer to previous frame  指向前一帧的指针
Frame* mpPrevFrame;
IMU::Preintegrated* mpImuPreintegratedFrame;

// Current and Next Frame id.
static long unsigned int nNextId; ///< Next Frame id.
long unsigned int mnId; ///< Current Frame id.

// Reference Keyframe.
KeyFrame* mpReferenceKF; //指针，指向参考关键帧

// Scale pyramid info.
int mnScaleLevels; //图像提金字塔的层数
float mfScaleFactor;//图像提金字塔的尺度因子
float mfLogScaleFactor;
vector<float> mvScaleFactors;
vector<float> mvInvScaleFactors;
vector<float> mvLevelSigma2;
vector<float> mvInvLevelSigma2;

// Undistorted Image Bounds (computed once).
// 用于确定画格子时的边界
static float mnMinX;
static float mnMaxX;
static float mnMinY;
static float mnMaxY;

static bool mbInitialComputations;

map<long unsigned int, cv::Point2f> mmProjectPoints;
map<long unsigned int, cv::Point2f> mmMatchedInImage;

string mNameFile;

int mnDataset;

GeometricCamera* mpCamera, *mpCamera2;

//Number of KeyPoints extracted in the left and right images
// 左右图像中提取的KeyPoint数
int Nleft, Nright;
//Number of Non Lapping Keypoints
int monoLeft, monoRight;

//For stereo matching
std::vector<int> mvLeftToRightMatch, mvRightToLeftMatch;

//For stereo fisheye matching
static cv::BFMatcher BFmatcher;

//Triangulated stereo observations using as reference the left camera. These are
//computed during ComputeStereoFishEyeMatches
std::vector<cv::Mat> mvStereo3Dpoints;

//Grid for the right image
std::vector<std::size_t> mGridRight[FRAME_GRID_COLS][FRAME_GRID_ROWS];

cv::Mat mTlr, mRlr, mtlr, mTrl;
cv::Matx34f mTrlx, mTlrx;

// 左右图像
cv::Mat imgLeft, imgRight;

构造：

重点描述了三种相机的构造，提取并校正特征，然后把特征点划分到网格中，这样做是为了让特征点在图像中分布得更均匀。
对于深度问题，双目相机使用了SAD去恢复深度，RGBD相机自身有深度，而单目无法获得深度，所以相应变量赋值-1，这样就完成了构造。

#define FRAME_GRID_ROWS 48
#define FRAME_GRID_COLS 64

无参构造

都给空

Frame::Frame(): mpcpi(NULL), mpImuPreintegrated(NULL), mpPrevFrame(NULL), mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast<KeyFrame*>(NULL)), mbImuPreintegrated(false)
{
#ifdef REGISTER_TIMES
    mTimeStereoMatch = 0;
    mTimeORB_Ext = 0;
#endif
}

拷贝构造

真正做到了深拷贝

Frame::Frame(const Frame &frame)
    :mpcpi(frame.mpcpi),mpORBvocabulary(frame.mpORBvocabulary), mpORBextractorLeft(frame.mpORBextractorLeft), mpORBextractorRight(frame.mpORBextractorRight),
     mTimeStamp(frame.mTimeStamp), mK(frame.mK.clone()), mDistCoef(frame.mDistCoef.clone()),
     mbf(frame.mbf), mb(frame.mb), mThDepth(frame.mThDepth), N(frame.N), mvKeys(frame.mvKeys),
     mvKeysRight(frame.mvKeysRight), mvKeysUn(frame.mvKeysUn), mvuRight(frame.mvuRight),
     mvDepth(frame.mvDepth), mBowVec(frame.mBowVec), mFeatVec(frame.mFeatVec),
     mDescriptors(frame.mDescriptors.clone()), mDescriptorsRight(frame.mDescriptorsRight.clone()),
     mvpMapPoints(frame.mvpMapPoints), mvbOutlier(frame.mvbOutlier), mImuCalib(frame.mImuCalib), mnCloseMPs(frame.mnCloseMPs),
     mpImuPreintegrated(frame.mpImuPreintegrated), mpImuPreintegratedFrame(frame.mpImuPreintegratedFrame), mImuBias(frame.mImuBias),
     mnId(frame.mnId), mpReferenceKF(frame.mpReferenceKF), mnScaleLevels(frame.mnScaleLevels),
     mfScaleFactor(frame.mfScaleFactor), mfLogScaleFactor(frame.mfLogScaleFactor),
     mvScaleFactors(frame.mvScaleFactors), mvInvScaleFactors(frame.mvInvScaleFactors), mNameFile(frame.mNameFile), mnDataset(frame.mnDataset),
     mvLevelSigma2(frame.mvLevelSigma2), mvInvLevelSigma2(frame.mvInvLevelSigma2), mpPrevFrame(frame.mpPrevFrame), mpLastKeyFrame(frame.mpLastKeyFrame), mbImuPreintegrated(frame.mbImuPreintegrated), mpMutexImu(frame.mpMutexImu),
     mpCamera(frame.mpCamera), mpCamera2(frame.mpCamera2), Nleft(frame.Nleft), Nright(frame.Nright),
     monoLeft(frame.monoLeft), monoRight(frame.monoRight), mvLeftToRightMatch(frame.mvLeftToRightMatch),
     mvRightToLeftMatch(frame.mvRightToLeftMatch), mvStereo3Dpoints(frame.mvStereo3Dpoints),
     mTlr(frame.mTlr.clone()), mRlr(frame.mRlr.clone()), mtlr(frame.mtlr.clone()), mTrl(frame.mTrl.clone()),
     mTrlx(frame.mTrlx), mTlrx(frame.mTlrx), mOwx(frame.mOwx), mRcwx(frame.mRcwx), mtcwx(frame.mtcwx)
{
	// Grid拷贝，完全独立
    for(int i=0;i<FRAME_GRID_COLS;i++)
        for(int j=0; j<FRAME_GRID_ROWS; j++){
            mGrid[i][j]=frame.mGrid[i][j];
            if(frame.Nleft > 0){
                mGridRight[i][j] = frame.mGridRight[i][j];
            }
        }

    if(!frame.mTcw.empty())
        SetPose(frame.mTcw);

    if(!frame.mVw.empty())
        mVw = frame.mVw.clone();

    mmProjectPoints = frame.mmProjectPoints;
    mmMatchedInImage = frame.mmMatchedInImage;

#ifdef REGISTER_TIMES
    mTimeStereoMatch = frame.mTimeStereoMatch;
    mTimeORB_Ext = frame.mTimeORB_Ext;
#endif
}

有参构造

立体相机的构造函数。
Frame(const cv::Mat &imLeft, const cv::Mat &imRight, const double &timeStamp, ORBextractor* extractorLeft, ORBextractor* extractorRight, ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera,Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
RGB-D 相机的构造.
Frame(const cv::Mat &imGray, const cv::Mat &imDepth, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, cv::Mat &K, cv::Mat &distCoef, const float &bf, const float &thDepth, GeometricCamera* pCamera,Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());
单目相机构造
Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, Frame* pPrevF = static_cast<Frame*>(NULL), const IMU::Calib &ImuCalib = IMU::Calib());

单目构造代码：

Param: 灰度图、时间戳、ORB特征提取、词袋数据、相机内参、图像校准参数、bf=双目基线*fx、这是深度值的阈值（按照特征点深度值大于或小于这个值，把他们分为close和far两类）

Frame::Frame(const cv::Mat &imGray, const double &timeStamp, ORBextractor* extractor,ORBVocabulary* voc, GeometricCamera* pCamera, cv::Mat &distCoef, const float &bf, const float &thDepth, Frame* pPrevF, const IMU::Calib &ImuCalib)
    :mpcpi(NULL),mpORBvocabulary(voc),mpORBextractorLeft(extractor),mpORBextractorRight(static_cast<ORBextractor*>(NULL)),
     mTimeStamp(timeStamp), mK(static_cast<Pinhole*>(pCamera)->toK()), mDistCoef(distCoef.clone()), mbf(bf), mThDepth(thDepth),
     mImuCalib(ImuCalib), mpImuPreintegrated(NULL),mpPrevFrame(pPrevF),mpImuPreintegratedFrame(NULL), mpReferenceKF(static_cast<KeyFrame*>(NULL)), mbImuPreintegrated(false), mpCamera(pCamera),
     mpCamera2(nullptr)
{
    // Frame ID
    mnId=nNextId++;

    // Scale Level Info 
    mnScaleLevels = mpORBextractorLeft->GetLevels();
    mfScaleFactor = mpORBextractorLeft->GetScaleFactor();
    mfLogScaleFactor = log(mfScaleFactor);
    mvScaleFactors = mpORBextractorLeft->GetScaleFactors();
    mvInvScaleFactors = mpORBextractorLeft->GetInverseScaleFactors();
    mvLevelSigma2 = mpORBextractorLeft->GetScaleSigmaSquares();
    mvInvLevelSigma2 = mpORBextractorLeft->GetInverseScaleSigmaSquares();

    /// step:1 ORB extraction
#ifdef REGISTER_TIMES
    std::chrono::steady_clock::time_point time_StartExtORB = std::chrono::steady_clock::now();
#endif
	/// 灰度图提取orb，提取时间可根据需要开启
    ExtractORB(0,imGray,0,1000);
#ifdef REGISTER_TIMES
    std::chrono::steady_clock::time_point time_EndExtORB = std::chrono::steady_clock::now();

    mTimeORB_Ext = std::chrono::duration_cast<std::chrono::duration<double,std::milli> >(time_EndExtORB - time_StartExtORB).count();
#endif

	// 若提取的特征值为 空，则return
    N = mvKeys.size();
    if(mvKeys.empty())
        return;
	
	/// step:2  根据观察点坐标计算理想点坐标，外参+失真参数
    UndistortKeyPoints();

	/// step:3 参数预设准备
    // Set no stereo information 没有右目和深度，赋值-1
    mvuRight = vector<float>(N,-1);
    mvDepth = vector<float>(N,-1);
    mnCloseMPs = 0;

	// 构造N个地图点，数据为 null
    mvpMapPoints = vector<MapPoint*>(N,static_cast<MapPoint*>(NULL));
    mmProjectPoints.clear();
    mmMatchedInImage.clear();
	// 外点标记
    mvbOutlier = vector<bool>(N,false);

    /// step 4: 这仅针对第一帧完成,赋值参数
    if(mbInitialComputations)
    {	
    	// 先得出图像的边界[0,0,x_max,y_max]
    	// 若图像无失真参数，直接返回
    	// 否则将这四个点带入失真函数cv::undistortPoints得到变换后的四个点，返回边界
        ComputeImageBounds(imGray);

		// 网格元素宽度/高度取逆 = 64_48/x_y
        mfGridElementWidthInv=static_cast<float>(FRAME_GRID_COLS)/static_cast<float>(mnMaxX-mnMinX);
        mfGridElementHeightInv=static_cast<float>(FRAME_GRID_ROWS)/static_cast<float>(mnMaxY-mnMinY);

		// 相机参数赋值
        fx = static_cast<Pinhole*>(mpCamera)->toK().at<float>(0,0);
        fy = static_cast<Pinhole*>(mpCamera)->toK().at<float>(1,1);
        cx = static_cast<Pinhole*>(mpCamera)->toK().at<float>(0,2);
        cy = static_cast<Pinhole*>(mpCamera)->toK().at<float>(1,2);
        invfx = 1.0f/fx;
        invfy = 1.0f/fy;

        mbInitialComputations=false;
    }

	
    mb = mbf/fx;

    //Set no stereo fisheye information 不设置双目相机数据
    Nleft = -1;
    Nright = -1;
    mvLeftToRightMatch = vector<int>(0);
    mvRightToLeftMatch = vector<int>(0);
    mTlr = cv::Mat(3,4,CV_32F);
    mTrl = cv::Mat(3,4,CV_32F);
    mvStereo3Dpoints = vector<cv::Mat>(0);
    monoLeft = -1;
    monoRight = -1;
	
	// 将特征分配给网络，下面为该函数
    AssignFeaturesToGrid();
    {
	    // 用点填充矩阵的个数
	    const int nCells = FRAME_GRID_COLS*FRAME_GRID_ROWS;
		//  每个格子分配的特征点数
	    int nReserve = 0.5f*N/(nCells);
	
	    for(unsigned int i=0; i<FRAME_GRID_COLS;i++)
	        for (unsigned int j=0; j<FRAME_GRID_ROWS;j++){
	            //  基于每个格子分配的特征点数分配内存
	            mGrid[i][j].reserve(nReserve);
	            if(Nleft != -1){  // 此处 Nleft== -1
	                mGridRight[i][j].reserve(nReserve);
	            }
	        }
	
		// 
	    for(int i=0;i<N;i++)
	    {
	    	// 取出特征点，这儿 Nleft == -1
	        const cv::KeyPoint &kp = (Nleft == -1) ? mvKeysUn[i]
	               : (i < Nleft) ? mvKeys[i]: mvKeysRight[i - Nleft];
	
	        int nGridPosX, nGridPosY;
	        // 特征点在Grid么，在时记录Grid_xy
	        if(PosInGrid(kp,nGridPosX,nGridPosY)){
	            if(Nleft == -1 || i < Nleft)
	                mGrid[nGridPosX][nGridPosY].push_back(i);
	            else
	                mGridRight[nGridPosX][nGridPosY].push_back(i - Nleft);
	        }
	    }

    }
	

    if(pPrevF)
    {
        if(!pPrevF->mVw.empty())
            mVw = pPrevF->mVw.clone();
    }
    else
    {
        mVw = cv::Mat::zeros(3,1,CV_32F);
    }

    mpMutexImu = new std::mutex();
}

opencv function

`undistortPoints`

根据观察点坐标计算理想点坐标
该函数类似于 cv::undistort 和 cv::initUndistortRectifyMap，但它对一组稀疏的点而不是光栅图像进行操作。该函数还对 projectPoints 执行反向转换。在 3D 对象的情况下，它不会重建其 3D 坐标，但对于平面对象，如果指定了适当的 R，它会重建平移向量。
对于每个观察点坐标 $(u, v)$ ，函数计算：
- ${x^{"} \leftarrow (u - c_x)/f_x}$
- ${y^{"} \leftarrow (v - c_y)/f_y}$
- $undistort(x^{"},y^{"}, \texttt{distCoeffs})}$
- ${{[X\,Y\,W]} ^T \leftarrow R*[x' \, y' \, 1]^T }$
- ${\texttt{only performed if P is specified:}}$
- $\leftarrow x {f'}_x + {c'}_x }$
- $\leftarrow y {f'}_y + {c'}_y}$
其中 undistort 是一种近似迭代算法，它从归一化的失真点坐标中估计归一化的原始点坐标（“归一化”意味着坐标不依赖于相机矩阵）。
函数参数：
- src 观察到的点坐标，1xN 或 Nx1 2 通道（CV_32FC2 或 CV_64FC2）。
- dst 输出无畸变和反向透视后的理想点坐标
- cameraMatrix 相机矩阵 : ${\begin{bmatrix} f_x & 0 & c_x\\ 0 & f_y & c_y \\ 0 & 0 & 1\end{bmatrix}}$
- distCoeffs 失真系数的输入向量: $(k_1, k_2, p_1, p_2[, k_3[, k_4, k_5, k_6[, s_1, s_2, s_3, s_4[, \tau_x, \tau_y]]]])$
  4、5、8、12 或 14 个元素。如果向量为 NULL/空，则假定零失真系数。
- R 对象空间（3x3 矩阵）中的整流变换。 R1 或 R2 计算公式
  cv::stereoRectify 可以在这里传递。如果矩阵为空，则使用恒等变换。
  通常是空 cv::Mat()
- P 新相机矩阵 (3x3) 或新投影矩阵 (3x4) $\begin{bmatrix} {f'}_x & 0 & {c'}_x & t_x \\ 0 & {f'}_y & { c'}_y & t_y \\ 0 & 0 & 1 & t_z \end{bmatrix}$ . P1 或 P2 计算公式
  小孔，跟上述一样

SAD algorithm

SAD 缩写：Sum of absolute differences
原理：在数字图像处理中，绝对差之和（SAD）是图像块之间相似性的度量。它是通过取原始块中每个像素与用于比较的块中相应像素之间的绝对差来计算的。将这些差异相加以创建块相似性的简单度量，即差异图像的 L1 范数或两个图像块之间的曼哈顿距离。
绝对差的总和可用于多种目的，例如对象识别、立体图像视差图的生成以及视频压缩的运动估计。
图像立体匹配：
- 图像立体匹配中常用的初级块匹配算法，其基本运算思想是求取相对应的左右两个像素块内像素值之差的绝对值之和。
物体识别：
- 绝对差异的总和提供了一种自动搜索图像内对象的简单方法，但由于环境因素（例如光照、颜色、观看方向、大小或形状的变化）的影响，可能不可靠。 SAD 可以与其他物体识别方法（例如边缘检测）结合使用，以提高结果的可靠性。
视屏识别：
- 由于其简单性，SAD 是一个非常快的度量标准；它实际上是考虑到块中每个像素的最简单的度量。因此，它对于许多不同块的宽运动搜索非常有效。 SAD 也很容易并行化，因为它分别分析每个像素，因此可以使用 ARM NEON 或 x86 SSE2 等指令轻松实现。例如，SSE 专门为此目的打包了绝对差和指令（PSADBW）。
- 一旦找到候选块，运动估计过程的最终细化通常使用其他更慢但更准确的度量来完成，这些度量更好地考虑了人类感知。这些包括绝对变换差之和 (SATD)、平方差之和 (SSD) 和率失真优化。

小函数

ExtractORB：提取特征点

把opencv自带的ORB提取功能多封装了一层，增加了一个flag，这个flag决定提取的是左目还是右目，从而调用不同的特征提取器。

void Frame::ExtractORB(int flag, const cv::Mat &im, const int x0, const int x1)
{
    vector<int> vLapping = {x0,x1};
    if(flag==0)
        monoLeft = (*mpORBextractorLeft)(im,cv::Mat(),mvKeys,mDescriptors,vLapping);
    else
        monoRight = (*mpORBextractorRight)(im,cv::Mat(),mvKeysRight,mDescriptorsRight,vLapping);
}

ComputeBoW 计算词袋数据

如果没有传入已有的词袋数据，则就用当前的描述子重新计算生成词袋数据
计算词包mBowVec和mFeatVec，其中mFeatVec记录了属于第i个node（在第4层）的ni个描述子。

void Frame::ComputeBoW()
{
    if(mBowVec.empty())
    {
        vector<cv::Mat> vCurrentDesc = Converter::toDescriptorVector(mDescriptors);
        mpORBvocabulary->transform(vCurrentDesc,mBowVec,mFeatVec,4);
    }
}

ProjectPointDistort 投影点扭曲

bool Frame::ProjectPointDistort(MapPoint* pMP, cv::Point2f &kp, float &u, float &v)
{

    // 3D in absolute coordinates 世界坐标系
    cv::Mat P = pMP->GetWorldPos();

    // 3D in camera coordinates 相机坐标系
    const cv::Mat Pc = mRcw*P+mtcw;
    const float &PcX = Pc.at<float>(0);
    const float &PcY= Pc.at<float>(1);
    const float &PcZ = Pc.at<float>(2);

    // Check positive depth
    if(PcZ<0.0f)
    {
        cout << "Negative depth: " << PcZ << endl;
        return false;
    }

    // Project in image and check it is not outside
    // 在图像中投影并检查它不在外面
    const float invz = 1.0f/PcZ;
    u=fx*PcX*invz+cx;
    v=fy*PcY*invz+cy;

    if(u<mnMinX || u>mnMaxX)
        return false;
    if(v<mnMinY || v>mnMaxY)
        return false;

    float u_distort, v_distort;

    float x = (u - cx) * invfx;
    float y = (v - cy) * invfy;
    float r2 = x * x + y * y;
    float k1 = mDistCoef.at<float>(0);
    float k2 = mDistCoef.at<float>(1);
    float p1 = mDistCoef.at<float>(2);
    float p2 = mDistCoef.at<float>(3);
    float k3 = 0;
    if(mDistCoef.total() == 5)
    {
        k3 = mDistCoef.at<float>(4);
    }

    // Radial distorsion 径向畸变
    float x_distort = x * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2);
    float y_distort = y * (1 + k1 * r2 + k2 * r2 * r2 + k3 * r2 * r2 * r2);

    // Tangential distorsion 切向畸变
    x_distort = x_distort + (2 * p1 * x * y + p2 * (r2 + 2 * x * x));
    y_distort = y_distort + (p1 * (r2 + 2 * y * y) + 2 * p2 * x * y);

    u_distort = x_distort * fx + cx;
    v_distort = y_distort * fy + cy;


    u = u_distort;
    v = v_distort;

    kp = cv::Point2f(u, v);

    return true;
}

ComputeStereoFishEyeMatches 鱼眼匹配

根据匹配重叠区域确定匹配范围，加速匹配
直接暴力匹配的出匹配对
对匹配进行三角匹配，检查视差和重投影错误以丢弃虚假匹配

void Frame::ComputeStereoFishEyeMatches() {
    //Speed it up by matching keypoints in the lapping area
    // 通过在重叠区域中的匹配关键点来加快速度
    vector<cv::KeyPoint> stereoLeft(mvKeys.begin() + monoLeft, mvKeys.end());
    vector<cv::KeyPoint> stereoRight(mvKeysRight.begin() + monoRight, mvKeysRight.end());

    cv::Mat stereoDescLeft = mDescriptors.rowRange(monoLeft, mDescriptors.rows);
    cv::Mat stereoDescRight = mDescriptorsRight.rowRange(monoRight, mDescriptorsRight.rows);

    mvLeftToRightMatch = vector<int>(Nleft,-1);
    mvRightToLeftMatch = vector<int>(Nright,-1);
    mvDepth = vector<float>(Nleft,-1.0f);
    mvuRight = vector<float>(Nleft,-1);
    mvStereo3Dpoints = vector<cv::Mat>(Nleft);
    mnCloseMPs = 0;

    //通过暴力匹配得到左右两帧图像的匹配点
    vector<vector<cv::DMatch>> matches;
    // cv::BFMatcher Frame::BFmatcher = cv::BFMatcher(cv::NORM_HAMMING);
    BFmatcher.knnMatch(stereoDescLeft,stereoDescRight,matches,2);

    int nMatches = 0;
    int descMatches = 0;

    //Check matches using Lowe's ratio
    for(vector<vector<cv::DMatch>>::iterator it = matches.begin(); it != matches.end(); ++it){
        if((*it).size() >= 2 && (*it)[0].distance < (*it)[1].distance * 0.7){
            //For every good match, check parallax and reprojection error to discard spurious matches
            cv::Mat p3D;
            descMatches++;
            // 取出 左右图塔层数，进行三角匹配得到深度
            float sigma1 = mvLevelSigma2[mvKeys[(*it)[0].queryIdx + monoLeft].octave], sigma2 = mvLevelSigma2[mvKeysRight[(*it)[0].trainIdx + monoRight].octave];
            float depth = static_cast<KannalaBrandt8*>(mpCamera)->TriangulateMatches(mpCamera2,mvKeys[(*it)[0].queryIdx + monoLeft],mvKeysRight[(*it)[0].trainIdx + monoRight],mRlr,mtlr,sigma1,sigma2,p3D);
            if(depth > 0.0001f){
                mvLeftToRightMatch[(*it)[0].queryIdx + monoLeft] = (*it)[0].trainIdx + monoRight;
                mvRightToLeftMatch[(*it)[0].trainIdx + monoRight] = (*it)[0].queryIdx + monoLeft;
                mvStereo3Dpoints[(*it)[0].queryIdx + monoLeft] = p3D.clone();
                mvDepth[(*it)[0].queryIdx + monoLeft] = depth;
                nMatches++;
            }
        }
    }
}

判断一个点是否在视野范围之内

核心步骤:

取出该点世界坐标系的3D坐标，并将其投影到相机坐标系下
进行一些条件判断：
1. 该点深度应该大于0
2. 图像坐标系下，该点应该在图像上
3. 计算MapPoint到相机中心的距离, 并判断是否在尺度变化的距离内
4. 计算当前视角和平均视角夹角的余弦值, 若小于cos(60), 即夹角大于60度则返回
根据深度预测尺度（对应特征点在一层）
赋值返回

isInFrustum

计算了重投影坐标，观测方向夹角，预测在当前帧的尺度
Param: MapPoint+视角和平均视角的方向阈值

bool Frame::isInFrustum(MapPoint *pMP, float viewingCosLimit)
{
    if(Nleft == -1){ 	// 如果是单目
        pMP->mbTrackInView = false;
        pMP->mTrackProjX = -1;
        pMP->mTrackProjY = -1;

        // 3D in absolute coordinates 
        // 取出3D点在绝对坐标中坐标
        cv::Matx31f Px = pMP->GetWorldPos2();

        // 3D in camera coordinates 3D点P在相机坐标系下的坐标
        const cv::Matx31f Pc = mRcwx * Px + mtcwx; // 这里的Rt是经过初步的优化后的
        const float Pc_dist = cv::norm(Pc);

        // Check positive depth 检测点的深度
        const float &PcZ = Pc(2);
        const float invz = 1.0f/PcZ;
        if(PcZ<0.0f)
            return false;
            
		// V-D 1) 将MapPoint投影到当前帧, 并判断是否在图像内
        const cv::Point2f uv = mpCamera->project(Pc);
        if(uv.x<mnMinX || uv.x>mnMaxX)
            return false;
        if(uv.y<mnMinY || uv.y>mnMaxY)
            return false;

        pMP->mTrackProjX = uv.x;
        pMP->mTrackProjY = uv.y;

        // Check distance is in the scale invariance region of the MapPoint
        // V-D 3) 计算MapPoint到相机中心的距离, 并判断是否在尺度变化的距离内
        // 每一个地图点都是对应于若干尺度的金字塔提取出来的，具有一定的有效深度
        const float maxDistance = pMP->GetMaxDistanceInvariance();
        const float minDistance = pMP->GetMinDistanceInvariance();
        // 世界坐标系下，相机到3D点P的向量, 向量方向由相机指向3D点P
        const cv::Matx31f PO = Px-mOwx;
        const float dist = cv::norm(PO);
        if(dist<minDistance || dist>maxDistance)
            return false;

        // Check viewing angle
        // V-D 2) 计算当前视角和平均视角夹角的余弦值, 若小于cos(60), 即夹角大于60度则返回
        cv::Matx31f Pnx = pMP->GetNormal2();
        const float viewCos = PO.dot(Pnx)/dist;
        if(viewCos<viewingCosLimit)
            return false;

        // Predict scale in the image
        // V-D 4) 根据深度预测尺度（对应特征点在一层）
        const int nPredictedLevel = pMP->PredictScale(dist,this);

        // Data used by the tracking // 标记该点将来要被投影
        pMP->mbTrackInView = true;
        pMP->mTrackProjX = uv.x;
        pMP->mTrackProjXR = uv.x - mbf*invz; //该3D点投影到双目右侧相机上的横坐标

        pMP->mTrackDepth = Pc_dist;

        pMP->mTrackProjY = uv.y;
        pMP->mnTrackScaleLevel= nPredictedLevel;
        pMP->mTrackViewCos = viewCos;


        return true;
    }
    else{	// 双目等
        pMP->mbTrackInView = false;
        pMP->mbTrackInViewR = false;
        pMP -> mnTrackScaleLevel = -1;
        pMP -> mnTrackScaleLevelR = -1;
		
		// 双目左右视眼都得检测
        pMP->mbTrackInView = isInFrustumChecks(pMP,viewingCosLimit);
        pMP->mbTrackInViewR = isInFrustumChecks(pMP,viewingCosLimit,true);

        return pMP->mbTrackInView || pMP->mbTrackInViewR;
    }
}

isInFrustumChecks

Param: 多了一个是否是右图标记

bool Frame::isInFrustumChecks(MapPoint *pMP, float viewingCosLimit, bool bRight) {
    // 3D in absolute coordinates
    // // 取出3D点在绝对坐标中坐标
    cv::Matx31f Px = pMP->GetWorldPos2();

    cv::Matx33f mRx;
    cv::Matx31f mtx, twcx;

    cv::Matx33f Rcw = mRcwx;
    cv::Matx33f Rwc = mRcwx.t();
    cv::Matx31f tcw = mOwx;

	// 得到3D世界点到图像坐标系的转换关系，分左右图
    if(bRight){
        cv::Matx33f Rrl = mTrlx.get_minor<3,3>(0,0);
        cv::Matx31f trl = mTrlx.get_minor<3,1>(0,3);
        mRx = Rrl * Rcw;
        mtx = Rrl * tcw + trl;
        twcx = Rwc * mTlrx.get_minor<3,1>(0,3) + tcw;
    }
    else{
        mRx = mRcwx;
        mtx = mtcwx;
        twcx = mOwx;
    }

    // 3D in camera coordinates
	// 将3D点转换到图像坐标系
    cv::Matx31f Pcx = mRx * Px + mtx;
    const float Pc_dist = cv::norm(Pcx);
    const float &PcZ = Pcx(2);

    // Check positive depth
    // 首先深度判断，若不符合直接return false
    if(PcZ<0.0f)
        return false;

    // Project in image and check it is not outside
    // 其次检测 该点是否超出图像的边界，超出时return false
    cv::Point2f uv;
    if(bRight) uv = mpCamera2->project(Pcx);
    else uv = mpCamera->project(Pcx);
    if(uv.x<mnMinX || uv.x>mnMaxX)
        return false;
    if(uv.y<mnMinY || uv.y>mnMaxY)
        return false;

    // Check distance is in the scale invariance region of the MapPoint
    // 接着计算 MapPoint到相机中心的距离, 并判断是否在尺度变化的距离内
    const float maxDistance = pMP->GetMaxDistanceInvariance();
    const float minDistance = pMP->GetMinDistanceInvariance();
    const cv::Matx31f POx = Px - twcx;
    const float dist = cv::norm(POx);
    if(dist<minDistance || dist>maxDistance)
        return false;

    // Check viewing angle
    // 然后 检测视场角度是否符合阈值
    cv::Matx31f Pnx = pMP->GetNormal2();

    const float viewCos = POx.dot(Pnx)/dist;

    if(viewCos<viewingCosLimit)
        return false;

    // Predict scale in the image
    // 最后基于深度预测尺度
    const int nPredictedLevel = pMP->PredictScale(dist,this);

    if(bRight){
        pMP->mTrackProjXR = uv.x;
        pMP->mTrackProjYR = uv.y;
        pMP->mnTrackScaleLevelR= nPredictedLevel;
        pMP->mTrackViewCosR = viewCos;
        pMP->mTrackDepthR = Pc_dist;
    }
    else{
        pMP->mTrackProjX = uv.x;
        pMP->mTrackProjY = uv.y;
        pMP->mnTrackScaleLevel= nPredictedLevel;
        pMP->mTrackViewCos = viewCos;
        pMP->mTrackDepth = Pc_dist;
    }

    return true;
}

GetFeaturesInArea

找到在以x,y为中心,边长为2r的方形内且在[minLevel, maxLevel]的特征点
- Param: 图像坐标u，图像坐标系v，边长，最小尺度，最大尺度
- return: 满足条件的特征点序号
步骤：
- 基于±r 确定栅格坐标系x,y的范围
- 遍历该矩形区域，找出每个区域的特征点
- 遍历这些特征点，若在尺度范围，且距离 uv距离小于r，则该特征满足条件
- 将该特征下表push进容器

vector<size_t> Frame::GetFeaturesInArea(const float &x, const float  &y, const float  &r, const int minLevel, const int maxLevel, const bool bRight) const
{	
	// 定义返回数据中所有特征的index
    vector<size_t> vIndices;
    vIndices.reserve(N);

    float factorX = r;
    float factorY = r;
	
	/// ±r后x方向 栅格的最小和最大index值
    const int nMinCellX = max(0,(int)floor((x-mnMinX-factorX)*mfGridElementWidthInv));
    if(nMinCellX>=FRAME_GRID_COLS)
    {
        return vIndices;
    }
    const int nMaxCellX = min((int)FRAME_GRID_COLS-1,(int)ceil((x-mnMinX+factorX)*mfGridElementWidthInv));
    if(nMaxCellX<0)
    {
        return vIndices;
    }

	// ±r后y方向 栅格的最小和最大index值
    const int nMinCellY = max(0,(int)floor((y-mnMinY-factorY)*mfGridElementHeightInv));
    if(nMinCellY>=FRAME_GRID_ROWS)
    {
        return vIndices;
    }

    const int nMaxCellY = min((int)FRAME_GRID_ROWS-1,(int)ceil((y-mnMinY+factorY)*mfGridElementHeightInv));
    if(nMaxCellY<0)
    {
        return vIndices;
    }
	
	// 最小尺度和最大尺度都应该大于等于0
    const bool bCheckLevels = (minLevel>0) || (maxLevel>=0);

	/// 遍历符合条件的区域栅格，
    for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
    {
        for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
        {	
        	// 取出该栅格内的所有特征标记
            const vector<size_t> vCell = (!bRight) ? mGrid[ix][iy] : mGridRight[ix][iy];
            if(vCell.empty()) // 若为空则continue
                continue;
			
			// 遍历该栅格内的所有特征
            for(size_t j=0, jend=vCell.size(); j<jend; j++)
            {	
            	// 取出特征点
                const cv::KeyPoint &kpUn = (Nleft == -1) ? mvKeysUn[vCell[j]]
                                                         : (!bRight) ? mvKeys[vCell[j]]
                                                                     : mvKeysRight[vCell[j]];
                // 金字塔层检测，超出最小或最大时continue
                if(bCheckLevels)	
                {
                    if(kpUn.octave<minLevel)
                        continue;
                    if(maxLevel>=0)
                        if(kpUn.octave>maxLevel)
                            continue;
                }
				
				// 筛出在 r 圆内的特征，并push到 vIndices
                const float distx = kpUn.pt.x-x;
                const float disty = kpUn.pt.y-y;
	
                if(fabs(distx)<factorX && fabs(disty)<factorY)
                    vIndices.push_back(vCell[j]);
            }
        }
    }

    return vIndices;
}

ComputeStereoMatches 双目匹配

目的：

为左图的每一个特征点在右图中找到匹配点

主要步骤：

根据基线(有冗余范围)上描述子距离找到匹配, 再进行SAD精确定位
然后利用抛物线拟合得到亚像素精度的匹配
匹配成功后会更新 mvuRight(ur) 和 mvDepth(Z)
最后对所有SAD的值进行排序, 剔除SAD值较大的匹配对

void Frame::ComputeStereoMatches()
{
    mvuRight = vector<float>(N,-1.0f);
    mvDepth = vector<float>(N,-1.0f);

    const int thOrbDist = (ORBmatcher::TH_HIGH+ORBmatcher::TH_LOW)/2;
	// 取出金字塔*零层*的行数
    const int nRows = mpORBextractorLeft->mvImagePyramid[0].rows;

    
    ///< 步骤1：建立特征点搜索范围对应表，一个特征点在一个带状区域内搜索匹配特征点
    // 匹配搜索的时候,不仅仅是在一条横线上而是在一条横向搜索带上搜索，搜索带的宽度与其所在层数有关
    
    //Assign keypoints to row table 将关键点分配给行表
    vector<vector<size_t> > vRowIndices(nRows,vector<size_t>());
    for(int i=0; i<nRows; i++)  // 每行内存初始分配200
        vRowIndices[i].reserve(200);

    const int Nr = mvKeysRight.size(); // 右图关键帧个数
	// 构建特征搜索带，一个特征对应临近多行
    for(int iR=0; iR<Nr; iR++) 
    {
    	// 在这个函数中没有对双目进行校正，双目校正是在外层程序中实现的
        const cv::KeyPoint &kp = mvKeysRight[iR]; 
        const float &kpY = kp.pt.y; // 右关键帧的行数
        // 计算匹配搜索的纵向宽度，尺度越大（层数越高，距离越近），搜索范围越大，其位置不确定性越高
        // 如果特征点在金字塔第一层，则搜索范围为:正负2
        const float r = 2.0f*mvScaleFactors[mvKeysRight[iR].octave];
        const int maxr = ceil(kpY+r);
        const int minr = floor(kpY-r);
		// 每行中加入可能为特征点的index
        for(int yi=minr;yi<=maxr;yi++)
            vRowIndices[yi].push_back(iR);
    }

    // Set limits for search 为左图在右图搜索中纵向确定范围
    const float minZ = mb; // 用的前一次的mb，因为构造在后面
    const float minD = 0;  // 最小视差,设置为0即可
    const float maxD = mbf/minZ; //最大视差,对应最小深度 mbf/minZ = mbf/(mbf/fx) = fx

    // For each left keypoint search a match in the right image
    // 对于每个左关键点，在右图像中搜索一个匹配项
    vector<pair<int, int> > vDistIdx;
    vDistIdx.reserve(N); ///< N KeyPoints数量

    
	///< 步骤2：对左目相机每个特征点，通过描述子在右目带状搜索区域找到匹配点, 再通过SAD做亚像素匹配
    
	// 2.1 所有可能的匹配点，找出最佳匹配点（描述子距离最小）
    for(int iL=0; iL<N; iL++)
    {	// 注意：这里是校正前的mvKeys，而不是校正后的mvKeysUn
        const cv::KeyPoint &kpL = mvKeys[iL]; 
        const int &levelL = kpL.octave;

        const float &vL = kpL.pt.y; //右关键帧图像 l\u
        const float &uL = kpL.pt.x; 
		
		// 取出右图中对应行可能的关键点帧indexs(已经包含了上下N行的可能)
        const vector<size_t> &vCandidates = vRowIndices[vL];
		
        if(vCandidates.empty()) 
            continue;

        const float minU = uL-maxD; //纵向最小匹配范围
        const float maxU = uL-minD; //纵向最大匹配范围

        if(maxU<0)	// 无匹配点直接跳过
            continue;

        int bestDist = ORBmatcher::TH_HIGH;
        size_t bestIdxR = 0;
		
		// 每个特征点描述子占一行，取出该特征点的描述子
        const cv::Mat &dL = mDescriptors.row(iL);

        // Compare descriptor to right keypoints 比较右图中所有匹配点
        for(size_t iC=0; iC<vCandidates.size(); iC++)
        {
            const size_t iR = vCandidates[iC];	//右图关键帧index
            const cv::KeyPoint &kpR = mvKeysRight[iR]; //右图关键帧
			// 仅对近邻尺度的特征点进行匹配 (上中下各一行)
            if(kpR.octave<levelL-1 || kpR.octave>levelL+1)
                continue;

            const float &uR = kpR.pt.x; //列数

            if(uR>=minU && uR<=maxU) // 列数符合
            {	
            	// 取出右描述子，并计算其距离
                const cv::Mat &dR = mDescriptorsRight.row(iR);
                const int dist = ORBmatcher::DescriptorDistance(dL,dR);
				// 最好的匹配的匹配误差存在bestDist，匹配点位置存在bestIdxR中
                if(dist<bestDist)
                {
                    bestDist = dist;
                    bestIdxR = iR;
                }
            }
        }

        // 2.2 通过SAD匹配提高像素匹配修正量bestincR
        if(bestDist<thOrbDist)	// 最好的匹配小于阈值
        {
            // coordinates in image pyramid at keypoint scale
            // kpL.pt.x对应金字塔最底层坐标，将最佳匹配的特征点对尺度变换到尺度对应层 
            const float uR0 = mvKeysRight[bestIdxR].pt.x;
            const float scaleFactor = mvInvScaleFactors[kpL.octave];
            const float scaleduL = round(kpL.pt.x*scaleFactor);
            const float scaledvL = round(kpL.pt.y*scaleFactor);
            const float scaleduR0 = round(uR0*scaleFactor);

            // sliding window search 滑动窗口的大小11*11 
            const int w = 5;
            cv::Mat IL = mpORBextractorLeft->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduL-w,scaleduL+w+1);
            
            // 窗口中的每个元素减去正中心的那个元素，简单归一化，减小光照强度影响 opencv3中去掉了
			**IL.convertTo(IL,CV_32F);
            **IL = IL - IL.at<float>(w,w) * cv::Mat::ones(IL.rows,IL.cols,CV_32F);
            
            int bestDist = INT_MAX;
            int bestincR = 0;
            const int L = 5;
            vector<float> vDists;
            vDists.resize(2*L+1); // 11
 			
            // 滑动窗口的滑动范围为 [0,2L+1)
            const float iniu = scaleduR0+L-w;
            const float endu = scaleduR0+L+w+1; 
            if(iniu<0 || endu >= mpORBextractorRight->mvImagePyramid[kpL.octave].cols)
                continue;	// 提前判断滑动窗口滑动过程中是否会越界
			
            for(int incR=-L; incR<=+L; incR++)
            {	
                // 只有列在动，即横向移动
                cv::Mat IR = mpORBextractorRight->mvImagePyramid[kpL.octave].rowRange(scaledvL-w,scaledvL+w+1).colRange(scaleduR0+incR-w,scaleduR0+incR+w+1);
                // 口中的每个元素减去正中心的那个元素，简单归一化，减小光照强度影响 opencv3中去掉了
                **IR.convertTo(IR,CV_32F);
                **IR = IR - IR.at<float>(w,w) * cv::Mat::ones(IR.rows,IR.cols,CV_32F);

				// 一范数，计算差的绝对值
                float dist = cv::norm(IL,IR,cv::NORM_L1);
                if(dist<bestDist)	// 得到最佳匹配位置
                {
                    bestDist =  dist;
                    bestincR = incR;
                }

                vDists[L+incR] = dist; //距离赋值vDists
            }

            // 整个滑动窗口过程中，SAD最小值应该以抛物线形式出现，否则确定为匹配失败
            // 最优质跑到了边界处，肯定不是抛物线的形式出现
            if(bestincR==-L || bestincR==L)
                continue;

            // Sub-pixel match (Parabola fitting)
            // 2.3：做抛物线拟合找谷底得到亚像素匹配deltaR
            const float dist1 = vDists[L+bestincR-1];
            const float dist2 = vDists[L+bestincR];
            const float dist3 = vDists[L+bestincR+1];
            // bestincR+deltaR就是抛物线谷底的位置，相对SAD匹配出的最小值bestincR的修正量为deltaR
            const float deltaR = (dist1-dist3)/(2.0f*(dist1+dist3-2.0f*dist2));
			
            // 抛物线拟合得到的修正量不能超过一个像素，否则放弃求该特征点的深度
            if(deltaR<-1 || deltaR>1)
                continue;

            // Re-scaled coordinate 《通过描述子匹配得到匹配点位置为scaleduR0+
            // 通过SAD匹配找到修正量bestincR+通过抛物线拟合找到亚像素修正量deltaR》
            float bestuR = mvScaleFactors[kpL.octave]*((float)scaleduR0+(float)bestincR+deltaR);
			
            float disparity = (uL-bestuR); // 计算出视差

            if(disparity>=minD && disparity<maxD)  // 最后判断视差是否在范围内
            {
                if(disparity<=0)
                {
                    disparity=0.01;
                    bestuR = uL-0.01;
                }
                mvDepth[iL]=mbf/disparity; // depth=baseline*fx/disparity
                mvuRight[iL] = bestuR; // 匹配对在右图的横坐标
                vDistIdx.push_back(pair<int,int>(bestDist,iL)); // 该特征点SAD匹配最小匹配偏差
            }
        }
    }
    
    
	///< 步骤3：剔除SAD匹配偏差较大的匹配特征点
    sort(vDistIdx.begin(),vDistIdx.end()); // 所有匹配对的SAD偏差进行排序
    const float median = vDistIdx[vDistIdx.size()/2].first; //取SAD中值偏差 
    // 计算自适应距离,大于此距离的匹配对将剔除
    const float thDist = 1.5f*1.4f*median; 
    for(int i=vDistIdx.size()-1;i>=0;i--)
    {
        if(vDistIdx[i].first<thDist)
            break;
        else
        {
            mvuRight[vDistIdx[i].second]=-1;
            mvDepth[vDistIdx[i].second]=-1;
        }
    }
}

KeyFrame

说明：

关键帧

关键帧：
- 关键帧是连续几帧普通帧中较具有代表性的一帧
作用：
- 降低局部相邻关键帧中的信息冗余度
- 一定程度上关键帧是普通帧滤波和优化的结果，防止无效和错误信息进入优化过程
- 面向后端优化的算力与精度的折中，提高计算资源的效率
选择：
- 本身质量高：清晰的画面、特征点充足、特征点分布均匀等
- 良好的联系网：与其他关键帧之间有一定的共视关系，同时不能重复度太高，即既存在约束，又要减少冗余的信息
选取方式：
- 与上一帧关键帧之间的时间（帧数）间隔是否合适
- 与上一帧之间的距离是否足够远
- 跟踪局部地图的质量（共视特征点的数量）

共视图

概念：
- 共视图：
  - 以任一关键帧为中心，与其共视关键帧及联系构建的共视网。
  - 共视图中的节点为关键帧；若两个关键帧共视地图点超过15个，则用一条边将两者相连接。用权重值θ表示两个关键帧能共同观测到的点云数量。
- 共视关键帧：
  - 任取一关键帧，与该关键帧能观测到相同地图点的关键帧（一级关键帧）。
  - 该关键帧的一级关键帧自身的一级关键帧称为该关键帧的二级关键帧。
  - 注：能观测到相同关键帧的数目由共视程度来表征
- 通常，只会使用一级相邻层级的共视关系及信息，而局部地图优化时用两级相邻共视关系进行优化。
作用：
- 增加地图点信息，以优化地图
- 表示关键帧之间的关系、联系的紧密程度
ORB应用场景：
- 跟踪局部地图，扩大搜索范围
- 局部建图中关键帧之间新建地图点
- 闭环检测、重定位检测
- 优化等

本质树

由父子关键帧构成，常用于本质图中

本质图

针对关键帧而构成的图像。
特点：
- 与共视图相比，本质图比较稀疏
- 节点表示所有关键帧，但连接的边只保留联系紧密关键帧之间的边，使得结果更加精确
- 图中包含的信息有：
  - 扩展树的连接关系
  - 形成闭环的连接关系
  - 闭环后，引起地图点变动而新增加的连接关系
  - 共视关系较好的连接关系（至少有100个共视地图点）
作用：
- 闭环矫正时，用相似变换（Sim3）来矫正尺度漂移，将闭环的误差均摊在本质图中
优势：
- 全局BA可能收敛不了，但本质图可以。
- 加快收敛且结果精度更高
- 本质图+全局优化一起优化，提高精度。

成员变量

分为：

1 个线程访问或从不更改（不需要互斥锁）
需要通过互斥体访问才能线程安全

// 以下变量仅从 1 个线程访问或从不更改（不需要互斥锁）
public:
	// nNextID表示上一个KeyFrame的ID号 
	// 初始定义： long unsigned int KeyFrame::nNextId=0; 
    static long unsigned int nNextId;
    // 在nNextID的基础上加1就得到了mnID，为当前KeyFrame的ID号
    long unsigned int mnId;

	// 每个KeyFrame基本属性是它是一个Frame，KeyFrame初始化的时候需要Frame，
    // mnFrameId记录了该KeyFrame是由哪个Frame初始化的
    const long unsigned int mnFrameId;

    const double mTimeStamp;

    // Grid (to speed up feature matching) 
	// 和Frame类中的定义相同，为了加快 特征匹配
    const int mnGridCols;
    const int mnGridRows;
    const float mfGridElementWidthInv;
    const float mfGridElementHeightInv;

    // Variables used by the tracking 
    long unsigned int mnTrackReferenceForFrame;
    long unsigned int mnFuseTargetForKF;

    // Variables used by the local mapping
    long unsigned int mnBALocalForKF;
    long unsigned int mnBAFixedForKF;

    //Number of optimizations by BA(amount of iterations in BA)
    long unsigned int mnNumberOfOpt;

    // Variables used by the keyframe database
    long unsigned int mnLoopQuery;
    int mnLoopWords;
    float mLoopScore;
    long unsigned int mnRelocQuery;
    int mnRelocWords;
    float mRelocScore;
    long unsigned int mnMergeQuery;
    int mnMergeWords;
    float mMergeScore;
    long unsigned int mnPlaceRecognitionQuery;
    int mnPlaceRecognitionWords;
    float mPlaceRecognitionScore;

    bool mbCurrentPlaceRecognition;


    // Variables used by loop closing
    cv::Mat mTcwGBA;
    cv::Mat mTcwBefGBA;
    cv::Mat mVwbGBA;
    cv::Mat mVwbBefGBA;
    IMU::Bias mBiasGBA;
    long unsigned int mnBAGlobalForKF;

    // Variables used by merging
    cv::Mat mTcwMerge;
    cv::Mat mTcwBefMerge;
    cv::Mat mTwcBefMerge;
    cv::Mat mVwbMerge;
    cv::Mat mVwbBefMerge;
    IMU::Bias mBiasMerge;
    long unsigned int mnMergeCorrectedForKF;
    long unsigned int mnMergeForKF;
    float mfScaleMerge;
    long unsigned int mnBALocalForMerge;

    float mfScale;

    // Calibration parameters
    const float fx, fy, cx, cy, invfx, invfy, mbf, mb, mThDepth;
    cv::Mat mDistCoef;

    // Number of KeyPoints
    const int N;

    // KeyPoints, stereo coordinate and descriptors (all associated by an index) 
	// 关键点、立体坐标和描述符（均由索引关联），与Frame类中定义相同
    const std::vector<cv::KeyPoint> mvKeys;
    const std::vector<cv::KeyPoint> mvKeysUn;
    const std::vector<float> mvuRight; // negative value for monocular points
    const std::vector<float> mvDepth; // negative value for monocular points
    const cv::Mat mDescriptors;

    //BoW
    DBoW2::BowVector mBowVec;
    DBoW2::FeatureVector mFeatVec;

    // Pose relative to parent (this is computed when bad flag is activated)
	// 相对于父的姿势（这是在激活坏标志时计算的）
    cv::Mat mTcp;

    // Scale
    const int mnScaleLevels;
    const float mfScaleFactor;
    const float mfLogScaleFactor;
    const std::vector<float> mvScaleFactors;// 尺度因子，scale^n，scale=1.2，n为层数
    const std::vector<float> mvLevelSigma2; // 尺度因子的平方
    const std::vector<float> mvInvLevelSigma2;

    // Image bounds and calibration
    const int mnMinX;
    const int mnMinY;
    const int mnMaxX;
    const int mnMaxY;
    const cv::Mat mK;

    // Preintegrated IMU measurements from previous keyframe
    KeyFrame* mPrevKF;
    KeyFrame* mNextKF;

    IMU::Preintegrated* mpImuPreintegrated;
    IMU::Calib mImuCalib;


    unsigned int mnOriginMapId;

    string mNameFile;

    int mnDataset;

    std::vector <KeyFrame*> mvpLoopCandKFs;
    std::vector <KeyFrame*> mvpMergeCandKFs;

    bool mbHasHessian;
    cv::Mat mHessianPose;

// 以下变量需要通过互斥体访问才能线程安全。
protected:

    // SE3 Pose and camera center 位姿和相机光心的坐标
    cv::Mat Tcw; // 当前相机的位姿，世界坐标系到相机坐标系
    cv::Mat Twc; // 当前相机位姿的逆
    cv::Mat Ow; // 相机光心(左目)在世界坐标系下的坐标,这里和普通帧中的定义是一样的
    cv::Mat Cw; // Stereo middel point. Only for visualization

    cv::Matx44f Tcw_, Twc_, Tlr_;
    cv::Matx31f Ow_;

    // IMU position
    cv::Mat Owb;

    // Velocity (Only used for inertial SLAM)
    cv::Mat Vw;

    // Imu bias
    IMU::Bias mImuBias;

    // 关键帧观测到的地图点
    std::vector<MapPoint*> mvpMapPoints;

    // BoW
    KeyFrameDatabase* mpKeyFrameDB; // BoW词典
    ORBVocabulary* mpORBvocabulary; // 视觉单词

    // Grid over the image to speed up feature matching
    // 其实应该说是二维的,第三维的 vector中保存的是这个网格内的特征点的索引
    std::vector< std::vector <std::vector<size_t> > > mGrid;
	// 与该关键帧连接（至少15个共视地图点）的关键帧与权重
    std::map<KeyFrame*,int> mConnectedKeyFrameWeights;  ///< 与该关键帧连接的关键帧与权重
    std::vector<KeyFrame*> mvpOrderedConnectedKeyFrames; ///< 排序后的共视图关键帧
    std::vector<int> mvOrderedWeights;///< 排序后的权重(从大到小)

    // Spanning Tree and Loop Edges
    bool mbFirstConnection;  // 是否是第一次生成树
    KeyFrame* mpParent; // 当前关键帧的父关键帧 （共视程度最高的）
	// set 比 vector 无重复，自动排序
    std::set<KeyFrame*> mspChildrens; // 存储当前关键帧的子关键帧，这个一般不止一个
    std::set<KeyFrame*> mspLoopEdges; // 和当前关键帧形成回环关系的关键帧
    std::set<KeyFrame*> mspMergeEdges; // 和当前关键帧合并的关键帧

    // Bad flags
    bool mbNotErase;  // 当前关键帧已经和其他的关键帧形成了回环关系，因此在各种优化的过程中不应该被删除
    bool mbToBeErased; // 将要被删除的标志
    bool mbBad;  // 关键帧为Bad的标志 

    float mHalfBaseline; // Only for visualization // 对于双目相机来说,双目相机基线长度的一半

    Map* mpMap; // 局部地图
    
	/// 在对位姿进行操作时相关的互斥锁
    std::mutex mMutexPose; // for pose, velocity and biases
    /// 在操作当前关键帧和其他关键帧的共视关系的时候使用到的互斥锁
    std::mutex mMutexConnections;
    /// 在操作和特征点有关的变量的时候的互斥锁
    std::mutex mMutexFeatures;
    std::mutex mMutexMap;

public:
    GeometricCamera* mpCamera, *mpCamera2;

    //Indexes of stereo observations correspondences
    std::vector<int> mvLeftToRightMatch, mvRightToLeftMatch;

    //Transformation matrix between cameras in stereo fisheye
	// 立体鱼眼中摄像机之间的变换矩阵
    cv::Mat mTlr;
    cv::Mat mTrl;

    //KeyPoints in the right image (for stereo fisheye, coordinates are needed)
	//右图的关键点（立体鱼眼，需要坐标）
    const std::vector<cv::KeyPoint> mvKeysRight;

    const int NLeft, NRight;

    std::vector< std::vector <std::vector<size_t> > > mGridRight;

构造

// Param: 当前帧+地图Map+关键帧数据集的指针
KeyFrame::KeyFrame(Frame &F, Map *pMap, KeyFrameDatabase *pKFDB):
    bImu(pMap->isImuInitialized()) //使用imu标记，bool
        , mnFrameId(F.mnId),  mTimeStamp(F.mTimeStamp) // 当前帧的Id+时间戳
        , mnGridCols(FRAME_GRID_COLS), mnGridRows(FRAME_GRID_ROWS) // 网格数，跟Frame一样，64*48
        , mfGridElementWidthInv(F.mfGridElementWidthInv), mfGridElementHeightInv(F.mfGridElementHeightInv)//每个网格占多少像素的逆
        , mnTrackReferenceForFrame(0), mnFuseTargetForKF(0), mnBALocalForKF(0), mnBAFixedForKF(0), mnBALocalForMerge(0),
    mnLoopQuery(0), mnLoopWords(0), mnRelocQuery(0), mnRelocWords(0), mnBAGlobalForKF(0), mnPlaceRecognitionQuery(0), mnPlaceRecognitionWords(0), mPlaceRecognitionScore(0),
    fx(F.fx), fy(F.fy), cx(F.cx), cy(F.cy), invfx(F.invfx), invfy(F.invfy), //相机参数
    mbf(F.mbf), mb(F.mb), mThDepth(F.mThDepth), N(F.N), mvKeys(F.mvKeys), mvKeysUn(F.mvKeysUn),
    mvuRight(F.mvuRight), mvDepth(F.mvDepth), mDescriptors(F.mDescriptors.clone()),
    mBowVec(F.mBowVec), mFeatVec(F.mFeatVec), mnScaleLevels(F.mnScaleLevels), mfScaleFactor(F.mfScaleFactor),
    mfLogScaleFactor(F.mfLogScaleFactor), mvScaleFactors(F.mvScaleFactors), mvLevelSigma2(F.mvLevelSigma2),
    mvInvLevelSigma2(F.mvInvLevelSigma2), mnMinX(F.mnMinX), mnMinY(F.mnMinY), mnMaxX(F.mnMaxX),
    mnMaxY(F.mnMaxY), mK(F.mK), mPrevKF(NULL), mNextKF(NULL), mpImuPreintegrated(F.mpImuPreintegrated),
    mImuCalib(F.mImuCalib), mvpMapPoints(F.mvpMapPoints), mpKeyFrameDB(pKFDB),
    mpORBvocabulary(F.mpORBvocabulary), mbFirstConnection(true), mpParent(NULL), mDistCoef(F.mDistCoef), mbNotErase(false), mnDataset(F.mnDataset),
    mbToBeErased(false), mbBad(false), mHalfBaseline(F.mb/2), mpMap(pMap), mbCurrentPlaceRecognition(false), mNameFile(F.mNameFile), mbHasHessian(false), mnMergeCorrectedForKF(0),
    mpCamera(F.mpCamera), mpCamera2(F.mpCamera2),
    mvLeftToRightMatch(F.mvLeftToRightMatch),mvRightToLeftMatch(F.mvRightToLeftMatch),mTlr(F.mTlr.clone()),
    mvKeysRight(F.mvKeysRight), NLeft(F.Nleft), NRight(F.Nright), mTrl(F.mTrl), mnNumberOfOpt(0)
{
	// 复制当前帧的左右图，深度拷贝
    imgLeft = F.imgLeft.clone();
    imgRight = F.imgRight.clone();
	
    // 将下一帧的帧号赋值给mnId，然后自增1
    mnId=nNextId++;
	
    // Grid 赋值
    mGrid.resize(mnGridCols);
    if(F.Nleft != -1)  mGridRight.resize(mnGridCols);
    for(int i=0; i<mnGridCols;i++)
    {
        mGrid[i].resize(mnGridRows);
        if(F.Nleft != -1) mGridRight[i].resize(mnGridRows);
        for(int j=0; j<mnGridRows; j++){
            mGrid[i][j] = F.mGrid[i][j];
            if(F.Nleft != -1){
                mGridRight[i][j] = F.mGridRight[i][j];
            }
        }
    }


	// IMU linear velocity
    if(F.mVw.empty())
        Vw = cv::Mat::zeros(3,1,CV_32F);
    else
        Vw = F.mVw.clone();

    mImuBias = F.mImuBias;
    SetPose(F.mTcw); //相机到世界坐标系的关系

    mnOriginMapId = pMap->GetId();	// 地图Id
	
        
    this->Tlr_ = cv::Matx44f(mTlr.at<float>(0,0),mTlr.at<float>(0,1),mTlr.at<float>(0,2),mTlr.at<float>(0,3),
                             mTlr.at<float>(1,0),mTlr.at<float>(1,1),mTlr.at<float>(1,2),mTlr.at<float>(1,3),
                             mTlr.at<float>(2,0),mTlr.at<float>(2,1),mTlr.at<float>(2,2),mTlr.at<float>(2,3),
                             mTlr.at<float>(3,0),mTlr.at<float>(3,1),mTlr.at<float>(3,2),mTlr.at<float>(3,3));

}

其他函数

SetPose 设置当前帧的位姿

void KeyFrame::SetPose(const cv::Mat &Tcw_)
{
    unique_lock<mutex> lock(mMutexPose);
    Tcw_.copyTo(Tcw);
    cv::Mat Rcw = Tcw.rowRange(0,3).colRange(0,3);
    cv::Mat tcw = Tcw.rowRange(0,3).col(3);
    cv::Mat Rwc = Rcw.t();
    Ow = -Rwc*tcw;

    Twc = cv::Mat::eye(4,4,Tcw.type());
    Rwc.copyTo(Twc.rowRange(0,3).colRange(0,3));
    Ow.copyTo(Twc.rowRange(0,3).col(3));
    // center为相机坐标系（左目）下，立体相机中心的坐标
    // 立体相机中心点坐标与左目相机坐标之间只是在x轴上相差mHalfBaseline,
    // 因此可以看出，立体相机中两个摄像头的连线为x轴，正方向为左目相机指向右目相机
    cv::Mat center = (cv::Mat_<float>(4,1) << mHalfBaseline, 0 , 0, 1);
    // 世界坐标系下，左目相机中心到立体相机中心的向量，方向由左目相机指向立体相机中心
    Cw = Twc*center;
}

AddConnection 为关键帧之间添加链接

添加当前帧的链接权重，mConnectedKeyFrameWeights
按照权重对连接的关键帧和权重进行排序，存放于：
1. mvpOrderedConnectedKeyFrames 关键帧
2. mvOrderedWeights 权重

// Param: 关键帧 + 权重(该关键帧与pKF共同观测到的3d点数量)
void KeyFrame::AddConnection(KeyFrame *pKF, const int &weight)
{
    {
        unique_lock<mutex> lock(mMutexConnections);
        // mConnectedKeyFrameWeights中没有pKF，之前没有连接，则进行连接，若已链接，则赋值新权重
        if(!mConnectedKeyFrameWeights.count(pKF)) 
            mConnectedKeyFrameWeights[pKF]=weight;
        else if(mConnectedKeyFrameWeights[pKF]!=weight)
            mConnectedKeyFrameWeights[pKF]=weight;
        else
            return;
    }
	// 按照权重对连接的关键帧进行排序
    UpdateBestCovisibles();
}

void KeyFrame::UpdateBestCovisibles()
{
    unique_lock<mutex> lock(mMutexConnections);

    vector<pair<int,KeyFrame*> > vPairs;
    vPairs.reserve(mConnectedKeyFrameWeights.size());
    // 取出所有连接的关键帧，vPairs变量将共视的3D点个数放在前面利于排序
    for(map<KeyFrame*,int>::iterator mit=mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
       vPairs.push_back(make_pair(mit->second,mit->first));

    // 按照权重进行排序
    sort(vPairs.begin(),vPairs.end());
    list<KeyFrame*> lKFs; // keyframe
    list<int> lWs; // weight
    for(size_t i=0, iend=vPairs.size(); i<iend;i++)
    {	// 排序从小到达，但向前插入
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }

    // 权重从大到小
    mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
    mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());
}

取关联关键帧

GetConnectedKeyFrames 取连接的关键帧
- mConnectedKeyFrameWeights <关键帧，权重>
- 将所有链接的关键帧都取了，此时关键帧未排序
GetVectorCovisibleKeyFrames 取按权重排序的关键帧
GetBestCovisibilityKeyFrames(N) 取按权重排序的N个关键帧，不足全取
GetCovisiblesByWeight(w) 取大于权重w的关联关键帧

GetWeight 取当前帧的权重

若当前帧与此关键帧关联，则返回权重，否则返回0

int KeyFrame::GetWeight(KeyFrame *pKF)
{
    unique_lock<mutex> lock(mMutexConnections);
    if(mConnectedKeyFrameWeights.count(pKF))
        return mConnectedKeyFrameWeights[pKF];
    else
        return 0;
}

GetNumberMPs 得到地图点个数

遍历当前帧地图点，number++

int KeyFrame::GetNumberMPs()
{
    unique_lock<mutex> lock(mMutexFeatures);
    int numberMPs = 0;
    for(size_t i=0, iend=mvpMapPoints.size(); i<iend; i++)
    {
        if(!mvpMapPoints[i])
            continue;
        numberMPs++;
    }
    return numberMPs;
}

AddMapPoint 添加地图点

Param:
- pMP: MapPoint
- idx: MapPoint在KeyFrame中的索引

void KeyFrame::AddMapPoint(MapPoint *pMP, const size_t &idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    mvpMapPoints[idx]=pMP;
}

EraseMapPointMatch 删除地图点的匹配

Param idx: MapPoint在KeyFrame中的索引
基于索引将地图点容器中数据赋值null

void KeyFrame::EraseMapPointMatch(const int &idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    mvpMapPoints[idx]=static_cast<MapPoint*>(NULL);
}

EraseMapPointMatch 删除地图点匹配

Param: 地图点

void KeyFrame::EraseMapPointMatch(MapPoint* pMP)
{	// 取出地图点Index
    tuple<size_t,size_t> indexes = pMP->GetIndexInKeyFrame(this);
    size_t leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
    // Index 不为空，则基于`index`将其数据赋值为0 
    if(leftIndex != -1)
        mvpMapPoints[leftIndex]=static_cast<MapPoint*>(NULL);
    if(rightIndex != -1)
        mvpMapPoints[rightIndex]=static_cast<MapPoint*>(NULL);
}

ReplaceMapPointMatch 替换地图点的匹配


void KeyFrame::ReplaceMapPointMatch(const int &idx, MapPoint* pMP)
{
    mvpMapPoints[idx]=pMP;
}

GetMapPoints 取出地图点

将好的地图点取出，返回set容器

set<MapPoint*> KeyFrame::GetMapPoints()
{
    unique_lock<mutex> lock(mMutexFeatures);
    set<MapPoint*> s;
    for(size_t i=0, iend=mvpMapPoints.size(); i<iend; i++)
    {
        if(!mvpMapPoints[i])
            continue;
        MapPoint* pMP = mvpMapPoints[i];
        if(!pMP->isBad())
            s.insert(pMP);
    }
    return s;
}

TrackedMapPoints 优质地图点的个数

关键帧中，大于等于minObs的MapPoints的数量
minObs就是一个阈值，大于minObs就表示该MapPoint是一个高质量的MapPoint
一个高质量的MapPoint会被多个KeyFrame观测到，minObs：最小观测

int KeyFrame::TrackedMapPoints(const int &minObs)
{
    unique_lock<mutex> lock(mMutexFeatures);

    int nPoints=0;
    const bool bCheckObs = minObs>0;
    for(int i=0; i<N; i++)
    {
        MapPoint* pMP = mvpMapPoints[i];
        if(pMP)
        {
            if(!pMP->isBad())
            {
                if(bCheckObs)
                {
                    // 该MapPoint是一个高质量的MapPoint
                    if(mvpMapPoints[i]->Observations()>=minObs)
                        nPoints++;
                }
                else
                    nPoints++;
            }
        }
    }
    return nPoints;
}

UpdateConnections 更新地图链接

在没有执行这个函数前，关键帧只和MapPoints之间有连接关系，这个函数可以更新关键帧之间的连接关系
该函数主要包含以下三部分内容：
- 首先获得该关键帧的所有MapPoint点，然后遍历观测到这些3d点的其它所有关键帧，对每一个找到的关键帧，先存储到相应的容器中。
- 计算所有共视帧与该帧的连接权重，权重即为共视的3d点的数量，对这些连接按照权重从大到小进行排序。当该权重必须大于一个阈值，便在两帧之间建立边，如果没有超过该阈值的权重，那么就只保留权重最大的边（与其它关键帧的共视程度比较高）
- 更新covisibility graph，即把计算的边用来给图赋值，然后设置spanning tree中该帧的父节点，即共视程度最高的那一帧

void KeyFrame::UpdateConnections(bool upParent)
{
    map<KeyFrame*,int> KFcounter;

    vector<MapPoint*> vpMP;

    {	// 获得该关键帧的所有3D点
        unique_lock<mutex> lockMPs(mMutexFeatures);
        vpMP = mvpMapPoints;
    }

    //For all map points in keyframe check in which other keyframes are they seen
    //Increase counter for those keyframes
    // 即统计每一个关键帧都有多少关键帧与它存在共视关系，统计结果放在KFcounter
    for(vector<MapPoint*>::iterator vit=vpMP.begin(), vend=vpMP.end(); vit!=vend; vit++)
    {
        MapPoint* pMP = *vit;
		
		// 该地图点非空，且不是坏点
        if(!pMP)      continue;
        if(pMP->isBad())     continue;
		// 对于每一个MapPoint点，observations记录了可以观测到该MapPoint的所有关键帧
        map<KeyFrame*,tuple<int,int>> observations = pMP->GetObservations();
		// 遍历地图点，记录共视关键帧的地图点个数
        for(map<KeyFrame*,tuple<int,int>>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
        {
        	// 除去自身，自己与自己不算共视
            if(mit->first->mnId==mnId || mit->first->isBad() || mit->first->GetMap() != mpMap)
                continue;
            KFcounter[mit->first]++;

        }
    }

    // This should not happen
    if(KFcounter.empty())
        return;

    //If the counter is greater than threshold add connection
    //In case no keyframe counter is over threshold add the one with maximum counter
    // 通过3D点间接统计可以观测到这些3D点的所有关键帧之间的共视程度
    int nmax=0;
    KeyFrame* pKFmax=NULL;
    int th = 15;
    
    // vPairs记录与其它关键帧共视帧数大于th的关键帧
    // pair<int,KeyFrame*>将关键帧的权重写在前面，关键帧写在后面方便后面排序
    vector<pair<int,KeyFrame*> > vPairs;
    vPairs.reserve(KFcounter.size());
    if(!upParent)
        cout << "UPDATE_CONN: current KF " << mnId << endl;
    for(map<KeyFrame*,int>::iterator mit=KFcounter.begin(), mend=KFcounter.end(); mit!=mend; mit++)
    {
        if(!upParent)
            cout << "  UPDATE_CONN: KF " << mit->first->mnId << " ; num matches: " << mit->second << endl;
        // 找到对应权重最大的关键帧（共视程度最高的关键帧）
        if(mit->second>nmax)
        {
            nmax=mit->second;
            pKFmax=mit->first;
        }
        // // 对应权重需要大于阈值，对这些关键帧建立连接
        if(mit->second>=th)
        {
            vPairs.push_back(make_pair(mit->second,mit->first));
            (mit->first)->AddConnection(this,mit->second);
        }
    }

	// 如果没有超过阈值的权重，则对权重最大的关键帧建立连接
    if(vPairs.empty())
    {
        vPairs.push_back(make_pair(nmax,pKFmax));
        pKFmax->AddConnection(this,nmax);
    }
	
	// vPairs里存的都是相互共视程度比较高的关键帧和共视权重，由大到小
    sort(vPairs.begin(),vPairs.end());
    list<KeyFrame*> lKFs;
    list<int> lWs;
    for(size_t i=0; i<vPairs.size();i++)
    {
        lKFs.push_front(vPairs[i].second);
        lWs.push_front(vPairs[i].first);
    }

	// 更新图的连接(权重)，更新生成树的连接
    {
        unique_lock<mutex> lockCon(mMutexConnections);

        mConnectedKeyFrameWeights = KFcounter;
        mvpOrderedConnectedKeyFrames = vector<KeyFrame*>(lKFs.begin(),lKFs.end());
        mvOrderedWeights = vector<int>(lWs.begin(), lWs.end());

        if(mbFirstConnection && mnId!=mpMap->GetInitKFid())
        {	// 初始化该关键帧的父关键帧为共视程度最高的那个关键帧
            mpParent = mvpOrderedConnectedKeyFrames.front();
            // 建立双向连接关系
            mpParent->AddChild(this);
            mbFirstConnection = false;
        }

    }
}

SetBadFlag 设置坏标记

需要删除的是该关键帧和其他所有帧、地图点之间的连接关系，但是删除会带来一个问题，就是它可能是其他节点的父节点，在删除之前需要告诉自己所有的子节点，换个爸爸，这个函数里绝大部分代码都是在完成这一步。

遍历所有和当前关键帧共视的关键帧，删除他们与当前关键帧的联系
遍历每一个当前关键帧的地图点，删除每一个地图点和当前关键帧的联系
清空和当前关键帧的共视关键帧集合和带顺序的关键帧集合
共视图更新完毕后，还需要更新生成树。删除当前关键帧之前，需要处理好父亲和儿子关键帧关系，不然会造成整个关键帧维护的图断裂，或者混乱，不能够为后端提供较好的初值。举例：
1. B–>A（B的父节点是A） C–>B（C的父节点是B） D–C（D与C相连） E–C（E与C相连） F–C（F与C相连） D–>A（D的父节点是A） E–>A（E的父节点是A）
2. 现在B挂了，于是C在与自己相连的D、E、F节点中找到父节点指向A的D
3. 此过程就是为了找到可以替换B的那个节点。
4. 上面例子中，B为当前要设置为SetBadFlag的关键帧
5. A为spcit，也即sParentCandidates
6. C为pKF,pC，也即mspChildrens中的一个
7. D、E、F为vpConnected中的变量，由于C与D间的权重比 C与E间的权重大，因此D为pP
遍历所有把当前关键帧当成父关键帧的子关键帧。重新为他们找父关键帧。设置一个候选父关键帧集合
对于每一个子关键帧，找到与它共视的关键帧集合，遍历它，看看是否有候选父帧集合里的帧，如果有，就把这个帧当做新的父帧
如果有子关键帧没有找到新的父帧，那么直接把当前帧的父帧（爷）当成它的父帧

void KeyFrame::SetBadFlag()
{
	/// 0： 若初始ID 或不应该删除时，直接return
    {
        unique_lock<mutex> lock(mMutexConnections);
        if(mnId==mpMap->GetInitKFid())
        {
            return;
        }
        // mbNotErase表示不应该擦除该KeyFrame，于是把mbToBeErased置为true，表示已经擦除了，其实没有擦除
        else if(mbNotErase)
        {
            mbToBeErased = true;
            return;
        }
    }
	/// 1： 遍历所有和当前关键帧共视的关键帧，删除他们与当前关键帧的联系
    for(map<KeyFrame*,int>::iterator mit = mConnectedKeyFrameWeights.begin(), mend=mConnectedKeyFrameWeights.end(); mit!=mend; mit++)
    {
        mit->first->EraseConnection(this);
    }

	/// 2： 遍历每一个当前关键帧的地图点，删除每一个地图点和当前关键帧的联系
    for(size_t i=0; i<mvpMapPoints.size(); i++)
    {
        if(mvpMapPoints[i])
        {
            mvpMapPoints[i]->EraseObservation(this);
        }
    }

    {
        unique_lock<mutex> lock(mMutexConnections);
        unique_lock<mutex> lock1(mMutexFeatures);
	/// 3： 清空和当前关键帧的共视关键帧集合和带顺序的关键帧集合
        mConnectedKeyFrameWeights.clear();
        mvpOrderedConnectedKeyFrames.clear();

        // Update Spanning Tree 更新生成树
        set<KeyFrame*> sParentCandidates;
        if(mpParent)	// 如果该节点有父节点时，记录候选值父节点
            sParentCandidates.insert(mpParent);

        // Assign at each iteration one children with a parent (the pair with highest covisibility weight)
        // Include that children as new parent candidate for the rest
        // 如果这个关键帧有子关键帧，则给子节点重新找父节点，若找到新的父节点，则删除其子节点
        while(!mspChildrens.empty())
        {
            bool bContinue = false;

            int max = -1;
            KeyFrame* pC;
            KeyFrame* pP;
			
			// 遍历每一个子关键帧，让它们更新它们指向的父关键帧
            for(set<KeyFrame*>::iterator sit=mspChildrens.begin(), send=mspChildrens.end(); sit!=send; sit++)
            {
                KeyFrame* pKF = *sit;
                if(pKF->isBad())
                    continue;

                // Check if a parent candidate is connected to the keyframe
                // 子关键帧遍历每一个与它相连的关键帧（共视关键帧），与子节点同层
                vector<KeyFrame*> vpConnected = pKF->GetVectorCovisibleKeyFrames();
                for(size_t i=0, iend=vpConnected.size(); i<iend; i++)
                {	
                    for(set<KeyFrame*>::iterator spcit=sParentCandidates.begin(), spcend=sParentCandidates.end(); spcit!=spcend; spcit++)
                    {	// 子节点指向其父节点，且取出权重最大的那一个
                        if(vpConnected[i]->mnId == (*spcit)->mnId)
                        {
                            int w = pKF->GetWeight(vpConnected[i]);
                            if(w>max)
                            {
                                pC = pKF;
                                pP = vpConnected[i];
                                max = w;
                                bContinue = true;
                            }
                        }
                    }
                }
            }

            if(bContinue)
            {	// 因为父节点死了，并且子节点找到了新的父节点，子节点更新自己的父节点
                pC->ChangeParent(pP);
                // 因为子节点找到了新的父节点并更新了父节点，那么该子节点升级，作为其它子节点的备选父节点
                sParentCandidates.insert(pC);
                // 该子节点处理完毕
                mspChildrens.erase(pC);
            }
            else
                break;
        }

        // If a children has no covisibility links with any parent candidate, assign to the original parent of this KF
        // 如果还有子节点没有找到新的父节点，直接把父节点的父节点作为自己的父节点
        if(!mspChildrens.empty())
        {	
            for(set<KeyFrame*>::iterator sit=mspChildrens.begin(); sit!=mspChildrens.end(); sit++)
            {
                (*sit)->ChangeParent(mpParent);
            }
        }

        if(mpParent){
            mpParent->EraseChild(this);
            mTcp = Tcw*mpParent->GetPoseInverse();
        }
        mbBad = true;
    }


    mpMap->EraseKeyFrame(this);
    mpKeyFrameDB->erase(this);
}

GetFeaturesInArea 得到区域内的特征点

首先确定区域范围，然后在找出区域范围内的特征，最后根据欧式距离得出最终符合区域的特征点

vector<size_t> KeyFrame::GetFeaturesInArea(const float &x, const float &y, const float &r) const
{
    vector<size_t> vIndices;
    vIndices.reserve(N);

    // floor向下取整，mfGridElementWidthInv为每个像素占多少个格子
    const int nMinCellX = max(0,(int)floor((x-mnMinX-r)*mfGridElementWidthInv));
    if(nMinCellX>=mnGridCols)
        return vIndices;

    // ceil向上取整
    const int nMaxCellX = min((int)mnGridCols-1,(int)ceil((x-mnMinX+r)*mfGridElementWidthInv));
    if(nMaxCellX<0)
        return vIndices;

    const int nMinCellY = max(0,(int)floor((y-mnMinY-r)*mfGridElementHeightInv));
    if(nMinCellY>=mnGridRows)
        return vIndices;

    const int nMaxCellY = min((int)mnGridRows-1,(int)ceil((y-mnMinY+r)*mfGridElementHeightInv));
    if(nMaxCellY<0)
        return vIndices;

    for(int ix = nMinCellX; ix<=nMaxCellX; ix++)
    {
        for(int iy = nMinCellY; iy<=nMaxCellY; iy++)
        {
            const vector<size_t> vCell = mGrid[ix][iy];
            for(size_t j=0, jend=vCell.size(); j<jend; j++)
            {
                const cv::KeyPoint &kpUn = mvKeysUn[vCell[j]];
                const float distx = kpUn.pt.x-x;
                const float disty = kpUn.pt.y-y;
				// 先矩形，在圆形确定
                if(fabs(distx)<r && fabs(disty)<r)
                    vIndices.push_back(vCell[j]);
            }
        }
    }

    return vIndices;
}

ComputeSceneMedianDepth 计算场景的中位数深度

评估当前关键帧场景深度，q=2表示中值

float KeyFrame::ComputeSceneMedianDepth(const int q)
{
    vector<MapPoint*> vpMapPoints;
    cv::Mat Tcw_;
    {
        unique_lock<mutex> lock(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPose);
        vpMapPoints = mvpMapPoints;
        Tcw_ = Tcw.clone();
    }

    vector<float> vDepths;
    vDepths.reserve(N);
    cv::Mat Rcw2 = Tcw_.row(2).colRange(0,3);
    Rcw2 = Rcw2.t();
    float zcw = Tcw_.at<float>(2,3);
    // 
    for(int i=0; i<N; i++)
    {
        if(mvpMapPoints[i])
        {
            MapPoint* pMP = mvpMapPoints[i];
            cv::Mat x3Dw = pMP->GetWorldPos();
            float z = Rcw2.dot(x3Dw)+zcw;
            vDepths.push_back(z);
        }
    }

    sort(vDepths.begin(),vDepths.end());

    return vDepths[(vDepths.size()-1)/q];
}

MapPoint

说明

MapPoint是地图中的特征点，它自身的参数是三维坐标和描述子。
地图点存储：
- 在世界坐标系下的3D位姿 mWorldPos
- 地图点的观测方向mNormalVector，为归一化后的平均观测方向
  (指连接该地图点和其他对应观测关键帧光心的单位方向向量的均值)
- 一个具有代表性ORB描述子mDescriptor
  - 如果MapPoint与很多帧图像特征点对应（由keyframe来构造时），那么距离其它描述子的平均距离最小的描述子是最佳描述子
  - MapPoint只与一帧的图像特征点对应（由frame来构造时），那么这个特征点的描述子就是该3D点的描述子
- 根据ORB特征点的尺度不变性得到观测该地图点的最大和最小距离
功能：
- 维护关键帧之间的共视关系
- 通过计算描述向量之间的距离，在多个关键帧的特征点中找最匹配的特征点
- 在闭环完成修正后，需要根据修正的主帧位姿修正特征点
- 对于非关键帧产生MapPoint，为Tracking临时使用
地图点构造：可以通过关键帧来构造，也可以通过普通帧构造，但是最终必须是和关键帧对应的。
通过普通帧构造的地图点只是临时被Tracking用来追踪用的。
地图点和关键帧之间的观测关系，参考关键帧是哪一帧，该地图点被哪些关键帧观测到，对应的哪个（idx）特征点？
通过两个成员维护：
- std::map<KeyFrame*,size_t> mObservations;
- 观测到该地图点的相机数 int obs;
添加地图点观测：能够观测到同一个地图点的关键帧之间存在共视关系。
删除地图点观测：从当前地图点的mObservation和nObs成员中删掉对应关键帧观测关系，若该关键帧恰好是参考帧，则需要重新指定。
- 当观测相机数小于等于2时，该地图点需要剔除。删掉观测关系，和删掉地图点（以及替换地图点），需要区分开。
  void MapPoint::EraseObservation(KeyFrame* pKF);
剔除MapPoint，不仅需要删掉地图点中维护的关键帧观测关系，还需要删掉对应关键中对应的地图点，以及Map中该地图点的内存：
几个和地图点相关的重要函数：
- 计算平均观测方向和尺度不变性距离 UpdateNormalAndDepth
  - 地图点到所有观测到的关键帧相机中心向量，归一化后相加
  - 深度范围：地图点到参考帧（只有一帧）相机中心距离，乘上参考帧中描述子获取时金字塔放大尺度，得到最大距离mfMaxDistance；最大距离除以整个金字塔最高层的放大尺度得到最小距离mfMinDistance。
  - 通常来说，距离较近的地图点，将在金字塔层数较高的地方提取出，距离较远的地图点，在金字塔层数较低的地方提取出（金字塔层数越低，分辨率越高，才能识别出远点）。
  - 因此通过地图点的信息（主要是对应描述子），我们可以获得该地图点对应的金字塔层级：
    pRefKF->mvKeysUn[observations[pRefKF]].octave;
- 计算最具代表性的描述子 ComputeDistinctiveDescriptors
  - 从mObservations中获取观察到当前地图点的关键帧及对应描述子，描述子放入vDescriptor描述子向量组成的向量中。在这些描述子中，选择距离（类似hamming距离）其他描述子最近的（中值距离最小）作为地图点的描述子mDescriptor。
- 根据地图点距离关键帧/普通帧相机光心的距离来预测地图点对应的观测应该在哪一层的金字塔 PredictScale
  - 注意金字塔ScaleFactor和距离的关系：当前特征点对应ScaleFactor为1.2的意思是：图片分辨率下降1.2倍后，可以提取出该特征点。
    （分辨率更高时，肯定也可以提取出，这里取金字塔中能够提取出该特征点最高层级作为该特征点的层级）

类成员变量

public:
    long unsigned int mnId;	///< Global ID for MapPoint
    static long unsigned int nNextId; /// static 方便计算下一个
    long int mnFirstKFid;///< 创建该MapPoint的关键帧ID
    long int mnFirstFrame;
    int nObs;

    // Variables used by the tracking 跟踪使用的变量
    float mTrackProjX;
    float mTrackProjY;
    float mTrackDepth;
    float mTrackDepthR;
    float mTrackProjXR;
    float mTrackProjYR;
    
	///< SearchByProjection中决定是否对该点进行投影的变量
	// mbTrackInView==false的点有几种：
    //    a 已经和当前帧经过匹配（TrackReferenceKeyFrame，TrackWithMotionModel）但在优化过程中认为是外点
    //    b 已经和当前帧经过匹配且为内点，这类点也不需要再进行投影
    //    c 不在当前相机视野中的点（即未通过isInFrustum判断）
    bool mbTrackInView, mbTrackInViewR;
    int mnTrackScaleLevel, mnTrackScaleLevelR;
    float mTrackViewCos, mTrackViewCosR;
    // UpdateLocalPoints中防止将MapPoints重复添加至mvpLocalMapPoints的标记
    long unsigned int mnTrackReferenceForFrame;
    
    ///< SearchLocalPoints中决定是否进行isInFrustum判断的变量
    // mnLastFrameSeen==mCurrentFrame.mnId的点有几种：
    //   a 已经和当前帧经过匹配（TrackReferenceKeyFrame，TrackWithMotionModel）但在优化过程中认为是外点
    //   b 已经和当前帧经过匹配且为内点，这类点也不需要再进行投影
    long unsigned int mnLastFrameSeen;

    // Variables used by local mapping
    long unsigned int mnBALocalForKF;
    long unsigned int mnFuseCandidateForKF;

    // Variables used by loop closing
    long unsigned int mnLoopPointForKF;
    long unsigned int mnCorrectedByKF;
    long unsigned int mnCorrectedReference;    
    cv::Mat mPosGBA;
    long unsigned int mnBAGlobalForKF;
    long unsigned int mnBALocalForMerge;

    // Variable used by merging
    cv::Mat mPosMerge;
    cv::Mat mNormalVectorMerge;

    // Fopr inverse depth optimization
    double mInvDepth;
    double mInitU;
    double mInitV;
    KeyFrame* mpHostKF;

    static std::mutex mGlobalMutex;

    unsigned int mnOriginMapId;

protected:    

     // Position in absolute coordinates
     cv::Mat mWorldPos;　// MapPoint在世界坐标系下的坐标
     cv::Matx31f mWorldPosx;

     // Keyframes observing the point and associated index in keyframe
     //　观测到该MapPoint的KF和该MapPoint在KF中的索引
     std::map<KeyFrame*,std::tuple<int,int> > mObservations;

     // Mean viewing direction　该MapPoint平均观测方向
     cv::Mat mNormalVector;
     cv::Matx31f mNormalVectorx;

     // Best descriptor to fast matching　
     ///<  每个3D点也有一个descriptor
     //     如果MapPoint与很多帧图像特征点对应（由keyframe来构造时），那么距离其它描述子的平均距离最小的描述子是最佳描述子
     //     MapPoint只与一帧的图像特征点对应（由frame来构造时），那么这个特征点的描述子就是该3D点的描述子
     cv::Mat mDescriptor;

     // Reference KeyFrame
     KeyFrame* mpRefKF;

     // Tracking counters
     int mnVisible;
     int mnFound;

     // Bad flag (we do not currently erase MapPoint from memory)
     bool mbBad;
     MapPoint* mpReplaced;

     // Scale invariance distances
     float mfMinDistance;
     float mfMaxDistance;

     Map* mpMap;

     std::mutex mMutexPos;
     std::mutex mMutexFeatures;
     std::mutex mMutexMap;

类成员函数

AddObservation 添加观测

记录哪些KeyFrame的那个特征点能观测到该MapPoint，并增加观测的相机数目nObs，单目+1，双目或者grbd+2
这个函数是建立关键帧共视关系的核心函数，能共同观测到某些MapPoints的关键帧是共视关键帧
Param:
- pKF KeyFrame
- idx MapPoint在KeyFrame中的索引

void MapPoint::AddObservation(KeyFrame* pKF, int idx)
{
    unique_lock<mutex> lock(mMutexFeatures);
    tuple<int,int> indexes;

    if(mObservations.count(pKF)){
        indexes = mObservations[pKF];
    }
    else{
        indexes = tuple<int,int>(-1,-1);
    }

    if(pKF -> NLeft != -1 && idx >= pKF -> NLeft){
        get<1>(indexes) = idx;
    }
    else{
        get<0>(indexes) = idx;
    }
    
	 // 记录下能观测到该MapPoint的KF和该MapPoint在KF中的索引
    mObservations[pKF]=indexes;
    
    if(!pKF->mpCamera2 && pKF->mvuRight[idx]>=0)
        nObs+=2;
    else
        nObs++;
}

EraseObservation 删除观测

void MapPoint::EraseObservation(KeyFrame* pKF)
{
    bool bBad=false;
    {
        unique_lock<mutex> lock(mMutexFeatures);
        if(mObservations.count(pKF))
        {
            tuple<int,int> indexes = mObservations[pKF];
            int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);

            if(leftIndex != -1){
                if(!pKF->mpCamera2 && pKF->mvuRight[leftIndex]>=0)
                    nObs-=2;
                else
                    nObs--;
            }
            if(rightIndex != -1){
                nObs--;
            }

            mObservations.erase(pKF);
		
			// 如果该keyFrame是参考帧，该Frame被删除后重新指定RefFrame
            if(mpRefKF==pKF)
                mpRefKF=mObservations.begin()->first;

            // If only 2 observations or less, discard point
            // 当观测到该点的相机数目少于2时，丢弃该点
            if(nObs<=2)
                bBad=true;
        }
    }

    if(bBad)
        SetBadFlag();
}

SetBadFlag 该地图点已被删除

void MapPoint::SetBadFlag()
{
    map<KeyFrame*, tuple<int,int>> obs;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        mbBad=true;
		 // 把mObservations转存到obs，obs和mObservations里存的是指针，赋值过程为浅拷贝
		 // 把mObservations指向的内存释放，obs作为局部变量之后自动删除
        obs = mObservations;
        mObservations.clear();
    }
    for(map<KeyFrame*, tuple<int,int>>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;
        int leftIndex = get<0>(mit -> second), rightIndex = get<1>(mit -> second);
        if(leftIndex != -1){
        	// 告诉可以观测到该MapPoint的KeyFrame，该MapPoint被删了
            pKF->EraseMapPointMatch(leftIndex);
        }
        if(rightIndex != -1){
            pKF->EraseMapPointMatch(rightIndex);
        }
    }
	// 擦除该MapPoint申请的内存
    mpMap->EraseMapPoint(this);
}

Replace 更新关键帧与地图点

在形成闭环的时候，会更新KeyFrame与MapPoint之间的关系

void MapPoint::Replace(MapPoint* pMP)
{
    if(pMP->mnId==this->mnId)
        return;

    int nvisible, nfound;
    map<KeyFrame*,tuple<int,int>> obs;
    {	// 这一段和SetBadFlag函数相同
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        obs=mObservations;
        mObservations.clear();
        mbBad=true;
        nvisible = mnVisible;
        nfound = mnFound;
        mpReplaced = pMP;
    }

	// 所有能观测到该MapPoint的keyframe都要替换
    for(map<KeyFrame*,tuple<int,int>>::iterator mit=obs.begin(), mend=obs.end(); mit!=mend; mit++)
    {
        // Replace measurement in keyframe
        KeyFrame* pKF = mit->first;

        tuple<int,int> indexes = mit -> second;
        int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);

        if(!pMP->IsInKeyFrame(pKF))
        {
            if(leftIndex != -1){
                pKF->ReplaceMapPointMatch(leftIndex, pMP); // 让KeyFrame用pMP替换掉原来的MapPoint
                pMP->AddObservation(pKF,leftIndex);// 让MapPoint替换掉对应的KeyFrame
            }
            if(rightIndex != -1){
                pKF->ReplaceMapPointMatch(rightIndex, pMP);
                pMP->AddObservation(pKF,rightIndex);
            }
        }
        else
        {
            if(leftIndex != -1){
            	// 产生冲突，即pKF中有两个特征点a,b（这两个特征点的描述子是近似相同的），这两个特征点对应两个MapPoint为this
            	// 然而在fuse的过程中pMP的观测更多，需要替换this，因此保留b与pMP的联系，去掉a与this的联系
                pKF->EraseMapPointMatch(leftIndex);
            }
            if(rightIndex != -1){
                pKF->EraseMapPointMatch(rightIndex);
            }
        }
    }
    pMP->IncreaseFound(nfound);
    pMP->IncreaseVisible(nvisible);
    pMP->ComputeDistinctiveDescriptors();

    mpMap->EraseMapPoint(this);
}

ComputeDistinctiveDescriptors 计算具有代表的描述子

由于一个MapPoint会被许多相机观测到，因此在插入关键帧后，需要判断是否更新当前点的最适合的描述子。
先获得当前点的所有描述子，然后计算描述子之间的两两距离，最好的描述子与其他描述子应该具有最小的距离中值。

void MapPoint::ComputeDistinctiveDescriptors()
{
    // Retrieve all observed descriptors
    vector<cv::Mat> vDescriptors;

    map<KeyFrame*,tuple<int,int>> observations;

    {
        unique_lock<mutex> lock1(mMutexFeatures);
        if(mbBad)
            return;
        observations=mObservations;
    }

    if(observations.empty())
        return;

    vDescriptors.reserve(observations.size());
    
	// 遍历观测到3d点的所有关键帧，获得orb描述子，并插入到vDescriptors中
    for(map<KeyFrame*,tuple<int,int>>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;

        if(!pKF->isBad()){
            tuple<int,int> indexes = mit -> second;
            int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);

            if(leftIndex != -1){
                vDescriptors.push_back(pKF->mDescriptors.row(leftIndex));
            }
            if(rightIndex != -1){
                vDescriptors.push_back(pKF->mDescriptors.row(rightIndex));
            }
        }
    }

    if(vDescriptors.empty())
        return;

    // Compute distances between them 获得这些描述子两两之间的距离
    const size_t N = vDescriptors.size();

    float Distances[N][N];
    for(size_t i=0;i<N;i++)
    {
        Distances[i][i]=0;
        for(size_t j=i+1;j<N;j++)
        {
            int distij = ORBmatcher::DescriptorDistance(vDescriptors[i],vDescriptors[j]);
            Distances[i][j]=distij;
            Distances[j][i]=distij;
        }
    }

    // Take the descriptor with least median distance to the rest
    // 取与其余部分距离中值最小的描述符
    int BestMedian = INT_MAX;
    int BestIdx = 0;
    for(size_t i=0;i<N;i++)
    {
        vector<int> vDists(Distances[i],Distances[i]+N);
        sort(vDists.begin(),vDists.end());
        int median = vDists[0.5*(N-1)];
		// 寻找最小的中值
        if(median<BestMedian)
        {
            BestMedian = median;
            BestIdx = i;
        }
    }

    {
    	// 最好的描述子，该描述子相对于其他描述子有最小的距离中值
        // 简化来讲，中值代表了这个描述子到其它描述子的平均距离
        // 最好的描述子就是和其它描述子的平均距离最小
        unique_lock<mutex> lock(mMutexFeatures);
        mDescriptor = vDescriptors[BestIdx].clone();
    }
}

UpdateNormalAndDepth 更新平均观测方向以及观测距离范围

由于一个MapPoint会被许多相机观测到，因此在插入关键帧后，需要更新相应变量

void MapPoint::UpdateNormalAndDepth()
{
    map<KeyFrame*,tuple<int,int>> observations;
    KeyFrame* pRefKF;
    cv::Mat Pos;
    {
        unique_lock<mutex> lock1(mMutexFeatures);
        unique_lock<mutex> lock2(mMutexPos);
        if(mbBad)
            return;
        observations=mObservations;// 获得观测到该3d点的所有关键帧
        pRefKF=mpRefKF; // 观测到该点的参考关键帧
        Pos = mWorldPos.clone();// 3d点在世界坐标系中的位置
    }

    if(observations.empty())
        return;

    cv::Mat normal = cv::Mat::zeros(3,1,CV_32F);
    int n=0;
    for(map<KeyFrame*,tuple<int,int>>::iterator mit=observations.begin(), mend=observations.end(); mit!=mend; mit++)
    {
        KeyFrame* pKF = mit->first;

        tuple<int,int> indexes = mit -> second;
        int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);

        if(leftIndex != -1){
            cv::Mat Owi = pKF->GetCameraCenter();
            cv::Mat normali = mWorldPos - Owi;
            //  对所有关键帧对该点的观测方向归一化为单位向量进行求和
            normal = normal + normali/cv::norm(normali);
            n++;
        }
        if(rightIndex != -1){
            cv::Mat Owi = pKF->GetRightCameraCenter();
            cv::Mat normali = mWorldPos - Owi;
            normal = normal + normali/cv::norm(normali);
            n++;
        }
    }
	// 参考关键帧相机指向3D点的向量（在世界坐标系下的表示）
    cv::Mat PC = Pos - pRefKF->GetCameraCenter();
    // 该点到参考关键帧相机的距离
    const float dist = cv::norm(PC);

    tuple<int ,int> indexes = observations[pRefKF];
    int leftIndex = get<0>(indexes), rightIndex = get<1>(indexes);
    int level;
    if(pRefKF -> NLeft == -1){
        level = pRefKF->mvKeysUn[leftIndex].octave;
    }
    else if(leftIndex != -1){
        level = pRefKF -> mvKeys[leftIndex].octave;
    }
    else{
        level = pRefKF -> mvKeysRight[rightIndex - pRefKF -> NLeft].octave;
    }

    const float levelScaleFactor =  pRefKF->mvScaleFactors[level];
    // 金字塔层数
    const int nLevels = pRefKF->mnScaleLevels;

    {
        unique_lock<mutex> lock3(mMutexPos);
        mfMaxDistance = dist*levelScaleFactor; // 观测到该点的距离下限
        mfMinDistance = mfMaxDistance/pRefKF->mvScaleFactors[nLevels-1];  //观测到该点的距离上限
        mNormalVector = normal/n; // 获得平均的观测方向
        mNormalVectorx = cv::Matx31f(mNormalVector.at<float>(0), mNormalVector.at<float>(1), mNormalVector.at<float>(2));
    }
}

PredictScale 预测金字塔的层数

金字塔示例

              ____
 Nearer      /____\     level:n-1 --> dmin
            /______\                       d/dmin = 1.2^(n-1-m)
           /________\   level:m   --> d
          /__________\                     dmax/d = 1.2^m
 Farther /____________\ level:0   --> dmax

           log(dmax/d)
 m = ceil(------------)
            log(1.2)

mfMaxDistance = ref_dist*levelScaleFactor为参考帧考虑上尺度后的距离
ratio = mfMaxDistance/currentDist = ref_dist/cur_dist

int MapPoint::PredictScale(const float &currentDist, KeyFrame* pKF)
{
    float ratio;
    {
        unique_lock<mutex> lock(mMutexPos);
        ratio = mfMaxDistance/currentDist;
    }

    int nScale = ceil(log(ratio)/pKF->mfLogScaleFactor);
    if(nScale<0)
        nScale = 0;
    else if(nScale>=pKF->mnScaleLevels)
        nScale = pKF->mnScaleLevels-1;

    return nScale;
}

int MapPoint::PredictScale(const float &currentDist, Frame* pF)
{
    float ratio;
    {
        unique_lock<mutex> lock(mMutexPos);
        ratio = mfMaxDistance/currentDist;
    }

    int nScale = ceil(log(ratio)/pF->mfLogScaleFactor);
    if(nScale<0)
        nScale = 0;
    else if(nScale>=pF->mnScaleLevels)
        nScale = pF->mnScaleLevels-1;

    return nScale;
}

xiaoma_bk

关注

3
点赞
踩
22

收藏

觉得还不错? 一键收藏
0
评论
ORB-SLAM3 IMU(李群)+Frame+KeyFrame+MapPoint

单目惯导IMUImuTypesIMU测量数据IMU bias估计IMU 标定集成 1个陀螺仪测量ImuTypesIMU测量数据线加速度+角速度+时间const float GRAVITY_VALUE=9.81;//IMU measurement (gyro, accelerometer and timestamp)class Point{public: Point(const float &acc_x, const float &acc_y, const float
复制链接

扫一扫