1.ImuTypes文件解析
1.1 Point类
在ORBSLAM3中，Point类表示一个IMU的测量数据：加速度、角速度及数据所对应的时间戳。其构造函数有两个形式，其都是构造一个imu的测量数据的。
// 这里指代一个imu的测量数据 class Point { //加速度 角速度 时间戳 public: 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 ×tamp): 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 ×tamp): a(Acc.x,Acc.y,Acc.z), w(Gyro.x,Gyro.y,Gyro.z), t(timestamp){} public: Eigen::Vector3f a; Eigen::Vector3f w; double t; EIGEN_MAKE_ALIGNED_OPERATOR_NEW };
1.2 Bias类
Bias类是偏置类：保存加速度和角速度的偏置
空构造时传入的都是0，否则传来的是加速度的偏置和角速度的偏置。
// IMU biases (gyro and accelerometer) // 保存加速度和角速度 class Bias { friend class boost::serialization::access; template<class Archive> void serialize(Archive & ar, const unsigned int version) { ar & bax; ar & bay; ar & baz; ar & bwx; ar & bwy; ar & bwz; } 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; EIGEN_MAKE_ALIGNED_OPERATOR_NEW };void serialize(Archive & ar, const unsigned int version)这个函数是保存地图的时候序列化反序列化的时候用到的。偏置里面也是保存六个数，关于加速度的偏置和关于角速度的偏置。
Bias::CopyFrom函数也是做的相同的事情。
** * @brief 赋值新的偏置 * @param b 偏置 */ void Bias::CopyFrom(Bias &b) { bax = b.bax; bay = b.bay; baz = b.baz; bwx = b.bwx; bwy = b.bwy; bwz = b.bwz; }
1.3 Calib类
Calib保存着IMU的外参，它是一个外参类：
***这里的b表示base（IMU），这点在之后阅读代码的时候要格外关注一下，这里主要保存了imu到相机的变换矩阵
和相机到imu的变换矩阵
。还存放了误差的协方差
和随机游走
的协方差矩阵。（协方差矩阵是用来算信息矩阵的！！）
// IMU calibration (Tbc, Tcb, noise) // 保存标定好的外参的类 class Calib { friend class boost::serialization::access; template<class Archive> void serialize(Archive & ar, const unsigned int version) { serializeSophusSE3(ar,mTcb,version); serializeSophusSE3(ar,mTbc,version); ar & boost::serialization::make_array(Cov.diagonal().data(), Cov.diagonal().size()); ar & boost::serialization::make_array(CovWalk.diagonal().data(), CovWalk.diagonal().size()); ar & mbIsSet; } public: Calib(const Sophus::SE3<float> &Tbc, const float &ng, const float &na, const float &ngw, const float &naw) { Set(Tbc,ng,na,ngw,naw); } Calib(const Calib &calib); Calib(){mbIsSet = false;} //void Set(const cv::Mat &cvTbc, const float &ng, const float &na, const float &ngw, const float &naw); void Set(const Sophus::SE3<float> &sophTbc, const float &ng, const float &na, const float &ngw, const float &naw); public: // Sophus/Eigen implementation // imu到相机的T Sophus::SE3<float> mTcb; // 相机到imu的T Sophus::SE3<float> mTbc; // 误差的协方差和随机游走的协方差 Eigen::DiagonalMatrix<float,6> Cov, CovWalk; bool mbIsSet; };/** * @brief 设置参数 * @param Tbc_ 位姿变换 * @param ng 噪声 * @param na 噪声 * @param ngw 随机游走 * @param naw 随机游走 */ void Calib::Set(const Sophus::SE3<float> &sophTbc, const float &ng, const float &na, const float &ngw, const float &naw) { mbIsSet = true; const float ng2 = ng * ng; const float na2 = na * na; const float ngw2 = ngw * ngw; const float naw2 = naw * naw; // Sophus/Eigen mTbc = sophTbc; mTcb = mTbc.inverse(); // 噪声协方差 Cov.diagonal() << ng2, ng2, ng2, na2, na2, na2; // 随机游走协方差 CovWalk.diagonal() << ngw2, ngw2, ngw2, naw2, naw2, naw2; }
1.4 IntegratedRotation类
角度的预积分类IntegratedRotation：单纯两个IMU数据之间的积分，不存在累计关系。
是去除了噪声后的IMU积分。这个类只做了一次积分。
// Integration of 1 gyro measurement // 关于角度的积分类,单纯两个IMU数据之间的积分,不存在累计关系 class IntegratedRotation { public: IntegratedRotation(){} IntegratedRotation(const Eigen::Vector3f &angVel, const Bias &imuBias, const float &time); public: float deltaT; //一次积分中的间隔derta T Eigen::Matrix3f deltaR; // Exp 里面中的值 Eigen::Matrix3f rightJ; // 右雅可比 EIGEN_MAKE_ALIGNED_OPERATOR_NEW };首先去偏置，然后根据d值计算。
/** * @brief 计算旋转角度积分量 * 对应Bias更新对预积分的影响一节 * @param[in] angVel 陀螺仪数据(角速度) * @param[in] imuBias 陀螺仪偏置(角速度偏置) * @param[in] time 两帧间的时间差,derta t */ IntegratedRotation::IntegratedRotation(const Eigen::Vector3f &angVel, const Bias &imuBias, const float &time) { // 得到考虑偏置后的角度旋转 const float x = (angVel(0) - imuBias.bwx) * time; const float y = (angVel(1) - imuBias.bwy) * time; const float z = (angVel(2) - imuBias.bwz) * time; // 计算旋转矩阵的模值，后面用罗德里格公式计算旋转矩阵时会用到 const float d2 = x * x + y * y + z * z; const float d = sqrt(d2); Eigen::Vector3f v; v << x, y, z; // 角度转成叉积的矩阵形式 Eigen::Matrix3f W = Sophus::SO3f::hat(v); // eps = 1e-4 是一个小量，根据罗德里格斯公式求极限，后面的高阶小量忽略掉得到此式 if (d < eps) { // 3*3 单位矩阵 + W(叉积矩阵的形式) deltaR = Eigen::Matrix3f::Identity() + W; // E rightJ = Eigen::Matrix3f::Identity(); } else { // 经典预积分计算公式 deltaR = Eigen::Matrix3f::Identity() + W * sin(d) / d + W * W * (1.0f - cos(d)) / d2; // 右雅可比矩阵 公式1.6 rightJ = Eigen::Matrix3f::Identity() - W * (1.0f - cos(d)) / d2 + W * W * (d - sin(d)) / (d2 * d); } }
1.5 Preintegrated类
预积分类Preintegrated：都有详细的标注，不再阐述
// Integration of 1 gyro measurement // 关于角度的积分类,单纯两个IMU数据之间的积分,不存在累计关系 class IntegratedRotation { public: IntegratedRotation(){} IntegratedRotation(const Eigen::Vector3f &angVel, const Bias &imuBias, const float &time); public: float deltaT; //一次积分中的间隔derta T Eigen::Matrix3f deltaR; // Exp 里面中的值 Eigen::Matrix3f rightJ; // 右雅可比 EIGEN_MAKE_ALIGNED_OPERATOR_NEW }; // Preintegration of Imu Measurements // 预积分类 class Preintegrated { friend class boost::serialization::access; template<class Archive> void serialize(Archive & ar, const unsigned int version) { ar & dT; ar & boost::serialization::make_array(C.data(), C.size()); ar & boost::serialization::make_array(Info.data(), Info.size()); ar & boost::serialization::make_array(Nga.diagonal().data(), Nga.diagonal().size()); ar & boost::serialization::make_array(NgaWalk.diagonal().data(), NgaWalk.diagonal().size()); ar & b; ar & boost::serialization::make_array(dR.data(), dR.size()); ar & boost::serialization::make_array(dV.data(), dV.size()); ar & boost::serialization::make_array(dP.data(), dP.size()); ar & boost::serialization::make_array(JRg.data(), JRg.size()); ar & boost::serialization::make_array(JVg.data(), JVg.size()); ar & boost::serialization::make_array(JVa.data(), JVa.size()); ar & boost::serialization::make_array(JPg.data(), JPg.size()); ar & boost::serialization::make_array(JPa.data(), JPa.size()); ar & boost::serialization::make_array(avgA.data(), avgA.size()); ar & boost::serialization::make_array(avgW.data(), avgW.size()); ar & bu; ar & boost::serialization::make_array(db.data(), db.size()); ar & mvMeasurements; } public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW Preintegrated(const Bias &b_, const Calib &calib); Preintegrated(Preintegrated* pImuPre); Preintegrated() {} ~Preintegrated() {} void CopyFrom(Preintegrated* pImuPre); void Initialize(const Bias &b_); void IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt); void Reintegrate(); void MergePrevious(Preintegrated* pPrev); void SetNewBias(const Bias &bu_); IMU::Bias GetDeltaBias(const Bias &b_); Eigen::Matrix3f GetDeltaRotation(const Bias &b_); Eigen::Vector3f GetDeltaVelocity(const Bias &b_); Eigen::Vector3f GetDeltaPosition(const Bias &b_); Eigen::Matrix3f GetUpdatedDeltaRotation(); Eigen::Vector3f GetUpdatedDeltaVelocity(); Eigen::Vector3f GetUpdatedDeltaPosition(); Eigen::Matrix3f GetOriginalDeltaRotation(); Eigen::Vector3f GetOriginalDeltaVelocity(); Eigen::Vector3f GetOriginalDeltaPosition(); Eigen::Matrix<float,6,1> GetDeltaBias(); Bias GetOriginalBias(); Bias GetUpdatedBias(); void printMeasurements() const { std::cout << "pint meas:\n"; for(int i=0; i<mvMeasurements.size(); i++){ std::cout << "meas " << mvMeasurements[i].t << std::endl; } std::cout << "end pint meas:\n"; } public: // 这一段的总时间 float dT; // 协方差矩阵 Eigen::Matrix<float,15,15> C; // 信息矩阵 Eigen::Matrix<float,15,15> Info; // 误差 随机游走 Eigen::DiagonalMatrix<float,6> Nga, NgaWalk; // Values for the original bias (when integration was computed) // 原始偏置、初始化的时候给的偏置 Bias b; // 预积分的值 Eigen::Matrix3f dR; Eigen::Vector3f dV, dP; // 旋转对陀螺仪偏置的雅可比、速度对陀螺仪偏置的雅可比、速度对加速度计偏置的雅可比、位置对陀螺仪偏置加速度计偏置的雅可比...... Eigen::Matrix3f JRg, JVg, JVa, JPg, JPa; // 加速度的平均值、角速度的平均值 Eigen::Vector3f avgA, avgW; private: Bias bu; //更新后的偏置值(优化的时候会更新) // Dif between original and updated bias // This is used to compute the updated values of the preintegration Eigen::Matrix<float,6,1> db; struct integrable { template<class Archive> void serialize(Archive & ar, const unsigned int version) { ar & boost::serialization::make_array(a.data(), a.size()); ar & boost::serialization::make_array(w.data(), w.size()); ar & t; } EIGEN_MAKE_ALIGNED_OPERATOR_NEW integrable(){} integrable(const Eigen::Vector3f &a_, const Eigen::Vector3f &w_ , const float &t_):a(a_),w(w_),t(t_){} Eigen::Vector3f a, w; float t; }; // 这段时间内的所有imu数据 std::vector<integrable> mvMeasurements; // 锁 std::mutex mMutex; }; // Lie Algebra Functions Eigen::Matrix3f RightJacobianSO3(const float &x, const float &y, const float &z); Eigen::Matrix3f RightJacobianSO3(const Eigen::Vector3f &v); Eigen::Matrix3f InverseRightJacobianSO3(const float &x, const float &y, const float &z); Eigen::Matrix3f InverseRightJacobianSO3(const Eigen::Vector3f &v); Eigen::Matrix3f NormalizeRotation(const Eigen::Matrix3f &R); }有好几个构造函数，我们依次看一看：这个主要是把协方差矩阵赋值了。Nga、NgaWalk是Preintegrated类的类内public变量。表示一段时间预积分的协方差矩阵。
/** * @brief 预积分类构造函数，根据输入的偏置初始化预积分参数 * @param b_ 偏置 * @param calib imu标定参数的类 */ Preintegrated::Preintegrated(const Bias &b_, const Calib &calib) { Nga = calib.Cov; NgaWalk = calib.CovWalk; Initialize(b_); }这里pImuPre是指上一段时间的预积分：这一段就是把上段预积分的每个变量取出来赋值到当前的预积分类的成员变量里面。
// Copy constructor // pImuPre 上一段时间的预积分 // 主要是将上一次预积分的量赋值到本次的成员变量中 Preintegrated::Preintegrated(Preintegrated *pImuPre) : dT(pImuPre->dT), C(pImuPre->C), Info(pImuPre->Info), Nga(pImuPre->Nga), NgaWalk(pImuPre->NgaWalk), b(pImuPre->b), dR(pImuPre->dR), dV(pImuPre->dV), dP(pImuPre->dP), JRg(pImuPre->JRg), JVg(pImuPre->JVg), JVa(pImuPre->JVa), JPg(pImuPre->JPg), JPa(pImuPre->JPa), avgA(pImuPre->avgA), avgW(pImuPre->avgW), bu(pImuPre->bu), db(pImuPre->db), mvMeasurements(pImuPre->mvMeasurements) { }这段差不多，也是和上面的做同样的事情：
/** * @brief 复制上一帧的预积分 * @param pImuPre 上一帧的预积分 */ void Preintegrated::CopyFrom(Preintegrated *pImuPre) { dT = pImuPre->dT; C = pImuPre->C; Info = pImuPre->Info; Nga = pImuPre->Nga; NgaWalk = pImuPre->NgaWalk; b.CopyFrom(pImuPre->b); dR = pImuPre->dR; dV = pImuPre->dV; dP = pImuPre->dP; // 旋转关于陀螺仪偏置变化的雅克比，以此类推 JRg = pImuPre->JRg; JVg = pImuPre->JVg; JVa = pImuPre->JVa; JPg = pImuPre->JPg; JPa = pImuPre->JPa; avgA = pImuPre->avgA; avgW = pImuPre->avgW; bu.CopyFrom(pImuPre->bu); db = pImuPre->db; mvMeasurements = pImuPre->mvMeasurements; }初始化预积分函数：就把偏置更新了一下。
/** * @brief 初始化预积分 * @param b_ 偏置 */ void Preintegrated::Initialize(const Bias &b_) { // 除了dR设置为单位矩阵,其他均设置为0矩阵 dR.setIdentity(); dV.setZero(); dP.setZero(); JRg.setZero(); JVg.setZero(); JVa.setZero(); JPg.setZero(); JPa.setZero(); C.setZero(); Info.setZero(); db.setZero(); b = b_; bu = b_; // 更新后的偏置 avgA.setZero(); // 平均加速度 avgW.setZero(); // 平均角速度 dT = 0.0f; mvMeasurements.clear(); // 存放imu数据及dt }重新积分函数：将这段时间的IMU数据取出来放到容器中，然后用更新后的偏置bu更新偏置，其他参数全部初始化，然后重新计算这段时间的预积分（这个函数是最重要的函数）。
/** * @brief 根据新的偏置重新算预积分mvMeasurements里的数据 Optimizer::InertialOptimization调用 */ void Preintegrated::Reintegrate() { std::unique_lock<std::mutex> lock(mMutex); // 将这段时间的所有imu数据取出来放在aux向量中 const std::vector<integrable> aux = mvMeasurements; // 根据最新的偏置bu重新初始化 Initialize(bu); // 计算预积分 for (size_t i = 0; i < aux.size(); i++) IntegrateNewMeasurement(aux[i].a, aux[i].w, aux[i].t); }
1.6 计算预积分 Preintegrated::IntegrateNewMeasurement函数解析
integrable是预积分类Preintegrated的一个结构体，其中包含的数据如下：
struct integrable { template<class Archive> // 没用 void serialize(Archive & ar, const unsigned int version) { ar & boost::serialization::make_array(a.data(), a.size()); ar & boost::serialization::make_array(w.data(), w.size()); ar & t; } EIGEN_MAKE_ALIGNED_OPERATOR_NEW integrable(){} // 加速度 角速度 integrable(const Eigen::Vector3f &a_, const Eigen::Vector3f &w_ , const float &t_):a(a_),w(w_),t(t_){} Eigen::Vector3f a, w; float t; };其实就是保存了一段时间的实测数据，加速度角速度和时间间隔。
/** * @brief 预积分计算，更新noise * * @param[in] acceleration 加速度计数据 * @param[in] angVel 陀螺仪数据,角速度 * @param[in] dt 两数据间的时间差 */ void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt) { // 保存imu数据，利用中值积分的结果构造一个预积分类保存在mvMeasurements中 mvMeasurements.push_back(integrable(acceleration, angVel, dt)); // Position is updated firstly, as it depends on previously computed velocity and rotation. // Velocity is updated secondly, as it depends on previously computed rotation. // Rotation is the last to be updated. // Matrices to compute covariance // Step 1.构造协方差矩阵 // 噪声矩阵的传递矩阵，这部分用于计算i到j-1历史噪声或者协方差 Eigen::Matrix<float, 9, 9> A; A.setIdentity(); // 噪声矩阵的传递矩阵，这部分用于计算j-1新的噪声或协方差，这两个矩阵里面的数都是当前时刻的，计算主要是为了下一时刻使用 Eigen::Matrix<float, 9, 6> B; B.setZero(); // 考虑偏置后的加速度、角速度，根据偏置得到数据 Eigen::Vector3f acc, accW; acc << acceleration(0) - b.bax, acceleration(1) - b.bay, acceleration(2) - b.baz; accW << angVel(0) - b.bwx, angVel(1) - b.bwy, angVel(2) - b.bwz; // 记录平均加速度和角速度 avgA = (dT * avgA + dR * acc * dt) / (dT + dt); avgW = (dT * avgW + accW * dt) / (dT + dt); // Update delta position dP and velocity dV (rely on no-updated delta rotation) // 更新位置和速度：根据没有更新的dR来更新dP与dV eq.(38) // 关于位置的预积分 dP = dP + dV * dt + 0.5f * dR * acc * dt * dt; dV = dV + dR * acc * dt; // Compute velocity and position parts of matrices A and B (rely on non-updated delta rotation) // 根据η_ij = A * η_i,j-1 + B_j-1 * η_j-1中的Ａ矩阵和Ｂ矩阵对速度和位移进行更新，角速度的反对陈矩阵 Eigen::Matrix<float, 3, 3> Wacc = Sophus::SO3f::hat(acc); A.block<3, 3>(3, 0) = -dR * dt * Wacc; A.block<3, 3>(6, 0) = -0.5f * dR * dt * dt * Wacc; A.block<3, 3>(6, 3) = Eigen::DiagonalMatrix<float, 3>(dt, dt, dt); B.block<3, 3>(3, 3) = dR * dt; B.block<3, 3>(6, 3) = 0.5f * dR * dt * dt; // Update position and velocity jacobians wrt bias correction // 因为随着时间推移，不可能每次都重新计算雅克比矩阵，所以需要做J(k+1) = j(k) + (~)这类事，分解方式与AB矩阵相同 // 论文作者对forster论文公式的基础上做了变形，然后递归更新，参见 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/212 // 计算预积分相对于偏置的雅可比矩阵 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 // Step 2. 构造函数，会根据更新后的bias进行角度积分 IntegratedRotation dRi(angVel, b, dt); // 强行归一化使其符合旋转矩阵的格式,更新角度的预积分 dR = NormalizeRotation(dR * dRi.deltaR); // Compute rotation parts of matrices A and B // 补充AB矩阵 A.block<3, 3>(0, 0) = dRi.deltaR.transpose(); B.block<3, 3>(0, 0) = dRi.rightJ * dt; // 小量delta初始为0，更新后通常也为0，故省略了小量的更新 // Update covariance // Step 3.更新协方差，frost经典预积分论文的第63个公式，推导了噪声（ηa, ηg）对dR dV dP 的影响 C.block<9, 9>(0, 0) = A * C.block<9, 9>(0, 0) * A.transpose() + B * Nga * B.transpose(); // B矩阵为9*6矩阵 Nga 6*6对角矩阵，3个陀螺仪噪声的平方，3个加速度计噪声的平方 // 这一部分最开始是0矩阵，随着积分次数增加，每次都加上随机游走，偏置的信息矩阵 C.block<6, 6>(9, 9) += NgaWalk; // Update rotation jacobian wrt bias correction // 计算偏置的雅克比矩阵，r对bg的导数，∂ΔRij/∂bg = (ΔRjj-1) * ∂ΔRij-1/∂bg - Jr(j-1)*t // 论文作者对forster论文公式的基础上做了变形，然后递归更新，参见 https://github.com/UZ-SLAMLab/ORB_SLAM3/issues/212 // ? 为什么先更新JPa、JPg、JVa、JVg最后更新JRg? 答：这里必须先更新dRi才能更新到这个值，但是为什么JPg和JVg依赖的上一个JRg值进行更新的？ JRg = dRi.deltaR.transpose() * JRg - dRi.rightJ * dt; // Total integrated time // 更新总时间 dT += dt; }在本段函数中，mvMeasurements.push_back(integrable(acceleration, angVel, dt));，mvMeasurements保存着一段时间的IMU数据，现保存上以备后续使用。
第一步我们定义A，B矩阵，因为A，B矩阵是实时变化的，对于每一帧都需要重新计算。
将偏置后的角速度、加速度及算出来。计算平均角速度和平均加速度。
我们先计算关于速度和位移的预积分，没有先去计算关于角度的预积分。
我们是用的上一时刻的dR来更新这一时刻的dV和dp：
第二我们算A、B矩阵：
A.block<3, 3>(3, 0) = -dR * dt * Wacc; 从第3行第0列开始的3*3矩阵。
对应着图中的第二行第一列的矩阵。-dR表示到上一时刻的dR值(i -> j-1)，Wacc是关于角速度的反对称矩阵。其他都是一样的..不再赘述。
第三算预积分相对于偏置的雅可比矩阵：
我们拿位置相对于陀螺仪偏置的项为例子
JPg = JPg + JVg * dt - 0.5f * dR * dt * dt * Wacc * JRg;
第四我们构建一个角度的预积分的类，将dR计算出来。（构造函数构造的时候已经算好了）
最后我们更新A、B矩阵及协方差矩阵，总时间。
A.block<3, 3>(0, 0) = dRi.deltaR.transpose(); 我们算得是j-1 -> j ，因此需要取一个转置。
B.block<3, 3>(0, 0) = dRi.rightJ * dt; 对应的
。当前时刻的右雅可比。
1.6 求右雅可比矩阵及逆矩阵
/** * @brief 计算右雅可比矩阵 * @param xyz 李代数 * @return Jr */ Eigen::Matrix3f RightJacobianSO3(const float &x, const float &y, const float &z) { Eigen::Matrix3f I; I.setIdentity(); const float d2 = x * x + y * y + z * z; const float d = sqrt(d2); Eigen::Vector3f v; v << x, y, z; Eigen::Matrix3f W = Sophus::SO3f::hat(v); if (d < eps) { return I; } else { return I - W * (1.0f - cos(d)) / d2 + W * W * (d - sin(d)) / (d2 * d); } }
/** * @brief 计算右雅可比矩阵的逆 * @param xyz so3 * @return Jr^-1 */ Eigen::Matrix3f InverseRightJacobianSO3(const float &x, const float &y, const float &z) { Eigen::Matrix3f I; I.setIdentity(); const float d2 = x * x + y * y + z * z; const float d = sqrt(d2); Eigen::Vector3f v; v << x, y, z; Eigen::Matrix3f W = Sophus::SO3f::hat(v); if (d < eps) { return I; } else { return I + W / 2 + W * W * (1.0f / d2 - (1.0f + cos(d)) / (2.0f * d * sin(d))); } }
1.7 融合预积分Preintegrated::MergePrevious
/** * @brief 融合两个预积分，发生在删除关键帧的时候，3帧变2帧，需要把两段预积分融合 * @param pPrev 前面的预积分(上一段的预积分 上上以帧 -> 上一帧) */ void Preintegrated::MergePrevious(Preintegrated *pPrev) { if (pPrev == this) return; std::unique_lock<std::mutex> lock1(mMutex); std::unique_lock<std::mutex> lock2(pPrev->mMutex); // 取到最新的偏置信息 Bias bav; bav.bwx = bu.bwx; bav.bwy = bu.bwy; bav.bwz = bu.bwz; bav.bax = bu.bax; bav.bay = bu.bay; bav.baz = bu.baz; // 存放上一段预积分中的所有数据 const std::vector<integrable> aux1 = pPrev->mvMeasurements; const std::vector<integrable> aux2 = mvMeasurements; // 重新初始化预积分 Initialize(bav); // 好像没啥用呢 for (size_t i = 0; i < aux1.size(); i++) IntegrateNewMeasurement(aux1[i].a, aux1[i].w, aux1[i].t); for (size_t i = 0; i < aux2.size(); i++) IntegrateNewMeasurement(aux2[i].a, aux2[i].w, aux2[i].t); }输入的是上一段时间的预积分（上上一帧到上一帧），先取到最新的积分利用偏置的改变重新初始化预积分。
1.8 利用偏置的改变量计算新的预积分（根据理论）
/** * @brief 根据新的偏置计算新的dR * @param b_ 新的偏置 * @return dR */ Eigen::Matrix3f Preintegrated::GetDeltaRotation(const Bias &b_) { std::unique_lock<std::mutex> lock(mMutex); // 计算偏置的变化量 Eigen::Vector3f dbg; dbg << b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz; // 考虑偏置后，dR对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13～P14 // Forster论文公式（44）yP17也有结果（但没有推导），后面两个函数GetDeltaPosition和GetDeltaPosition也是基于此推导的 // 更新之前的dR * Exp(雅可比矩阵 * 偏置改变量) return NormalizeRotation(dR * Sophus::SO3f::exp(JRg * dbg).matrix()); } /** * @brief 根据新的偏置计算新的dV * @param b_ 新的偏置 * @return dV */ Eigen::Vector3f Preintegrated::GetDeltaVelocity(const Bias &b_) { std::unique_lock<std::mutex> lock(mMutex); Eigen::Vector3f dbg, dba; dbg << b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz; dba << b_.bax - b.bax, b_.bay - b.bay, b_.baz - b.baz; // 考虑偏置后，dV对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13，JPg和JPa在预积分处理中更新 return dV + JVg * dbg + JVa * dba; } /** * @brief 根据新的偏置计算新的dP * @param b_ 新的偏置 * @return dP */ Eigen::Vector3f Preintegrated::GetDeltaPosition(const Bias &b_) { std::unique_lock<std::mutex> lock(mMutex); Eigen::Vector3f dbg, dba; dbg << b_.bwx - b.bwx, b_.bwy - b.bwy, b_.bwz - b.bwz; dba << b_.bax - b.bax, b_.bay - b.bay, b_.baz - b.baz; // 考虑偏置后，dP对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13，JPg和JPa在预积分处理中更新 return dP + JPg * dbg + JPa * dba; }/** * @brief 返回经过db(δba, δbg)更新后的dR,与上面是一个意思 * @return dR */ Eigen::Matrix3f Preintegrated::GetUpdatedDeltaRotation() { std::unique_lock<std::mutex> lock(mMutex); return NormalizeRotation(dR * Sophus::SO3f::exp(JRg * db.head(3)).matrix()); } /** * @brief 返回经过db(δba, δbg)更新后的dV,与上面是一个意思 * @return dV */ Eigen::Vector3f Preintegrated::GetUpdatedDeltaVelocity() { std::unique_lock<std::mutex> lock(mMutex); return dV + JVg * db.head(3) + JVa * db.tail(3); } /** * @brief 返回经过db(δba, δbg)更新后的dP,与上面是一个意思 * @return dP */ Eigen::Vector3f Preintegrated::GetUpdatedDeltaPosition() { std::unique_lock<std::mutex> lock(mMutex); return dP + JPg * db.head(3) + JPa * db.tail(3); }
2. IMU初始化
2.1 为什么要进行IMU的初始化
如果不做IMU的初始化，那么随机的就是以第一帧图像作为世界坐标系，这么以来重力的方向是不知道的，导致计算速度和位置的时候我们利用加速度来算积分的时候重力是没有办法剔除的，因为IMU数据本身就包含重力。如果重力方向未知的情况下我们就无法平衡重力对积分的影响了，导致飘飞。
初始化的目的就是要把第一帧（世界坐标系下的）拉到与重力方向（世界坐标系的Z轴）平行的状态下。这样一来我们就可以在后面平衡重力计算状态。
IMU初始化在Local Mapping线程中调用，IMU初始化分为三个阶段：
// Step 7 当前关键帧所在地图未完成IMU初始化（第一阶段） // 执行IMU第一阶段初始化。目的:快速初始化IMU,尽快用IMU来跟踪 if(!mpCurrentKeyFrame->GetMap()->isImuInitialized() && mbInertial) { // 在函数InitializeIMU里设置IMU成功初始化标志 SetImuInitialized // IMU第一次初始化 if (mbMonocular) InitializeIMU(1e2, 1e10, true); else InitializeIMU(1e2, 1e5, true); }完成第一阶段初始化之后将mpCurrentKeyFrame->GetMap()->isImuInitialized()置为true
第二阶段、第三阶段初始化如下：
// Step 9 如果距离IMU第一阶段初始化成功累计时间差小于100s，进行VIBA // 当第一阶段初始化后到这里小于100秒的时候且在imu模式下，开始第二阶段初始化 if ((mTinit<50.0f) && mbInertial) { // Step 9.1 根据条件判断是否进行VIBA1（IMU第二次初始化） // 条件：1、当前关键帧所在的地图完成第一阶段IMU初始化---并且--------2、正常跟踪状态---------- if(mpCurrentKeyFrame->GetMap()->isImuInitialized() && mpTracker->mState==Tracking::OK) // Enter here everytime local-mapping is called { // 当前关键帧所在的地图还未完成VIBA 1 if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA1()){ // 第一阶段初始化大于5秒，开始VIBA1（IMU第二阶段初始化） 执行IMU第二阶段初始化。目的:快速修正IMU,短时间内使得IMU参数相对靠谱 if (mTinit>5.0f) { cout << "start VIBA 1" << endl; // mbIMU_BA1 = true mpCurrentKeyFrame->GetMap()->SetIniertialBA1(); if (mbMonocular) InitializeIMU(1.f, 1e5, true); else InitializeIMU(1.f, 1e5, true); cout << "end VIBA 1" << endl; } } // Step 9.2 根据条件判断是否进行VIBA2（IMU第三次初始化） // 当前关键帧所在的地图还未完成VIBA 2 else if(!mpCurrentKeyFrame->GetMap()->GetIniertialBA2()){ if (mTinit>15.0f){ cout << "start VIBA 2" << endl; mpCurrentKeyFrame->GetMap()->SetIniertialBA2(); if (mbMonocular) InitializeIMU(0.f, 0.f, true); else InitializeIMU(0.f, 0.f, true); cout << "end VIBA 2" << endl; } } // scale refinement 对应论文中：初始化之后尺度仍然不是很好 这里面做的尺度更新的工作 // Step 9.3 在关键帧小于100时，会在满足一定时间间隔后多次进行尺度、重力方向优化 if (((mpAtlas->KeyFramesInMap())<=200) && ((mTinit>25.0f && mTinit<25.5f)|| (mTinit>35.0f && mTinit<35.5f)|| (mTinit>45.0f && mTinit<45.5f)|| (mTinit>55.0f && mTinit<55.5f)|| (mTinit>65.0f && mTinit<65.5f)|| (mTinit>75.0f && mTinit<75.5f))){ if (mbMonocular) // 使用了所有关键帧，但只优化尺度和重力方向以及速度和偏执（其实就是一切跟惯性相关的量） ScaleRefinement(); } } } }
2.2 初始化IMU函数 LocalMapping::InitializeIMU解析
首先是纯视觉的初始化方式，保证地图关键帧大于一定数量才能进行初始化。即从时间及帧数上限制初始化，不满足下面的不进行初始化。
// 从时间及帧数上限制初始化，不满足下面的不进行初始化 if (mbMonocular) { minTime = 2.0; nMinKF = 10; } else { // 只有大于10关键帧才考虑imu初始化 minTime = 1.0; nMinKF = 10; } // 当前地图大于10帧才进行初始化 if(mpAtlas->KeyFramesInMap()<nMinKF) return;在第一次初始化没做的时候，我们计算
（表示重力坐标系向世界坐标系的变换矩阵）以及记录第一阶段初始化的时间
。
// 2. 正式IMU初始化,N是关键帧的数量 const int N = vpKF.size(); // 最开始的时候将imu的偏置置为0 IMU::Bias b(0,0,0,0,0,0); // Compute and KF velocities mRwg estimation // 在IMU连一次初始化都没有做的情况下,主要目的是算重力方向的初始量 if (!mpCurrentKeyFrame->GetMap()->isImuInitialized()) { // 保存算好的重力方向,w表示世界坐标系 g表示对齐之后的重力方向，表示重力坐标系向世界坐标系的变换矩阵 Eigen::Matrix3f Rwg; Eigen::Vector3f dirG; dirG.setZero(); int have_imu_num = 0; for(vector<KeyFrame*>::iterator itKF = vpKF.begin(); itKF!=vpKF.end(); itKF++) { // 当前关键帧到他上一个关键帧这段时间的预积分,我们把这帧pass掉 if (!(*itKF)->mpImuPreintegrated) continue; // 这个帧上一帧的预积分,如果这是第一帧根本没有上一帧 if (!(*itKF)->mPrevKF) continue; have_imu_num++; // dirG求重力方向 // 初始化时关于速度的预积分定义Ri.t()*(s*Vj - s*Vi - Rwg*g*tij) // 角度预积分 * 速度预积分 dirG -= (*itKF)->mPrevKF->GetImuRotation() * (*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity(); // 求取实际的速度，位移/时间,GetImuPosition存放着imu在世界坐标系下的位置 Eigen::Vector3f _vel = ((*itKF)->GetImuPosition() - (*itKF)->mPrevKF->GetImuPosition())/(*itKF)->mpImuPreintegrated->dT; (*itKF)->SetVelocity(_vel); (*itKF)->mPrevKF->SetVelocity(_vel); } if (have_imu_num < 6) { cout << "imu初始化失败, 由于带有imu预积分信息的关键帧数量太少" << endl; bInitializing=false; mbBadImu = true; return; } // dirG = sV1 - sVn + n*Rwg*g*t // 归一化，约等于重力在世界坐标系下的方向 dirG = dirG/dirG.norm(); // 原本的重力方向,单位化过了 Eigen::Vector3f gI(0.0f, 0.0f, -1.0f); // 求“重力在重力坐标系下的方向”与的“重力在世界坐标系（纯视觉）下的方向”叉乘 Eigen::Vector3f v = gI.cross(dirG); // 求叉乘模长 const float nv = v.norm(); // 求转角ang大小 gI(我们最终要平衡的重力坐标系) dirG const float cosg = gI.dot(dirG); const float ang = acos(cosg); // v/nv 表示垂直于两个向量的轴 ang 表示转的角度，组成角轴，从g到w的角轴 Eigen::Vector3f vzg = v*ang/nv; // 获得重力坐标系到世界坐标系的旋转矩阵的初值 // 将旋转向量转换成旋转矩阵 Rwg = Sophus::SO3f::exp(vzg).matrix(); mRwg = Rwg.cast<double>(); // 记录第一阶段初始化的时间 mTinit = mpCurrentKeyFrame->mTimeStamp-mFirstTs; } else // 在第一次初始化的时候没有设置偏置,第二次初始化的时候才设置 { mRwg = Eigen::Matrix3d::Identity(); mbg = mpCurrentKeyFrame->GetGyroBias().cast<double>(); mba = mpCurrentKeyFrame->GetAccBias().cast<double>(); }
进行优化，这个在之后会说，现在先跳过：
std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now(); // 3. 计算残差及偏置差，优化尺度、重力方向、速度、偏置，偏置先验为0，双目时不优化尺度 Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale, mbg, mba, mbMonocular, infoInertial, false, false, priorG, priorA); std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();进行善后工作，将
// 到此时为止，前面做的东西没有改变map,善后工作,将更新应用在地图上 { unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate); // 尺度变化超过设定值，或者非单目时（无论带不带imu，但这个函数只在带imu时才执行，所以这个可以理解为双目imu） // 尺度变化优化后和优化前差很多的情况下才会执行 if ((fabs(mScale - 1.f) > 0.00001) || !mbMonocular) { // 4.1 恢复重力方向与尺度信息 Sophus::SE3f Twg(mRwg.cast<float>().transpose(), Eigen::Vector3f::Zero()); // Twg重力方向 mScale尺度 true mpAtlas->GetCurrentMap()->ApplyScaledRotation(Twg, mScale, true); // 4.2 更新普通帧的位姿，主要是当前帧与上一帧 mpTracker->UpdateFrameIMU(mScale, vpKF[0]->GetImuBias(), mpCurrentKeyFrame); } // Check if initialization OK // 即使初始化成功后面还会执行这个函数重新初始化 // 在之前没有初始化成功情况下（此时刚刚初始化成功）对每一帧都标记，后面的kf全部都在tracking里面标记为true // 也就是初始化之前的那些关键帧即使有imu信息也不算 if (!mpAtlas->isImuInitialized()) for (int i = 0; i < N; i++) { KeyFrame *pKF2 = vpKF[i]; pKF2->bImu = true; } }2.3节中有Map::ApplyScaledRotation的详细解释。其主要思想是我们更新所有相机的位姿和地图点的坐标，使其在重力坐标下。
2.4节有Tracking::UpdateFrameIMU的详细解释。
/** * @brief imu初始化 * @param priorG 陀螺仪偏置的信息矩阵系数，主动设置时一般bInit为true，也就是只优化最后一帧的偏置，这个数会作为计算信息矩阵时使用 * @param priorA 加速度计偏置的信息矩阵系数 * @param bFIBA 是否做BA优化，目前都为true */ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) { // 1. 将所有关键帧放入列表及向量里，且查看是否满足初始化条件 // 是否要reset,界面的那个按钮按一下就reset了 if (mbResetRequested) return; float minTime; int nMinKF; // 从时间及帧数上限制初始化，不满足下面的不进行初始化 if (mbMonocular) { minTime = 2.0; nMinKF = 10; } else { // 只有大于10关键帧才考虑imu初始化 minTime = 1.0; nMinKF = 10; } // 当前地图大于10帧才进行初始化 if(mpAtlas->KeyFramesInMap()<nMinKF) return; // Retrieve all keyframe in temporal order // 按照顺序存放目前地图里的关键帧,顺序按照前后顺序来,包括当前关键帧,时间最早排在list的前面当前关键帧排在list的最后面 list<KeyFrame*> lpKF; KeyFrame* pKF = mpCurrentKeyFrame; while(pKF->mPrevKF) { lpKF.push_front(pKF); pKF = pKF->mPrevKF; } lpKF.push_front(pKF); // 同样内容再构建一个和lpKF一样的容器vpKF vector<KeyFrame*> vpKF(lpKF.begin(),lpKF.end()); if(vpKF.size()<nMinKF) return; // 若当前帧的时间戳 - 第一帧的时间戳不满足条件不进行IMU初始化 mFirstTs=vpKF.front()->mTimeStamp; if(mpCurrentKeyFrame->mTimeStamp-mFirstTs<minTime) return; // 正在做IMU的初始化，在tracking里面使用，如果为true，暂不向LocalMapping线程添加关键帧 bInitializing = true; // 由于是多线程,因此在Localmapping线程到现在为止有Tracking线程新到的关键帧。处理先处理新关键帧，防止堆积且保证数据量充足 // tracking线程可能在闲暇时间向LocalMapping线程输送了几帧关键帧,这里我们把未处理的KF放到地图里面 while(CheckNewKeyFrames()) { ProcessNewKeyFrame(); vpKF.push_back(mpCurrentKeyFrame); lpKF.push_back(mpCurrentKeyFrame); } // 2. 正式IMU初始化,N是关键帧的数量 const int N = vpKF.size(); // 最开始的时候将imu的偏置置为0 IMU::Bias b(0,0,0,0,0,0); // Compute and KF velocities mRwg estimation // 在IMU连一次初始化都没有做的情况下,主要目的是算重力方向的初始量 if (!mpCurrentKeyFrame->GetMap()->isImuInitialized()) { // 保存算好的重力方向,w表示世界坐标系 g表示对齐之后的重力方向，表示重力坐标系向世界坐标系的变换矩阵 Eigen::Matrix3f Rwg; Eigen::Vector3f dirG; dirG.setZero(); int have_imu_num = 0; for(vector<KeyFrame*>::iterator itKF = vpKF.begin(); itKF!=vpKF.end(); itKF++) { // 当前关键帧到他上一个关键帧这段时间的预积分,我们把这帧pass掉 if (!(*itKF)->mpImuPreintegrated) continue; // 这个帧上一帧的预积分,如果这是第一帧根本没有上一帧 if (!(*itKF)->mPrevKF) continue; have_imu_num++; // dirG求重力方向 // 初始化时关于速度的预积分定义Ri.t()*(s*Vj - s*Vi - Rwg*g*tij) // 角度预积分 * 速度预积分 dirG -= (*itKF)->mPrevKF->GetImuRotation() * (*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity(); // 求取实际的速度，位移/时间,GetImuPosition存放着imu在世界坐标系下的位置 Eigen::Vector3f _vel = ((*itKF)->GetImuPosition() - (*itKF)->mPrevKF->GetImuPosition())/(*itKF)->mpImuPreintegrated->dT; (*itKF)->SetVelocity(_vel); (*itKF)->mPrevKF->SetVelocity(_vel); } if (have_imu_num < 6) { cout << "imu初始化失败, 由于带有imu预积分信息的关键帧数量太少" << endl; bInitializing=false; mbBadImu = true; return; } // dirG = sV1 - sVn + n*Rwg*g*t // 归一化，约等于重力在世界坐标系下的方向 dirG = dirG/dirG.norm(); // 原本的重力方向,单位化过了 Eigen::Vector3f gI(0.0f, 0.0f, -1.0f); // 求“重力在重力坐标系下的方向”与的“重力在世界坐标系（纯视觉）下的方向”叉乘 Eigen::Vector3f v = gI.cross(dirG); // 求叉乘模长 const float nv = v.norm(); // 求转角ang大小 gI(我们最终要平衡的重力坐标系) dirG const float cosg = gI.dot(dirG); const float ang = acos(cosg); // v/nv 表示垂直于两个向量的轴 ang 表示转的角度，组成角轴，从g到w的角轴 Eigen::Vector3f vzg = v*ang/nv; // 获得重力坐标系到世界坐标系的旋转矩阵的初值 // 将旋转向量转换成旋转矩阵 Rwg = Sophus::SO3f::exp(vzg).matrix(); mRwg = Rwg.cast<double>(); // 记录第一阶段初始化的时间 mTinit = mpCurrentKeyFrame->mTimeStamp-mFirstTs; } else // 在第一次初始化的时候没有设置偏置,第二次初始化的时候才设置 { mRwg = Eigen::Matrix3d::Identity(); // 偏置:第一次初始化的时候直接初始化为0 IMU::Bias b(0,0,0,0,0,0); mbg = mpCurrentKeyFrame->GetGyroBias().cast<double>(); mba = mpCurrentKeyFrame->GetAccBias().cast<double>(); } mScale=1.0; // 暂时没发现在别的地方出现过 mInitTime = mpTracker->mLastFrame.mTimeStamp-vpKF.front()->mTimeStamp; std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now(); // 3. 计算残差及偏置差，优化尺度、重力方向、速度、偏置，偏置先验为0，双目时不优化尺度 纯惯性优化 Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale, mbg, mba, mbMonocular, infoInertial, false, false, priorG, priorA); std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); // 尺度太小的话初始化认为失败 if (mScale<1e-1) { cout << "scale too small" << endl; bInitializing=false; return; } // 到此时为止，前面做的东西没有改变map,善后工作,将更新应用在地图上 { unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate); // 尺度变化超过设定值，或者非单目时（无论带不带imu，但这个函数只在带imu时才执行，所以这个可以理解为双目imu） // 尺度变化优化后和优化前差很多的情况下才会执行 if ((fabs(mScale - 1.f) > 0.00001) || !mbMonocular) { // 4.1 恢复重力方向与尺度信息 Sophus::SE3f Twg(mRwg.cast<float>().transpose(), Eigen::Vector3f::Zero()); // Twg重力方向 mScale尺度 true mpAtlas->GetCurrentMap()->ApplyScaledRotation(Twg, mScale, true); // 4.2 更新普通帧的位姿，主要是当前帧与上一帧 mpTracker->UpdateFrameIMU(mScale, vpKF[0]->GetImuBias(), mpCurrentKeyFrame); } // Check if initialization OK // 即使初始化成功后面还会执行这个函数重新初始化 // 在之前没有初始化成功情况下（此时刚刚初始化成功）对每一帧都标记，后面的kf全部都在tracking里面标记为true // 也就是初始化之前的那些关键帧即使有imu信息也不算 if (!mpAtlas->isImuInitialized()) for (int i = 0; i < N; i++) { KeyFrame *pKF2 = vpKF[i]; pKF2->bImu = true; } } // TODO 这步更新是否有必要做待研究，0.4版本是放在FullInertialBA下面做的 // 这个版本FullInertialBA不直接更新位姿及三位点了 mpTracker->UpdateFrameIMU(1.0,vpKF[0]->GetImuBias(),mpCurrentKeyFrame); // 设置经过初始化了 if (!mpAtlas->isImuInitialized()) { // ! 重要！标记初始化成功 mpAtlas->SetImuInitialized(); mpTracker->t0IMU = mpTracker->mCurrentFrame.mTimeStamp; mpCurrentKeyFrame->bImu = true; } std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now(); // 代码里都为true 惯性+视觉优化 if (bFIBA) { // 5. 承接上一步纯imu优化，按照之前的结果更新了尺度信息及适应重力方向，所以要结合地图进行一次视觉加imu的全局优化，这次带了MP等信息 // ! 1.0版本里面不直接赋值了，而是将所有优化后的信息保存到变量里面 if (priorA!=0.f) Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 100, false, mpCurrentKeyFrame->mnId, NULL, true, priorG, priorA); else Optimizer::FullInertialBA(mpAtlas->GetCurrentMap(), 100, false, mpCurrentKeyFrame->mnId, NULL, false); } std::chrono::steady_clock::time_point t5 = std::chrono::steady_clock::now(); Verbose::PrintMess("Global Bundle Adjustment finished\nUpdating map ...", Verbose::VERBOSITY_NORMAL); // Get Map Mutex unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate); unsigned long GBAid = mpCurrentKeyFrame->mnId; // Process keyframes in the queue // 6. 处理一下新来的关键帧，这些关键帧没有参与优化，但是这部分bInitializing为true，只在第2次跟第3次初始化会有新的关键帧进来 // 这部分关键帧也需要被更新 while(CheckNewKeyFrames()) { ProcessNewKeyFrame(); vpKF.push_back(mpCurrentKeyFrame); lpKF.push_back(mpCurrentKeyFrame); } // Correct keyframes starting at map first keyframe // 7. 更新位姿与三维点 // 获取地图中初始关键帧，第一帧肯定经过修正的 list<KeyFrame*> lpKFtoCheck(mpAtlas->GetCurrentMap()->mvpKeyFrameOrigins.begin(),mpAtlas->GetCurrentMap()->mvpKeyFrameOrigins.end()); // 初始就一个关键帧，顺藤摸瓜找到父子相连的所有关键帧 // 类似于树的广度优先搜索，其实也就是根据父子关系遍历所有的关键帧，有的参与了FullInertialBA有的没参与 while(!lpKFtoCheck.empty()) { // 7.1 获得这个关键帧的子关键帧 KeyFrame* pKF = lpKFtoCheck.front(); const set<KeyFrame*> sChilds = pKF->GetChilds(); Sophus::SE3f Twc = pKF->GetPoseInverse(); // 获得关键帧的优化前的位姿 // 7.2 遍历这个关键帧所有的子关键帧 for(set<KeyFrame*>::const_iterator sit=sChilds.begin();sit!=sChilds.end();sit++) { // 确认是否能用 KeyFrame* pChild = *sit; if(!pChild || pChild->isBad()) continue; // 这个判定为true表示pChild没有参与前面的优化，因此要根据已经优化过的更新，结果全部暂存至变量 if(pChild->mnBAGlobalForKF!=GBAid) { // pChild->GetPose()也是优化前的位姿，Twc也是优化前的位姿 // 7.3 因此他们的相对位姿是比较准的，可以用于更新pChild的位姿 Sophus::SE3f Tchildc = pChild->GetPose() * Twc; // 使用相对位姿，根据pKF优化后的位姿更新pChild位姿，最后结果都暂时放于mTcwGBA pChild->mTcwGBA = Tchildc * pKF->mTcwGBA; // 7.4 使用相同手段更新速度 Sophus::SO3f Rcor = pChild->mTcwGBA.so3().inverse() * pChild->GetPose().so3(); if(pChild->isVelocitySet()){ pChild->mVwbGBA = Rcor * pChild->GetVelocity(); } else { Verbose::PrintMess("Child velocity empty!! ", Verbose::VERBOSITY_NORMAL); } pChild->mBiasGBA = pChild->GetImuBias(); pChild->mnBAGlobalForKF = GBAid; } // 加入到list中，再去寻找pChild的子关键帧 lpKFtoCheck.push_back(pChild); } // 7.5 此时pKF的利用价值就没了，但是里面的数值还都是优化前的，优化后的全部放于类似mTcwGBA这样的变量中 // 所以要更新到正式的状态里，另外mTcwBefGBA要记录更新前的位姿，用于同样的手段更新三维点用 pKF->mTcwBefGBA = pKF->GetPose(); pKF->SetPose(pKF->mTcwGBA); // 速度偏置同样更新 if(pKF->bImu) { pKF->mVwbBefGBA = pKF->GetVelocity(); pKF->SetVelocity(pKF->mVwbGBA); pKF->SetNewBias(pKF->mBiasGBA); } else { cout << "KF " << pKF->mnId << " not set to inertial!! \n"; } // pop lpKFtoCheck.pop_front(); } // Correct MapPoints // 8. 更新三维点，三维点在优化后同样没有正式的更新，而是找了个中间变量保存了优化后的数值 const vector<MapPoint*> vpMPs = mpAtlas->GetCurrentMap()->GetAllMapPoints(); for(size_t i=0; i<vpMPs.size(); i++) { MapPoint* pMP = vpMPs[i]; if(pMP->isBad()) continue; // 8.1 如果这个点参与了全局优化，那么直接使用优化后的值来赋值 if(pMP->mnBAGlobalForKF==GBAid) { // If optimized by Global BA, just update pMP->SetWorldPos(pMP->mPosGBA); } // 如果没有参与，与关键帧的更新方式类似 else { // Update according to the correction of its reference keyframe KeyFrame* pRefKF = pMP->GetReferenceKeyFrame(); if(pRefKF->mnBAGlobalForKF!=GBAid) continue; // Map to non-corrected camera // 8.2 根据优化前的世界坐标系下三维点的坐标以及优化前的关键帧位姿计算这个点在关键帧下的坐标 Eigen::Vector3f Xc = pRefKF->mTcwBefGBA * pMP->GetWorldPos(); // Backproject using corrected camera // 8.3 根据优化后的位姿转到世界坐标系下作为这个点优化后的三维坐标 pMP->SetWorldPos(pRefKF->GetPoseInverse() * Xc); } } Verbose::PrintMess("Map updated!", Verbose::VERBOSITY_NORMAL); mnKFs=vpKF.size(); mIdxInit++; // 9. 再有新的来就不要了~不然陷入无限套娃了 for(list<KeyFrame*>::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++) { (*lit)->SetBadFlag(); delete *lit; } mlNewKeyFrames.clear(); mpTracker->mState=Tracking::OK; bInitializing = false; mpCurrentKeyFrame->GetMap()->IncreaseChangeIndex(); return; }
2.3 Map::ApplyScaledRotation解析
// 恢复尺度及重力方向,设置关键帧的坐标(重力2相机) 设置地图点的坐标(世界坐标系2重力坐标系) 设置速度(世界坐标系2重力坐标系) /** imu在localmapping中初始化，LocalMapping::InitializeIMU中使用，误差包含三个残差与两个偏置 * mpAtlas->GetCurrentMap()->ApplyScaledRotation(Twg, mScale, true); * 地图融合时也会使用 * @param R 初始化时为Rgw * @param s 尺度 * @param bScaledVel 将尺度更新到速度 * @param t 默认cv::Mat::zeros(cv::Size(1,3),CV_32F),缺省值默认为0,0,0 */ void Map::ApplyScaledRotation(const Sophus::SE3f &T, const float s, const bool bScaledVel) { unique_lock<mutex> lock(mMutexMap); // Body position (IMU) of first keyframe is fixed to (0,0,0) // y可以理解为带重力方向的坐标系 Loopclosing : T是指由当前帧坐标系到融合帧的变换 Sophus::SE3f Tyw = T; Eigen::Matrix3f Ryw = Tyw.rotationMatrix(); Eigen::Vector3f tyw = Tyw.translation(); // 取出当前地图所有关键帧 for (set<KeyFrame *>::iterator sit = mspKeyFrames.begin(); sit != mspKeyFrames.end(); sit++) { // 更新关键帧位姿 /** * | Rw2w1 tw2w1 | * | Rw1c s*tw1c | = | Rw2c s*Rw2w1*tw1c + tw2w1 | * | 0 1 | | 0 1 | | 0 1 | * 这么做比正常乘在旋转上少了个s，后面不需要这个s了，因为所有mp在下面已经全部转到了w2坐标系下，不存在尺度变化了 * * | s*Rw2w1 tw2w1 | * | Rw1c tw1c | = | s*Rw2c s*Rw2w1*tw1c + tw2w1 | * | 0 1 | | 0 1 | | 0 1 | */ KeyFrame * pKF = *sit; // 取出相机坐标系向世界坐标系1的变换 Sophus::SE3f Twc = pKF->GetPoseInverse(); Twc.translation() *= s; // | Ryc s*Ryw*twc + tyw | // | 0 1 | // Loopclosing : 相机位姿 -> 世界坐标系1 -> 世界坐标系2 即把左图的帧向世界坐标系2投影 Sophus::SE3f Tyc = Tyw * Twc; // 世界坐标系2下的相机坐标 Sophus::SE3f Tcy = Tyc.inverse(); // 重力坐标系向相机坐标系的转换 pKF->SetPose(Tcy); // 更新关键帧速度,将视觉坐标系变换成重力坐标系乘以尺度 Eigen::Vector3f Vw = pKF->GetVelocity(); if (!bScaledVel) pKF->SetVelocity(Ryw * Vw); else pKF->SetVelocity(Ryw * Vw * s); } // mspMapPoints存储着所有的三维点 for (set<MapPoint *>::iterator sit = mspMapPoints.begin(); sit != mspMapPoints.end(); sit++) { // 更新每一个mp在世界坐标系下的坐标 MapPoint *pMP = *sit; if (!pMP || pMP->isBad()) continue; // 跟重力对齐, pMP->SetWorldPos(s * Ryw * pMP->GetWorldPos() + tyw); pMP->UpdateNormalAndDepth(); } mnMapChange++; }在IMU初始化调用时,Tyc = Tyw * Twc 相机 -> 世界坐标 -> 重力坐标系 Tcy 表示相机在重力坐标系下的坐标 原本是在世界坐标系下的坐标，为了平衡重力我们将世界坐标系做了一个偏移。
也就是说，这把世界坐标系给弄没了......以后我们所有的帧所在的坐标系都是参考我们算出来的重力坐标系而言的。
在这个函数中，我们更新所有相机的位姿和地图点的坐标。使其在重力坐标下。
2.4 Tracking::UpdateFrameIMU解析
解释几个变量：
mlRelativeFramePoses：参考帧（关键帧）到当前帧的平移矩阵
。
mlpReference：当前帧的参考关键帧。
由于在后端优化的时候我们只是对关键帧进行位姿更新，对于普通帧就享受不到更新所带来的位姿精度的提升。
void Tracking::UpdateLastFrame() { // Update pose according to reference keyframe // Step 1：利用参考关键帧更新上一帧在世界坐标系下的位姿 // 上一普通帧的参考关键帧，注意这里用的是参考关键帧（位姿准）而不是上上一帧的普通帧 KeyFrame* pRef = mLastFrame.mpReferenceKF; // ref_keyframe 到 lastframe的位姿变换 Sophus::SE3f Tlr = mlRelativeFramePoses.back(); // 将上一帧的世界坐标系下的位姿计算出来 // l:last, r:reference, w:world // Tlw = Tlr*Trw mLastFrame.SetPose(Tlr * pRef->GetPose());比如在这块，我们把关键帧到普通帧的位姿保存起来，当在后段关键帧位姿更新后，可以根据上述这种方式得到普通帧的更新后的旋转平移矩阵。
这个函数的主要作用是更新当前帧和上一帧的R，t，速度信息：
长表示关键帧，短条表示普通帧 其中变量mpImuPreintegrated表示从上一关键帧积分到当前普通帧一段的预积分。
mLastFrame.SetImuPoseVelocity(IMU::NormalizeRotation(Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaRotation()), twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaPosition(), Vwb1 + Gz*t12 + Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaVelocity());这里状态的计算是由上一个关键帧的IMU的状态：
mpImuPreintegrated->GetUpdatedDeltaRotation()取到了这段时间的
，也就是上一段时间的旋转
（上一个关键帧中IMU坐标系在世界坐标系下的坐标） * 这段时间的预积分
3. 尺度更新函数LocalMapping::ScaleRefinement
/** * @brief 通过BA优化进行尺度更新，关键帧小于100，使用了所有关键帧的信息，但只优化尺度和重力方向。每10s在这里的时间段内时多次进行尺度更新 */ void LocalMapping::ScaleRefinement() { // Minimum number of keyframes to compute a solution // Minimum time (seconds) between first and last keyframe to compute a solution. Make the difference between monocular and stereo // unique_lock<mutex> lock0(mMutexImuInit); // 判断是否按按钮reset了 if (mbResetRequested) return; // Retrieve all keyframes in temporal order // 1. 检索所有的关键帧（当前地图） list<KeyFrame*> lpKF; KeyFrame* pKF = mpCurrentKeyFrame; while(pKF->mPrevKF) { lpKF.push_front(pKF); pKF = pKF->mPrevKF; } lpKF.push_front(pKF); vector<KeyFrame*> vpKF(lpKF.begin(),lpKF.end()); // 加入新添加的帧 while(CheckNewKeyFrames()) { ProcessNewKeyFrame(); vpKF.push_back(mpCurrentKeyFrame); lpKF.push_back(mpCurrentKeyFrame); } const int N = vpKF.size(); // 2. 更新旋转与尺度 // 待优化变量的初值 : mRwg mRwg = Eigen::Matrix3d::Identity(); mScale=1.0; std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now(); // 优化重力方向与尺度 Optimizer::InertialOptimization(mpAtlas->GetCurrentMap(), mRwg, mScale); std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now(); if (mScale<1e-1) // 1e-1 { cout << "scale too small" << endl; bInitializing=false; return; } Sophus::SO3d so3wg(mRwg); // Before this line we are not changing the map // 3. 开始更新地图 unique_lock<mutex> lock(mpAtlas->GetCurrentMap()->mMutexMapUpdate); std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now(); // 3.1 如果尺度更新较多，或是在双目imu情况下更新地图 // 0.4版本这个值为0.00001 if ((fabs(mScale-1.f)>0.002)||!mbMonocular) { Sophus::SE3f Tgw(mRwg.cast<float>().transpose(),Eigen::Vector3f::Zero()); mpAtlas->GetCurrentMap()->ApplyScaledRotation(Tgw,mScale,true); mpTracker->UpdateFrameIMU(mScale,mpCurrentKeyFrame->GetImuBias(),mpCurrentKeyFrame); } std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now(); // 3.2 优化的这段时间新进来的kf全部清空不要 for(list<KeyFrame*>::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++) { (*lit)->SetBadFlag(); delete *lit; } mlNewKeyFrames.clear(); double t_inertial_only = std::chrono::duration_cast<std::chrono::duration<double> >(t1 - t0).count(); // To perform pose-inertial opt w.r.t. last keyframe mpCurrentKeyFrame->GetMap()->IncreaseChangeIndex(); return; }与前面重复不再赘述。