修改回环bug

Examples/Stereo/KITTI00-02.yaml

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

include/LoopClosing.h

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

src/CameraModels/KannalaBrandt8.cpp

@@ -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)
 {
     Eigen::Vector3f p3D;
+    // 用三角化出点并验证的这个过程代替极线验证
     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,
                                             Sophus::SE3f &Tcw1, Sophus::SE3f &Tcw2,
                                             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 float unc, Eigen::Vector3f &p3D)
 {
-    // 1. 得到对应特征点的相平面坐标
+    // 1. 得到对应特征点的归一化平面坐标
     Eigen::Vector3f r1 = this->unprojectEig(kp1.pt);
     Eigen::Vector3f r2 = pCamera2->unprojectEig(kp2.pt);
 

src/LoopClosing.cc

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