修改回环bug
wuqing
parent
0b0f2d49ed
commit
5d57b1d807
|
|
@ -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
|
||||
|
||||
|
|
|
|||
|
|
@ -180,7 +180,7 @@ protected:
|
|||
std::vector<KeyFrame*> mvpCurrentConnectedKFs;
|
||||
std::vector<MapPoint*> mvpCurrentMatchedPoints;
|
||||
std::vector<MapPoint*> mvpLoopMapPoints;
|
||||
cv::Mat mScw;
|
||||
|
||||
g2o::Sim3 mg2oScw;
|
||||
|
||||
//-------
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
||||
|
|
|
|||
|
|
@ -179,8 +179,8 @@ void LoopClosing::Run()
|
|||
// If inertial, force only yaw
|
||||
// 如果是imu模式并且完成了初始化,强制将焊接变换的 roll 和 pitch 设为0
|
||||
// 通过物理约束来保证两个坐标轴都是水平的
|
||||
if ((mpTracker->mSensor==System::IMU_MONOCULAR ||mpTracker->mSensor==System::IMU_STEREO) &&
|
||||
mpCurrentKF->GetMap()->GetIniertialBA1()) // TODO, maybe with GetIniertialBA1
|
||||
if ((mpTracker->mSensor==System::IMU_MONOCULAR || mpTracker->mSensor==System::IMU_STEREO || mpTracker->mSensor==System::IMU_RGBD) &&
|
||||
mpCurrentKF->GetMap()->GetIniertialBA1())
|
||||
{
|
||||
Eigen::Vector3d phi = LogSO3(mSold_new.rotation().toRotationMatrix());
|
||||
phi(0)=0;
|
||||
|
|
@ -693,8 +693,7 @@ bool LoopClosing::DetectAndReffineSim3FromLastKF(KeyFrame* pCurrentKF, KeyFrame*
|
|||
if(numOptMatches > nProjOptMatches)
|
||||
{
|
||||
//!bug, 以下gScw_estimation应该通过上述sim3优化后的位姿来更新。以下mScw应该改为 gscm * gswm^-1
|
||||
g2o::Sim3 gScw_estimation(Converter::toMatrix3d(mScw.rowRange(0, 3).colRange(0, 3)),
|
||||
Converter::toVector3d(mScw.rowRange(0, 3).col(3)),1.0);
|
||||
g2o::Sim3 gScw_estimation(gScw.rotation(), gScw.translation(),1.0);
|
||||
|
||||
vector<MapPoint*> vpMatchedMP;
|
||||
vpMatchedMP.resize(mpCurrentKF->GetMapPointMatches().size(), static_cast<MapPoint*>(NULL));
|
||||
|
|
|
|||
638
src/Settings.cc
638
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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include "Settings.h"
|
||||
|
||||
|
|
@ -30,136 +30,165 @@
|
|||
|
||||
using namespace std;
|
||||
|
||||
namespace ORB_SLAM3 {
|
||||
namespace ORB_SLAM3
|
||||
{
|
||||
|
||||
template<>
|
||||
float Settings::readParameter<float>(cv::FileStorage& fSettings, const std::string& name, bool& found, const bool required){
|
||||
template <>
|
||||
float Settings::readParameter<float>(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required)
|
||||
{
|
||||
cv::FileNode node = fSettings[name];
|
||||
if(node.empty()){
|
||||
if(required){
|
||||
if (node.empty())
|
||||
{
|
||||
if (required)
|
||||
{
|
||||
std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
else{
|
||||
else
|
||||
{
|
||||
std::cerr << name << " optional parameter does not exist..." << std::endl;
|
||||
found = false;
|
||||
return 0.0f;
|
||||
}
|
||||
}
|
||||
else if(!node.isReal()){
|
||||
else if (!node.isReal())
|
||||
{
|
||||
std::cerr << name << " parameter must be a real number, aborting..." << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
else{
|
||||
else
|
||||
{
|
||||
found = true;
|
||||
return node.real();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template<>
|
||||
int Settings::readParameter<int>(cv::FileStorage& fSettings, const std::string& name, bool& found, const bool required){
|
||||
template <>
|
||||
int Settings::readParameter<int>(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required)
|
||||
{
|
||||
cv::FileNode node = fSettings[name];
|
||||
if(node.empty()){
|
||||
if(required){
|
||||
if (node.empty())
|
||||
{
|
||||
if (required)
|
||||
{
|
||||
std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
else{
|
||||
else
|
||||
{
|
||||
std::cerr << name << " optional parameter does not exist..." << std::endl;
|
||||
found = false;
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
else if(!node.isInt()){
|
||||
else if (!node.isInt())
|
||||
{
|
||||
std::cerr << name << " parameter must be an integer number, aborting..." << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
else{
|
||||
else
|
||||
{
|
||||
found = true;
|
||||
return node.operator int();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template<>
|
||||
string Settings::readParameter<string>(cv::FileStorage& fSettings, const std::string& name, bool& found, const bool required){
|
||||
template <>
|
||||
string Settings::readParameter<string>(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required)
|
||||
{
|
||||
cv::FileNode node = fSettings[name];
|
||||
if(node.empty()){
|
||||
if(required){
|
||||
if (node.empty())
|
||||
{
|
||||
if (required)
|
||||
{
|
||||
std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
else{
|
||||
else
|
||||
{
|
||||
std::cerr << name << " optional parameter does not exist..." << std::endl;
|
||||
found = false;
|
||||
return string();
|
||||
}
|
||||
}
|
||||
else if(!node.isString()){
|
||||
else if (!node.isString())
|
||||
{
|
||||
std::cerr << name << " parameter must be a string, aborting..." << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
else{
|
||||
else
|
||||
{
|
||||
found = true;
|
||||
return node.string();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
template<>
|
||||
cv::Mat Settings::readParameter<cv::Mat>(cv::FileStorage& fSettings, const std::string& name, bool& found, const bool required){
|
||||
template <>
|
||||
cv::Mat Settings::readParameter<cv::Mat>(cv::FileStorage &fSettings, const std::string &name, bool &found, const bool required)
|
||||
{
|
||||
cv::FileNode node = fSettings[name];
|
||||
if(node.empty()){
|
||||
if(required){
|
||||
if (node.empty())
|
||||
{
|
||||
if (required)
|
||||
{
|
||||
std::cerr << name << " required parameter does not exist, aborting..." << std::endl;
|
||||
exit(-1);
|
||||
}
|
||||
else{
|
||||
else
|
||||
{
|
||||
std::cerr << name << " optional parameter does not exist..." << std::endl;
|
||||
found = false;
|
||||
return cv::Mat();
|
||||
}
|
||||
}
|
||||
else{
|
||||
else
|
||||
{
|
||||
found = true;
|
||||
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;
|
||||
|
||||
//Open settings file
|
||||
// Open settings file
|
||||
cv::FileStorage fSettings(configFile, cv::FileStorage::READ);
|
||||
if (!fSettings.isOpened()) {
|
||||
if (!fSettings.isOpened())
|
||||
{
|
||||
cerr << "[ERROR]: could not open configuration file at: " << configFile << endl;
|
||||
cerr << "Aborting..." << endl;
|
||||
|
||||
exit(-1);
|
||||
}
|
||||
else{
|
||||
else
|
||||
{
|
||||
cout << "Loading settings from " << configFile << endl;
|
||||
}
|
||||
|
||||
//Read first camera
|
||||
// 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){
|
||||
// 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
|
||||
// Read image info
|
||||
readImageInfo(fSettings);
|
||||
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);
|
||||
cout << "\t-Loaded IMU calibration" << endl;
|
||||
}
|
||||
|
||||
if(sensor_ == System::RGBD || sensor_ == System::IMU_RGBD){
|
||||
if (sensor_ == System::RGBD || sensor_ == System::IMU_RGBD)
|
||||
{
|
||||
readRGBD(fSettings);
|
||||
cout << "\t-Loaded RGB-D calibration" << endl;
|
||||
}
|
||||
|
|
@ -173,427 +202,482 @@ namespace ORB_SLAM3 {
|
|||
readOtherParameters(fSettings);
|
||||
cout << "\t-Loaded misc parameters" << endl;
|
||||
|
||||
if(bNeedToRectify_){
|
||||
if (bNeedToRectify_)
|
||||
{
|
||||
precomputeRectificationMaps();
|
||||
cout << "\t-Computed rectification maps" << endl;
|
||||
}
|
||||
|
||||
cout << "----------------------------------" << endl;
|
||||
}
|
||||
}
|
||||
|
||||
void Settings::readCamera1(cv::FileStorage &fSettings) {
|
||||
void Settings::readCamera1(cv::FileStorage &fSettings)
|
||||
{
|
||||
bool found;
|
||||
|
||||
//Read camera model
|
||||
string cameraModel = readParameter<string>(fSettings,"Camera.type",found);
|
||||
// Read camera model
|
||||
string cameraModel = readParameter<string>(fSettings, "Camera.type", found);
|
||||
|
||||
vector<float> vCalibration;
|
||||
if (cameraModel == "PinHole") {
|
||||
if (cameraModel == "PinHole")
|
||||
{
|
||||
cameraType_ = PinHole;
|
||||
|
||||
//Read intrinsic parameters
|
||||
float fx = readParameter<float>(fSettings,"Camera1.fx",found);
|
||||
float fy = readParameter<float>(fSettings,"Camera1.fy",found);
|
||||
float cx = readParameter<float>(fSettings,"Camera1.cx",found);
|
||||
float cy = readParameter<float>(fSettings,"Camera1.cy",found);
|
||||
// Read intrinsic parameters
|
||||
float fx = readParameter<float>(fSettings, "Camera1.fx", found);
|
||||
float fy = readParameter<float>(fSettings, "Camera1.fy", found);
|
||||
float cx = readParameter<float>(fSettings, "Camera1.cx", found);
|
||||
float cy = readParameter<float>(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<float>(fSettings,"Camera1.k1",found,false);
|
||||
if(found){
|
||||
readParameter<float>(fSettings,"Camera1.k3",found,false);
|
||||
if(found){
|
||||
// Check if it is a distorted PinHole
|
||||
readParameter<float>(fSettings, "Camera1.k1", found, false);
|
||||
if (found)
|
||||
{
|
||||
readParameter<float>(fSettings, "Camera1.k3", found, false);
|
||||
if (found)
|
||||
{
|
||||
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_[0] = readParameter<float>(fSettings,"Camera1.k1",found);
|
||||
vPinHoleDistorsion1_[1] = readParameter<float>(fSettings,"Camera1.k2",found);
|
||||
vPinHoleDistorsion1_[2] = readParameter<float>(fSettings,"Camera1.p1",found);
|
||||
vPinHoleDistorsion1_[3] = readParameter<float>(fSettings,"Camera1.p2",found);
|
||||
vPinHoleDistorsion1_[0] = readParameter<float>(fSettings, "Camera1.k1", found);
|
||||
vPinHoleDistorsion1_[1] = readParameter<float>(fSettings, "Camera1.k2", found);
|
||||
vPinHoleDistorsion1_[2] = readParameter<float>(fSettings, "Camera1.p1", found);
|
||||
vPinHoleDistorsion1_[3] = readParameter<float>(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){
|
||||
// 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"){
|
||||
else if (cameraModel == "Rectified")
|
||||
{
|
||||
cameraType_ = Rectified;
|
||||
|
||||
//Read intrinsic parameters
|
||||
float fx = readParameter<float>(fSettings,"Camera1.fx",found);
|
||||
float fy = readParameter<float>(fSettings,"Camera1.fy",found);
|
||||
float cx = readParameter<float>(fSettings,"Camera1.cx",found);
|
||||
float cy = readParameter<float>(fSettings,"Camera1.cy",found);
|
||||
// Read intrinsic parameters
|
||||
float fx = readParameter<float>(fSettings, "Camera1.fx", found);
|
||||
float fy = readParameter<float>(fSettings, "Camera1.fy", found);
|
||||
float cx = readParameter<float>(fSettings, "Camera1.cx", found);
|
||||
float cy = readParameter<float>(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)
|
||||
// Rectified images are assumed to be ideal PinHole images (no distortion)
|
||||
}
|
||||
else if(cameraModel == "KannalaBrandt8"){
|
||||
else if (cameraModel == "KannalaBrandt8")
|
||||
{
|
||||
cameraType_ = KannalaBrandt;
|
||||
|
||||
//Read intrinsic parameters
|
||||
float fx = readParameter<float>(fSettings,"Camera1.fx",found);
|
||||
float fy = readParameter<float>(fSettings,"Camera1.fy",found);
|
||||
float cx = readParameter<float>(fSettings,"Camera1.cx",found);
|
||||
float cy = readParameter<float>(fSettings,"Camera1.cy",found);
|
||||
// Read intrinsic parameters
|
||||
float fx = readParameter<float>(fSettings, "Camera1.fx", found);
|
||||
float fy = readParameter<float>(fSettings, "Camera1.fy", found);
|
||||
float cx = readParameter<float>(fSettings, "Camera1.cx", found);
|
||||
float cy = readParameter<float>(fSettings, "Camera1.cy", found);
|
||||
|
||||
float k0 = readParameter<float>(fSettings,"Camera1.k1",found);
|
||||
float k1 = readParameter<float>(fSettings,"Camera1.k2",found);
|
||||
float k2 = readParameter<float>(fSettings,"Camera1.k3",found);
|
||||
float k3 = readParameter<float>(fSettings,"Camera1.k4",found);
|
||||
float k0 = readParameter<float>(fSettings, "Camera1.k1", found);
|
||||
float k1 = readParameter<float>(fSettings, "Camera1.k2", found);
|
||||
float k2 = readParameter<float>(fSettings, "Camera1.k3", 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);
|
||||
originalCalib1_ = new KannalaBrandt8(vCalibration);
|
||||
|
||||
if(sensor_ == System::STEREO || sensor_ == System::IMU_STEREO){
|
||||
int colBegin = readParameter<int>(fSettings,"Camera1.overlappingBegin",found);
|
||||
int colEnd = readParameter<int>(fSettings,"Camera1.overlappingEnd",found);
|
||||
if (sensor_ == System::STEREO || sensor_ == System::IMU_STEREO)
|
||||
{
|
||||
int colBegin = readParameter<int>(fSettings, "Camera1.overlappingBegin", found);
|
||||
int colEnd = readParameter<int>(fSettings, "Camera1.overlappingEnd", found);
|
||||
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;
|
||||
exit(-1);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Settings::readCamera2(cv::FileStorage &fSettings) {
|
||||
void Settings::readCamera2(cv::FileStorage &fSettings)
|
||||
{
|
||||
bool found;
|
||||
vector<float> vCalibration;
|
||||
if (cameraType_ == PinHole) {
|
||||
if (cameraType_ == PinHole)
|
||||
{
|
||||
bNeedToRectify_ = true;
|
||||
|
||||
//Read intrinsic parameters
|
||||
float fx = readParameter<float>(fSettings,"Camera2.fx",found);
|
||||
float fy = readParameter<float>(fSettings,"Camera2.fy",found);
|
||||
float cx = readParameter<float>(fSettings,"Camera2.cx",found);
|
||||
float cy = readParameter<float>(fSettings,"Camera2.cy",found);
|
||||
|
||||
// Read intrinsic parameters
|
||||
float fx = readParameter<float>(fSettings, "Camera2.fx", found);
|
||||
float fy = readParameter<float>(fSettings, "Camera2.fy", found);
|
||||
float cx = readParameter<float>(fSettings, "Camera2.cx", found);
|
||||
float cy = readParameter<float>(fSettings, "Camera2.cy", found);
|
||||
|
||||
vCalibration = {fx, fy, cx, cy};
|
||||
|
||||
calibration2_ = new Pinhole(vCalibration);
|
||||
originalCalib2_ = new Pinhole(vCalibration);
|
||||
|
||||
//Check if it is a distorted PinHole
|
||||
readParameter<float>(fSettings,"Camera2.k1",found,false);
|
||||
if(found){
|
||||
readParameter<float>(fSettings,"Camera2.k3",found,false);
|
||||
if(found){
|
||||
// Check if it is a distorted PinHole
|
||||
readParameter<float>(fSettings, "Camera2.k1", found, false);
|
||||
if (found)
|
||||
{
|
||||
readParameter<float>(fSettings, "Camera2.k3", found, false);
|
||||
if (found)
|
||||
{
|
||||
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_[0] = readParameter<float>(fSettings,"Camera2.k1",found);
|
||||
vPinHoleDistorsion2_[1] = readParameter<float>(fSettings,"Camera2.k2",found);
|
||||
vPinHoleDistorsion2_[2] = readParameter<float>(fSettings,"Camera2.p1",found);
|
||||
vPinHoleDistorsion2_[3] = readParameter<float>(fSettings,"Camera2.p2",found);
|
||||
vPinHoleDistorsion2_[0] = readParameter<float>(fSettings, "Camera2.k1", found);
|
||||
vPinHoleDistorsion2_[1] = readParameter<float>(fSettings, "Camera2.k2", found);
|
||||
vPinHoleDistorsion2_[2] = readParameter<float>(fSettings, "Camera2.p1", found);
|
||||
vPinHoleDistorsion2_[3] = readParameter<float>(fSettings, "Camera2.p2", found);
|
||||
}
|
||||
}
|
||||
else if(cameraType_ == KannalaBrandt){
|
||||
//Read intrinsic parameters
|
||||
float fx = readParameter<float>(fSettings,"Camera2.fx",found);
|
||||
float fy = readParameter<float>(fSettings,"Camera2.fy",found);
|
||||
float cx = readParameter<float>(fSettings,"Camera2.cx",found);
|
||||
float cy = readParameter<float>(fSettings,"Camera2.cy",found);
|
||||
else if (cameraType_ == KannalaBrandt)
|
||||
{
|
||||
// Read intrinsic parameters
|
||||
float fx = readParameter<float>(fSettings, "Camera2.fx", found);
|
||||
float fy = readParameter<float>(fSettings, "Camera2.fy", found);
|
||||
float cx = readParameter<float>(fSettings, "Camera2.cx", found);
|
||||
float cy = readParameter<float>(fSettings, "Camera2.cy", found);
|
||||
|
||||
float k0 = readParameter<float>(fSettings,"Camera1.k1",found);
|
||||
float k1 = readParameter<float>(fSettings,"Camera1.k2",found);
|
||||
float k2 = readParameter<float>(fSettings,"Camera1.k3",found);
|
||||
float k3 = readParameter<float>(fSettings,"Camera1.k4",found);
|
||||
float k0 = readParameter<float>(fSettings, "Camera1.k1", found);
|
||||
float k1 = readParameter<float>(fSettings, "Camera1.k2", found);
|
||||
float k2 = readParameter<float>(fSettings, "Camera1.k3", 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);
|
||||
originalCalib2_ = new KannalaBrandt8(vCalibration);
|
||||
|
||||
int colBegin = readParameter<int>(fSettings,"Camera2.overlappingBegin",found);
|
||||
int colEnd = readParameter<int>(fSettings,"Camera2.overlappingEnd",found);
|
||||
int colBegin = readParameter<int>(fSettings, "Camera2.overlappingBegin", found);
|
||||
int colEnd = readParameter<int>(fSettings, "Camera2.overlappingEnd", found);
|
||||
vector<int> vOverlapping = {colBegin, colEnd};
|
||||
|
||||
static_cast<KannalaBrandt8*>(calibration2_)->mvLappingArea = vOverlapping;
|
||||
static_cast<KannalaBrandt8 *>(calibration2_)->mvLappingArea = vOverlapping;
|
||||
}
|
||||
|
||||
//Load stereo extrinsic calibration
|
||||
if(cameraType_ == Rectified){
|
||||
b_ = readParameter<float>(fSettings,"Stereo.b",found);
|
||||
// Load stereo extrinsic calibration
|
||||
if (cameraType_ == Rectified)
|
||||
{
|
||||
b_ = readParameter<float>(fSettings, "Stereo.b", found);
|
||||
bf_ = b_ * calibration1_->getParameter(0);
|
||||
}
|
||||
else{
|
||||
cv::Mat cvTlr = readParameter<cv::Mat>(fSettings,"Stereo.T_c1_c2",found);
|
||||
else
|
||||
{
|
||||
cv::Mat cvTlr = readParameter<cv::Mat>(fSettings, "Stereo.T_c1_c2", found);
|
||||
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();
|
||||
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;
|
||||
//Read original and desired image dimensions
|
||||
int originalRows = readParameter<int>(fSettings,"Camera.height",found);
|
||||
int originalCols = readParameter<int>(fSettings,"Camera.width",found);
|
||||
// Read original and desired image dimensions
|
||||
int originalRows = readParameter<int>(fSettings, "Camera.height", found);
|
||||
int originalCols = readParameter<int>(fSettings, "Camera.width", found);
|
||||
originalImSize_.width = originalCols;
|
||||
originalImSize_.height = originalRows;
|
||||
|
||||
newImSize_ = originalImSize_;
|
||||
int newHeigh = readParameter<int>(fSettings,"Camera.newHeight",found,false);
|
||||
if(found){
|
||||
int newHeigh = readParameter<int>(fSettings, "Camera.newHeight", found, false);
|
||||
if (found)
|
||||
{
|
||||
bNeedToResize1_ = true;
|
||||
newImSize_.height = newHeigh;
|
||||
|
||||
if(!bNeedToRectify_){
|
||||
//Update calibration
|
||||
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((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(3) * scaleRowFactor, 3);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
int newWidth = readParameter<int>(fSettings,"Camera.newWidth",found,false);
|
||||
if(found){
|
||||
int newWidth = readParameter<int>(fSettings, "Camera.newWidth", found, false);
|
||||
if (found)
|
||||
{
|
||||
bNeedToResize1_ = true;
|
||||
newImSize_.width = newWidth;
|
||||
|
||||
if(!bNeedToRectify_){
|
||||
//Update calibration
|
||||
float scaleColFactor = (float)newImSize_.width /(float) originalImSize_.width;
|
||||
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){
|
||||
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<KannalaBrandt8*>(calibration1_)->mvLappingArea[0] *= scaleColFactor;
|
||||
static_cast<KannalaBrandt8*>(calibration1_)->mvLappingArea[1] *= scaleColFactor;
|
||||
if (cameraType_ == KannalaBrandt)
|
||||
{
|
||||
static_cast<KannalaBrandt8 *>(calibration1_)->mvLappingArea[0] *= scaleColFactor;
|
||||
static_cast<KannalaBrandt8 *>(calibration1_)->mvLappingArea[1] *= scaleColFactor;
|
||||
|
||||
static_cast<KannalaBrandt8*>(calibration2_)->mvLappingArea[0] *= scaleColFactor;
|
||||
static_cast<KannalaBrandt8*>(calibration2_)->mvLappingArea[1] *= scaleColFactor;
|
||||
static_cast<KannalaBrandt8 *>(calibration2_)->mvLappingArea[0] *= scaleColFactor;
|
||||
static_cast<KannalaBrandt8 *>(calibration2_)->mvLappingArea[1] *= scaleColFactor;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
fps_ = readParameter<int>(fSettings,"Camera.fps",found);
|
||||
bRGB_ = (bool) readParameter<int>(fSettings,"Camera.RGB",found);
|
||||
}
|
||||
fps_ = readParameter<int>(fSettings, "Camera.fps", found);
|
||||
bRGB_ = (bool)readParameter<int>(fSettings, "Camera.RGB", found);
|
||||
}
|
||||
|
||||
void Settings::readIMU(cv::FileStorage &fSettings) {
|
||||
void Settings::readIMU(cv::FileStorage &fSettings)
|
||||
{
|
||||
bool found;
|
||||
noiseGyro_ = readParameter<float>(fSettings,"IMU.NoiseGyro",found);
|
||||
noiseAcc_ = readParameter<float>(fSettings,"IMU.NoiseAcc",found);
|
||||
gyroWalk_ = readParameter<float>(fSettings,"IMU.GyroWalk",found);
|
||||
accWalk_ = readParameter<float>(fSettings,"IMU.AccWalk",found);
|
||||
imuFrequency_ = readParameter<float>(fSettings,"IMU.Frequency",found);
|
||||
noiseGyro_ = readParameter<float>(fSettings, "IMU.NoiseGyro", found);
|
||||
noiseAcc_ = readParameter<float>(fSettings, "IMU.NoiseAcc", found);
|
||||
gyroWalk_ = readParameter<float>(fSettings, "IMU.GyroWalk", found);
|
||||
accWalk_ = readParameter<float>(fSettings, "IMU.AccWalk", 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);
|
||||
|
||||
readParameter<int>(fSettings,"IMU.InsertKFsWhenLost",found,false);
|
||||
if(found){
|
||||
insertKFsWhenLost_ = (bool) readParameter<int>(fSettings,"IMU.InsertKFsWhenLost",found,false);
|
||||
readParameter<int>(fSettings, "IMU.InsertKFsWhenLost", found, false);
|
||||
if (found)
|
||||
{
|
||||
insertKFsWhenLost_ = (bool)readParameter<int>(fSettings, "IMU.InsertKFsWhenLost", found, false);
|
||||
}
|
||||
else{
|
||||
else
|
||||
{
|
||||
insertKFsWhenLost_ = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Settings::readRGBD(cv::FileStorage& fSettings) {
|
||||
void Settings::readRGBD(cv::FileStorage &fSettings)
|
||||
{
|
||||
bool found;
|
||||
|
||||
depthMapFactor_ = readParameter<float>(fSettings,"RGBD.DepthMapFactor",found);
|
||||
thDepth_ = readParameter<float>(fSettings,"Stereo.ThDepth",found);
|
||||
b_ = readParameter<float>(fSettings,"Stereo.b",found);
|
||||
depthMapFactor_ = readParameter<float>(fSettings, "RGBD.DepthMapFactor", found);
|
||||
thDepth_ = readParameter<float>(fSettings, "Stereo.ThDepth", found);
|
||||
b_ = readParameter<float>(fSettings, "Stereo.b", found);
|
||||
bf_ = b_ * calibration1_->getParameter(0);
|
||||
}
|
||||
}
|
||||
|
||||
void Settings::readORB(cv::FileStorage &fSettings) {
|
||||
void Settings::readORB(cv::FileStorage &fSettings)
|
||||
{
|
||||
bool found;
|
||||
|
||||
nFeatures_ = readParameter<int>(fSettings,"ORBextractor.nFeatures",found);
|
||||
scaleFactor_ = readParameter<float>(fSettings,"ORBextractor.scaleFactor",found);
|
||||
nLevels_ = readParameter<int>(fSettings,"ORBextractor.nLevels",found);
|
||||
initThFAST_ = readParameter<int>(fSettings,"ORBextractor.iniThFAST",found);
|
||||
minThFAST_ = readParameter<int>(fSettings,"ORBextractor.minThFAST",found);
|
||||
}
|
||||
nFeatures_ = readParameter<int>(fSettings, "ORBextractor.nFeatures", found);
|
||||
scaleFactor_ = readParameter<float>(fSettings, "ORBextractor.scaleFactor", found);
|
||||
nLevels_ = readParameter<int>(fSettings, "ORBextractor.nLevels", found);
|
||||
initThFAST_ = readParameter<int>(fSettings, "ORBextractor.iniThFAST", found);
|
||||
minThFAST_ = readParameter<int>(fSettings, "ORBextractor.minThFAST", found);
|
||||
}
|
||||
|
||||
void Settings::readViewer(cv::FileStorage &fSettings) {
|
||||
void Settings::readViewer(cv::FileStorage &fSettings)
|
||||
{
|
||||
bool found;
|
||||
|
||||
keyFrameSize_ = readParameter<float>(fSettings,"Viewer.KeyFrameSize",found);
|
||||
keyFrameLineWidth_ = readParameter<float>(fSettings,"Viewer.KeyFrameLineWidth",found);
|
||||
graphLineWidth_ = readParameter<float>(fSettings,"Viewer.GraphLineWidth",found);
|
||||
pointSize_ = readParameter<float>(fSettings,"Viewer.PointSize",found);
|
||||
cameraSize_ = readParameter<float>(fSettings,"Viewer.CameraSize",found);
|
||||
cameraLineWidth_ = readParameter<float>(fSettings,"Viewer.CameraLineWidth",found);
|
||||
viewPointX_ = readParameter<float>(fSettings,"Viewer.ViewpointX",found);
|
||||
viewPointY_ = readParameter<float>(fSettings,"Viewer.ViewpointY",found);
|
||||
viewPointZ_ = readParameter<float>(fSettings,"Viewer.ViewpointZ",found);
|
||||
viewPointF_ = readParameter<float>(fSettings,"Viewer.ViewpointF",found);
|
||||
imageViewerScale_ = readParameter<float>(fSettings,"Viewer.imageViewScale",found,false);
|
||||
keyFrameSize_ = readParameter<float>(fSettings, "Viewer.KeyFrameSize", found);
|
||||
keyFrameLineWidth_ = readParameter<float>(fSettings, "Viewer.KeyFrameLineWidth", found);
|
||||
graphLineWidth_ = readParameter<float>(fSettings, "Viewer.GraphLineWidth", found);
|
||||
pointSize_ = readParameter<float>(fSettings, "Viewer.PointSize", found);
|
||||
cameraSize_ = readParameter<float>(fSettings, "Viewer.CameraSize", found);
|
||||
cameraLineWidth_ = readParameter<float>(fSettings, "Viewer.CameraLineWidth", found);
|
||||
viewPointX_ = readParameter<float>(fSettings, "Viewer.ViewpointX", found);
|
||||
viewPointY_ = readParameter<float>(fSettings, "Viewer.ViewpointY", found);
|
||||
viewPointZ_ = readParameter<float>(fSettings, "Viewer.ViewpointZ", found);
|
||||
viewPointF_ = readParameter<float>(fSettings, "Viewer.ViewpointF", found);
|
||||
imageViewerScale_ = readParameter<float>(fSettings, "Viewer.imageViewScale", found, false);
|
||||
|
||||
if(!found)
|
||||
if (!found)
|
||||
imageViewerScale_ = 1.0f;
|
||||
}
|
||||
}
|
||||
|
||||
void Settings::readLoadAndSave(cv::FileStorage &fSettings) {
|
||||
void Settings::readLoadAndSave(cv::FileStorage &fSettings)
|
||||
{
|
||||
bool found;
|
||||
|
||||
sLoadFrom_ = readParameter<string>(fSettings,"System.LoadAtlasFromFile",found,false);
|
||||
sSaveto_ = readParameter<string>(fSettings,"System.SaveAtlasToFile",found,false);
|
||||
}
|
||||
sLoadFrom_ = readParameter<string>(fSettings, "System.LoadAtlasFromFile", found, false);
|
||||
sSaveto_ = readParameter<string>(fSettings, "System.SaveAtlasToFile", found, false);
|
||||
}
|
||||
|
||||
void Settings::readOtherParameters(cv::FileStorage& fSettings) {
|
||||
void Settings::readOtherParameters(cv::FileStorage &fSettings)
|
||||
{
|
||||
bool found;
|
||||
|
||||
thFarPoints_ = readParameter<float>(fSettings,"System.thFarPoints",found,false);
|
||||
}
|
||||
thFarPoints_ = readParameter<float>(fSettings, "System.thFarPoints", found, false);
|
||||
}
|
||||
|
||||
void Settings::precomputeRectificationMaps() {
|
||||
//Precompute rectification maps, new calibrations, ...
|
||||
cv::Mat K1 = static_cast<Pinhole*>(calibration1_)->toK();
|
||||
K1.convertTo(K1,CV_64F);
|
||||
cv::Mat K2 = static_cast<Pinhole*>(calibration2_)->toK();
|
||||
K2.convertTo(K2,CV_64F);
|
||||
void Settings::precomputeRectificationMaps()
|
||||
{
|
||||
// Precompute rectification maps, new calibrations, ...
|
||||
cv::Mat K1 = static_cast<Pinhole *>(calibration1_)->toK();
|
||||
K1.convertTo(K1, CV_64F);
|
||||
cv::Mat K2 = static_cast<Pinhole *>(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::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_,
|
||||
cv::stereoRectify(K1, camera1DistortionCoef(), K2, camera2DistortionCoef(), newImSize_,
|
||||
R12, t12,
|
||||
R_r1_u1,R_r2_u2,P1,P2,Q,
|
||||
cv::CALIB_ZERO_DISPARITY,-1,newImSize_);
|
||||
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<double>(0,0), 0);
|
||||
calibration1_->setParameter(P1.at<double>(1,1), 1);
|
||||
calibration1_->setParameter(P1.at<double>(0,2), 2);
|
||||
calibration1_->setParameter(P1.at<double>(1,2), 3);
|
||||
// Update calibration
|
||||
calibration1_->setParameter(P1.at<double>(0, 0), 0);
|
||||
calibration1_->setParameter(P1.at<double>(1, 1), 1);
|
||||
calibration1_->setParameter(P1.at<double>(0, 2), 2);
|
||||
calibration1_->setParameter(P1.at<double>(1, 2), 3);
|
||||
|
||||
//Update bf
|
||||
bf_ = b_ * P1.at<double>(0,0);
|
||||
// Update bf
|
||||
bf_ = b_ * P1.at<double>(0, 0);
|
||||
|
||||
//Update relative pose between camera 1 and IMU if necessary
|
||||
if(sensor_ == System::IMU_STEREO){
|
||||
// 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());
|
||||
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){
|
||||
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){
|
||||
if (settings.cameraType_ == Settings::PinHole || settings.cameraType_ == Settings::Rectified)
|
||||
{
|
||||
output << "Pinhole";
|
||||
}
|
||||
else{
|
||||
else
|
||||
{
|
||||
output << "Kannala-Brandt";
|
||||
}
|
||||
output << ")" << ": [";
|
||||
for(size_t i = 0; i < settings.originalCalib1_->size(); i++){
|
||||
output << ")"
|
||||
<< ": [";
|
||||
for (size_t i = 0; i < settings.originalCalib1_->size(); i++)
|
||||
{
|
||||
output << " " << settings.originalCalib1_->getParameter(i);
|
||||
}
|
||||
output << " ]" << endl;
|
||||
|
||||
if(!settings.vPinHoleDistorsion1_.empty()){
|
||||
if (!settings.vPinHoleDistorsion1_.empty())
|
||||
{
|
||||
output << "\t-Camera 1 distortion parameters: [ ";
|
||||
for(float d : settings.vPinHoleDistorsion1_){
|
||||
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_){
|
||||
if (settings.bNeedToRectify_)
|
||||
{
|
||||
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 << " ]" << endl;
|
||||
}
|
||||
else if(settings.bNeedToResize1_){
|
||||
else if (settings.bNeedToResize1_)
|
||||
{
|
||||
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 << " ]" << endl;
|
||||
|
||||
if((settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO) &&
|
||||
settings.cameraType_ == Settings::KannalaBrandt){
|
||||
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++){
|
||||
for (size_t i = 0; i < settings.calibration2_->size(); i++)
|
||||
{
|
||||
output << " " << settings.calibration2_->getParameter(i);
|
||||
}
|
||||
output << " ]" << endl;
|
||||
|
|
@ -602,20 +686,23 @@ namespace ORB_SLAM3 {
|
|||
|
||||
output << "\t-Sequence FPS: " << settings.fps_ << endl;
|
||||
|
||||
//Stereo stuff
|
||||
if(settings.sensor_ == System::STEREO || settings.sensor_ == System::IMU_STEREO){
|
||||
// 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<KannalaBrandt8*>(settings.calibration1_)->mvLappingArea;
|
||||
auto vOverlapping2 = static_cast<KannalaBrandt8*>(settings.calibration2_)->mvLappingArea;
|
||||
if (settings.cameraType_ == Settings::KannalaBrandt)
|
||||
{
|
||||
auto vOverlapping1 = static_cast<KannalaBrandt8 *>(settings.calibration1_)->mvLappingArea;
|
||||
auto vOverlapping2 = static_cast<KannalaBrandt8 *>(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) {
|
||||
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;
|
||||
|
|
@ -623,7 +710,8 @@ namespace ORB_SLAM3 {
|
|||
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;
|
||||
}
|
||||
|
||||
|
|
@ -634,5 +722,5 @@ namespace ORB_SLAM3 {
|
|||
output << "\t-Min FAST threshold: " << settings.minThFAST_ << endl;
|
||||
|
||||
return output;
|
||||
}
|
||||
}
|
||||
};
|
||||
|
|
|
|||
Loading…
Reference in New Issue