12345qiupeng
/
orb_slam3_details
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix

master
electech6 2022-04-14 23:07:08 +08:00
parent 5609871832
commit 81c07bfe77
6 changed files with 18 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
include/ImuTypes.h
View File

@ -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<float,6,1> db;

12
src/G2oTypes.cc
View File

@ -579,12 +579,12 @@ EdgeInertial::EdgeInertial(IMU::Preintegrated *pInt):JRg(pInt->JRg.cast<double>(
void EdgeInertial::computeError()
{
// TODO Maybe Reintegrate inertial measurments when difference between linearization point and current estimate is too big
const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]);
const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]);
const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]);
const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]);
const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]);
const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_vertices[5]);
const VertexPose* VP1 = static_cast<const VertexPose*>(_vertices[0]); //位姿Ti
const VertexVelocity* VV1= static_cast<const VertexVelocity*>(_vertices[1]); //速度vi
const VertexGyroBias* VG1= static_cast<const VertexGyroBias*>(_vertices[2]); //零偏Bgi
const VertexAccBias* VA1= static_cast<const VertexAccBias*>(_vertices[3]); //零偏Bai
const VertexPose* VP2 = static_cast<const VertexPose*>(_vertices[4]); //位姿Tj
const VertexVelocity* VV2 = static_cast<const VertexVelocity*>(_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<double>();
const Eigen::Vector3d dV = mpInt->GetDeltaVelocity(b1).cast<double>();

2
src/ImuTypes.cc
View File

@ -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)
{

4
src/LocalMapping.cc
View File

@ -1547,10 +1547,8 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
pKF = pKF->mPrevKF;
}
lpKF.push_front(pKF);
// 以相同内容再构建一个vector
// 同样内容再构建一个和lpKF一样的容器vpKF
vector<KeyFrame*> vpKF(lpKF.begin(),lpKF.end());
// TODO 跟上面重复？
if(vpKF.size()<nMinKF)
return;

13
src/LoopClosing.cc
View File

@ -506,7 +506,8 @@ bool LoopClosing::NewDetectCommonRegions()
//Merge candidates
bool bMergeDetectedInKF = false;
// Step 3.2 融合的时序几何校验: 注意初始化时mnMergeNumCoincidences=0, 所以可以先跳过看后面
// 如果成功验证总次数大于0
// mnMergeNumCoincidences表示成功校验总次数，初始化为0
// 会先经过后面共视几何校验，如果小于3，会进到如下判断开始时序几何校验
if(mnMergeNumCoincidences > 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)

2
src/Tracking.cc
View File

@ -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());
}
}