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));

282
src/Settings.cc
View File

@ -30,113 +30,139 @@
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) : Settings::Settings(const std::string &configFile, const int &sensor) : bNeedToUndistort_(false), bNeedToRectify_(false), bNeedToResize1_(false), bNeedToResize2_(false)
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;
} }
@ -145,7 +171,8 @@ namespace ORB_SLAM3 {
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;
} }
@ -154,12 +181,14 @@ namespace ORB_SLAM3 {
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;
} }
@ -173,7 +202,8 @@ namespace ORB_SLAM3 {
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;
} }
@ -181,14 +211,16 @@ namespace ORB_SLAM3 {
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
@ -204,13 +236,16 @@ namespace ORB_SLAM3 {
// 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);
@ -220,11 +255,13 @@ namespace ORB_SLAM3 {
} }
// Check if we need to correct distortion from the images // Check if we need to correct distortion from the images
if((sensor_ == System::MONOCULAR || sensor_ == System::IMU_MONOCULAR) && vPinHoleDistorsion1_.size() != 0){ if ((sensor_ == System::MONOCULAR || sensor_ == System::IMU_MONOCULAR) && 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
@ -240,7 +277,8 @@ namespace ORB_SLAM3 {
// 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
@ -259,7 +297,8 @@ namespace ORB_SLAM3 {
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};
@ -267,16 +306,19 @@ namespace ORB_SLAM3 {
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
@ -285,7 +327,6 @@ namespace ORB_SLAM3 {
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);
@ -293,13 +334,16 @@ namespace ORB_SLAM3 {
// 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);
@ -308,7 +352,8 @@ namespace ORB_SLAM3 {
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);
@ -320,7 +365,6 @@ namespace ORB_SLAM3 {
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);
@ -334,11 +378,13 @@ namespace ORB_SLAM3 {
} }
// 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);
@ -349,11 +395,10 @@ namespace ORB_SLAM3 {
} }
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);
@ -363,18 +408,20 @@ namespace ORB_SLAM3 {
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);
} }
@ -382,21 +429,25 @@ namespace ORB_SLAM3 {
} }
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;
@ -411,7 +462,8 @@ namespace ORB_SLAM3 {
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);
@ -423,15 +475,18 @@ namespace ORB_SLAM3 {
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);
@ -440,7 +495,8 @@ namespace ORB_SLAM3 {
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);
@ -450,7 +506,8 @@ namespace ORB_SLAM3 {
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);
@ -469,20 +526,23 @@ namespace ORB_SLAM3 {
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);
@ -518,7 +578,8 @@ namespace ORB_SLAM3 {
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());
@ -526,74 +587,97 @@ namespace ORB_SLAM3 {
} }
} }
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 || settings.cameraType_ == Settings::Rectified){ if (settings.cameraType_ == Settings::PinHole)
{
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;
@ -603,11 +687,13 @@ namespace ORB_SLAM3 {
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;
@ -615,7 +701,8 @@ namespace ORB_SLAM3 {
} }
} }
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;
@ -623,7 +710,8 @@ namespace ORB_SLAM3 {
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;
} }