From 2f4447471564d2aecb0ca46a15713c5dfe071a08 Mon Sep 17 00:00:00 2001 From: ZhouJiafen Date: Tue, 29 Dec 2020 23:55:04 -0800 Subject: [PATCH] =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E8=B7=9F=E8=B8=AA=E6=97=B6IM?= =?UTF-8?q?U=E6=95=B0=E6=8D=AE=E9=A2=84=E6=B5=8B=E4=BD=8D=E5=A7=BF?= =?UTF-8?q?=E7=9A=84=E6=B3=A8=E9=87=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/Frame.cc | 11 ++++++++++- src/ImuTypes.cc | 6 ++++++ src/Tracking.cc | 21 +++++++++++++++++---- 3 files changed, 33 insertions(+), 5 deletions(-) diff --git a/src/Frame.cc b/src/Frame.cc index 70d2c2c..562453e 100644 --- a/src/Frame.cc +++ b/src/Frame.cc @@ -488,15 +488,24 @@ void Frame::SetVelocity(const cv::Mat &Vwb) { mVw = Vwb.clone(); } - +/** + * @brief 设置IMU的速度预测位姿，最后用imu到相机的位姿和世界坐标系到imu的位姿更新相机和世界坐标系的位姿 + * 目的：得到相机在世界坐标系的位姿 + * + * @param[in] Rwb 旋转值 + * @param[in] twb 位移值 + * @param[in] Vwb 速度值 + */ void Frame::SetImuPoseVelocity(const cv::Mat &Rwb, const cv::Mat &twb, const cv::Mat &Vwb) { + //Twb -> Tbw 求逆的过程，根据旋转矩阵的逆求得，参见十四讲（第一版）P44公式（3.13） mVw = Vwb.clone(); cv::Mat Rbw = Rwb.t(); cv::Mat tbw = -Rbw*twb; cv::Mat Tbw = cv::Mat::eye(4,4,CV_32F); Rbw.copyTo(Tbw.rowRange(0,3).colRange(0,3)); tbw.copyTo(Tbw.rowRange(0,3).col(3)); + // 外参值mImuCalib.Tcb与Tbw求mTcw mTcw = mImuCalib.Tcb*Tbw; UpdatePoseMatrices(); } diff --git a/src/ImuTypes.cc b/src/ImuTypes.cc index d90d609..2df1a32 100644 --- a/src/ImuTypes.cc +++ b/src/ImuTypes.cc @@ -343,6 +343,7 @@ void Preintegrated::IntegrateNewMeasurement(const cv::Point3f &acceleration, con // 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.t()*JRg - dRi.rightJ*dt; // Total integrated time @@ -398,7 +399,10 @@ IMU::Bias Preintegrated::GetDeltaBias(const Bias &b_) cv::Mat Preintegrated::GetDeltaRotation(const Bias &b_) { std::unique_lock lock(mMutex); + // ? 计算偏置的变化量 cv::Mat dbg = (cv::Mat_(3,1) << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz); + // ? 考虑偏置后，dR对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13～P14 + // ? Forster论文公式（44）yP17也有结果（但没有推导），后面两个函数GetDeltaPosition和GetDeltaPosition也是基于此推导的 return NormalizeRotation(dR*ExpSO3(JRg*dbg)); } @@ -407,6 +411,7 @@ cv::Mat Preintegrated::GetDeltaVelocity(const Bias &b_) std::unique_lock lock(mMutex); cv::Mat dbg = (cv::Mat_(3,1) << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz); cv::Mat dba = (cv::Mat_(3,1) << b_.bax-b.bax,b_.bay-b.bay,b_.baz-b.baz); + // ? 考虑偏置后，dV对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13，JPg和JPa在预积分处理中更新 return dV + JVg*dbg + JVa*dba; } @@ -415,6 +420,7 @@ cv::Mat Preintegrated::GetDeltaPosition(const Bias &b_) std::unique_lock lock(mMutex); cv::Mat dbg = (cv::Mat_(3,1) << b_.bwx-b.bwx,b_.bwy-b.bwy,b_.bwz-b.bwz); cv::Mat dba = (cv::Mat_(3,1) << b_.bax-b.bax,b_.bay-b.bay,b_.baz-b.baz); + // ? 考虑偏置后，dP对偏置线性化的近似求解,邱笑晨《预积分总结与公式推导》P13，JPg和JPa在预积分处理中更新 return dP + JPg*dbg + JPa*dba; } diff --git a/src/Tracking.cc b/src/Tracking.cc index ee0dc2a..7170746 100644 --- a/src/Tracking.cc +++ b/src/Tracking.cc @@ -1276,7 +1276,12 @@ void Tracking::PreintegrateIMU() Verbose::PrintMess("Preintegration is finished!! ", Verbose::VERBOSITY_DEBUG); } - +/** + * @brief 跟踪不成功的时候，用初始化好的imu数据做跟踪处理 + * + * @return true + * @return false + */ bool Tracking::PredictStateIMU() { if(!mCurrentFrame.mpPrevFrame) @@ -1284,20 +1289,26 @@ bool Tracking::PredictStateIMU() Verbose::PrintMess("No last frame", Verbose::VERBOSITY_NORMAL); return false; } - + // 地图更新时，并且上一个图像关键帧存在 if(mbMapUpdated && mpLastKeyFrame) - { + { const cv::Mat twb1 = mpLastKeyFrame->GetImuPosition(); const cv::Mat Rwb1 = mpLastKeyFrame->GetImuRotation(); const cv::Mat Vwb1 = mpLastKeyFrame->GetVelocity(); const cv::Mat Gz = (cv::Mat_(3,1) << 0,0,-IMU::GRAVITY_VALUE); + // ? mpImuPreintegratedFromLastKF是将图像帧转成imu格式的？ const float t12 = mpImuPreintegratedFromLastKF->dT; - + // 计算当前帧在世界坐标系的位姿,原理都是用预积分的位姿（预积分的值不会变化）与上一帧的位姿（会迭代变化）进行更新 + // 旋转 R_wb2 = R_wb1 * R_b1b2 cv::Mat Rwb2 = IMU::NormalizeRotation(Rwb1*mpImuPreintegratedFromLastKF->GetDeltaRotation(mpLastKeyFrame->GetImuBias())); + // 位移 cv::Mat twb2 = twb1 + Vwb1*t12 + 0.5f*t12*t12*Gz+ Rwb1*mpImuPreintegratedFromLastKF->GetDeltaPosition(mpLastKeyFrame->GetImuBias()); + // 速度 cv::Mat Vwb2 = Vwb1 + t12*Gz + Rwb1*mpImuPreintegratedFromLastKF->GetDeltaVelocity(mpLastKeyFrame->GetImuBias()); + // 设置当前帧的世界坐标系的相机位姿 mCurrentFrame.SetImuPoseVelocity(Rwb2,twb2,Vwb2); + // ? 预测当前帧imu在世界坐标系的位姿，以及记录偏置bias和bias更新 mCurrentFrame.mPredRwb = Rwb2.clone(); mCurrentFrame.mPredtwb = twb2.clone(); mCurrentFrame.mPredVwb = Vwb2.clone(); @@ -1305,12 +1316,14 @@ bool Tracking::PredictStateIMU() mCurrentFrame.mPredBias = mCurrentFrame.mImuBias; return true; } + // 地图未更新时 else if(!mbMapUpdated) { const cv::Mat twb1 = mLastFrame.GetImuPosition(); const cv::Mat Rwb1 = mLastFrame.GetImuRotation(); const cv::Mat Vwb1 = mLastFrame.mVw; const cv::Mat Gz = (cv::Mat_(3,1) << 0,0,-IMU::GRAVITY_VALUE); + // ? mpImuPreintegratedFrame是当前帧上一帧，未必是关键帧的图像帧？ const float t12 = mCurrentFrame.mpImuPreintegratedFrame->dT; cv::Mat Rwb2 = IMU::NormalizeRotation(Rwb1*mCurrentFrame.mpImuPreintegratedFrame->GetDeltaRotation(mLastFrame.mImuBias));