From 5d57b1d807ebd8fcd4588bb3f0d516699e888ab3 Mon Sep 17 00:00:00 2001 From: wuqing <741299292@qq.com> Date: Sat, 28 May 2022 15:05:22 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E5=9B=9E=E7=8E=AFbug?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Examples/Stereo/KITTI00-02.yaml | 2 +- include/LoopClosing.h | 2 +- src/CameraModels/KannalaBrandt8.cpp | 4 +- src/LoopClosing.cc | 7 +- src/Settings.cc | 1170 ++++++++++++++------------- 5 files changed, 637 insertions(+), 548 deletions(-) diff --git a/Examples/Stereo/KITTI00-02.yaml b/Examples/Stereo/KITTI00-02.yaml index 64d5141..d523a7f 100644 --- a/Examples/Stereo/KITTI00-02.yaml +++ b/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 diff --git a/include/LoopClosing.h b/include/LoopClosing.h index c9ccb26..3077aa3 100644 --- a/include/LoopClosing.h +++ b/include/LoopClosing.h @@ -180,7 +180,7 @@ protected: std::vector mvpCurrentConnectedKFs; std::vector mvpCurrentMatchedPoints; std::vector mvpLoopMapPoints; - cv::Mat mScw; + g2o::Sim3 mg2oScw; //------- diff --git a/src/CameraModels/KannalaBrandt8.cpp b/src/CameraModels/KannalaBrandt8.cpp index f4bfafb..cd0297a 100644 --- a/src/CameraModels/KannalaBrandt8.cpp +++ b/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); diff --git a/src/LoopClosing.cc b/src/LoopClosing.cc index 560dd08..ee101f8 100644 --- a/src/LoopClosing.cc +++ b/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 vpMatchedMP; vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast(NULL)); diff --git a/src/Settings.cc b/src/Settings.cc index 93a50be..603f369 100644 --- a/src/Settings.cc +++ b/src/Settings.cc @@ -1,20 +1,20 @@ /** -* 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) 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 -* License as published by the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* 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 -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License along with ORB-SLAM3. -* If not, see . -*/ + * 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) 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 + * License as published by the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * 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 + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License along with ORB-SLAM3. + * If not, see . + */ #include "Settings.h" @@ -30,609 +30,697 @@ using namespace std; -namespace ORB_SLAM3 { +namespace ORB_SLAM3 +{ - template<> - float Settings::readParameter(cv::FileStorage& fSettings, const std::string& name, bool& found, const bool required){ - cv::FileNode node = fSettings[name]; - if(node.empty()){ - if(required){ - std::cerr << name << " required parameter does not exist, aborting..." << std::endl; - exit(-1); - } - else{ - std::cerr << name << " optional parameter does not exist..." << std::endl; - found = false; - return 0.0f; - } - } - else if(!node.isReal()){ - std::cerr << name << " parameter must be a real number, aborting..." << std::endl; +template <> +float Settings::readParameter(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required) +{ + cv::FileNode node = fSettings[name]; + if (node.empty()) + { + if (required) + { + std::cerr << name << " required parameter does not exist, aborting..." << std::endl; exit(-1); } - else{ - found = true; - return node.real(); + else + { + std::cerr << name << " optional parameter does not exist..." << std::endl; + found = false; + return 0.0f; } } + else if (!node.isReal()) + { + std::cerr << name << " parameter must be a real number, aborting..." << std::endl; + exit(-1); + } + else + { + found = true; + return node.real(); + } +} - template<> - int Settings::readParameter(cv::FileStorage& fSettings, const std::string& name, bool& found, const bool required){ - cv::FileNode node = fSettings[name]; - if(node.empty()){ - if(required){ - std::cerr << name << " required parameter does not exist, aborting..." << std::endl; - exit(-1); - } - else{ - std::cerr << name << " optional parameter does not exist..." << std::endl; - found = false; - return 0; - } - } - else if(!node.isInt()){ - std::cerr << name << " parameter must be an integer number, aborting..." << std::endl; +template <> +int Settings::readParameter(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required) +{ + cv::FileNode node = fSettings[name]; + if (node.empty()) + { + if (required) + { + std::cerr << name << " required parameter does not exist, aborting..." << std::endl; exit(-1); } - else{ - found = true; - return node.operator int(); + else + { + std::cerr << name << " optional parameter does not exist..." << std::endl; + found = false; + return 0; } } + else if (!node.isInt()) + { + std::cerr << name << " parameter must be an integer number, aborting..." << std::endl; + exit(-1); + } + else + { + found = true; + return node.operator int(); + } +} - template<> - string Settings::readParameter(cv::FileStorage& fSettings, const std::string& name, bool& found, const bool required){ - cv::FileNode node = fSettings[name]; - if(node.empty()){ - if(required){ - std::cerr << name << " required parameter does not exist, aborting..." << std::endl; - exit(-1); - } - else{ - std::cerr << name << " optional parameter does not exist..." << std::endl; - found = false; - return string(); - } - } - else if(!node.isString()){ - std::cerr << name << " parameter must be a string, aborting..." << std::endl; +template <> +string Settings::readParameter(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required) +{ + cv::FileNode node = fSettings[name]; + if (node.empty()) + { + if (required) + { + std::cerr << name << " required parameter does not exist, aborting..." << std::endl; exit(-1); } - else{ - found = true; - return node.string(); + else + { + std::cerr << name << " optional parameter does not exist..." << std::endl; + found = false; + return string(); } } - - template<> - cv::Mat Settings::readParameter(cv::FileStorage& fSettings, const std::string& name, bool& found, const bool required){ - cv::FileNode node = fSettings[name]; - if(node.empty()){ - if(required){ - std::cerr << name << " required parameter does not exist, aborting..." << std::endl; - exit(-1); - } - else{ - std::cerr << name << " optional parameter does not exist..." << std::endl; - found = false; - return cv::Mat(); - } - } - else{ - found = true; - return node.mat(); - } + else if (!node.isString()) + { + std::cerr << name << " parameter must be a string, aborting..." << std::endl; + exit(-1); } + else + { + found = true; + return node.string(); + } +} - Settings::Settings(const std::string &configFile, const int& sensor) : - bNeedToUndistort_(false), bNeedToRectify_(false), bNeedToResize1_(false), bNeedToResize2_(false) { - sensor_ = sensor; - - //Open settings file - cv::FileStorage fSettings(configFile, cv::FileStorage::READ); - if (!fSettings.isOpened()) { - cerr << "[ERROR]: could not open configuration file at: " << configFile << endl; - cerr << "Aborting..." << endl; - +template <> +cv::Mat Settings::readParameter(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required) +{ + cv::FileNode node = fSettings[name]; + if (node.empty()) + { + if (required) + { + std::cerr << name << " required parameter does not exist, aborting..." << std::endl; exit(-1); } - else{ - cout << "Loading settings from " << configFile << endl; - } - - //Read first camera - readCamera1(fSettings); - cout << "\t-Loaded camera 1" << endl; - - //Read second camera if stereo (not rectified) - if(sensor_ == System::STEREO || sensor_ == System::IMU_STEREO){ - readCamera2(fSettings); - cout << "\t-Loaded camera 2" << endl; - } - - //Read image info - readImageInfo(fSettings); - cout << "\t-Loaded image info" << endl; - - if(sensor_ == System::IMU_MONOCULAR || sensor_ == System::IMU_STEREO || sensor_ == System::IMU_RGBD){ - readIMU(fSettings); - cout << "\t-Loaded IMU calibration" << endl; - } - - if(sensor_ == System::RGBD || sensor_ == System::IMU_RGBD){ - readRGBD(fSettings); - cout << "\t-Loaded RGB-D calibration" << endl; - } - - readORB(fSettings); - cout << "\t-Loaded ORB settings" << endl; - readViewer(fSettings); - cout << "\t-Loaded viewer settings" << endl; - readLoadAndSave(fSettings); - cout << "\t-Loaded Atlas settings" << endl; - readOtherParameters(fSettings); - cout << "\t-Loaded misc parameters" << endl; - - if(bNeedToRectify_){ - precomputeRectificationMaps(); - cout << "\t-Computed rectification maps" << endl; - } - - cout << "----------------------------------" << endl; - } - - void Settings::readCamera1(cv::FileStorage &fSettings) { - bool found; - - //Read camera model - string cameraModel = readParameter(fSettings,"Camera.type",found); - - vector vCalibration; - if (cameraModel == "PinHole") { - cameraType_ = PinHole; - - //Read intrinsic parameters - float fx = readParameter(fSettings,"Camera1.fx",found); - float fy = readParameter(fSettings,"Camera1.fy",found); - float cx = readParameter(fSettings,"Camera1.cx",found); - float cy = readParameter(fSettings,"Camera1.cy",found); - - vCalibration = {fx, fy, cx, cy}; - - calibration1_ = new Pinhole(vCalibration); - originalCalib1_ = new Pinhole(vCalibration); - - //Check if it is a distorted PinHole - readParameter(fSettings,"Camera1.k1",found,false); - if(found){ - readParameter(fSettings,"Camera1.k3",found,false); - if(found){ - vPinHoleDistorsion1_.resize(5); - vPinHoleDistorsion1_[4] = readParameter(fSettings,"Camera1.k3",found); - } - else{ - vPinHoleDistorsion1_.resize(4); - } - vPinHoleDistorsion1_[0] = readParameter(fSettings,"Camera1.k1",found); - vPinHoleDistorsion1_[1] = readParameter(fSettings,"Camera1.k2",found); - vPinHoleDistorsion1_[2] = readParameter(fSettings,"Camera1.p1",found); - vPinHoleDistorsion1_[3] = readParameter(fSettings,"Camera1.p2",found); - } - - //Check if we need to correct distortion from the images - if((sensor_ == System::MONOCULAR || sensor_ == System::IMU_MONOCULAR) && vPinHoleDistorsion1_.size() != 0){ - bNeedToUndistort_ = true; - } - } - else if(cameraModel == "Rectified"){ - cameraType_ = Rectified; - - //Read intrinsic parameters - float fx = readParameter(fSettings,"Camera1.fx",found); - float fy = readParameter(fSettings,"Camera1.fy",found); - float cx = readParameter(fSettings,"Camera1.cx",found); - float cy = readParameter(fSettings,"Camera1.cy",found); - - vCalibration = {fx, fy, cx, cy}; - - calibration1_ = new Pinhole(vCalibration); - originalCalib1_ = new Pinhole(vCalibration); - - //Rectified images are assumed to be ideal PinHole images (no distortion) - } - else if(cameraModel == "KannalaBrandt8"){ - cameraType_ = KannalaBrandt; - - //Read intrinsic parameters - float fx = readParameter(fSettings,"Camera1.fx",found); - float fy = readParameter(fSettings,"Camera1.fy",found); - float cx = readParameter(fSettings,"Camera1.cx",found); - float cy = readParameter(fSettings,"Camera1.cy",found); - - float k0 = readParameter(fSettings,"Camera1.k1",found); - float k1 = readParameter(fSettings,"Camera1.k2",found); - float k2 = readParameter(fSettings,"Camera1.k3",found); - float k3 = readParameter(fSettings,"Camera1.k4",found); - - vCalibration = {fx,fy,cx,cy,k0,k1,k2,k3}; - - calibration1_ = new KannalaBrandt8(vCalibration); - originalCalib1_ = new KannalaBrandt8(vCalibration); - - if(sensor_ == System::STEREO || sensor_ == System::IMU_STEREO){ - int colBegin = readParameter(fSettings,"Camera1.overlappingBegin",found); - int colEnd = readParameter(fSettings,"Camera1.overlappingEnd",found); - vector vOverlapping = {colBegin, colEnd}; - - static_cast(calibration1_)->mvLappingArea = vOverlapping; - } - } - else{ - cerr << "Error: " << cameraModel << " not known" << endl; - exit(-1); + else + { + std::cerr << name << " optional parameter does not exist..." << std::endl; + found = false; + return cv::Mat(); } } + else + { + found = true; + return node.mat(); + } +} - void Settings::readCamera2(cv::FileStorage &fSettings) { - bool found; - vector vCalibration; - if (cameraType_ == PinHole) { - bNeedToRectify_ = true; +Settings::Settings(const std::string &configFile, const int &sensor) : bNeedToUndistort_(false), bNeedToRectify_(false), bNeedToResize1_(false), bNeedToResize2_(false) +{ + sensor_ = sensor; - //Read intrinsic parameters - float fx = readParameter(fSettings,"Camera2.fx",found); - float fy = readParameter(fSettings,"Camera2.fy",found); - float cx = readParameter(fSettings,"Camera2.cx",found); - float cy = readParameter(fSettings,"Camera2.cy",found); + // Open settings file + cv::FileStorage fSettings(configFile, cv::FileStorage::READ); + if (!fSettings.isOpened()) + { + cerr << "[ERROR]: could not open configuration file at: " << configFile << endl; + cerr << "Aborting..." << endl; + exit(-1); + } + else + { + cout << "Loading settings from " << configFile << endl; + } - vCalibration = {fx, fy, cx, cy}; + // Read first camera + readCamera1(fSettings); + cout << "\t-Loaded camera 1" << endl; - calibration2_ = new Pinhole(vCalibration); - originalCalib2_ = new Pinhole(vCalibration); + // Read second camera if stereo (not rectified) + if (sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) + { + readCamera2(fSettings); + cout << "\t-Loaded camera 2" << endl; + } - //Check if it is a distorted PinHole - readParameter(fSettings,"Camera2.k1",found,false); - if(found){ - readParameter(fSettings,"Camera2.k3",found,false); - if(found){ - vPinHoleDistorsion2_.resize(5); - vPinHoleDistorsion2_[4] = readParameter(fSettings,"Camera2.k3",found); - } - else{ - vPinHoleDistorsion2_.resize(4); - } - vPinHoleDistorsion2_[0] = readParameter(fSettings,"Camera2.k1",found); - vPinHoleDistorsion2_[1] = readParameter(fSettings,"Camera2.k2",found); - vPinHoleDistorsion2_[2] = readParameter(fSettings,"Camera2.p1",found); - vPinHoleDistorsion2_[3] = readParameter(fSettings,"Camera2.p2",found); + // Read image info + readImageInfo(fSettings); + cout << "\t-Loaded image info" << endl; + + if (sensor_ == System::IMU_MONOCULAR || sensor_ == System::IMU_STEREO || sensor_ == System::IMU_RGBD) + { + readIMU(fSettings); + cout << "\t-Loaded IMU calibration" << endl; + } + + if (sensor_ == System::RGBD || sensor_ == System::IMU_RGBD) + { + readRGBD(fSettings); + cout << "\t-Loaded RGB-D calibration" << endl; + } + + readORB(fSettings); + cout << "\t-Loaded ORB settings" << endl; + readViewer(fSettings); + cout << "\t-Loaded viewer settings" << endl; + readLoadAndSave(fSettings); + cout << "\t-Loaded Atlas settings" << endl; + readOtherParameters(fSettings); + cout << "\t-Loaded misc parameters" << endl; + + if (bNeedToRectify_) + { + precomputeRectificationMaps(); + cout << "\t-Computed rectification maps" << endl; + } + + cout << "----------------------------------" << endl; +} + +void Settings::readCamera1(cv::FileStorage &fSettings) +{ + bool found; + + // Read camera model + string cameraModel = readParameter(fSettings, "Camera.type", found); + + vector vCalibration; + if (cameraModel == "PinHole") + { + cameraType_ = PinHole; + + // Read intrinsic parameters + float fx = readParameter(fSettings, "Camera1.fx", found); + float fy = readParameter(fSettings, "Camera1.fy", found); + float cx = readParameter(fSettings, "Camera1.cx", found); + float cy = readParameter(fSettings, "Camera1.cy", found); + + vCalibration = {fx, fy, cx, cy}; + + calibration1_ = new Pinhole(vCalibration); + originalCalib1_ = new Pinhole(vCalibration); + + // Check if it is a distorted PinHole + readParameter(fSettings, "Camera1.k1", found, false); + if (found) + { + readParameter(fSettings, "Camera1.k3", found, false); + if (found) + { + vPinHoleDistorsion1_.resize(5); + vPinHoleDistorsion1_[4] = readParameter(fSettings, "Camera1.k3", found); } + else + { + vPinHoleDistorsion1_.resize(4); + } + vPinHoleDistorsion1_[0] = readParameter(fSettings, "Camera1.k1", found); + vPinHoleDistorsion1_[1] = readParameter(fSettings, "Camera1.k2", found); + vPinHoleDistorsion1_[2] = readParameter(fSettings, "Camera1.p1", found); + vPinHoleDistorsion1_[3] = readParameter(fSettings, "Camera1.p2", found); } - else if(cameraType_ == KannalaBrandt){ - //Read intrinsic parameters - float fx = readParameter(fSettings,"Camera2.fx",found); - float fy = readParameter(fSettings,"Camera2.fy",found); - float cx = readParameter(fSettings,"Camera2.cx",found); - float cy = readParameter(fSettings,"Camera2.cy",found); - float k0 = readParameter(fSettings,"Camera1.k1",found); - float k1 = readParameter(fSettings,"Camera1.k2",found); - float k2 = readParameter(fSettings,"Camera1.k3",found); - float k3 = readParameter(fSettings,"Camera1.k4",found); + // Check if we need to correct distortion from the images + if ((sensor_ == System::MONOCULAR || sensor_ == System::IMU_MONOCULAR) && vPinHoleDistorsion1_.size() != 0) + { + bNeedToUndistort_ = true; + } + } + else if (cameraModel == "Rectified") + { + cameraType_ = Rectified; + // Read intrinsic parameters + float fx = readParameter(fSettings, "Camera1.fx", found); + float fy = readParameter(fSettings, "Camera1.fy", found); + float cx = readParameter(fSettings, "Camera1.cx", found); + float cy = readParameter(fSettings, "Camera1.cy", found); - vCalibration = {fx,fy,cx,cy,k0,k1,k2,k3}; + vCalibration = {fx, fy, cx, cy}; - calibration2_ = new KannalaBrandt8(vCalibration); - originalCalib2_ = new KannalaBrandt8(vCalibration); + calibration1_ = new Pinhole(vCalibration); + originalCalib1_ = new Pinhole(vCalibration); - int colBegin = readParameter(fSettings,"Camera2.overlappingBegin",found); - int colEnd = readParameter(fSettings,"Camera2.overlappingEnd",found); + // Rectified images are assumed to be ideal PinHole images (no distortion) + } + else if (cameraModel == "KannalaBrandt8") + { + cameraType_ = KannalaBrandt; + + // Read intrinsic parameters + float fx = readParameter(fSettings, "Camera1.fx", found); + float fy = readParameter(fSettings, "Camera1.fy", found); + float cx = readParameter(fSettings, "Camera1.cx", found); + float cy = readParameter(fSettings, "Camera1.cy", found); + + float k0 = readParameter(fSettings, "Camera1.k1", found); + float k1 = readParameter(fSettings, "Camera1.k2", found); + float k2 = readParameter(fSettings, "Camera1.k3", found); + float k3 = readParameter(fSettings, "Camera1.k4", found); + + vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3}; + + calibration1_ = new KannalaBrandt8(vCalibration); + originalCalib1_ = new KannalaBrandt8(vCalibration); + + if (sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) + { + int colBegin = readParameter(fSettings, "Camera1.overlappingBegin", found); + int colEnd = readParameter(fSettings, "Camera1.overlappingEnd", found); vector vOverlapping = {colBegin, colEnd}; - static_cast(calibration2_)->mvLappingArea = vOverlapping; + static_cast(calibration1_)->mvLappingArea = vOverlapping; } - - //Load stereo extrinsic calibration - if(cameraType_ == Rectified){ - b_ = readParameter(fSettings,"Stereo.b",found); - bf_ = b_ * calibration1_->getParameter(0); - } - else{ - cv::Mat cvTlr = readParameter(fSettings,"Stereo.T_c1_c2",found); - Tlr_ = Converter::toSophus(cvTlr); - - //TODO: also search for Trl and invert if necessary - - b_ = Tlr_.translation().norm(); - bf_ = b_ * calibration1_->getParameter(0); - } - - thDepth_ = readParameter(fSettings,"Stereo.ThDepth",found); - - } + else + { + cerr << "Error: " << cameraModel << " not known" << endl; + exit(-1); + } +} - void Settings::readImageInfo(cv::FileStorage &fSettings) { - bool found; - //Read original and desired image dimensions - int originalRows = readParameter(fSettings,"Camera.height",found); - int originalCols = readParameter(fSettings,"Camera.width",found); - originalImSize_.width = originalCols; - originalImSize_.height = originalRows; +void Settings::readCamera2(cv::FileStorage &fSettings) +{ + bool found; + vector vCalibration; + if (cameraType_ == PinHole) + { + bNeedToRectify_ = true; - newImSize_ = originalImSize_; - int newHeigh = readParameter(fSettings,"Camera.newHeight",found,false); - if(found){ - bNeedToResize1_ = true; - newImSize_.height = newHeigh; + // Read intrinsic parameters + float fx = readParameter(fSettings, "Camera2.fx", found); + float fy = readParameter(fSettings, "Camera2.fy", found); + float cx = readParameter(fSettings, "Camera2.cx", found); + float cy = readParameter(fSettings, "Camera2.cy", found); - if(!bNeedToRectify_){ - //Update calibration - float scaleRowFactor = (float)newImSize_.height / (float)originalImSize_.height; - calibration1_->setParameter(calibration1_->getParameter(1) * scaleRowFactor, 1); - calibration1_->setParameter(calibration1_->getParameter(3) * scaleRowFactor, 3); + vCalibration = {fx, fy, cx, cy}; + calibration2_ = new Pinhole(vCalibration); + originalCalib2_ = new Pinhole(vCalibration); - if((sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) && cameraType_ != Rectified){ - calibration2_->setParameter(calibration2_->getParameter(1) * scaleRowFactor, 1); - calibration2_->setParameter(calibration2_->getParameter(3) * scaleRowFactor, 3); - } + // Check if it is a distorted PinHole + readParameter(fSettings, "Camera2.k1", found, false); + if (found) + { + readParameter(fSettings, "Camera2.k3", found, false); + if (found) + { + vPinHoleDistorsion2_.resize(5); + vPinHoleDistorsion2_[4] = readParameter(fSettings, "Camera2.k3", found); } - } - - int newWidth = readParameter(fSettings,"Camera.newWidth",found,false); - if(found){ - bNeedToResize1_ = true; - newImSize_.width = newWidth; - - if(!bNeedToRectify_){ - //Update calibration - float scaleColFactor = (float)newImSize_.width /(float) originalImSize_.width; - calibration1_->setParameter(calibration1_->getParameter(0) * scaleColFactor, 0); - calibration1_->setParameter(calibration1_->getParameter(2) * scaleColFactor, 2); - - if((sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) && cameraType_ != Rectified){ - calibration2_->setParameter(calibration2_->getParameter(0) * scaleColFactor, 0); - calibration2_->setParameter(calibration2_->getParameter(2) * scaleColFactor, 2); - - if(cameraType_ == KannalaBrandt){ - static_cast(calibration1_)->mvLappingArea[0] *= scaleColFactor; - static_cast(calibration1_)->mvLappingArea[1] *= scaleColFactor; - - static_cast(calibration2_)->mvLappingArea[0] *= scaleColFactor; - static_cast(calibration2_)->mvLappingArea[1] *= scaleColFactor; - } - } + else + { + vPinHoleDistorsion2_.resize(4); } - } - - fps_ = readParameter(fSettings,"Camera.fps",found); - bRGB_ = (bool) readParameter(fSettings,"Camera.RGB",found); - } - - void Settings::readIMU(cv::FileStorage &fSettings) { - bool found; - noiseGyro_ = readParameter(fSettings,"IMU.NoiseGyro",found); - noiseAcc_ = readParameter(fSettings,"IMU.NoiseAcc",found); - gyroWalk_ = readParameter(fSettings,"IMU.GyroWalk",found); - accWalk_ = readParameter(fSettings,"IMU.AccWalk",found); - imuFrequency_ = readParameter(fSettings,"IMU.Frequency",found); - - cv::Mat cvTbc = readParameter(fSettings,"IMU.T_b_c1",found); - Tbc_ = Converter::toSophus(cvTbc); - - readParameter(fSettings,"IMU.InsertKFsWhenLost",found,false); - if(found){ - insertKFsWhenLost_ = (bool) readParameter(fSettings,"IMU.InsertKFsWhenLost",found,false); - } - else{ - insertKFsWhenLost_ = true; + vPinHoleDistorsion2_[0] = readParameter(fSettings, "Camera2.k1", found); + vPinHoleDistorsion2_[1] = readParameter(fSettings, "Camera2.k2", found); + vPinHoleDistorsion2_[2] = readParameter(fSettings, "Camera2.p1", found); + vPinHoleDistorsion2_[3] = readParameter(fSettings, "Camera2.p2", found); } } + else if (cameraType_ == KannalaBrandt) + { + // Read intrinsic parameters + float fx = readParameter(fSettings, "Camera2.fx", found); + float fy = readParameter(fSettings, "Camera2.fy", found); + float cx = readParameter(fSettings, "Camera2.cx", found); + float cy = readParameter(fSettings, "Camera2.cy", found); - void Settings::readRGBD(cv::FileStorage& fSettings) { - bool found; + float k0 = readParameter(fSettings, "Camera1.k1", found); + float k1 = readParameter(fSettings, "Camera1.k2", found); + float k2 = readParameter(fSettings, "Camera1.k3", found); + float k3 = readParameter(fSettings, "Camera1.k4", found); - depthMapFactor_ = readParameter(fSettings,"RGBD.DepthMapFactor",found); - thDepth_ = readParameter(fSettings,"Stereo.ThDepth",found); - b_ = readParameter(fSettings,"Stereo.b",found); + vCalibration = {fx, fy, cx, cy, k0, k1, k2, k3}; + + calibration2_ = new KannalaBrandt8(vCalibration); + originalCalib2_ = new KannalaBrandt8(vCalibration); + + int colBegin = readParameter(fSettings, "Camera2.overlappingBegin", found); + int colEnd = readParameter(fSettings, "Camera2.overlappingEnd", found); + vector vOverlapping = {colBegin, colEnd}; + + static_cast(calibration2_)->mvLappingArea = vOverlapping; + } + + // Load stereo extrinsic calibration + if (cameraType_ == Rectified) + { + b_ = readParameter(fSettings, "Stereo.b", found); + bf_ = b_ * calibration1_->getParameter(0); + } + else + { + cv::Mat cvTlr = readParameter(fSettings, "Stereo.T_c1_c2", found); + Tlr_ = Converter::toSophus(cvTlr); + + // TODO: also search for Trl and invert if necessary + + b_ = Tlr_.translation().norm(); bf_ = b_ * calibration1_->getParameter(0); } - void Settings::readORB(cv::FileStorage &fSettings) { - bool found; + thDepth_ = readParameter(fSettings, "Stereo.ThDepth", found); +} - nFeatures_ = readParameter(fSettings,"ORBextractor.nFeatures",found); - scaleFactor_ = readParameter(fSettings,"ORBextractor.scaleFactor",found); - nLevels_ = readParameter(fSettings,"ORBextractor.nLevels",found); - initThFAST_ = readParameter(fSettings,"ORBextractor.iniThFAST",found); - minThFAST_ = readParameter(fSettings,"ORBextractor.minThFAST",found); - } +void Settings::readImageInfo(cv::FileStorage &fSettings) +{ + bool found; + // Read original and desired image dimensions + int originalRows = readParameter(fSettings, "Camera.height", found); + int originalCols = readParameter(fSettings, "Camera.width", found); + originalImSize_.width = originalCols; + originalImSize_.height = originalRows; - void Settings::readViewer(cv::FileStorage &fSettings) { - bool found; + newImSize_ = originalImSize_; + int newHeigh = readParameter(fSettings, "Camera.newHeight", found, false); + if (found) + { + bNeedToResize1_ = true; + newImSize_.height = newHeigh; - keyFrameSize_ = readParameter(fSettings,"Viewer.KeyFrameSize",found); - keyFrameLineWidth_ = readParameter(fSettings,"Viewer.KeyFrameLineWidth",found); - graphLineWidth_ = readParameter(fSettings,"Viewer.GraphLineWidth",found); - pointSize_ = readParameter(fSettings,"Viewer.PointSize",found); - cameraSize_ = readParameter(fSettings,"Viewer.CameraSize",found); - cameraLineWidth_ = readParameter(fSettings,"Viewer.CameraLineWidth",found); - viewPointX_ = readParameter(fSettings,"Viewer.ViewpointX",found); - viewPointY_ = readParameter(fSettings,"Viewer.ViewpointY",found); - viewPointZ_ = readParameter(fSettings,"Viewer.ViewpointZ",found); - viewPointF_ = readParameter(fSettings,"Viewer.ViewpointF",found); - imageViewerScale_ = readParameter(fSettings,"Viewer.imageViewScale",found,false); + if (!bNeedToRectify_) + { + // Update calibration + float scaleRowFactor = (float)newImSize_.height / (float)originalImSize_.height; + calibration1_->setParameter(calibration1_->getParameter(1) * scaleRowFactor, 1); + calibration1_->setParameter(calibration1_->getParameter(3) * scaleRowFactor, 3); - if(!found) - imageViewerScale_ = 1.0f; - } - - void Settings::readLoadAndSave(cv::FileStorage &fSettings) { - bool found; - - sLoadFrom_ = readParameter(fSettings,"System.LoadAtlasFromFile",found,false); - sSaveto_ = readParameter(fSettings,"System.SaveAtlasToFile",found,false); - } - - void Settings::readOtherParameters(cv::FileStorage& fSettings) { - bool found; - - thFarPoints_ = readParameter(fSettings,"System.thFarPoints",found,false); - } - - void Settings::precomputeRectificationMaps() { - //Precompute rectification maps, new calibrations, ... - cv::Mat K1 = static_cast(calibration1_)->toK(); - K1.convertTo(K1,CV_64F); - cv::Mat K2 = static_cast(calibration2_)->toK(); - K2.convertTo(K2,CV_64F); - - cv::Mat cvTlr; - cv::eigen2cv(Tlr_.inverse().matrix3x4(),cvTlr); - cv::Mat R12 = cvTlr.rowRange(0,3).colRange(0,3); - R12.convertTo(R12,CV_64F); - cv::Mat t12 = cvTlr.rowRange(0,3).col(3); - t12.convertTo(t12,CV_64F); - - cv::Mat R_r1_u1, R_r2_u2; - cv::Mat P1, P2, Q; - - cv::stereoRectify(K1,camera1DistortionCoef(),K2,camera2DistortionCoef(),newImSize_, - R12, t12, - R_r1_u1,R_r2_u2,P1,P2,Q, - cv::CALIB_ZERO_DISPARITY,-1,newImSize_); - cv::initUndistortRectifyMap(K1, camera1DistortionCoef(), R_r1_u1, P1.rowRange(0, 3).colRange(0, 3), - newImSize_, CV_32F, M1l_, M2l_); - cv::initUndistortRectifyMap(K2, camera2DistortionCoef(), R_r2_u2, P2.rowRange(0, 3).colRange(0, 3), - newImSize_, CV_32F, M1r_, M2r_); - - //Update calibration - calibration1_->setParameter(P1.at(0,0), 0); - calibration1_->setParameter(P1.at(1,1), 1); - calibration1_->setParameter(P1.at(0,2), 2); - calibration1_->setParameter(P1.at(1,2), 3); - - //Update bf - bf_ = b_ * P1.at(0,0); - - //Update relative pose between camera 1 and IMU if necessary - if(sensor_ == System::IMU_STEREO){ - Eigen::Matrix3f eigenR_r1_u1; - cv::cv2eigen(R_r1_u1,eigenR_r1_u1); - Sophus::SE3f T_r1_u1(eigenR_r1_u1,Eigen::Vector3f::Zero()); - Tbc_ = Tbc_ * T_r1_u1.inverse(); + if ((sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) && cameraType_ != Rectified) + { + calibration2_->setParameter(calibration2_->getParameter(1) * scaleRowFactor, 1); + calibration2_->setParameter(calibration2_->getParameter(3) * scaleRowFactor, 3); + } } } - ostream &operator<<(std::ostream& output, const Settings& settings){ - output << "SLAM settings: " << endl; + int newWidth = readParameter(fSettings, "Camera.newWidth", found, false); + if (found) + { + bNeedToResize1_ = true; + newImSize_.width = newWidth; - output << "\t-Camera 1 parameters ("; - if(settings.cameraType_ == Settings::PinHole || settings.cameraType_ == Settings::Rectified){ - output << "Pinhole"; + if (!bNeedToRectify_) + { + // Update calibration + float scaleColFactor = (float)newImSize_.width / (float)originalImSize_.width; + calibration1_->setParameter(calibration1_->getParameter(0) * scaleColFactor, 0); + calibration1_->setParameter(calibration1_->getParameter(2) * scaleColFactor, 2); + + if ((sensor_ == System::STEREO || sensor_ == System::IMU_STEREO) && cameraType_ != Rectified) + { + calibration2_->setParameter(calibration2_->getParameter(0) * scaleColFactor, 0); + calibration2_->setParameter(calibration2_->getParameter(2) * scaleColFactor, 2); + + if (cameraType_ == KannalaBrandt) + { + static_cast(calibration1_)->mvLappingArea[0] *= scaleColFactor; + static_cast(calibration1_)->mvLappingArea[1] *= scaleColFactor; + + static_cast(calibration2_)->mvLappingArea[0] *= scaleColFactor; + static_cast(calibration2_)->mvLappingArea[1] *= scaleColFactor; + } + } } - else{ - output << "Kannala-Brandt"; - } - output << ")" << ": ["; - for(size_t i = 0; i < settings.originalCalib1_->size(); i++){ - output << " " << settings.originalCalib1_->getParameter(i); + } + + fps_ = readParameter(fSettings, "Camera.fps", found); + bRGB_ = (bool)readParameter(fSettings, "Camera.RGB", found); +} + +void Settings::readIMU(cv::FileStorage &fSettings) +{ + bool found; + noiseGyro_ = readParameter(fSettings, "IMU.NoiseGyro", found); + noiseAcc_ = readParameter(fSettings, "IMU.NoiseAcc", found); + gyroWalk_ = readParameter(fSettings, "IMU.GyroWalk", found); + accWalk_ = readParameter(fSettings, "IMU.AccWalk", found); + imuFrequency_ = readParameter(fSettings, "IMU.Frequency", found); + + cv::Mat cvTbc = readParameter(fSettings, "IMU.T_b_c1", found); + Tbc_ = Converter::toSophus(cvTbc); + + readParameter(fSettings, "IMU.InsertKFsWhenLost", found, false); + if (found) + { + insertKFsWhenLost_ = (bool)readParameter(fSettings, "IMU.InsertKFsWhenLost", found, false); + } + else + { + insertKFsWhenLost_ = true; + } +} + +void Settings::readRGBD(cv::FileStorage &fSettings) +{ + bool found; + + depthMapFactor_ = readParameter(fSettings, "RGBD.DepthMapFactor", found); + thDepth_ = readParameter(fSettings, "Stereo.ThDepth", found); + b_ = readParameter(fSettings, "Stereo.b", found); + bf_ = b_ * calibration1_->getParameter(0); +} + +void Settings::readORB(cv::FileStorage &fSettings) +{ + bool found; + + nFeatures_ = readParameter(fSettings, "ORBextractor.nFeatures", found); + scaleFactor_ = readParameter(fSettings, "ORBextractor.scaleFactor", found); + nLevels_ = readParameter(fSettings, "ORBextractor.nLevels", found); + initThFAST_ = readParameter(fSettings, "ORBextractor.iniThFAST", found); + minThFAST_ = readParameter(fSettings, "ORBextractor.minThFAST", found); +} + +void Settings::readViewer(cv::FileStorage &fSettings) +{ + bool found; + + keyFrameSize_ = readParameter(fSettings, "Viewer.KeyFrameSize", found); + keyFrameLineWidth_ = readParameter(fSettings, "Viewer.KeyFrameLineWidth", found); + graphLineWidth_ = readParameter(fSettings, "Viewer.GraphLineWidth", found); + pointSize_ = readParameter(fSettings, "Viewer.PointSize", found); + cameraSize_ = readParameter(fSettings, "Viewer.CameraSize", found); + cameraLineWidth_ = readParameter(fSettings, "Viewer.CameraLineWidth", found); + viewPointX_ = readParameter(fSettings, "Viewer.ViewpointX", found); + viewPointY_ = readParameter(fSettings, "Viewer.ViewpointY", found); + viewPointZ_ = readParameter(fSettings, "Viewer.ViewpointZ", found); + viewPointF_ = readParameter(fSettings, "Viewer.ViewpointF", found); + imageViewerScale_ = readParameter(fSettings, "Viewer.imageViewScale", found, false); + + if (!found) + imageViewerScale_ = 1.0f; +} + +void Settings::readLoadAndSave(cv::FileStorage &fSettings) +{ + bool found; + + sLoadFrom_ = readParameter(fSettings, "System.LoadAtlasFromFile", found, false); + sSaveto_ = readParameter(fSettings, "System.SaveAtlasToFile", found, false); +} + +void Settings::readOtherParameters(cv::FileStorage &fSettings) +{ + bool found; + + thFarPoints_ = readParameter(fSettings, "System.thFarPoints", found, false); +} + +void Settings::precomputeRectificationMaps() +{ + // Precompute rectification maps, new calibrations, ... + cv::Mat K1 = static_cast(calibration1_)->toK(); + K1.convertTo(K1, CV_64F); + cv::Mat K2 = static_cast(calibration2_)->toK(); + K2.convertTo(K2, CV_64F); + + cv::Mat cvTlr; + cv::eigen2cv(Tlr_.inverse().matrix3x4(), cvTlr); + cv::Mat R12 = cvTlr.rowRange(0, 3).colRange(0, 3); + R12.convertTo(R12, CV_64F); + cv::Mat t12 = cvTlr.rowRange(0, 3).col(3); + t12.convertTo(t12, CV_64F); + + cv::Mat R_r1_u1, R_r2_u2; + cv::Mat P1, P2, Q; + + cv::stereoRectify(K1, camera1DistortionCoef(), K2, camera2DistortionCoef(), newImSize_, + R12, t12, + R_r1_u1, R_r2_u2, P1, P2, Q, + cv::CALIB_ZERO_DISPARITY, -1, newImSize_); + cv::initUndistortRectifyMap(K1, camera1DistortionCoef(), R_r1_u1, P1.rowRange(0, 3).colRange(0, 3), + newImSize_, CV_32F, M1l_, M2l_); + cv::initUndistortRectifyMap(K2, camera2DistortionCoef(), R_r2_u2, P2.rowRange(0, 3).colRange(0, 3), + newImSize_, CV_32F, M1r_, M2r_); + + // Update calibration + calibration1_->setParameter(P1.at(0, 0), 0); + calibration1_->setParameter(P1.at(1, 1), 1); + calibration1_->setParameter(P1.at(0, 2), 2); + calibration1_->setParameter(P1.at(1, 2), 3); + + // Update bf + bf_ = b_ * P1.at(0, 0); + + // Update relative pose between camera 1 and IMU if necessary + if (sensor_ == System::IMU_STEREO) + { + Eigen::Matrix3f eigenR_r1_u1; + cv::cv2eigen(R_r1_u1, eigenR_r1_u1); + Sophus::SE3f T_r1_u1(eigenR_r1_u1, Eigen::Vector3f::Zero()); + Tbc_ = Tbc_ * T_r1_u1.inverse(); + } +} + +ostream &operator<<(std::ostream &output, const Settings &settings) +{ + output << "SLAM settings: " << endl; + + output << "\t-Camera 1 parameters ("; + if (settings.cameraType_ == Settings::PinHole || settings.cameraType_ == Settings::Rectified) + { + output << "Pinhole"; + } + else + { + output << "Kannala-Brandt"; + } + output << ")" + << ": ["; + for (size_t i = 0; i < settings.originalCalib1_->size(); i++) + { + output << " " << settings.originalCalib1_->getParameter(i); + } + output << " ]" << endl; + + if (!settings.vPinHoleDistorsion1_.empty()) + { + output << "\t-Camera 1 distortion parameters: [ "; + for (float d : settings.vPinHoleDistorsion1_) + { + output << " " << d; } output << " ]" << endl; + } - if(!settings.vPinHoleDistorsion1_.empty()){ - output << "\t-Camera 1 distortion parameters: [ "; - for(float d : settings.vPinHoleDistorsion1_){ - output << " " << d; - } - 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 ("; - if(settings.cameraType_ == Settings::PinHole || settings.cameraType_ == Settings::Rectified){ + if (settings.cameraType_ == Settings::PinHole) + { output << "Pinhole"; } - else{ + else + { output << "Kannala-Brandt"; } - output << "" << ": ["; - for(size_t i = 0; i < settings.originalCalib2_->size(); i++){ + output << "" + << ": ["; + for (size_t i = 0; i < settings.originalCalib2_->size(); i++) + { output << " " << settings.originalCalib2_->getParameter(i); } output << " ]" << endl; - if(!settings.vPinHoleDistorsion2_.empty()){ + if (!settings.vPinHoleDistorsion2_.empty()) + { output << "\t-Camera 1 distortion parameters: [ "; - for(float d : settings.vPinHoleDistorsion2_){ + for (float d : settings.vPinHoleDistorsion2_) + { output << " " << d; } output << " ]" << 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; - - if(settings.bNeedToRectify_){ - output << "\t-Camera 1 parameters after rectification: [ "; - for(size_t i = 0; i < settings.calibration1_->size(); i++){ - output << " " << settings.calibration1_->getParameter(i); - } - output << " ]" << endl; - } - else if(settings.bNeedToResize1_){ - output << "\t-Camera 1 parameters after resize: [ "; - for(size_t i = 0; i < settings.calibration1_->size(); i++){ - output << " " << settings.calibration1_->getParameter(i); - } - output << " ]" << endl; - - if((settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO) && - settings.cameraType_ == Settings::KannalaBrandt){ - output << "\t-Camera 2 parameters after resize: [ "; - for(size_t i = 0; i < settings.calibration2_->size(); i++){ - output << " " << settings.calibration2_->getParameter(i); - } - output << " ]" << endl; - } - } - - output << "\t-Sequence FPS: " << settings.fps_ << endl; - - //Stereo stuff - if(settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO){ - output << "\t-Stereo baseline: " << settings.b_ << endl; - output << "\t-Stereo depth threshold : " << settings.thDepth_ << endl; - - if(settings.cameraType_ == Settings::KannalaBrandt){ - auto vOverlapping1 = static_cast(settings.calibration1_)->mvLappingArea; - auto vOverlapping2 = static_cast(settings.calibration2_)->mvLappingArea; - output << "\t-Camera 1 overlapping area: [ " << vOverlapping1[0] << " , " << vOverlapping1[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) { - output << "\t-Gyro noise: " << settings.noiseGyro_ << endl; - output << "\t-Accelerometer noise: " << settings.noiseAcc_ << endl; - output << "\t-Gyro walk: " << settings.gyroWalk_ << endl; - output << "\t-Accelerometer walk: " << settings.accWalk_ << endl; - output << "\t-IMU frequency: " << settings.imuFrequency_ << endl; - } - - if(settings.sensor_ == System::RGBD || settings.sensor_ == System::IMU_RGBD){ - output << "\t-RGB-D depth map factor: " << settings.depthMapFactor_ << endl; - } - - output << "\t-Features per image: " << settings.nFeatures_ << endl; - output << "\t-ORB scale factor: " << settings.scaleFactor_ << endl; - output << "\t-ORB number of scales: " << settings.nLevels_ << endl; - output << "\t-Initial FAST threshold: " << settings.initThFAST_ << endl; - output << "\t-Min FAST threshold: " << settings.minThFAST_ << endl; - - return output; } + + output << "\t-Original image size: [ " << settings.originalImSize_.width << " , " << settings.originalImSize_.height << " ]" << endl; + output << "\t-Current image size: [ " << settings.newImSize_.width << " , " << settings.newImSize_.height << " ]" << endl; + + if (settings.bNeedToRectify_) + { + output << "\t-Camera 1 parameters after rectification: [ "; + for (size_t i = 0; i < settings.calibration1_->size(); i++) + { + output << " " << settings.calibration1_->getParameter(i); + } + output << " ]" << endl; + } + else if (settings.bNeedToResize1_) + { + output << "\t-Camera 1 parameters after resize: [ "; + for (size_t i = 0; i < settings.calibration1_->size(); i++) + { + output << " " << settings.calibration1_->getParameter(i); + } + output << " ]" << endl; + + if ((settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO) && + settings.cameraType_ == Settings::KannalaBrandt) + { + output << "\t-Camera 2 parameters after resize: [ "; + for (size_t i = 0; i < settings.calibration2_->size(); i++) + { + output << " " << settings.calibration2_->getParameter(i); + } + output << " ]" << endl; + } + } + + output << "\t-Sequence FPS: " << settings.fps_ << endl; + + // Stereo stuff + if (settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO) + { + output << "\t-Stereo baseline: " << settings.b_ << endl; + output << "\t-Stereo depth threshold : " << settings.thDepth_ << endl; + + if (settings.cameraType_ == Settings::KannalaBrandt) + { + auto vOverlapping1 = static_cast(settings.calibration1_)->mvLappingArea; + auto vOverlapping2 = static_cast(settings.calibration2_)->mvLappingArea; + output << "\t-Camera 1 overlapping area: [ " << vOverlapping1[0] << " , " << vOverlapping1[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) + { + output << "\t-Gyro noise: " << settings.noiseGyro_ << endl; + output << "\t-Accelerometer noise: " << settings.noiseAcc_ << endl; + output << "\t-Gyro walk: " << settings.gyroWalk_ << endl; + output << "\t-Accelerometer walk: " << settings.accWalk_ << endl; + output << "\t-IMU frequency: " << settings.imuFrequency_ << endl; + } + + if (settings.sensor_ == System::RGBD || settings.sensor_ == System::IMU_RGBD) + { + output << "\t-RGB-D depth map factor: " << settings.depthMapFactor_ << endl; + } + + output << "\t-Features per image: " << settings.nFeatures_ << endl; + output << "\t-ORB scale factor: " << settings.scaleFactor_ << endl; + output << "\t-ORB number of scales: " << settings.nLevels_ << endl; + output << "\t-Initial FAST threshold: " << settings.initThFAST_ << endl; + output << "\t-Min FAST threshold: " << settings.minThFAST_ << endl; + + return output; +} };