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