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

Compare commits

base: 12345qiupeng:a9f462bcbd4a782066db35135657c809509dc3e2
Branches Tags
12345qiupeng:master
..
compare: 12345qiupeng:0b0f2d49edd8eebe0f4556316d4da0cae719ae07
Branches Tags
12345qiupeng:master

No commits in common. "a9f462bcbd4a782066db35135657c809509dc3e2" and "0b0f2d49edd8eebe0f4556316d4da0cae719ae07" have entirely different histories.
a9f462bcbd ... 0b0f2d49ed

16 changed files with 4830 additions and 5976 deletions
Show Stats Expand all files Collapse all files

10
CMakeLists.txt
View File

@ -14,12 +14,12 @@ set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} -march=native")
# Check C++11 or C++0x support # Check C++11 or C++0x support
include(CheckCXXCompilerFlag) include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11) if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
add_definitions(-DCOMPILEDWITHC11) add_definitions(-DCOMPILEDWITHC11)
message(STATUS "Using flag -std=c++14.") message(STATUS "Using flag -std=c++11.")
elseif(COMPILER_SUPPORTS_CXX0X) elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
add_definitions(-DCOMPILEDWITHC0X) add_definitions(-DCOMPILEDWITHC0X)
@ -32,9 +32,9 @@ LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules)
# 要保证整个工程的opencv版本一致，包括dbow，源码以及ros相关的 # 要保证整个工程的opencv版本一致，包括dbow，源码以及ros相关的
# 3 4 都可以正常运行 # 3 4 都可以正常运行
find_package(OpenCV 4.2) find_package(OpenCV 3.2)
if(NOT OpenCV_FOUND) if(NOT OpenCV_FOUND)
message(FATAL_ERROR "OpenCV > 4.2 not found.") message(FATAL_ERROR "OpenCV > 3.2 not found.")
endif() endif()
MESSAGE("OPENCV VERSION:") MESSAGE("OPENCV VERSION:")

12
Examples/ROS/ORB_SLAM3/CMakeLists.txt
View File

@ -14,12 +14,12 @@ set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall -O3 -march=native")
# Check C++11 or C++0x support # Check C++11 or C++0x support
include(CheckCXXCompilerFlag) include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14) CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X) CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX14) if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
add_definitions(-DCOMPILEDWITHC11) add_definitions(-DCOMPILEDWITHC11)
message(STATUS "Using flag -std=c++14.") message(STATUS "Using flag -std=c++11.")
elseif(COMPILER_SUPPORTS_CXX0X) elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x") set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
add_definitions(-DCOMPILEDWITHC0X) add_definitions(-DCOMPILEDWITHC0X)
@ -32,9 +32,9 @@ LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules)
# 要保证整个工程的opencv版本一致，包括dbow，源码以及ros相关的 # 要保证整个工程的opencv版本一致，包括dbow，源码以及ros相关的
# 3 4 都可以正常运行 # 3 4 都可以正常运行
find_package(OpenCV 4.2) find_package(OpenCV 3.2)
if(NOT OpenCV_FOUND) if(NOT OpenCV_FOUND)
message(FATAL_ERROR "OpenCV > 4.2 not found.") message(FATAL_ERROR "OpenCV > 3.2 not found.")
endif() endif()
find_package(Eigen3 3.1.0 REQUIRED) find_package(Eigen3 3.1.0 REQUIRED)

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.0 Viewer.ViewpointY: -100
Viewer.ViewpointZ: -0.1 Viewer.ViewpointZ: -0.1
Viewer.ViewpointF: 2000.0 Viewer.ViewpointF: 2000.0

6
README.md
View File

@ -1,10 +1,12 @@
# ORB-SLAM3 超详细注释 # ORB-SLAM3 超详细注释
-by 计算机视觉life 公众号旗下开源学习小组：SLAM研习社
![DEMO](https://github.com/electech6/ORB_SLAM3_detailed_comments/blob/master/demo.gif) ![DEMO](https://github.com/electech6/ORB_SLAM3_detailed_comments/blob/master/demo.gif)
独家视频课程《VIO灭霸：ORB-SLAM3公式推导+源码逐行解析+算法改进》！ [点击进入官网](https://cvlife.net/) ![课程大纲](https://github.com/electech6/ORB_SLAM3_detailed_comments/blob/master/outline.png)
点击进入 [小六的机器人SLAM学习圈](https://mp.weixin.qq.com/s?__biz=MzIxOTczOTM4NA==&mid=2247567299&idx=1&sn=2d6b673b6d5e396bd87ec2a28cd3d27d&chksm=97d52254a0a2ab42ff833b5fa733f2213a1ad1ec8ebdb199d96c237aa4cb21fe8e05cc77db8a#rd)，4500+人都在这里交流！ [ORB-SLAM3 逐行源码讲解](https://cvlife.net/detail/term_6255372036a53_o5VfgR/25)
---- ----
# ORB-SLAM3 # ORB-SLAM3

5
Thirdparty/DBoW2/CMakeLists.txt vendored
View File

@ -30,9 +30,12 @@ set(SRCS_DUTILS
# 要保证整个工程的opencv版本一致，包括dbow，源码以及ros相关的 # 要保证整个工程的opencv版本一致，包括dbow，源码以及ros相关的
# 3 4 都可以正常运行 # 3 4 都可以正常运行
find_package(OpenCV 4.2 QUIET) find_package(OpenCV 3.2 QUIET)
if(NOT OpenCV_FOUND) if(NOT OpenCV_FOUND)
find_package(OpenCV 3.0 QUIET)
if(NOT OpenCV_FOUND)
message(FATAL_ERROR "OpenCV > 3.0 not found.") message(FATAL_ERROR "OpenCV > 3.0 not found.")
endif()
endif() endif()
set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib) set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)

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
include/MapPoint.h
View File

@ -123,12 +123,12 @@ public:
int Observations(); int Observations();
void AddObservation(KeyFrame* pKF,int idx); void AddObservation(KeyFrame* pKF,int idx);
void EraseObservation(KeyFrame* pKF, bool erase = true); void EraseObservation(KeyFrame* pKF);
std::tuple<int,int> GetIndexInKeyFrame(KeyFrame* pKF); std::tuple<int,int> GetIndexInKeyFrame(KeyFrame* pKF);
bool IsInKeyFrame(KeyFrame* pKF); bool IsInKeyFrame(KeyFrame* pKF);
void SetBadFlag(bool erase = true); void SetBadFlag();
bool isBad(); bool isBad();
void Replace(MapPoint* pMP); void Replace(MapPoint* pMP);

BIN
outline.png Normal file
View File

Binary file not shown.
Side by Side

After

Width:  |  Height:  |  Size: 103 KiB

4
src/CameraModels/KannalaBrandt8.cpp
View File

@ -323,11 +323,9 @@ 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,
@ -441,7 +439,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);

4
src/G2oTypes.cc
View File

@ -831,8 +831,8 @@ void EdgeInertialGS::linearizeOplus()
// Jacobians wrt scale factor // Jacobians wrt scale factor
// _jacobianOplus[3] 9*1矩阵 总体来说就是三个残差分别对尺度求导 // _jacobianOplus[3] 9*1矩阵 总体来说就是三个残差分别对尺度求导
_jacobianOplus[7].setZero(); _jacobianOplus[7].setZero();
_jacobianOplus[7].block<3,1>(3,0) = Rbw1*(VV2->estimate()-VV1->estimate()) * s; _jacobianOplus[7].block<3,1>(3,0) = Rbw1*(VV2->estimate()-VV1->estimate());
_jacobianOplus[7].block<3,1>(6,0) = Rbw1*(VP2->estimate().twb-VP1->estimate().twb-VV1->estimate()*dt) * s; _jacobianOplus[7].block<3,1>(6,0) = Rbw1*(VP2->estimate().twb-VP1->estimate().twb-VV1->estimate()*dt);
} }
/** /**

2
src/LocalMapping.cc
View File

@ -1609,7 +1609,7 @@ void LocalMapping::InitializeIMU(float priorG, float priorA, bool bFIBA)
dirG = dirG/dirG.norm(); dirG = dirG/dirG.norm();
// 原本的重力方向 // 原本的重力方向
Eigen::Vector3f gI(0.0f, 0.0f, -1.0f); Eigen::Vector3f gI(0.0f, 0.0f, -1.0f);
// 求“重力在重力坐标系下的方向”与的“重力在世界坐标系（纯视觉）下的方向”叉乘 // 求重力在世界坐标系下的方向与重力在重力坐标系下的方向的叉乘
Eigen::Vector3f v = gI.cross(dirG); Eigen::Vector3f v = gI.cross(dirG);
// 求叉乘模长 // 求叉乘模长
const float nv = v.norm(); const float nv = v.norm();

18
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 || mpTracker->mSensor==System::IMU_RGBD) && if ((mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO) &&
mpCurrentKF->GetMap()->GetIniertialBA1()) mpCurrentKF->GetMap()->GetIniertialBA1()) // TODO, maybe with GetIniertialBA1
{ {
Eigen::Vector3d phi = LogSO3(mSold_new.rotation().toRotationMatrix()); Eigen::Vector3d phi = LogSO3(mSold_new.rotation().toRotationMatrix());
phi(0)=0; phi(0)=0;
@ -693,7 +693,8 @@ 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(gScw.rotation(), gScw.translation(),1.0); g2o::Sim3 gScw_estimation(Converter::toMatrix3d(mScw.rowRange(0, 3).colRange(0, 3)),
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));
@ -1217,16 +1218,11 @@ int LoopClosing::FindMatchesByProjection(
{ {
spCheckKFs.insert(vpKFs[j]); spCheckKFs.insert(vpKFs[j]);
++nInserted; ++nInserted;
// 改成这样
vpCovKFm.push_back(vpKFs[j]);
} }
++j; ++j;
} }
// 这里是原来的代码，这么写不太合适，会出现重复
// 所以下面的插入可以改成放在if里面
// 把每个帧的共视关键帧都加到窗口内 // 把每个帧的共视关键帧都加到窗口内
// vpCovKFm.insert(vpCovKFm.end(), vpKFs.begin(), vpKFs.end()); // 已放上面 vpCovKFm.insert(vpCovKFm.end(), vpKFs.begin(), vpKFs.end());
} }
} }
@ -2279,8 +2275,8 @@ void LoopClosing::MergeLocal()
// Essential graph 优化后可以重新开始局部建图了 // Essential graph 优化后可以重新开始局部建图了
mpLocalMapper->Release(); mpLocalMapper->Release();
// 全局的BA，后面一串的判断都为true // 全局的BA（永远不会执行）
// !pCurrentMap->isImuInitialized()一定是true // 这里没有imu, 所以isImuInitialized一定是false, 此时地图融合Atlas至少2个地图，所以第二个条件也一定是false
// Step 9 全局BA // Step 9 全局BA
if(bRelaunchBA && (!pCurrentMap->isImuInitialized() || (pCurrentMap->KeyFramesInMap()<200 && mpAtlas->CountMaps()==1))) if(bRelaunchBA && (!pCurrentMap->isImuInitialized() || (pCurrentMap->KeyFramesInMap()<200 && mpAtlas->CountMaps()==1)))
{ {

6
src/Map.cc
View File

@ -311,8 +311,6 @@ void Map::ApplyScaledRotation(const Sophus::SE3f &T, const float s, const bool b
{ {
// 更新每一个mp在世界坐标系下的坐标 // 更新每一个mp在世界坐标系下的坐标
MapPoint *pMP = *sit; MapPoint *pMP = *sit;
if (!pMP || pMP->isBad())
continue;
pMP->SetWorldPos(s * Ryw * pMP->GetWorldPos() + tyw); pMP->SetWorldPos(s * Ryw * pMP->GetWorldPos() + tyw);
pMP->UpdateNormalAndDepth(); pMP->UpdateNormalAndDepth();
} }
@ -414,9 +412,9 @@ void Map::PreSave(std::set<GeometricCamera *> &spCams)
map<KeyFrame *, std::tuple<int, int>> mpObs = pMPi->GetObservations(); map<KeyFrame *, std::tuple<int, int>> mpObs = pMPi->GetObservations();
for (map<KeyFrame *, std::tuple<int, int>>::iterator it = mpObs.begin(), end = mpObs.end(); it != end; ++it) for (map<KeyFrame *, std::tuple<int, int>>::iterator it = mpObs.begin(), end = mpObs.end(); it != end; ++it)
{ {
if (!it->first || it->first->GetMap() != this || it->first->isBad()) if (it->first->GetMap() != this || it->first->isBad())
{ {
pMPi->EraseObservation(it->first, false); pMPi->EraseObservation(it->first);
} }
} }
} }

16
src/MapPoint.cc
View File

@ -201,7 +201,7 @@ void MapPoint::AddObservation(KeyFrame* pKF, int idx)
} }
// 删除某个关键帧对当前地图点的观测 // 删除某个关键帧对当前地图点的观测
void MapPoint::EraseObservation(KeyFrame* pKF, bool erase) void MapPoint::EraseObservation(KeyFrame* pKF)
{ {
bool bBad=false; bool bBad=false;
{ {
@ -236,7 +236,7 @@ void MapPoint::EraseObservation(KeyFrame* pKF, bool erase)
} }
// 告知可以观测到该MapPoint的Frame，该MapPoint已被删除 // 告知可以观测到该MapPoint的Frame，该MapPoint已被删除
if(bBad) if(bBad)
SetBadFlag(erase); SetBadFlag();
} }
// 能够观测到当前地图点的所有关键帧及该地图点在KF中的索引 // 能够观测到当前地图点的所有关键帧及该地图点在KF中的索引
@ -260,7 +260,7 @@ int MapPoint::Observations()
* @brief 告知可以观测到该MapPoint的Frame，该MapPoint已被删除 * @brief 告知可以观测到该MapPoint的Frame，该MapPoint已被删除
* *
*/ */
void MapPoint::SetBadFlag(bool erase) void MapPoint::SetBadFlag()
{ {
map<KeyFrame*, tuple<int,int>> obs; map<KeyFrame*, tuple<int,int>> obs;
{ {
@ -285,7 +285,6 @@ void MapPoint::SetBadFlag(bool erase)
} }
// 擦除该MapPoint申请的内存 // 擦除该MapPoint申请的内存
if (erase)
mpMap->EraseMapPoint(this); mpMap->EraseMapPoint(this);
} }
@ -770,9 +769,7 @@ void MapPoint::PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP)
mBackupObservationsId1.clear(); mBackupObservationsId1.clear();
mBackupObservationsId2.clear(); mBackupObservationsId2.clear();
// Save the id and position in each KF who view it // Save the id and position in each KF who view it
std::vector<KeyFrame*> erase_kfs; for(std::map<KeyFrame*,std::tuple<int,int> >::const_iterator it = mObservations.begin(), end = mObservations.end(); it != end; ++it)
for(std::map<KeyFrame*,std::tuple<int,int> >::const_iterator it = mObservations.begin(),
end = mObservations.end(); it != end; ++it)
{ {
KeyFrame* pKFi = it->first; KeyFrame* pKFi = it->first;
if(spKF.find(pKFi) != spKF.end()) if(spKF.find(pKFi) != spKF.end())
@ -782,11 +779,10 @@ void MapPoint::PreSave(set<KeyFrame*>& spKF,set<MapPoint*>& spMP)
} }
else else
{ {
erase_kfs.push_back(pKFi); EraseObservation(pKFi);
} }
} }
for (auto pKFi : erase_kfs)
EraseObservation(pKFi, false);
// Save the id of the reference KF // Save the id of the reference KF
// 3. 备份参考关键帧ID // 3. 备份参考关键帧ID
if(spKF.find(mpRefKF) != spKF.end()) if(spKF.find(mpRefKF) != spKF.end())

9222
src/Optimizer.cc
View File

File diff suppressed because it is too large Load Diff

643
src/Settings.cc
View File

@ -1,20 +1,20 @@
/** /**
* This file is part of ORB-SLAM3 * This file is part of ORB-SLAM3
* *
* Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. * Copyright (C) 2017-2021 Carlos Campos, Richard Elvira, Juan J. Gómez Rodríguez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza. * Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
* *
* ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public * ORB-SLAM3 is free software: you can redistribute it and/or modify it under the terms of the GNU General Public
* License as published by the Free Software Foundation, either version 3 of the License, or * License as published by the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version. * (at your option) any later version.
* *
* ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even * ORB-SLAM3 is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even
* the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details. * GNU General Public License for more details.
* *
* You should have received a copy of the GNU General Public License along with ORB-SLAM3. * You should have received a copy of the GNU General Public License along with ORB-SLAM3.
* If not, see <http://www.gnu.org/licenses/>. * If not, see <http://www.gnu.org/licenses/>.
*/ */
#include "Settings.h" #include "Settings.h"
@ -30,165 +30,136 @@
using namespace std; using namespace std;
namespace ORB_SLAM3 namespace ORB_SLAM3 {
{
template <> template<>
float Settings::readParameter<float>(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required) float Settings::readParameter<float>(cv::FileStorage& fSettings, const std::string& name, bool& found, const bool required){
{
cv::FileNode node = fSettings[name]; cv::FileNode node = fSettings[name];
if (node.empty()) if(node.empty()){
{ if(required){
if (required)
{
std::cerr << name << " required parameter does not exist, aborting..." << std::endl; std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
exit(-1); exit(-1);
} }
else else{
{
std::cerr << name << " optional parameter does not exist..." << std::endl; std::cerr << name << " optional parameter does not exist..." << std::endl;
found = false; found = false;
return 0.0f; return 0.0f;
} }
} }
else if (!node.isReal()) else if(!node.isReal()){
{
std::cerr << name << " parameter must be a real number, aborting..." << std::endl; std::cerr << name << " parameter must be a real number, aborting..." << std::endl;
exit(-1); exit(-1);
} }
else else{
{
found = true; found = true;
return node.real(); return node.real();
} }
} }
template <> template<>
int Settings::readParameter<int>(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required) int Settings::readParameter<int>(cv::FileStorage& fSettings, const std::string& name, bool& found, const bool required){
{
cv::FileNode node = fSettings[name]; cv::FileNode node = fSettings[name];
if (node.empty()) if(node.empty()){
{ if(required){
if (required)
{
std::cerr << name << " required parameter does not exist, aborting..." << std::endl; std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
exit(-1); exit(-1);
} }
else else{
{
std::cerr << name << " optional parameter does not exist..." << std::endl; std::cerr << name << " optional parameter does not exist..." << std::endl;
found = false; found = false;
return 0; return 0;
} }
} }
else if (!node.isInt()) else if(!node.isInt()){
{
std::cerr << name << " parameter must be an integer number, aborting..." << std::endl; std::cerr << name << " parameter must be an integer number, aborting..." << std::endl;
exit(-1); exit(-1);
} }
else else{
{
found = true; found = true;
return node.operator int(); return node.operator int();
} }
} }
template <> template<>
string Settings::readParameter<string>(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required) string Settings::readParameter<string>(cv::FileStorage& fSettings, const std::string& name, bool& found, const bool required){
{
cv::FileNode node = fSettings[name]; cv::FileNode node = fSettings[name];
if (node.empty()) if(node.empty()){
{ if(required){
if (required)
{
std::cerr << name << " required parameter does not exist, aborting..." << std::endl; std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
exit(-1); exit(-1);
} }
else else{
{
std::cerr << name << " optional parameter does not exist..." << std::endl; std::cerr << name << " optional parameter does not exist..." << std::endl;
found = false; found = false;
return string(); return string();
} }
} }
else if (!node.isString()) else if(!node.isString()){
{
std::cerr << name << " parameter must be a string, aborting..." << std::endl; std::cerr << name << " parameter must be a string, aborting..." << std::endl;
exit(-1); exit(-1);
} }
else else{
{
found = true; found = true;
return node.string(); return node.string();
} }
} }
template <> template<>
cv::Mat Settings::readParameter<cv::Mat>(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required) cv::Mat Settings::readParameter<cv::Mat>(cv::FileStorage& fSettings, const std::string& name, bool& found, const bool required){
{
cv::FileNode node = fSettings[name]; cv::FileNode node = fSettings[name];
if (node.empty()) if(node.empty()){
{ if(required){
if (required)
{
std::cerr << name << " required parameter does not exist, aborting..." << std::endl; std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
exit(-1); exit(-1);
} }
else else{
{
std::cerr << name << " optional parameter does not exist..." << std::endl; std::cerr << name << " optional parameter does not exist..." << std::endl;
found = false; found = false;
return cv::Mat(); return cv::Mat();
} }
} }
else else{
{
found = true; found = true;
return node.mat(); return node.mat();
} }
} }
Settings::Settings(const std::string &configFile, const int &sensor) : bNeedToUndistort_(false), bNeedToRectify_(false), bNeedToResize1_(false), bNeedToResize2_(false) Settings::Settings(const std::string &configFile, const int& sensor) :
{ bNeedToUndistort_(false), bNeedToRectify_(false), bNeedToResize1_(false), bNeedToResize2_(false) {
sensor_ = sensor; sensor_ = sensor;
// Open settings file //Open settings file
cv::FileStorage fSettings(configFile, cv::FileStorage::READ); cv::FileStorage fSettings(configFile, cv::FileStorage::READ);
if (!fSettings.isOpened()) if (!fSettings.isOpened()) {
{
cerr << "[ERROR]: could not open configuration file at: " << configFile << endl; cerr << "[ERROR]: could not open configuration file at: " << configFile << endl;
cerr << "Aborting..." << endl; cerr << "Aborting..." << endl;
exit(-1); exit(-1);
} }
else else{
{
cout << "Loading settings from " << configFile << endl; cout << "Loading settings from " << configFile << endl;
} }
// Read first camera //Read first camera
readCamera1(fSettings); readCamera1(fSettings);
cout << "\t-Loaded camera 1" << endl; cout << "\t-Loaded camera 1" << endl;
// Read second camera if stereo (not rectified) //Read second camera if stereo (not rectified)
if (sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) if(sensor_ == System::STEREO || sensor_ == System::IMU_STEREO){
{
readCamera2(fSettings); readCamera2(fSettings);
cout << "\t-Loaded camera 2" << endl; cout << "\t-Loaded camera 2" << endl;
} }
// Read image info //Read image info
readImageInfo(fSettings); readImageInfo(fSettings);
cout << "\t-Loaded image info" << endl; cout << "\t-Loaded image info" << endl;
if (sensor_ == System::IMU_MONOCULAR || sensor_ == System::IMU_STEREO || sensor_ == System::IMU_RGBD) if(sensor_ == System::IMU_MONOCULAR || sensor_ == System::IMU_STEREO || sensor_ == System::IMU_RGBD){
{
readIMU(fSettings); readIMU(fSettings);
cout << "\t-Loaded IMU calibration" << endl; cout << "\t-Loaded IMU calibration" << endl;
} }
if (sensor_ == System::RGBD || sensor_ == System::IMU_RGBD) if(sensor_ == System::RGBD || sensor_ == System::IMU_RGBD){
{
readRGBD(fSettings); readRGBD(fSettings);
cout << "\t-Loaded RGB-D calibration" << endl; cout << "\t-Loaded RGB-D calibration" << endl;
} }
@ -202,487 +173,427 @@ Settings::Settings(const std::string &configFile, const int &sensor) : bNeedToUn
readOtherParameters(fSettings); readOtherParameters(fSettings);
cout << "\t-Loaded misc parameters" << endl; cout << "\t-Loaded misc parameters" << endl;
if (bNeedToRectify_) if(bNeedToRectify_){
{
precomputeRectificationMaps(); precomputeRectificationMaps();
cout << "\t-Computed rectification maps" << endl; cout << "\t-Computed rectification maps" << endl;
} }
cout << "----------------------------------" << endl; cout << "----------------------------------" << endl;
} }
void Settings::readCamera1(cv::FileStorage &fSettings) void Settings::readCamera1(cv::FileStorage &fSettings) {
{
bool found; bool found;
// Read camera model //Read camera model
string cameraModel = readParameter<string>(fSettings, "Camera.type", found); string cameraModel = readParameter<string>(fSettings,"Camera.type",found);
vector<float> vCalibration; vector<float> vCalibration;
if (cameraModel == "PinHole") if (cameraModel == "PinHole") {
{
cameraType_ = PinHole; cameraType_ = PinHole;
// Read intrinsic parameters //Read intrinsic parameters
float fx = readParameter<float>(fSettings, "Camera1.fx", found); float fx = readParameter<float>(fSettings,"Camera1.fx",found);
float fy = readParameter<float>(fSettings, "Camera1.fy", found); float fy = readParameter<float>(fSettings,"Camera1.fy",found);
float cx = readParameter<float>(fSettings, "Camera1.cx", found); float cx = readParameter<float>(fSettings,"Camera1.cx",found);
float cy = readParameter<float>(fSettings, "Camera1.cy", found); float cy = readParameter<float>(fSettings,"Camera1.cy",found);
vCalibration = {fx, fy, cx, cy}; vCalibration = {fx, fy, cx, cy};
calibration1_ = new Pinhole(vCalibration); calibration1_ = new Pinhole(vCalibration);
originalCalib1_ = new Pinhole(vCalibration); originalCalib1_ = new Pinhole(vCalibration);
// Check if it is a distorted PinHole //Check if it is a distorted PinHole
readParameter<float>(fSettings, "Camera1.k1", found, false); readParameter<float>(fSettings,"Camera1.k1",found,false);
if (found) if(found){
{ readParameter<float>(fSettings,"Camera1.k3",found,false);
readParameter<float>(fSettings, "Camera1.k3", found, false); if(found){
if (found)
{
vPinHoleDistorsion1_.resize(5); vPinHoleDistorsion1_.resize(5);
vPinHoleDistorsion1_[4] = readParameter<float>(fSettings, "Camera1.k3", found); vPinHoleDistorsion1_[4] = readParameter<float>(fSettings,"Camera1.k3",found);
} }
else else{
{
vPinHoleDistorsion1_.resize(4); vPinHoleDistorsion1_.resize(4);
} }
vPinHoleDistorsion1_[0] = readParameter<float>(fSettings, "Camera1.k1", found); vPinHoleDistorsion1_[0] = readParameter<float>(fSettings,"Camera1.k1",found);
vPinHoleDistorsion1_[1] = readParameter<float>(fSettings, "Camera1.k2", found); vPinHoleDistorsion1_[1] = readParameter<float>(fSettings,"Camera1.k2",found);
vPinHoleDistorsion1_[2] = readParameter<float>(fSettings, "Camera1.p1", found); vPinHoleDistorsion1_[2] = readParameter<float>(fSettings,"Camera1.p1",found);
vPinHoleDistorsion1_[3] = readParameter<float>(fSettings, "Camera1.p2", found); vPinHoleDistorsion1_[3] = readParameter<float>(fSettings,"Camera1.p2",found);
} }
// Check if we need to correct distortion from the images //Check if we need to correct distortion from the images
if ( if((sensor_ == System::MONOCULAR || sensor_ == System::IMU_MONOCULAR) && vPinHoleDistorsion1_.size() != 0){
(sensor_ == System::MONOCULAR ||
sensor_ == System::IMU_MONOCULAR ||
sensor_ == System::RGBD ||
sensor_ == System::IMU_RGBD) &&
vPinHoleDistorsion1_.size() != 0)
{
bNeedToUndistort_ = true; bNeedToUndistort_ = true;
} }
} }
else if (cameraModel == "Rectified") else if(cameraModel == "Rectified"){
{
cameraType_ = Rectified; cameraType_ = Rectified;
// Read intrinsic parameters //Read intrinsic parameters
float fx = readParameter<float>(fSettings, "Camera1.fx", found); float fx = readParameter<float>(fSettings,"Camera1.fx",found);
float fy = readParameter<float>(fSettings, "Camera1.fy", found); float fy = readParameter<float>(fSettings,"Camera1.fy",found);
float cx = readParameter<float>(fSettings, "Camera1.cx", found); float cx = readParameter<float>(fSettings,"Camera1.cx",found);
float cy = readParameter<float>(fSettings, "Camera1.cy", found); float cy = readParameter<float>(fSettings,"Camera1.cy",found);
vCalibration = {fx, fy, cx, cy}; vCalibration = {fx, fy, cx, cy};
calibration1_ = new Pinhole(vCalibration); calibration1_ = new Pinhole(vCalibration);
originalCalib1_ = new Pinhole(vCalibration); originalCalib1_ = new Pinhole(vCalibration);
// Rectified images are assumed to be ideal PinHole images (no distortion) //Rectified images are assumed to be ideal PinHole images (no distortion)
} }
else if (cameraModel == "KannalaBrandt8") else if(cameraModel == "KannalaBrandt8"){
{
cameraType_ = KannalaBrandt; cameraType_ = KannalaBrandt;
// Read intrinsic parameters //Read intrinsic parameters
float fx = readParameter<float>(fSettings, "Camera1.fx", found); float fx = readParameter<float>(fSettings,"Camera1.fx",found);
float fy = readParameter<float>(fSettings, "Camera1.fy", found); float fy = readParameter<float>(fSettings,"Camera1.fy",found);
float cx = readParameter<float>(fSettings, "Camera1.cx", found); float cx = readParameter<float>(fSettings,"Camera1.cx",found);
float cy = readParameter<float>(fSettings, "Camera1.cy", found); float cy = readParameter<float>(fSettings,"Camera1.cy",found);
float k0 = readParameter<float>(fSettings, "Camera1.k1", found); float k0 = readParameter<float>(fSettings,"Camera1.k1",found);
float k1 = readParameter<float>(fSettings, "Camera1.k2", found); float k1 = readParameter<float>(fSettings,"Camera1.k2",found);
float k2 = readParameter<float>(fSettings, "Camera1.k3", found); float k2 = readParameter<float>(fSettings,"Camera1.k3",found);
float k3 = readParameter<float>(fSettings, "Camera1.k4", found); float k3 = readParameter<float>(fSettings,"Camera1.k4",found);
vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3}; vCalibration = {fx,fy,cx,cy,k0,k1,k2,k3};
calibration1_ = new KannalaBrandt8(vCalibration); calibration1_ = new KannalaBrandt8(vCalibration);
originalCalib1_ = new KannalaBrandt8(vCalibration); originalCalib1_ = new KannalaBrandt8(vCalibration);
if (sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) if(sensor_ == System::STEREO || sensor_ == System::IMU_STEREO){
{ int colBegin = readParameter<int>(fSettings,"Camera1.overlappingBegin",found);
int colBegin = readParameter<int>(fSettings, "Camera1.overlappingBegin", found); int colEnd = readParameter<int>(fSettings,"Camera1.overlappingEnd",found);
int colEnd = readParameter<int>(fSettings, "Camera1.overlappingEnd", found);
vector<int> vOverlapping = {colBegin, colEnd}; vector<int> vOverlapping = {colBegin, colEnd};
static_cast<KannalaBrandt8 *>(calibration1_)->mvLappingArea = vOverlapping; static_cast<KannalaBrandt8*>(calibration1_)->mvLappingArea = vOverlapping;
} }
} }
else else{
{
cerr << "Error: " << cameraModel << " not known" << endl; cerr << "Error: " << cameraModel << " not known" << endl;
exit(-1); exit(-1);
} }
} }
void Settings::readCamera2(cv::FileStorage &fSettings) void Settings::readCamera2(cv::FileStorage &fSettings) {
{
bool found; bool found;
vector<float> vCalibration; vector<float> vCalibration;
if (cameraType_ == PinHole) if (cameraType_ == PinHole) {
{
bNeedToRectify_ = true; bNeedToRectify_ = true;
// Read intrinsic parameters //Read intrinsic parameters
float fx = readParameter<float>(fSettings, "Camera2.fx", found); float fx = readParameter<float>(fSettings,"Camera2.fx",found);
float fy = readParameter<float>(fSettings, "Camera2.fy", found); float fy = readParameter<float>(fSettings,"Camera2.fy",found);
float cx = readParameter<float>(fSettings, "Camera2.cx", found); float cx = readParameter<float>(fSettings,"Camera2.cx",found);
float cy = readParameter<float>(fSettings, "Camera2.cy", found); float cy = readParameter<float>(fSettings,"Camera2.cy",found);
vCalibration = {fx, fy, cx, cy}; vCalibration = {fx, fy, cx, cy};
calibration2_ = new Pinhole(vCalibration); calibration2_ = new Pinhole(vCalibration);
originalCalib2_ = new Pinhole(vCalibration); originalCalib2_ = new Pinhole(vCalibration);
// Check if it is a distorted PinHole //Check if it is a distorted PinHole
readParameter<float>(fSettings, "Camera2.k1", found, false); readParameter<float>(fSettings,"Camera2.k1",found,false);
if (found) if(found){
{ readParameter<float>(fSettings,"Camera2.k3",found,false);
readParameter<float>(fSettings, "Camera2.k3", found, false); if(found){
if (found)
{
vPinHoleDistorsion2_.resize(5); vPinHoleDistorsion2_.resize(5);
vPinHoleDistorsion2_[4] = readParameter<float>(fSettings, "Camera2.k3", found); vPinHoleDistorsion2_[4] = readParameter<float>(fSettings,"Camera2.k3",found);
} }
else else{
{
vPinHoleDistorsion2_.resize(4); vPinHoleDistorsion2_.resize(4);
} }
vPinHoleDistorsion2_[0] = readParameter<float>(fSettings, "Camera2.k1", found); vPinHoleDistorsion2_[0] = readParameter<float>(fSettings,"Camera2.k1",found);
vPinHoleDistorsion2_[1] = readParameter<float>(fSettings, "Camera2.k2", found); vPinHoleDistorsion2_[1] = readParameter<float>(fSettings,"Camera2.k2",found);
vPinHoleDistorsion2_[2] = readParameter<float>(fSettings, "Camera2.p1", found); vPinHoleDistorsion2_[2] = readParameter<float>(fSettings,"Camera2.p1",found);
vPinHoleDistorsion2_[3] = readParameter<float>(fSettings, "Camera2.p2", found); vPinHoleDistorsion2_[3] = readParameter<float>(fSettings,"Camera2.p2",found);
} }
} }
else if (cameraType_ == KannalaBrandt) else if(cameraType_ == KannalaBrandt){
{ //Read intrinsic parameters
// Read intrinsic parameters float fx = readParameter<float>(fSettings,"Camera2.fx",found);
float fx = readParameter<float>(fSettings, "Camera2.fx", found); float fy = readParameter<float>(fSettings,"Camera2.fy",found);
float fy = readParameter<float>(fSettings, "Camera2.fy", found); float cx = readParameter<float>(fSettings,"Camera2.cx",found);
float cx = readParameter<float>(fSettings, "Camera2.cx", found); float cy = readParameter<float>(fSettings,"Camera2.cy",found);
float cy = readParameter<float>(fSettings, "Camera2.cy", found);
float k0 = readParameter<float>(fSettings, "Camera1.k1", found); float k0 = readParameter<float>(fSettings,"Camera1.k1",found);
float k1 = readParameter<float>(fSettings, "Camera1.k2", found); float k1 = readParameter<float>(fSettings,"Camera1.k2",found);
float k2 = readParameter<float>(fSettings, "Camera1.k3", found); float k2 = readParameter<float>(fSettings,"Camera1.k3",found);
float k3 = readParameter<float>(fSettings, "Camera1.k4", found); float k3 = readParameter<float>(fSettings,"Camera1.k4",found);
vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3};
vCalibration = {fx,fy,cx,cy,k0,k1,k2,k3};
calibration2_ = new KannalaBrandt8(vCalibration); calibration2_ = new KannalaBrandt8(vCalibration);
originalCalib2_ = new KannalaBrandt8(vCalibration); originalCalib2_ = new KannalaBrandt8(vCalibration);
int colBegin = readParameter<int>(fSettings, "Camera2.overlappingBegin", found); int colBegin = readParameter<int>(fSettings,"Camera2.overlappingBegin",found);
int colEnd = readParameter<int>(fSettings, "Camera2.overlappingEnd", found); int colEnd = readParameter<int>(fSettings,"Camera2.overlappingEnd",found);
vector<int> vOverlapping = {colBegin, colEnd}; vector<int> vOverlapping = {colBegin, colEnd};
static_cast<KannalaBrandt8 *>(calibration2_)->mvLappingArea = vOverlapping; static_cast<KannalaBrandt8*>(calibration2_)->mvLappingArea = vOverlapping;
} }
// Load stereo extrinsic calibration //Load stereo extrinsic calibration
if (cameraType_ == Rectified) if(cameraType_ == Rectified){
{ b_ = readParameter<float>(fSettings,"Stereo.b",found);
b_ = readParameter<float>(fSettings, "Stereo.b", found);
bf_ = b_ * calibration1_->getParameter(0); bf_ = b_ * calibration1_->getParameter(0);
} }
else else{
{ cv::Mat cvTlr = readParameter<cv::Mat>(fSettings,"Stereo.T_c1_c2",found);
cv::Mat cvTlr = readParameter<cv::Mat>(fSettings, "Stereo.T_c1_c2", found);
Tlr_ = Converter::toSophus(cvTlr); Tlr_ = Converter::toSophus(cvTlr);
// TODO: also search for Trl and invert if necessary //TODO: also search for Trl and invert if necessary
b_ = Tlr_.translation().norm(); b_ = Tlr_.translation().norm();
bf_ = b_ * calibration1_->getParameter(0); bf_ = b_ * calibration1_->getParameter(0);
} }
thDepth_ = readParameter<float>(fSettings, "Stereo.ThDepth", found); thDepth_ = readParameter<float>(fSettings,"Stereo.ThDepth",found);
}
void Settings::readImageInfo(cv::FileStorage &fSettings)
{ }
void Settings::readImageInfo(cv::FileStorage &fSettings) {
bool found; bool found;
// Read original and desired image dimensions //Read original and desired image dimensions
int originalRows = readParameter<int>(fSettings, "Camera.height", found); int originalRows = readParameter<int>(fSettings,"Camera.height",found);
int originalCols = readParameter<int>(fSettings, "Camera.width", found); int originalCols = readParameter<int>(fSettings,"Camera.width",found);
originalImSize_.width = originalCols; originalImSize_.width = originalCols;
originalImSize_.height = originalRows; originalImSize_.height = originalRows;
newImSize_ = originalImSize_; newImSize_ = originalImSize_;
int newHeigh = readParameter<int>(fSettings, "Camera.newHeight", found, false); int newHeigh = readParameter<int>(fSettings,"Camera.newHeight",found,false);
if (found) if(found){
{
bNeedToResize1_ = true; bNeedToResize1_ = true;
newImSize_.height = newHeigh; newImSize_.height = newHeigh;
if (!bNeedToRectify_) if(!bNeedToRectify_){
{ //Update calibration
// Update calibration
float scaleRowFactor = (float)newImSize_.height / (float)originalImSize_.height; float scaleRowFactor = (float)newImSize_.height / (float)originalImSize_.height;
calibration1_->setParameter(calibration1_->getParameter(1) * scaleRowFactor, 1); calibration1_->setParameter(calibration1_->getParameter(1) * scaleRowFactor, 1);
calibration1_->setParameter(calibration1_->getParameter(3) * scaleRowFactor, 3); calibration1_->setParameter(calibration1_->getParameter(3) * scaleRowFactor, 3);
if ((sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) && cameraType_ != Rectified)
{ if((sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) && cameraType_ != Rectified){
calibration2_->setParameter(calibration2_->getParameter(1) * scaleRowFactor, 1); calibration2_->setParameter(calibration2_->getParameter(1) * scaleRowFactor, 1);
calibration2_->setParameter(calibration2_->getParameter(3) * scaleRowFactor, 3); calibration2_->setParameter(calibration2_->getParameter(3) * scaleRowFactor, 3);
} }
} }
} }
int newWidth = readParameter<int>(fSettings, "Camera.newWidth", found, false); int newWidth = readParameter<int>(fSettings,"Camera.newWidth",found,false);
if (found) if(found){
{
bNeedToResize1_ = true; bNeedToResize1_ = true;
newImSize_.width = newWidth; newImSize_.width = newWidth;
if (!bNeedToRectify_) if(!bNeedToRectify_){
{ //Update calibration
// Update calibration float scaleColFactor = (float)newImSize_.width /(float) originalImSize_.width;
float scaleColFactor = (float)newImSize_.width / (float)originalImSize_.width;
calibration1_->setParameter(calibration1_->getParameter(0) * scaleColFactor, 0); calibration1_->setParameter(calibration1_->getParameter(0) * scaleColFactor, 0);
calibration1_->setParameter(calibration1_->getParameter(2) * scaleColFactor, 2); calibration1_->setParameter(calibration1_->getParameter(2) * scaleColFactor, 2);
if ((sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) && cameraType_ != Rectified) if((sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) && cameraType_ != Rectified){
{
calibration2_->setParameter(calibration2_->getParameter(0) * scaleColFactor, 0); calibration2_->setParameter(calibration2_->getParameter(0) * scaleColFactor, 0);
calibration2_->setParameter(calibration2_->getParameter(2) * scaleColFactor, 2); calibration2_->setParameter(calibration2_->getParameter(2) * scaleColFactor, 2);
if (cameraType_ == KannalaBrandt) if(cameraType_ == KannalaBrandt){
{ static_cast<KannalaBrandt8*>(calibration1_)->mvLappingArea[0] *= scaleColFactor;
static_cast<KannalaBrandt8 *>(calibration1_)->mvLappingArea[0] *= scaleColFactor; static_cast<KannalaBrandt8*>(calibration1_)->mvLappingArea[1] *= scaleColFactor;
static_cast<KannalaBrandt8 *>(calibration1_)->mvLappingArea[1] *= scaleColFactor;
static_cast<KannalaBrandt8 *>(calibration2_)->mvLappingArea[0] *= scaleColFactor; static_cast<KannalaBrandt8*>(calibration2_)->mvLappingArea[0] *= scaleColFactor;
static_cast<KannalaBrandt8 *>(calibration2_)->mvLappingArea[1] *= scaleColFactor; static_cast<KannalaBrandt8*>(calibration2_)->mvLappingArea[1] *= scaleColFactor;
} }
} }
} }
} }
fps_ = readParameter<int>(fSettings, "Camera.fps", found); fps_ = readParameter<int>(fSettings,"Camera.fps",found);
bRGB_ = (bool)readParameter<int>(fSettings, "Camera.RGB", found); bRGB_ = (bool) readParameter<int>(fSettings,"Camera.RGB",found);
} }
void Settings::readIMU(cv::FileStorage &fSettings) void Settings::readIMU(cv::FileStorage &fSettings) {
{
bool found; bool found;
noiseGyro_ = readParameter<float>(fSettings, "IMU.NoiseGyro", found); noiseGyro_ = readParameter<float>(fSettings,"IMU.NoiseGyro",found);
noiseAcc_ = readParameter<float>(fSettings, "IMU.NoiseAcc", found); noiseAcc_ = readParameter<float>(fSettings,"IMU.NoiseAcc",found);
gyroWalk_ = readParameter<float>(fSettings, "IMU.GyroWalk", found); gyroWalk_ = readParameter<float>(fSettings,"IMU.GyroWalk",found);
accWalk_ = readParameter<float>(fSettings, "IMU.AccWalk", found); accWalk_ = readParameter<float>(fSettings,"IMU.AccWalk",found);
imuFrequency_ = readParameter<float>(fSettings, "IMU.Frequency", found); imuFrequency_ = readParameter<float>(fSettings,"IMU.Frequency",found);
cv::Mat cvTbc = readParameter<cv::Mat>(fSettings, "IMU.T_b_c1", found); cv::Mat cvTbc = readParameter<cv::Mat>(fSettings,"IMU.T_b_c1",found);
Tbc_ = Converter::toSophus(cvTbc); Tbc_ = Converter::toSophus(cvTbc);
readParameter<int>(fSettings, "IMU.InsertKFsWhenLost", found, false); readParameter<int>(fSettings,"IMU.InsertKFsWhenLost",found,false);
if (found) if(found){
{ insertKFsWhenLost_ = (bool) readParameter<int>(fSettings,"IMU.InsertKFsWhenLost",found,false);
insertKFsWhenLost_ = (bool)readParameter<int>(fSettings, "IMU.InsertKFsWhenLost", found, false);
} }
else else{
{
insertKFsWhenLost_ = true; insertKFsWhenLost_ = true;
} }
} }
void Settings::readRGBD(cv::FileStorage &fSettings) void Settings::readRGBD(cv::FileStorage& fSettings) {
{
bool found; bool found;
depthMapFactor_ = readParameter<float>(fSettings, "RGBD.DepthMapFactor", found); depthMapFactor_ = readParameter<float>(fSettings,"RGBD.DepthMapFactor",found);
thDepth_ = readParameter<float>(fSettings, "Stereo.ThDepth", found); thDepth_ = readParameter<float>(fSettings,"Stereo.ThDepth",found);
b_ = readParameter<float>(fSettings, "Stereo.b", found); b_ = readParameter<float>(fSettings,"Stereo.b",found);
bf_ = b_ * calibration1_->getParameter(0); bf_ = b_ * calibration1_->getParameter(0);
} }
void Settings::readORB(cv::FileStorage &fSettings) void Settings::readORB(cv::FileStorage &fSettings) {
{
bool found; bool found;
nFeatures_ = readParameter<int>(fSettings, "ORBextractor.nFeatures", found); nFeatures_ = readParameter<int>(fSettings,"ORBextractor.nFeatures",found);
scaleFactor_ = readParameter<float>(fSettings, "ORBextractor.scaleFactor", found); scaleFactor_ = readParameter<float>(fSettings,"ORBextractor.scaleFactor",found);
nLevels_ = readParameter<int>(fSettings, "ORBextractor.nLevels", found); nLevels_ = readParameter<int>(fSettings,"ORBextractor.nLevels",found);
initThFAST_ = readParameter<int>(fSettings, "ORBextractor.iniThFAST", found); initThFAST_ = readParameter<int>(fSettings,"ORBextractor.iniThFAST",found);
minThFAST_ = readParameter<int>(fSettings, "ORBextractor.minThFAST", found); minThFAST_ = readParameter<int>(fSettings,"ORBextractor.minThFAST",found);
} }
void Settings::readViewer(cv::FileStorage &fSettings) void Settings::readViewer(cv::FileStorage &fSettings) {
{
bool found; bool found;
keyFrameSize_ = readParameter<float>(fSettings, "Viewer.KeyFrameSize", found); keyFrameSize_ = readParameter<float>(fSettings,"Viewer.KeyFrameSize",found);
keyFrameLineWidth_ = readParameter<float>(fSettings, "Viewer.KeyFrameLineWidth", found); keyFrameLineWidth_ = readParameter<float>(fSettings,"Viewer.KeyFrameLineWidth",found);
graphLineWidth_ = readParameter<float>(fSettings, "Viewer.GraphLineWidth", found); graphLineWidth_ = readParameter<float>(fSettings,"Viewer.GraphLineWidth",found);
pointSize_ = readParameter<float>(fSettings, "Viewer.PointSize", found); pointSize_ = readParameter<float>(fSettings,"Viewer.PointSize",found);
cameraSize_ = readParameter<float>(fSettings, "Viewer.CameraSize", found); cameraSize_ = readParameter<float>(fSettings,"Viewer.CameraSize",found);
cameraLineWidth_ = readParameter<float>(fSettings, "Viewer.CameraLineWidth", found); cameraLineWidth_ = readParameter<float>(fSettings,"Viewer.CameraLineWidth",found);
viewPointX_ = readParameter<float>(fSettings, "Viewer.ViewpointX", found); viewPointX_ = readParameter<float>(fSettings,"Viewer.ViewpointX",found);
viewPointY_ = readParameter<float>(fSettings, "Viewer.ViewpointY", found); viewPointY_ = readParameter<float>(fSettings,"Viewer.ViewpointY",found);
viewPointZ_ = readParameter<float>(fSettings, "Viewer.ViewpointZ", found); viewPointZ_ = readParameter<float>(fSettings,"Viewer.ViewpointZ",found);
viewPointF_ = readParameter<float>(fSettings, "Viewer.ViewpointF", found); viewPointF_ = readParameter<float>(fSettings,"Viewer.ViewpointF",found);
imageViewerScale_ = readParameter<float>(fSettings, "Viewer.imageViewScale", found, false); imageViewerScale_ = readParameter<float>(fSettings,"Viewer.imageViewScale",found,false);
if (!found) if(!found)
imageViewerScale_ = 1.0f; imageViewerScale_ = 1.0f;
} }
void Settings::readLoadAndSave(cv::FileStorage &fSettings) void Settings::readLoadAndSave(cv::FileStorage &fSettings) {
{
bool found; bool found;
sLoadFrom_ = readParameter<string>(fSettings, "System.LoadAtlasFromFile", found, false); sLoadFrom_ = readParameter<string>(fSettings,"System.LoadAtlasFromFile",found,false);
sSaveto_ = readParameter<string>(fSettings, "System.SaveAtlasToFile", found, false); sSaveto_ = readParameter<string>(fSettings,"System.SaveAtlasToFile",found,false);
} }
void Settings::readOtherParameters(cv::FileStorage &fSettings) void Settings::readOtherParameters(cv::FileStorage& fSettings) {
{
bool found; bool found;
thFarPoints_ = readParameter<float>(fSettings, "System.thFarPoints", found, false); thFarPoints_ = readParameter<float>(fSettings,"System.thFarPoints",found,false);
} }
void Settings::precomputeRectificationMaps() void Settings::precomputeRectificationMaps() {
{ //Precompute rectification maps, new calibrations, ...
// Precompute rectification maps, new calibrations, ... cv::Mat K1 = static_cast<Pinhole*>(calibration1_)->toK();
cv::Mat K1 = static_cast<Pinhole *>(calibration1_)->toK(); K1.convertTo(K1,CV_64F);
K1.convertTo(K1, CV_64F); cv::Mat K2 = static_cast<Pinhole*>(calibration2_)->toK();
cv::Mat K2 = static_cast<Pinhole *>(calibration2_)->toK(); K2.convertTo(K2,CV_64F);
K2.convertTo(K2, CV_64F);
cv::Mat cvTlr; cv::Mat cvTlr;
cv::eigen2cv(Tlr_.inverse().matrix3x4(), cvTlr); cv::eigen2cv(Tlr_.inverse().matrix3x4(),cvTlr);
cv::Mat R12 = cvTlr.rowRange(0, 3).colRange(0, 3); cv::Mat R12 = cvTlr.rowRange(0,3).colRange(0,3);
R12.convertTo(R12, CV_64F); R12.convertTo(R12,CV_64F);
cv::Mat t12 = cvTlr.rowRange(0, 3).col(3); cv::Mat t12 = cvTlr.rowRange(0,3).col(3);
t12.convertTo(t12, CV_64F); t12.convertTo(t12,CV_64F);
cv::Mat R_r1_u1, R_r2_u2; cv::Mat R_r1_u1, R_r2_u2;
cv::Mat P1, P2, Q; cv::Mat P1, P2, Q;
cv::stereoRectify(K1, camera1DistortionCoef(), K2, camera2DistortionCoef(), newImSize_, cv::stereoRectify(K1,camera1DistortionCoef(),K2,camera2DistortionCoef(),newImSize_,
R12, t12, R12, t12,
R_r1_u1, R_r2_u2, P1, P2, Q, R_r1_u1,R_r2_u2,P1,P2,Q,
cv::CALIB_ZERO_DISPARITY, -1, newImSize_); cv::CALIB_ZERO_DISPARITY,-1,newImSize_);
cv::initUndistortRectifyMap(K1, camera1DistortionCoef(), R_r1_u1, P1.rowRange(0, 3).colRange(0, 3), cv::initUndistortRectifyMap(K1, camera1DistortionCoef(), R_r1_u1, P1.rowRange(0, 3).colRange(0, 3),
newImSize_, CV_32F, M1l_, M2l_); newImSize_, CV_32F, M1l_, M2l_);
cv::initUndistortRectifyMap(K2, camera2DistortionCoef(), R_r2_u2, P2.rowRange(0, 3).colRange(0, 3), cv::initUndistortRectifyMap(K2, camera2DistortionCoef(), R_r2_u2, P2.rowRange(0, 3).colRange(0, 3),
newImSize_, CV_32F, M1r_, M2r_); newImSize_, CV_32F, M1r_, M2r_);
// Update calibration //Update calibration
calibration1_->setParameter(P1.at<double>(0, 0), 0); calibration1_->setParameter(P1.at<double>(0,0), 0);
calibration1_->setParameter(P1.at<double>(1, 1), 1); calibration1_->setParameter(P1.at<double>(1,1), 1);
calibration1_->setParameter(P1.at<double>(0, 2), 2); calibration1_->setParameter(P1.at<double>(0,2), 2);
calibration1_->setParameter(P1.at<double>(1, 2), 3); calibration1_->setParameter(P1.at<double>(1,2), 3);
// Update bf //Update bf
bf_ = b_ * P1.at<double>(0, 0); bf_ = b_ * P1.at<double>(0,0);
// Update relative pose between camera 1 and IMU if necessary //Update relative pose between camera 1 and IMU if necessary
if (sensor_ == System::IMU_STEREO) if(sensor_ == System::IMU_STEREO){
{
Eigen::Matrix3f eigenR_r1_u1; Eigen::Matrix3f eigenR_r1_u1;
cv::cv2eigen(R_r1_u1, eigenR_r1_u1); cv::cv2eigen(R_r1_u1,eigenR_r1_u1);
Sophus::SE3f T_r1_u1(eigenR_r1_u1, Eigen::Vector3f::Zero()); Sophus::SE3f T_r1_u1(eigenR_r1_u1,Eigen::Vector3f::Zero());
Tbc_ = Tbc_ * T_r1_u1.inverse(); Tbc_ = Tbc_ * T_r1_u1.inverse();
} }
} }
ostream &operator<<(std::ostream &output, const Settings &settings) ostream &operator<<(std::ostream& output, const Settings& settings){
{
output << "SLAM settings: " << endl; output << "SLAM settings: " << endl;
output << "\t-Camera 1 parameters ("; output << "\t-Camera 1 parameters (";
if (settings.cameraType_ == Settings::PinHole || settings.cameraType_ == Settings::Rectified) if(settings.cameraType_ == Settings::PinHole || settings.cameraType_ == Settings::Rectified){
{
output << "Pinhole"; output << "Pinhole";
} }
else else{
{
output << "Kannala-Brandt"; output << "Kannala-Brandt";
} }
output << ")" output << ")" << ": [";
<< ": ["; for(size_t i = 0; i < settings.originalCalib1_->size(); i++){
for (size_t i = 0; i < settings.originalCalib1_->size(); i++)
{
output << " " << settings.originalCalib1_->getParameter(i); output << " " << settings.originalCalib1_->getParameter(i);
} }
output << " ]" << endl; output << " ]" << endl;
if (!settings.vPinHoleDistorsion1_.empty()) if(!settings.vPinHoleDistorsion1_.empty()){
{
output << "\t-Camera 1 distortion parameters: [ "; output << "\t-Camera 1 distortion parameters: [ ";
for (float d : settings.vPinHoleDistorsion1_) for(float d : settings.vPinHoleDistorsion1_){
{
output << " " << d; output << " " << d;
} }
output << " ]" << endl; output << " ]" << endl;
} }
if (settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO) if(settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO){
{
if (settings.cameraType_ != Settings::Rectified)
{
output << "\t-Camera 2 parameters ("; output << "\t-Camera 2 parameters (";
if (settings.cameraType_ == Settings::PinHole) if(settings.cameraType_ == Settings::PinHole || settings.cameraType_ == Settings::Rectified){
{
output << "Pinhole"; output << "Pinhole";
} }
else else{
{
output << "Kannala-Brandt"; output << "Kannala-Brandt";
} }
output << "" output << "" << ": [";
<< ": ["; for(size_t i = 0; i < settings.originalCalib2_->size(); i++){
for (size_t i = 0; i < settings.originalCalib2_->size(); i++)
{
output << " " << settings.originalCalib2_->getParameter(i); output << " " << settings.originalCalib2_->getParameter(i);
} }
output << " ]" << endl; output << " ]" << endl;
if (!settings.vPinHoleDistorsion2_.empty()) if(!settings.vPinHoleDistorsion2_.empty()){
{
output << "\t-Camera 1 distortion parameters: [ "; output << "\t-Camera 1 distortion parameters: [ ";
for (float d : settings.vPinHoleDistorsion2_) for(float d : settings.vPinHoleDistorsion2_){
{
output << " " << d; output << " " << d;
} }
output << " ]" << endl; output << " ]" << endl;
} }
} }
}
output << "\t-Original image size: [ " << settings.originalImSize_.width << " , " << settings.originalImSize_.height << " ]" << endl; output << "\t-Original image size: [ " << settings.originalImSize_.width << " , " << settings.originalImSize_.height << " ]" << endl;
output << "\t-Current image size: [ " << settings.newImSize_.width << " , " << settings.newImSize_.height << " ]" << endl; output << "\t-Current image size: [ " << settings.newImSize_.width << " , " << settings.newImSize_.height << " ]" << endl;
if (settings.bNeedToRectify_) if(settings.bNeedToRectify_){
{
output << "\t-Camera 1 parameters after rectification: [ "; output << "\t-Camera 1 parameters after rectification: [ ";
for (size_t i = 0; i < settings.calibration1_->size(); i++) for(size_t i = 0; i < settings.calibration1_->size(); i++){
{
output << " " << settings.calibration1_->getParameter(i); output << " " << settings.calibration1_->getParameter(i);
} }
output << " ]" << endl; output << " ]" << endl;
} }
else if (settings.bNeedToResize1_) else if(settings.bNeedToResize1_){
{
output << "\t-Camera 1 parameters after resize: [ "; output << "\t-Camera 1 parameters after resize: [ ";
for (size_t i = 0; i < settings.calibration1_->size(); i++) for(size_t i = 0; i < settings.calibration1_->size(); i++){
{
output << " " << settings.calibration1_->getParameter(i); output << " " << settings.calibration1_->getParameter(i);
} }
output << " ]" << endl; output << " ]" << endl;
if ((settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO) && if((settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO) &&
settings.cameraType_ == Settings::KannalaBrandt) settings.cameraType_ == Settings::KannalaBrandt){
{
output << "\t-Camera 2 parameters after resize: [ "; output << "\t-Camera 2 parameters after resize: [ ";
for (size_t i = 0; i < settings.calibration2_->size(); i++) for(size_t i = 0; i < settings.calibration2_->size(); i++){
{
output << " " << settings.calibration2_->getParameter(i); output << " " << settings.calibration2_->getParameter(i);
} }
output << " ]" << endl; output << " ]" << endl;
@ -691,23 +602,20 @@ ostream &operator<<(std::ostream &output, const Settings &settings)
output << "\t-Sequence FPS: " << settings.fps_ << endl; output << "\t-Sequence FPS: " << settings.fps_ << endl;
// Stereo stuff //Stereo stuff
if (settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO) if(settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO){
{
output << "\t-Stereo baseline: " << settings.b_ << endl; output << "\t-Stereo baseline: " << settings.b_ << endl;
output << "\t-Stereo depth threshold : " << settings.thDepth_ << endl; output << "\t-Stereo depth threshold : " << settings.thDepth_ << endl;
if (settings.cameraType_ == Settings::KannalaBrandt) if(settings.cameraType_ == Settings::KannalaBrandt){
{ auto vOverlapping1 = static_cast<KannalaBrandt8*>(settings.calibration1_)->mvLappingArea;
auto vOverlapping1 = static_cast<KannalaBrandt8 *>(settings.calibration1_)->mvLappingArea; auto vOverlapping2 = static_cast<KannalaBrandt8*>(settings.calibration2_)->mvLappingArea;
auto vOverlapping2 = static_cast<KannalaBrandt8 *>(settings.calibration2_)->mvLappingArea;
output << "\t-Camera 1 overlapping area: [ " << vOverlapping1[0] << " , " << vOverlapping1[1] << " ]" << endl; output << "\t-Camera 1 overlapping area: [ " << vOverlapping1[0] << " , " << vOverlapping1[1] << " ]" << endl;
output << "\t-Camera 2 overlapping area: [ " << vOverlapping2[0] << " , " << vOverlapping2[1] << " ]" << endl; output << "\t-Camera 2 overlapping area: [ " << vOverlapping2[0] << " , " << vOverlapping2[1] << " ]" << endl;
} }
} }
if (settings.sensor_ == System::IMU_MONOCULAR || settings.sensor_ == System::IMU_STEREO || settings.sensor_ == System::IMU_RGBD) if(settings.sensor_ == System::IMU_MONOCULAR || settings.sensor_ == System::IMU_STEREO || settings.sensor_ == System::IMU_RGBD) {
{
output << "\t-Gyro noise: " << settings.noiseGyro_ << endl; output << "\t-Gyro noise: " << settings.noiseGyro_ << endl;
output << "\t-Accelerometer noise: " << settings.noiseAcc_ << endl; output << "\t-Accelerometer noise: " << settings.noiseAcc_ << endl;
output << "\t-Gyro walk: " << settings.gyroWalk_ << endl; output << "\t-Gyro walk: " << settings.gyroWalk_ << endl;
@ -715,8 +623,7 @@ ostream &operator<<(std::ostream &output, const Settings &settings)
output << "\t-IMU frequency: " << settings.imuFrequency_ << endl; output << "\t-IMU frequency: " << settings.imuFrequency_ << endl;
} }
if (settings.sensor_ == System::RGBD || settings.sensor_ == System::IMU_RGBD) if(settings.sensor_ == System::RGBD || settings.sensor_ == System::IMU_RGBD){
{
output << "\t-RGB-D depth map factor: " << settings.depthMapFactor_ << endl; output << "\t-RGB-D depth map factor: " << settings.depthMapFactor_ << endl;
} }
@ -727,5 +634,5 @@ ostream &operator<<(std::ostream &output, const Settings &settings)
output << "\t-Min FAST threshold: " << settings.minThFAST_ << endl; output << "\t-Min FAST threshold: " << settings.minThFAST_ << endl;
return output; return output;
} }
}; };