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

修改回环bug

master
wuqing 2022-05-28 15:05:22 +08:00
parent 0b0f2d49ed
commit 5d57b1d807
5 changed files with 637 additions and 548 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
Examples/Stereo/KITTI00-02.yaml
View File

@ -57,7 +57,7 @@ Viewer.PointSize: 2.0
Viewer.CameraSize: 0.7 Viewer.CameraSize: 0.7
Viewer.CameraLineWidth: 3.0 Viewer.CameraLineWidth: 3.0
Viewer.ViewpointX: 0.0 Viewer.ViewpointX: 0.0
Viewer.ViewpointY: -100 Viewer.ViewpointY: -100.0
Viewer.ViewpointZ: -0.1 Viewer.ViewpointZ: -0.1
Viewer.ViewpointF: 2000.0 Viewer.ViewpointF: 2000.0

2
include/LoopClosing.h
View File

@ -180,7 +180,7 @@ protected:
std::vector<KeyFrame*> mvpCurrentConnectedKFs; std::vector<KeyFrame*> mvpCurrentConnectedKFs;
std::vector<MapPoint*> mvpCurrentMatchedPoints; std::vector<MapPoint*> mvpCurrentMatchedPoints;
std::vector<MapPoint*> mvpLoopMapPoints; std::vector<MapPoint*> mvpLoopMapPoints;
cv::Mat mScw;
g2o::Sim3 mg2oScw; g2o::Sim3 mg2oScw;
//------- //-------

4
src/CameraModels/KannalaBrandt8.cpp
View File

@ -323,9 +323,11 @@ bool KannalaBrandt8::epipolarConstrain(GeometricCamera *pCamera2, const cv::KeyP
const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12, const float sigmaLevel, const float unc) const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12, const float sigmaLevel, const float unc)
{ {
Eigen::Vector3f p3D; Eigen::Vector3f p3D;
// 用三角化出点并验证的这个过程代替极线验证
return this->TriangulateMatches(pCamera2, kp1, kp2, R12, t12, sigmaLevel, unc, p3D) > 0.0001f; return this->TriangulateMatches(pCamera2, kp1, kp2, R12, t12, sigmaLevel, unc, p3D) > 0.0001f;
} }
// 没用
bool KannalaBrandt8::matchAndtriangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, GeometricCamera *pOther, bool KannalaBrandt8::matchAndtriangulate(const cv::KeyPoint &kp1, const cv::KeyPoint &kp2, GeometricCamera *pOther,
Sophus::SE3f &Tcw1, Sophus::SE3f &Tcw2, Sophus::SE3f &Tcw1, Sophus::SE3f &Tcw2,
const float sigmaLevel1, const float sigmaLevel2, const float sigmaLevel1, const float sigmaLevel2,
@ -439,7 +441,7 @@ float KannalaBrandt8::TriangulateMatches(
const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12, const float sigmaLevel, const Eigen::Matrix3f &R12, const Eigen::Vector3f &t12, const float sigmaLevel,
const float unc, Eigen::Vector3f &p3D) const float unc, Eigen::Vector3f &p3D)
{ {
// 1. 得到对应特征点的相平面坐标 // 1. 得到对应特征点的归一化平面坐标
Eigen::Vector3f r1 = this->unprojectEig(kp1.pt); Eigen::Vector3f r1 = this->unprojectEig(kp1.pt);
Eigen::Vector3f r2 = pCamera2->unprojectEig(kp2.pt); Eigen::Vector3f r2 = pCamera2->unprojectEig(kp2.pt);

7
src/LoopClosing.cc
View File

@ -179,8 +179,8 @@ void LoopClosing::Run()
// If inertial, force only yaw // If inertial, force only yaw
// 如果是imu模式并且完成了初始化,强制将焊接变换的 roll 和 pitch 设为0 // 如果是imu模式并且完成了初始化,强制将焊接变换的 roll 和 pitch 设为0
// 通过物理约束来保证两个坐标轴都是水平的 // 通过物理约束来保证两个坐标轴都是水平的
if ((mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO) && if ((mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) &&
mpCurrentKF->GetMap()->GetIniertialBA1()) // TODO, maybe with GetIniertialBA1 mpCurrentKF->GetMap()->GetIniertialBA1())
{ {
Eigen::Vector3d phi = LogSO3(mSold_new.rotation().toRotationMatrix()); Eigen::Vector3d phi = LogSO3(mSold_new.rotation().toRotationMatrix());
phi(0)=0; phi(0)=0;
@ -693,8 +693,7 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame*
if(numOptMatches > nProjOptMatches) if(numOptMatches > nProjOptMatches)
{ {
//!bug, 以下gScw_estimation应该通过上述sim3优化后的位姿来更新。以下mScw应该改为 gscm * gswm^-1 //!bug, 以下gScw_estimation应该通过上述sim3优化后的位姿来更新。以下mScw应该改为 gscm * gswm^-1
g2o::Sim3 gScw_estimation(Converter::toMatrix3d(mScw.rowRange(0, 3).colRange(0, 3)), g2o::Sim3 gScw_estimation(gScw.rotation(), gScw.translation(),1.0);
Converter::toVector3d(mScw.rowRange(0, 3).col(3)),1.0);
vector<MapPoint*> vpMatchedMP; vector<MapPoint*> vpMatchedMP;
vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL)); vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));

1170
src/Settings.cc
View File

File diff suppressed because it is too large Load Diff