diff --git a/include/ImuTypes.h b/include/ImuTypes.h index ac78d36..79d9e4a 100644 --- a/include/ImuTypes.h +++ b/include/ImuTypes.h @@ -223,7 +223,7 @@ public: private: // Updated bias - Bias bu; + Bias bu; //更新后的零偏 // Dif between original and updated bias // This is used to compute the updated values of the preintegration Eigen::Matrix db; diff --git a/src/G2oTypes.cc b/src/G2oTypes.cc index 38bd286..34f39c0 100644 --- a/src/G2oTypes.cc +++ b/src/G2oTypes.cc @@ -579,12 +579,12 @@ EdgeInertial::EdgeInertial(IMU::Preintegrated *pInt):JRg(pInt->JRg.cast( void EdgeInertial::computeError() { // TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big - const VertexPose* VP1 = static_cast(_vertices[0]); - const VertexVelocity* VV1= static_cast(_vertices[1]); - const VertexGyroBias* VG1= static_cast(_vertices[2]); - const VertexAccBias* VA1= static_cast(_vertices[3]); - const VertexPose* VP2 = static_cast(_vertices[4]); - const VertexVelocity* VV2 = static_cast(_vertices[5]); + const VertexPose* VP1 = static_cast(_vertices[0]); //位姿Ti + const VertexVelocity* VV1= static_cast(_vertices[1]); //速度vi + const VertexGyroBias* VG1= static_cast(_vertices[2]); //零偏Bgi + const VertexAccBias* VA1= static_cast(_vertices[3]); //零偏Bai + const VertexPose* VP2 = static_cast(_vertices[4]); //位姿Tj + const VertexVelocity* VV2 = static_cast(_vertices[5]); //速度vj const IMU::Bias b1(VA1->estimate()[0],VA1->estimate()[1],VA1->estimate()[2],VG1->estimate()[0],VG1->estimate()[1],VG1->estimate()[2]); const Eigen::Matrix3d dR = mpInt->GetDeltaRotation(b1).cast(); const Eigen::Vector3d dV = mpInt->GetDeltaVelocity(b1).cast(); diff --git a/src/ImuTypes.cc b/src/ImuTypes.cc index 6dd0a93..0656edf 100644 --- a/src/ImuTypes.cc +++ b/src/ImuTypes.cc @@ -242,7 +242,7 @@ void Preintegrated::Reintegrate() * * @param[in] acceleration 加速度计数据 * @param[in] angVel 陀螺仪数据 - * @param[in] dt 两帧之间时间差 + * @param[in] dt 两图像 帧之间时间差 */ void Preintegrated::IntegrateNewMeasurement(const Eigen::Vector3f &acceleration, const Eigen::Vector3f &angVel, const float &dt) { diff --git a/src/LocalMapping.cc b/src/LocalMapping.cc index 0fb31b7..a2e3455 100644 --- a/src/LocalMapping.cc +++ b/src/LocalMapping.cc @@ -1547,10 +1547,8 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA) pKF = pKF->mPrevKF; } lpKF.push_front(pKF); - // 以相同内容再构建一个vector + // 同样内容再构建一个和lpKF一样的容器vpKF vector vpKF(lpKF.begin(),lpKF.end()); - - // TODO 跟上面重复? if(vpKF.size() 0) { // Find from the last KF candidates @@ -535,7 +536,7 @@ bool LoopClosing::NewDetectCommonRegions() mpMergeLastCurrentKF = mpCurrentKF; mg2oMergeSlw = gScw; mvpMergeMatchedMPs = vpMatchedMPs; - // 如果验证数大于等于3则为成功回环 + // 如果验证数大于等于3则为成功 mbMergeDetected = mnMergeNumCoincidences >= 3; } // 如果没找到共同区域(时序验证失败一次) @@ -1550,8 +1551,8 @@ void LoopClosing::CorrectLoop() mpAtlas->InformNewBigChange(); // Add loop edge - // Step 8. 添加当前帧与闭环匹配帧之间的边(这个连接关系不优化) - // !感觉这两句话应该放在OptimizeEssentialGraph之前,因为OptimizeEssentialGraph的步骤4.2中有优化 + // Step 7:添加当前帧与闭环匹配帧之间的边(这个连接关系不优化) + // 它在下一次的Essential Graph里面使用 mpLoopMatchedKF->AddLoopEdge(mpCurrentKF); mpCurrentKF->AddLoopEdge(mpLoopMatchedKF); @@ -2112,8 +2113,8 @@ void LoopClosing::MergeLocal() bool bStop = false; // 为Local BA的接口, 把set转为vector // Step 7 在缝合(Welding)区域进行local BA - vpLocalCurrentWindowKFs.clear(); - vpMergeConnectedKFs.clear(); + vpLocalCurrentWindowKFs.clear(); //当前关键帧的窗口 + vpMergeConnectedKFs.clear(); //融合关键帧的窗口 std::copy(spLocalWindowKFs.begin(), spLocalWindowKFs.end(), std::back_inserter(vpLocalCurrentWindowKFs)); std::copy(spMergeConnectedKFs.begin(), spMergeConnectedKFs.end(), std::back_inserter(vpMergeConnectedKFs)); if (mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) diff --git a/src/Tracking.cc b/src/Tracking.cc index 56b206b..36014ab 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -3515,11 +3515,13 @@ bool Tracking::TrackLocalMap() if(!mbMapUpdated) // && (mnMatchesInliers>30)) { Verbose::PrintMess("TLM: PoseInertialOptimizationLastFrame ", Verbose::VERBOSITY_DEBUG); + // 使用上一普通帧以及当前帧的视觉信息和IMU信息联合优化当前帧位姿、速度和IMU零偏 inliers = Optimizer::PoseInertialOptimizationLastFrame(&mCurrentFrame); // , !mpLastKeyFrame->GetMap()->GetIniertialBA1()); } else { Verbose::PrintMess("TLM: PoseInertialOptimizationLastKeyFrame ", Verbose::VERBOSITY_DEBUG); + // 使用上一关键帧以及当前帧的视觉信息和IMU信息联合优化当前帧位姿、速度和IMU零偏 inliers = Optimizer::PoseInertialOptimizationLastKeyFrame(&mCurrentFrame); // , !mpLastKeyFrame->GetMap()->GetIniertialBA1()); } }