From 8dcefb55673d7ada72caef1822789d72e120c6ef Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E5=B0=8F=E7=99=BD=E9=80=86=E8=A2=AD?= <741299292@qq.com> Date: Tue, 26 Oct 2021 23:30:10 +0800 Subject: [PATCH] =?UTF-8?q?=E6=9B=B4=E6=96=B0=E6=A0=87=E6=B3=A8?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- include/G2oTypes.h | 104 +++++++++++++++++++++++++++++++++++++------- src/G2oTypes.cc | 14 ++++-- src/LocalMapping.cc | 17 +++++--- src/Map.cc | 30 ++++++++++++- src/Optimizer.cc | 3 +- src/Tracking.cc | 30 ++++++++++--- 6 files changed, 163 insertions(+), 35 deletions(-) diff --git a/include/G2oTypes.h b/include/G2oTypes.h index f132b11..98428ad 100644 --- a/include/G2oTypes.h +++ b/include/G2oTypes.h @@ -67,7 +67,7 @@ Eigen::Matrix3d InverseRightJacobianSO3(const double x, const double y, const do Eigen::Matrix3d NormalizeRotation(const Eigen::Matrix3d &R); - +// 相关节点中使用，存放的是imu与cam的内外参 class ImuCamPose { public: @@ -103,9 +103,10 @@ public: Eigen::Matrix3d Rwb0; Eigen::Matrix3d DR; - int its; + int its; // 记录更新次数 }; +// 逆深度点，后面节点虽然有用到，但是声明的节点并没有使用到，暂时不看 class InvDepthPoint { public: @@ -124,6 +125,7 @@ public: }; // Optimizable parameters are IMU pose +// 优化中关于位姿的节点，6自由度 class VertexPose : public g2o::BaseVertex<6,ImuCamPose> { public: @@ -140,15 +142,21 @@ public: virtual bool read(std::istream& is); virtual bool write(std::ostream& os) const; + // 重置函数,设定被优化变量的原始值 virtual void setToOriginImpl() { } + // 更新 virtual void oplusImpl(const double* update_){ _estimate.Update(update_); + // https://github.com/RainerKuemmerle/g2o/blob/master/doc/README_IF_IT_WAS_WORKING_AND_IT_DOES_NOT.txt + // 官方讲解cache + // 需要在oplusImpl与setEstimate函数中添加 updateCache(); } }; +// 优化中关于位姿的节点，4自由度 3个平移加一个航偏 class VertexPose4DoF : public g2o::BaseVertex<4,ImuCamPose> { // Translation and yaw are the only optimizable variables @@ -172,6 +180,7 @@ public: virtual void setToOriginImpl() { } + // 强制让旋转的前两维的更新量为0 virtual void oplusImpl(const double* update_){ double update6DoF[6]; update6DoF[0] = 0; @@ -185,6 +194,9 @@ public: } }; +/** + * @brief 速度节点 + */ class VertexVelocity : public g2o::BaseVertex<3,Eigen::Vector3d> { public: @@ -206,6 +218,9 @@ public: } }; +/** + * @brief 陀螺仪偏置节点 + */ class VertexGyroBias : public g2o::BaseVertex<3,Eigen::Vector3d> { public: @@ -227,7 +242,9 @@ public: } }; - +/** + * @brief 加速度计偏置节点 + */ class VertexAccBias : public g2o::BaseVertex<3,Eigen::Vector3d> { public: @@ -251,6 +268,7 @@ public: // Gravity direction vertex +// 重力方向 class GDirection { public: @@ -268,6 +286,9 @@ public: int its; }; +/** + * @brief 重力方向节点 + */ class VertexGDir : public g2o::BaseVertex<2,GDirection> { public: @@ -290,6 +311,9 @@ public: }; // scale vertex +/** + * @brief 尺度节点 + */ class VertexScale : public g2o::BaseVertex<1,double> { public: @@ -315,6 +339,9 @@ public: // Inverse depth point (just one parameter, inverse depth at the host frame) +/** + * @brief 没用 + */ class VertexInvDepth : public g2o::BaseVertex<1,InvDepthPoint> { public: @@ -337,12 +364,13 @@ public: }; /** - * @brief 视觉重投影的边 + * @brief 单目视觉重投影的边 * 这里或许会有疑问，OptimizableTypes.h 里面不是定义了视觉重投影的边么？ * 原因是这样，那里面定义的边也用，不过只是在纯视觉时，没有imu情况下，因为用已经定义好的节点就好 * 但是加入imu时，优化要有残差的边与重投影的边同时存在，且两个边可能连接同一个位姿节点，所以需要重新弄一个包含imu位姿的节点 * 因此，边也需要重新写，并且在imu优化时使用这个边 */ +// 误差为2维， 类型为Eigen::Vector2d， 节点1类型为g2o::VertexSBAPointXYZ，节点二类型为VertexPose class EdgeMono : public g2o::BaseBinaryEdge<2,Eigen::Vector2d,g2o::VertexSBAPointXYZ,VertexPose> { public: @@ -354,11 +382,12 @@ public: virtual bool read(std::istream& is){return false;} virtual bool write(std::ostream& os) const{return false;} + // 计算重投影误差 void computeError(){ const g2o::VertexSBAPointXYZ* VPoint = static_cast(_vertices[0]); const VertexPose* VPose = static_cast(_vertices[1]); const Eigen::Vector2d obs(_measurement); - _error = obs - VPose->estimate().Project(VPoint->estimate(),cam_idx); + _error = obs - VPose->estimate().Project(VPoint->estimate(),cam_idx); // 投影 } @@ -379,6 +408,7 @@ public: return J; } + // 由2*2的像素点信息矩阵变成了9*9的关于旋转平移与三维点坐标的信息矩阵 Eigen::Matrix GetHessian(){ linearizeOplus(); Eigen::Matrix J; @@ -391,6 +421,9 @@ public: const int cam_idx; }; +/** + * @brief 单目纯位姿一元边 + */ class EdgeMonoOnlyPose : public g2o::BaseUnaryEdge<2,Eigen::Vector2d,VertexPose> { public: @@ -426,11 +459,15 @@ public: const int cam_idx; }; +/** + * @brief 双目位姿三维点二元边 + */ class EdgeStereo : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,g2o::VertexSBAPointXYZ,VertexPose> { public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // 代码中都是0 EdgeStereo(int cam_idx_=0): cam_idx(cam_idx_){} virtual bool read(std::istream& is){return false;} @@ -466,7 +503,9 @@ public: const int cam_idx; }; - +/** + * @brief 双目纯位姿一元边 + */ class EdgeStereoOnlyPose : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexPose> { public: @@ -497,6 +536,9 @@ public: }; // TO COMMENT +/** + * @brief 惯性边（误差为残差） + */ class EdgeInertial : public g2o::BaseMultiEdge<9,Vector9d> { public: @@ -510,6 +552,7 @@ public: void computeError(); virtual void linearizeOplus(); + // 关于pose1与2 的旋转平移速度，以及之间的偏置的信息矩阵 Eigen::Matrix GetHessian(){ linearizeOplus(); Eigen::Matrix J; @@ -522,6 +565,7 @@ public: return J.transpose()*information()*J; } + // 没用 Eigen::Matrix GetHessianNoPose1(){ linearizeOplus(); Eigen::Matrix J; @@ -533,6 +577,7 @@ public: return J.transpose()*information()*J; } + // 关于pose2 的旋转平移信息矩阵 Eigen::Matrix GetHessian2(){ linearizeOplus(); Eigen::Matrix J; @@ -541,14 +586,18 @@ public: return J.transpose()*information()*J; } + // 预积分中对应的状态对偏置的雅可比 const Eigen::Matrix3d JRg, JVg, JPg; const Eigen::Matrix3d JVa, JPa; - IMU::Preintegrated* mpInt; - const double dt; - Eigen::Vector3d g; + + IMU::Preintegrated* mpInt; // 预积分 + const double dt; // 预积分时间 + Eigen::Vector3d g; // 0, 0, -IMU::GRAVITY_VALUE }; - +/** + * @brief 初始化惯性边（误差为残差） + */ // Edge inertial whre gravity is included as optimizable variable and it is not supposed to be pointing in -z axis, as well as scale class EdgeInertialGS : public g2o::BaseMultiEdge<9,Vector9d> { @@ -570,6 +619,7 @@ public: const double dt; Eigen::Vector3d g, gI; + // 关于pose1与2 的旋转平移速度，以及之间的偏置，还有重力方向与尺度的信息矩阵 Eigen::Matrix GetHessian(){ linearizeOplus(); Eigen::Matrix J; @@ -584,6 +634,7 @@ public: return J.transpose()*information()*J; } + // 与上面摆放不同 Eigen::Matrix GetHessian2(){ linearizeOplus(); Eigen::Matrix J; @@ -598,6 +649,7 @@ public: return J.transpose()*information()*J; } + // 关于偏置，重力方向与尺度的信息矩阵 Eigen::Matrix GetHessian3(){ linearizeOplus(); Eigen::Matrix J; @@ -609,7 +661,7 @@ public: } - + // 下面的没有用到，其实也是获取状态的信息矩阵 Eigen::Matrix GetHessianScale(){ linearizeOplus(); Eigen::Matrix J = _jacobianOplus[7]; @@ -636,7 +688,9 @@ public: }; - +/** + * @brief 陀螺仪偏置的二元边，除了残差及重投影误差外的第三个边，控制偏置变化 + */ class EdgeGyroRW : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,VertexGyroBias,VertexGyroBias> { public: @@ -672,7 +726,9 @@ public: } }; - +/** + * @brief 加速度计偏置的二元边，除了残差及重投影误差外的第三个边，控制偏置变化 + */ class EdgeAccRW : public g2o::BaseBinaryEdge<3,Eigen::Vector3d,VertexAccBias,VertexAccBias> { public: @@ -708,6 +764,9 @@ public: } }; +/** + * @brief 先验类 + */ class ConstraintPoseImu { public: @@ -717,7 +776,8 @@ public: const Eigen::Vector3d &bg_, const Eigen::Vector3d &ba_, const Matrix15d &H_): Rwb(Rwb_), twb(twb_), vwb(vwb_), bg(bg_), ba(ba_), H(H_) { - H = (H+H)/2; + H = (H+H)/2; // 对称化 + // 令特征值小的位置变为0 Eigen::SelfAdjointEigenSolver > es(H); Eigen::Matrix eigs = es.eigenvalues(); for(int i=0;i<15;i++) @@ -725,6 +785,8 @@ public: eigs[i]=0; H = es.eigenvectors()*eigs.asDiagonal()*es.eigenvectors().transpose(); } + + // 形式不同 ConstraintPoseImu(const cv::Mat &Rwb_, const cv::Mat &twb_, const cv::Mat &vwb_, const IMU::Bias &b, const cv::Mat &H_) { @@ -753,6 +815,9 @@ public: Matrix15d H; }; +/** + * @brief 先验边，前端优化单帧用到 + */ class EdgePriorPoseImu : public g2o::BaseMultiEdge<15,Vector15d> { public: @@ -775,6 +840,7 @@ public: return J.transpose()*information()*J; } + // 没用到，除了旋转平移外的信息矩阵 Eigen::Matrix GetHessianNoPose(){ linearizeOplus(); Eigen::Matrix J; @@ -789,6 +855,9 @@ public: }; // Priors for biases +/** + * @brief 根据给定值的加速度计先验边，目的是把优化量维持在先验附近 + */ class EdgePriorAcc : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexAccBias> { public: @@ -813,6 +882,9 @@ public: const Eigen::Vector3d bprior; }; +/** + * @brief 根据给定值的陀螺仪先验边，目的是把优化量维持在先验附近 + */ class EdgePriorGyro : public g2o::BaseUnaryEdge<3,Eigen::Vector3d,VertexGyroBias> { public: @@ -837,7 +909,9 @@ public: const Eigen::Vector3d bprior; }; - +/** + * @brief 4DOF的二元边，误差为给定的旋转平移改变量 与 两个输入节点之间的旋转平移改变量的偏差 + */ class Edge4DoF : public g2o::BaseBinaryEdge<6,Vector6d,VertexPose4DoF,VertexPose4DoF> { public: diff --git a/src/G2oTypes.cc b/src/G2oTypes.cc index 1a52da8..19933a1 100644 --- a/src/G2oTypes.cc +++ b/src/G2oTypes.cc @@ -167,6 +167,9 @@ void ImuCamPose::SetParam(const std::vector &_Rcw, const std::v bf = _bf; } +/** + * @brief 单目投影 + */ Eigen::Vector2d ImuCamPose::Project(const Eigen::Vector3d &Xw, int cam_idx) const { Eigen::Vector3d Xc = Rcw[cam_idx]*Xw+tcw[cam_idx]; @@ -174,6 +177,10 @@ Eigen::Vector2d ImuCamPose::Project(const Eigen::Vector3d &Xw, int cam_idx) cons return pCamera[cam_idx]->project(Xc); } +/** + * @brief 双目投影，都是0 + * @return u v u` + */ Eigen::Vector3d ImuCamPose::ProjectStereo(const Eigen::Vector3d &Xw, int cam_idx) const { Eigen::Vector3d Pc = Rcw[cam_idx]*Xw+tcw[cam_idx]; @@ -219,6 +226,7 @@ void ImuCamPose::Update(const double *pu) } +// 更新世界坐标系 void ImuCamPose::UpdateW(const double *pu) { Eigen::Vector3d ur, ut; @@ -606,7 +614,7 @@ void EdgeInertial::linearizeOplus() _jacobianOplus[3].block<3,3>(6,0) = -JPa; // OK // Jacobians wrt Pose 2 - // _jacobianOplus[3] 9*6矩阵 总体来说就是三个残差分别对pose2的旋转与平移（p）求导 + // _jacobianOplus[4] 9*6矩阵 总体来说就是三个残差分别对pose2的旋转与平移（p）求导 _jacobianOplus[4].setZero(); // rotation _jacobianOplus[4].block<3,3>(0,0) = invJr; // OK @@ -614,7 +622,7 @@ void EdgeInertial::linearizeOplus() _jacobianOplus[4].block<3,3>(6,3) = Rbw1*Rwb2; // OK // Jacobians wrt Velocity 2 - // _jacobianOplus[3] 9*3矩阵 总体来说就是三个残差分别对pose2的速度求导 + // _jacobianOplus[5] 9*3矩阵 总体来说就是三个残差分别对pose2的速度求导 _jacobianOplus[5].setZero(); _jacobianOplus[5].block<3,3>(3,0) = Rbw1; // OK } @@ -930,6 +938,6 @@ Eigen::Matrix3d NormalizeRotation(const Eigen::Matrix3d &R) // 2. 对于行列数不同的矩阵，例如3*4 或者 4*3 矩阵只有3个奇异向量，计算的时候如果是Thin 那么得出的UV矩阵列数只能是3，如果是full那么就是4 // 3. thin会损失一部分数据，但是会加快计算，对于大型矩阵解算方程时，可以用thin加速得到结果 Eigen::JacobiSVD svd(R,Eigen::ComputeFullU | Eigen::ComputeFullV); - return svd.matrixU()*svd.matrixV(); + return svd.matrixU()*svd.matrixV().transpose(); } } diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc index 376761f..7270cf9 100644 --- a/src/LocalMapping.cc +++ b/src/LocalMapping.cc @@ -1600,7 +1600,8 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) mFirstTs=vpKF.front()->mTimeStamp; if(mpCurrentKeyFrame->mTimeStamp-mFirstTsmPrevKF) continue; - // Rwb（imu坐标转到初始化前世界坐标系下的坐标）*更新偏置后的速度，可以理解为在世界坐标系下的速度矢量 + // 初始化时关于速度的预积分定义Ri.t()*(s*Vj - s*Vi - Rwg*g*tij) dirG -= (*itKF)->mPrevKF->GetImuRotation()*(*itKF)->mpImuPreintegrated->GetUpdatedDeltaVelocity(); // 求取实际的速度，位移/时间 cv::Mat _vel = ((*itKF)->GetImuPosition() - (*itKF)->mPrevKF->GetImuPosition())/(*itKF)->mpImuPreintegrated->dT; (*itKF)->SetVelocity(_vel); (*itKF)->mPrevKF->SetVelocity(_vel); } + // dirG = sV1 - sVn + n*Rwg*g*t // 归一化 dirG = dirG/cv::norm(dirG); // 原本的重力方向 cv::Mat gI = (cv::Mat_(3,1) << 0.0f, 0.0f, -1.0f); // 求速度方向与重力方向的角轴 cv::Mat v = gI.cross(dirG); - // 求角轴长度 + // 求角轴模长 const float nv = cv::norm(v); // 求转角大小 const float cosg = gI.dot(dirG); @@ -1664,7 +1666,7 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) mInitTime = mpTracker->mLastFrame.mTimeStamp-vpKF.front()->mTimeStamp; std::chrono::steady_clock::time_point t0 = std::chrono::steady_clock::now(); - // 计算残差及偏置差，优化尺度重力方向及Ri Rj Vi Vj Pi Pj + // 计算残差及偏置差，优化尺度重力方向及速度偏置，偏置先验为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(); @@ -1736,10 +1738,11 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) } - mbNewInit=true; - mnKFs=vpKF.size(); - mIdxInit++; + mbNewInit=true; // 没用到 + mnKFs=vpKF.size(); // 没用到 + mIdxInit++; // 没用到 + // 里面存放的是来到Local线程，但还未处理的关键帧 for(list::iterator lit = mlNewKeyFrames.begin(), lend=mlNewKeyFrames.end(); lit!=lend; lit++) { (*lit)->SetBadFlag(); diff --git a/src/Map.cc b/src/Map.cc index d3193b4..1095883 100644 --- a/src/Map.cc +++ b/src/Map.cc @@ -298,6 +298,14 @@ void Map::RotateMap(const cv::Mat &R) } } +// 恢复尺度及重力方向 +/** imu在localmapping中初始化，LocalMapping::InitializeIMU中使用，误差包含三个残差与两个偏置 + * 地图融合时也会使用 + * @param R 初始化时为Rgw + * @param s 尺度 + * @param bScaledVel 将尺度更新到速度 + * @param t 默认cv::Mat::zeros(cv::Size(1,3),CV_32F) + */ void Map::ApplyScaledRotation(const cv::Mat &R, const float s, const bool bScaledVel, const cv::Mat t) { unique_lock lock(mMutexMap); @@ -310,19 +318,36 @@ void Map::ApplyScaledRotation(const cv::Mat &R, const float s, const bool bScale cv::Mat Tyw = Tyx*Txw; Tyw.rowRange(0,3).col(3) = Tyw.rowRange(0,3).col(3)+t; - cv::Mat Ryw = Tyw.rowRange(0,3).colRange(0,3); - cv::Mat tyw = Tyw.rowRange(0,3).col(3); + // Tyw 中旋转部分等于R，平移部分等于t + // 做了很多操作，到最后还是得出了一个R跟t？感觉上面的操作像是预留了一些东西，比如在世界坐标系与第一帧有一定Rt时，只不过暂时没有用到 + cv::Mat Ryw = Tyw.rowRange(0,3).colRange(0,3); // R + cv::Mat tyw = Tyw.rowRange(0,3).col(3); // t for(set::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; cv::Mat Twc = pKF->GetPoseInverse(); Twc.rowRange(0,3).col(3)*=s; + + // | Ryc s*Ryw*twc + tyw | + // | 0 1 | cv::Mat Tyc = Tyw*Twc; + cv::Mat Tcy = cv::Mat::eye(4,4,CV_32F); Tcy.rowRange(0,3).colRange(0,3) = Tyc.rowRange(0,3).colRange(0,3).t(); Tcy.rowRange(0,3).col(3) = -Tcy.rowRange(0,3).colRange(0,3)*Tyc.rowRange(0,3).col(3); pKF->SetPose(Tcy); + // 更新关键帧速度 cv::Mat Vw = pKF->GetVelocity(); if(!bScaledVel) pKF->SetVelocity(Ryw*Vw); @@ -332,6 +357,7 @@ void Map::ApplyScaledRotation(const cv::Mat &R, const float s, const bool bScale } for(set::iterator sit=mspMapPoints.begin(); sit!=mspMapPoints.end(); sit++) { + // 更新每一个mp在世界坐标系下的坐标 MapPoint* pMP = *sit; pMP->SetWorldPos(s*Ryw*pMP->GetWorldPos()+tyw); pMP->UpdateNormalAndDepth(); diff --git a/src/Optimizer.cc b/src/Optimizer.cc index 74fbfab..e281cac 100644 --- a/src/Optimizer.cc +++ b/src/Optimizer.cc @@ -5069,7 +5069,8 @@ Eigen::MatrixXd Optimizer::Sparsify(const Eigen::MatrixXd &H, const int &start1, } -void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba, bool bMono, Eigen::MatrixXd &covInertial, bool bFixedVel, bool bGauss, float priorG, float priorA) +void Optimizer::InertialOptimization(Map *pMap, Eigen::Matrix3d &Rwg, double &scale, Eigen::Vector3d &bg, Eigen::Vector3d &ba, + bool bMono, Eigen::MatrixXd &covInertial, bool bFixedVel, bool bGauss, float priorG, float priorA) { Verbose::PrintMess("inertial optimization", Verbose::VERBOSITY_NORMAL); int its = 200; // Check number of iterations diff --git a/src/Tracking.cc b/src/Tracking.cc index 1b3dfb3..7679c2d 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -4582,12 +4582,25 @@ void Tracking::InformOnlyTracking(const bool &flag) mbOnlyTracking = flag; } +/** + * @brief 更新了关键帧的位姿，但需要修改普通帧的位姿，因为正常跟踪需要普通帧 + * localmapping中初始化imu中使用，速度的走向（仅在imu模式使用），最开始速度定义于imu初始化时，每个关键帧都根据位移除以时间得到，经过非线性优化保存于KF中. + * 之后使用本函数，让上一帧与当前帧分别与他们对应的上一关键帧做速度叠加得到，后面新的frame速度由上一个帧速度决定，如果使用匀速模型（大多数情况下），通过imu积分更新速度。 + * 新的关键帧继承于对应帧 + * @param s 尺度 + * @param b 初始化后第一帧的偏置 + * @param pCurrentKeyFrame 当前关键帧 + */ void Tracking::UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurrentKeyFrame) { Map * pMap = pCurrentKeyFrame->GetMap(); - unsigned int index = mnFirstFrameId; + unsigned int index = mnFirstFrameId; // unused + + // 每一帧的参考关键帧 list::iterator lRit = mlpReferences.begin(); - list::iterator lbL = mlbLost.begin(); + list::iterator lbL = mlbLost.begin(); // 对应帧是否跟踪丢失 + // mlRelativeFramePoses 存放的是Tcr + // 三个变量一一对应 for(list::iterator lit=mlRelativeFramePoses.begin(),lend=mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lbL++) { if(*lbL) @@ -4605,11 +4618,11 @@ void Tracking::UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurr (*lit).rowRange(0,3).col(3)=(*lit).rowRange(0,3).col(3)*s; } } - + // 设置偏置 mLastBias = b; - + // 设置上一关键帧，如果说mpLastKeyFrame已经是经过添加的新的kf，而pCurrentKeyFrame还是上一个kf，mpLastKeyFrame直接指向之前的kf mpLastKeyFrame = pCurrentKeyFrame; - + // 更新偏置 mLastFrame.SetNewBias(mLastBias); mCurrentFrame.SetNewBias(mLastBias); @@ -4622,10 +4635,12 @@ void Tracking::UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurr while(!mCurrentFrame.imuIsPreintegrated()) { + // 当前帧需要预积分完毕，这段函数实在localmapping里调用的 usleep(500); } - + // frame的mpLastKeyFrame只是用于预积分（imu模式） + // TODO 如果上一帧正好是上一帧的上一关键帧（mLastFrame.mpLastKeyFrame与mLastFrame不可能是一个，可以验证一下） if(mLastFrame.mnId == mLastFrame.mpLastKeyFrame->mnFrameId) { mLastFrame.SetImuPoseVelocity(mLastFrame.mpLastKeyFrame->GetImuRotation(), @@ -4638,12 +4653,13 @@ void Tracking::UpdateFrameIMU(const float s, const IMU::Bias &b, KeyFrame* pCurr Rwb1 = mLastFrame.mpLastKeyFrame->GetImuRotation(); Vwb1 = mLastFrame.mpLastKeyFrame->GetVelocity(); t12 = mLastFrame.mpImuPreintegrated->dT; - + // 根据mLastFrame的上一个关键帧的信息（此时已经经过imu初始化了，所以关键帧的信息都是校正后的）以及imu的预积分重新计算上一帧的位姿 mLastFrame.SetImuPoseVelocity(Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaRotation(), twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaPosition(), Vwb1 + Gz*t12 + Rwb1*mLastFrame.mpImuPreintegrated->GetUpdatedDeltaVelocity()); } + // 当前帧是否做了预积分 if (mCurrentFrame.mpImuPreintegrated) { twb1 = mCurrentFrame.mpLastKeyFrame->GetImuPosition();