From 47c73159f70762185e2add3661f8e2a3b7f1b243 Mon Sep 17 00:00:00 2001 From: matlabbe Date: Thu, 18 Jun 2026 17:43:40 -0700 Subject: [PATCH] Added multi camera support for CameraStereoImages. Refactored single calib per frame option. --- corelib/include/rtabmap/core/CameraModel.h | 6 +- .../include/rtabmap/core/StereoCameraModel.h | 4 +- .../rtabmap/core/camera/CameraImages.h | 40 +- .../rtabmap/core/camera/CameraStereoImages.h | 27 + corelib/src/CameraModel.cpp | 8 +- corelib/src/StereoCameraModel.cpp | 6 +- corelib/src/camera/CameraImages.cpp | 491 +++++++++++------- corelib/src/camera/CameraStereoImages.cpp | 255 ++++++++- guilib/src/PreferencesDialog.cpp | 8 +- guilib/src/ui/preferencesDialog.ui | 11 +- 10 files changed, 618 insertions(+), 238 deletions(-) diff --git a/corelib/include/rtabmap/core/CameraModel.h b/corelib/include/rtabmap/core/CameraModel.h index b24b971b..7689c5ab 100644 --- a/corelib/include/rtabmap/core/CameraModel.h +++ b/corelib/include/rtabmap/core/CameraModel.h @@ -126,8 +126,10 @@ public: double verticalFOV() const; // in degrees bool isFisheye() const {return D_.cols == 6;} - bool load(const std::string & filePath); - bool load(const std::string & directory, const std::string & cameraName); + // Set initRectificationMaps=false to skip building the (potentially large) + // rectification maps when rectification won't be used (saves time and memory). + bool load(const std::string & filePath, bool initRectificationMaps = true); + bool load(const std::string & directory, const std::string & cameraName, bool initRectificationMaps = true); bool save(const std::string & directory) const; std::vector serialize() const; unsigned int deserialize(const std::vector& data); diff --git a/corelib/include/rtabmap/core/StereoCameraModel.h b/corelib/include/rtabmap/core/StereoCameraModel.h index 07cf2e77..c8d531a0 100644 --- a/corelib/include/rtabmap/core/StereoCameraModel.h +++ b/corelib/include/rtabmap/core/StereoCameraModel.h @@ -94,7 +94,9 @@ public: // backward compatibility void setImageSize(const cv::Size & size) {left_.setImageSize(size); right_.setImageSize(size);} - bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true); + // Set initRectificationMaps=false to skip building the (potentially large) left/right + // rectification maps when rectification won't be used (saves time and memory). + bool load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform = true, bool initRectificationMaps = true); bool save(const std::string & directory, bool ignoreStereoTransform = true) const; bool saveStereoTransform(const std::string & directory) const; std::vector serialize() const; diff --git a/corelib/include/rtabmap/core/camera/CameraImages.h b/corelib/include/rtabmap/core/camera/CameraImages.h index eb6e9036..f3bf01ef 100644 --- a/corelib/include/rtabmap/core/camera/CameraImages.h +++ b/corelib/include/rtabmap/core/camera/CameraImages.h @@ -76,16 +76,21 @@ public: { _hasConfigForEachFrame = value; } + bool isConfigForEachFrame() const {return _hasConfigForEachFrame;} // Enable multi-camera mode. Each image in the folder is expected to be the // horizontal concatenation of N sub-camera images of a rig, sharing the same // base name (timestamp or node id), e.g. "1780687370.031791.jpg" or "1.jpg". // One calibration file per sub-camera must exist in the calibrationFolder - // passed to init(), named "_.yaml" with index starting - // at 0 (e.g. "1_0.yaml", "1_1.yaml", ...). The number of cameras is - // auto-detected from the first frame. Each sub-image width is taken from the - // corresponding model's calibrated image size; if a model has no size, a - // uniform split (stackedWidth / N) is assumed instead. + // passed to init(), named "_.yaml" with index starting at 0 + // (e.g. "1_0.yaml", "1_1.yaml", ...). The number of cameras is auto-detected. + // Each sub-image width is taken from the corresponding model's calibrated image + // size; if a model has no size, a uniform split (stackedWidth / N) is assumed. + // If setConfigForEachFrame() is enabled, one calibration set is loaded per frame + // using each image's base name as prefix. Otherwise a single calibration set is + // loaded and reused for all frames, using the cameraName passed to init() as + // prefix (it may differ from any image name), falling back to the first image's + // base name when cameraName is empty. void setMultiCameraCalibration(bool enabled) { _multiCameraCalib = enabled; @@ -135,6 +140,11 @@ public: protected: virtual SensorData captureImage(SensorCaptureInfo * info = 0); + // File name (with extension, no directory) of the image returned by the last + // captureImage() call. Used by subclasses (e.g. stereo) to key per-frame + // calibration loaded on demand. Empty if no image was read. + const std::string & lastImageFileName() const {return _lastImageFileName;} + private: bool readPoses( std::list & outputPoses, @@ -143,6 +153,18 @@ private: int format, double maxTimeDiff) const; + // Load the sub-camera models of a multi-camera rig from calibration files named + // "_.yaml" (index 0.._multiCameraCount-1) in _calibrationFolder. + // Returns an empty vector (and logs an error) if any model is missing or invalid. + // Used to load per-frame calibration lazily in captureImage() instead of keeping + // all frames' models (and their rectification maps) in memory. + std::vector loadMultiCameraModels(const std::string & baseName) const; + + // Load the single-camera model for one frame from a per-frame config file (RTAB-Map + // calibration or 3DScannerApp format). Returns an invalid model (and logs an error) + // on failure. Used to load config-for-each-frame calibration on demand in captureImage(). + CameraModel loadConfigModel(const std::string & filePath); + private: std::string _path; int _startAt; @@ -158,6 +180,7 @@ private: int _framesPublished; UDirectory * _dir; std::string _lastFileName; + std::string _lastImageFileName; // file name of the image returned by the last captureImage() int _countScan; UDirectory * _scanDir; @@ -173,6 +196,7 @@ private: bool _filenamesAreTimestamps; bool _hasConfigForEachFrame; bool _multiCameraCalib; + int _multiCameraCount; // number of sub-cameras detected in multi-camera mode std::string _timestampsPath; bool _syncImageRateWithStamps; @@ -188,8 +212,10 @@ private: std::list covariances_; std::list groundTruth_; CameraModel _model; - std::list _models; - std::list > _multiModels; // one vector of sub-camera models per frame (multi-camera mode) + std::list _modelFileNames; // per-frame single-camera config file paths (config-for-each-frame), loaded on demand + bool _configLocalTransformWarned; // warn only once when a per-frame config has no local_transform + std::list > _multiModels; // sub-camera models, shared by all frames (multi-camera mode); empty when loaded per-frame + std::string _calibrationFolder; // kept to load per-frame calibration on demand UTimer _captureTimer; double _captureDelay; diff --git a/corelib/include/rtabmap/core/camera/CameraStereoImages.h b/corelib/include/rtabmap/core/camera/CameraStereoImages.h index 6571d8b6..4fa19f0a 100644 --- a/corelib/include/rtabmap/core/camera/CameraStereoImages.h +++ b/corelib/include/rtabmap/core/camera/CameraStereoImages.h @@ -58,6 +58,21 @@ public: void setRightGrayScale(bool enabled = true) {rightGrayScale_ = enabled;} + // Enable multi-camera stereo mode. Each left/right image in the folders is + // expected to be the horizontal concatenation of N sub-camera images of a + // stereo rig, all sharing the same base name (timestamp or node id), e.g. + // "1779318290.502227.jpg". Two calibration files per sub-camera must exist in + // the calibrationFolder passed to init(), named "__left.yaml" + // and "__right.yaml" with index starting at 0. The number of + // cameras is auto-detected. Sub-images are split using a uniform width + // (stackedWidth / N). + // If setConfigForEachFrame() is enabled, one calibration set is loaded per frame + // using each image's base name as prefix. Otherwise a single calibration set is + // loaded and reused for all frames, using the cameraName passed to init() as + // prefix (it may differ from any image name), falling back to the first image's + // base name when cameraName is empty. + void setMultiCameraCalibration(bool enabled) {multiCameraCalib_ = enabled;} + virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = ""); virtual bool isCalibrated() const; virtual std::string getSerial() const; @@ -68,10 +83,22 @@ public: protected: virtual SensorData captureImage(SensorCaptureInfo * info = 0); +private: + // Load the sub-camera stereo models of a frame from calibration files named + // "__{left,right}.yaml" (index 0..multiCameraCount_-1) in + // calibrationFolder_. Returns an empty vector (and logs an error) on failure. + // Used to load per-frame calibration on demand in captureImage() instead of + // keeping all frames' models (and their rectification maps) in memory. + std::vector loadStereoCameraModels(const std::string & baseName, bool rectify) const; + private: CameraImages * camera2_; StereoCameraModel stereoModel_; bool rightGrayScale_; + bool multiCameraCalib_; + std::list > multiStereoModels_; // sub-camera stereo models shared by all frames (multi-camera mode); empty when loaded per-frame + std::string calibrationFolder_; // kept to load per-frame multi-camera calibration on demand + int multiCameraCount_; // number of sub-cameras detected in multi-camera mode }; diff --git a/corelib/src/CameraModel.cpp b/corelib/src/CameraModel.cpp index 04b77329..de1e2c06 100644 --- a/corelib/src/CameraModel.cpp +++ b/corelib/src/CameraModel.cpp @@ -212,7 +212,7 @@ void CameraModel::setImageSize(const cv::Size & size) } } -bool CameraModel::load(const std::string & filePath) +bool CameraModel::load(const std::string & filePath, bool initRectificationMaps) { K_ = cv::Mat(); D_ = cv::Mat(); @@ -377,7 +377,7 @@ bool CameraModel::load(const std::string & filePath) fs.release(); - if(isValidForRectification()) + if(initRectificationMaps && isValidForRectification()) { initRectificationMap(); } @@ -396,9 +396,9 @@ bool CameraModel::load(const std::string & filePath) return false; } -bool CameraModel::load(const std::string & directory, const std::string & cameraName) +bool CameraModel::load(const std::string & directory, const std::string & cameraName, bool initRectificationMaps) { - return load(directory+"/"+cameraName+".yaml"); + return load(directory+"/"+cameraName+".yaml", initRectificationMaps); } bool CameraModel::save(const std::string & directory) const diff --git a/corelib/src/StereoCameraModel.cpp b/corelib/src/StereoCameraModel.cpp index 421d3f4c..9c338174 100644 --- a/corelib/src/StereoCameraModel.cpp +++ b/corelib/src/StereoCameraModel.cpp @@ -220,11 +220,11 @@ void StereoCameraModel::updateStereoRectification() right_ = CameraModel(right_.name(), right_.imageSize(), right_.K_raw(), right_.D_raw(), R2, P2, right_.localTransform()); } -bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform) +bool StereoCameraModel::load(const std::string & directory, const std::string & cameraName, bool ignoreStereoTransform, bool initRectificationMaps) { name_ = cameraName; - bool leftLoaded = left_.load(directory, cameraName+"_"+getLeftSuffix()); - bool rightLoaded = right_.load(directory, cameraName+"_"+getRightSuffix()); + bool leftLoaded = left_.load(directory, cameraName+"_"+getLeftSuffix(), initRectificationMaps); + bool rightLoaded = right_.load(directory, cameraName+"_"+getRightSuffix(), initRectificationMaps); if(leftLoaded && rightLoaded) { if(ignoreStereoTransform) diff --git a/corelib/src/camera/CameraImages.cpp b/corelib/src/camera/CameraImages.cpp index 2a76be64..91eab69d 100644 --- a/corelib/src/camera/CameraImages.cpp +++ b/corelib/src/camera/CameraImages.cpp @@ -61,6 +61,7 @@ CameraImages::CameraImages() : _filenamesAreTimestamps(false), _hasConfigForEachFrame(false), _multiCameraCalib(false), + _multiCameraCount(0), _syncImageRateWithStamps(true), _odometryFormat(0), _groundTruthFormat(0), @@ -93,6 +94,7 @@ CameraImages::CameraImages(const std::string & path, _filenamesAreTimestamps(false), _hasConfigForEachFrame(false), _multiCameraCalib(false), + _multiCameraCount(0), _syncImageRateWithStamps(true), _odometryFormat(0), _groundTruthFormat(0), @@ -113,15 +115,22 @@ CameraImages::~CameraImages() bool CameraImages::init(const std::string & calibrationFolder, const std::string & cameraName) { _lastFileName.clear(); + _lastImageFileName.clear(); _lastScanFileName.clear(); _count = 0; _countScan = 0; _captureDelay = 0.0; _framesPublished=0; _model = cameraModel(); - _models.clear(); + _modelFileNames.clear(); + _configLocalTransformWarned = false; _multiModels.clear(); + _calibrationFolder.clear(); + _multiCameraCount = 0; covariances_.clear(); + _stamps.clear(); + odometry_.clear(); + groundTruth_.clear(); UDEBUG(""); if(_dir) @@ -202,24 +211,52 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string return false; } + bool success = _dir|| _scanDir; + if(_dir) { if(_multiCameraCalib) { // Multi-camera mode: each image is a horizontal stack of N sub-camera // images. Load one CameraModel per sub-camera from calibration files - // named "_.yaml" (index starting at 0). + // named "_.yaml" (index starting at 0). In config-for-each-frame + // mode the prefix is each image's base name (one calibration set per frame); + // otherwise a single calibration set is shared by all frames, using cameraName + // as the prefix when provided (it may differ from any image name), falling back + // to the first image's base name. if(calibrationFolder.empty()) { UERROR("Multi-camera calibration is enabled but no calibration folder was provided."); return false; } const std::list & imageFiles = _dir->getFileNames(); - - // auto-detect the number of cameras from the first frame - int numCameras = 0; std::string firstBase = imageFiles.front().substr(0, imageFiles.front().find_last_of('.')); - while(UFile::exists(calibrationFolder + "/" + firstBase + "_" + uNumber2Str(numCameras) + ".yaml")) + + // In config-for-each-frame mode the prefix is each image's base name. Otherwise + // the shared prefix is cameraName when provided. cameraName may point to a specific + // sub-camera calibration "_" (e.g. when a single calibration file is + // selected); in that case strip the trailing "_" to recover the rig prefix + // shared by all sub-cameras. Fall back to the first image's base name if empty. + std::string sharedBase = cameraName; + if(!sharedBase.empty() && !UFile::exists(calibrationFolder + "/" + sharedBase + "_0.yaml")) + { + std::size_t us = sharedBase.find_last_of('_'); + if(us != std::string::npos && us+1 < sharedBase.size() && + sharedBase.find_first_not_of("0123456789", us+1) == std::string::npos && + UFile::exists(calibrationFolder + "/" + sharedBase.substr(0, us) + "_0.yaml")) + { + sharedBase = sharedBase.substr(0, us); + } + } + if(sharedBase.empty()) + { + sharedBase = firstBase; + } + + // auto-detect the number of cameras + int numCameras = 0; + std::string detectBase = _hasConfigForEachFrame ? firstBase : sharedBase; + while(UFile::exists(calibrationFolder + "/" + detectBase + "_" + uNumber2Str(numCameras) + ".yaml")) { ++numCameras; } @@ -227,54 +264,132 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string if(numCameras == 0) { UERROR("Multi-camera calibration is enabled but no calibration file matching " - "\"%s/%s_.yaml\" was found for the first image \"%s\".", - calibrationFolder.c_str(), firstBase.c_str(), imageFiles.front().c_str()); + "\"%s/%s_.yaml\" was found.", + calibrationFolder.c_str(), detectBase.c_str()); return false; } - UINFO("Multi-camera mode: %d sub-cameras detected from \"%s\".", numCameras, calibrationFolder.c_str()); - for(std::list::const_iterator iter=imageFiles.begin(); iter!=imageFiles.end(); ++iter) + UINFO("Multi-camera mode: %d sub-cameras detected from \"%s\" (%s).", numCameras, calibrationFolder.c_str(), + _hasConfigForEachFrame?"one calibration per frame, loaded on demand": + uFormat("calibration \"%s_\" reused for all frames", sharedBase.c_str()).c_str()); + _calibrationFolder = calibrationFolder; + _multiCameraCount = numCameras; + if(_hasConfigForEachFrame) { - std::string base = iter->substr(0, iter->find_last_of('.')); - std::vector models(numCameras); - for(int i=0; ic_str()); - _multiModels.clear(); - return false; - } - if(models[i].localTransform().isNull()) - { - // In multi-camera mode the rig extrinsics are required: each - // sub-camera calibration must provide a "local_transform". - UERROR("Calibration \"%s/%s.yaml\" has no \"local_transform\"; it is " - "required in multi-camera mode (rig extrinsics).", - calibrationFolder.c_str(), name.c_str()); - _multiModels.clear(); - return false; - } - if(_rectifyImages && !models[i].isValidForRectification()) - { - UERROR("Parameter \"rectifyImages\" is set, but calibration \"%s/%s.yaml\" is not valid for rectification.", - calibrationFolder.c_str(), name.c_str()); - _multiModels.clear(); - return false; - } + return false; + } + } + else + { + // A single calibration set is shared by all frames: load it once. + std::vector models = loadMultiCameraModels(sharedBase); + if(models.empty()) + { + return false; } _multiModels.push_back(models); } } + else if(_hasConfigForEachFrame) + { + // Per-frame calibration: one config file per image, taken from the + // calibration folder (same convention as the single/multi-camera cases). + // If the config files are in the same folder than the images, just set the + // calibration folder to the images folder. +#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && CV_MAJOR_VERSION < 2) + UDirectory dirJson(calibrationFolder, "yaml xml"); +#else + UDirectory dirJson(calibrationFolder, "yaml xml json"); +#endif + if(dirJson.getFileNames().size() == _dir->getFileNames().size()) + { + // The per-frame models are loaded on demand in captureImage() (see + // loadConfigModel) so we don't keep every frame's model - and its + // rectification map - in memory. Only the stamps and poses of the + // 3DScannerApp format are read eagerly here (needed for synchronization). + for(std::list::const_iterator iter=dirJson.getFileNames().begin(); iter!=dirJson.getFileNames().end() && success; ++iter) + { + std::string filePath = calibrationFolder+"/"+*iter; + cv::FileStorage fs(filePath, 0); + cv::FileNode poseNode = fs["cameraPoseARFrame"]; // Check if it is 3DScannerApp(iOS) format + if(!poseNode.isNone()) + { + cv::FileNode timeNode = fs["time"]; + cv::FileNode intrinsicsNode = fs["intrinsics"]; + if(poseNode.size() != 16) + { + UERROR("Failed reading \"cameraPoseARFrame\" parameter, it should have 16 values (file=%s)", filePath.c_str()); + success = false; + break; + } + else if(timeNode.isNone() || !timeNode.isReal()) + { + UERROR("Failed reading \"time\" parameter (file=%s)", filePath.c_str()); + success = false; + break; + } + else if(intrinsicsNode.isNone() || intrinsicsNode.size()!=9) + { + UERROR("Failed reading \"intrinsics\" parameter (file=%s)", filePath.c_str()); + success = false; + break; + } + else + { + _stamps.push_back((double)timeNode); + // we need to rotate from opengl world to rtabmap world + Transform pose( + (float)poseNode[0], (float)poseNode[1], (float)poseNode[2], (float)poseNode[3], + (float)poseNode[4], (float)poseNode[5], (float)poseNode[6], (float)poseNode[7], + (float)poseNode[8], (float)poseNode[9], (float)poseNode[10], (float)poseNode[11]); + pose = Transform::rtabmap_T_opengl() * pose * Transform::opengl_T_rtabmap(); + odometry_.push_back(pose); + } + } + _modelFileNames.push_back(filePath); + } + // Validate the first frame's calibration now to fail early. + if(success && !_modelFileNames.empty() && !loadConfigModel(_modelFileNames.front()).isValidForProjection()) + { + success = false; + } + if(!success) + { + odometry_.clear(); + _stamps.clear(); + _modelFileNames.clear(); + } + } + else + { + std::string opencv32warn; +#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && CV_MAJOR_VERSION < 2) + opencv32warn = " RTAB-Map is currently built with OpenCV < 3.2, only xml and yaml files are supported (not json)."; +#endif + UERROR("Parameter \"Config for each frame\" is true, but the " + "number of config files (%d) is not equal to number " + "of images (%d) in this directory \"%s\".%s", + (int)dirJson.getFileNames().size(), + (int)_dir->getFileNames().size(), + calibrationFolder.c_str(), + opencv32warn.c_str()); + success = false; + } + } else { + // Config-for-each-frame is disabled: load a single general calibration + // model (the per-frame branch above handles the config-for-each-frame case). // look for calibration files UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str()); if(!calibrationFolder.empty() && !cameraName.empty()) { - if(!_model.load(calibrationFolder, cameraName)) + if(!_model.load(calibrationFolder, cameraName, _rectifyImages)) { UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!", cameraName.c_str(), calibrationFolder.c_str()); @@ -297,150 +412,24 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string this->setLocalTransform(_model.localTransform()); } } + + // Only validate rectification when a general calibration was requested. + // Without it (e.g. images that are already rectified) the single model is + // expected to be empty and must not fail init here. + if(_rectifyImages && !_model.isValidForRectification()) + { + UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid."); + return false; + } } _model.setName(cameraName); _model.setLocalTransform(this->getLocalTransform()); - if(_rectifyImages && !_model.isValidForRectification()) - { - UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid."); - return false; - } } } - bool success = _dir|| _scanDir; - _stamps.clear(); - odometry_.clear(); - groundTruth_.clear(); if(success) { - if(_dir && _hasConfigForEachFrame && !_multiCameraCalib) - { -#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && CV_MAJOR_VERSION < 2) - UDirectory dirJson(_path, "yaml xml"); -#else - UDirectory dirJson(_path, "yaml xml json"); -#endif - if(dirJson.getFileNames().size() == _dir->getFileNames().size()) - { - bool modelsWarned = false; - bool localTWarned = false; - for(std::list::const_iterator iter=dirJson.getFileNames().begin(); iter!=dirJson.getFileNames().end() && success; ++iter) - { - std::string filePath = _path+"/"+*iter; - cv::FileStorage fs(filePath, 0); - cv::FileNode poseNode = fs["cameraPoseARFrame"]; // Check if it is 3DScannerApp(iOS) format - if(poseNode.isNone()) - { - cv::FileNode n = fs["local_transform"]; - bool hasLocalTransform = !n.isNone(); - - fs.release(); - if(_model.isValidForProjection() && !modelsWarned) - { - UWARN("Camera model loaded for each frame is overridden by " - "general calibration file provided. Remove general calibration " - "file to use camera model of each frame. This warning will " - "be shown only one time."); - modelsWarned = true; - } - else - { - CameraModel model; - model.load(filePath); - - if(!hasLocalTransform) - { - if(!localTWarned) - { - UWARN("Loaded calibration file doesn't have local_transform field, " - "the global local_transform parameter is used by default (%s).", - this->getLocalTransform().prettyPrint().c_str()); - localTWarned = true; - } - model.setLocalTransform(this->getLocalTransform()); - } - - _models.push_back(model); - } - } - else - { - cv::FileNode timeNode = fs["time"]; - cv::FileNode intrinsicsNode = fs["intrinsics"]; - if(poseNode.isNone() || poseNode.size() != 16) - { - UERROR("Failed reading \"cameraPoseARFrame\" parameter, it should have 16 values (file=%s)", filePath.c_str()); - success = false; - break; - } - else if(timeNode.isNone() || !timeNode.isReal()) - { - UERROR("Failed reading \"time\" parameter (file=%s)", filePath.c_str()); - success = false; - break; - } - else if(intrinsicsNode.isNone() || intrinsicsNode.size()!=9) - { - UERROR("Failed reading \"intrinsics\" parameter (file=%s)", filePath.c_str()); - success = false; - break; - } - else - { - _stamps.push_back((double)timeNode); - if(_model.isValidForProjection() && !modelsWarned) - { - UWARN("Camera model loaded for each frame is overridden by " - "general calibration file provided. Remove general calibration " - "file to use camera model of each frame. This warning will " - "be shown only one time."); - modelsWarned = true; - } - else - { - _models.push_back(CameraModel( - (double)intrinsicsNode[0], //fx - (double)intrinsicsNode[4], //fy - (double)intrinsicsNode[2], //cx - (double)intrinsicsNode[5], //cy - CameraModel::opticalRotation())); - } - // we need to rotate from opengl world to rtabmap world - Transform pose( - (float)poseNode[0], (float)poseNode[1], (float)poseNode[2], (float)poseNode[3], - (float)poseNode[4], (float)poseNode[5], (float)poseNode[6], (float)poseNode[7], - (float)poseNode[8], (float)poseNode[9], (float)poseNode[10], (float)poseNode[11]); - pose = Transform::rtabmap_T_opengl() * pose * Transform::opengl_T_rtabmap(); - odometry_.push_back(pose); - } - } - } - if(!success) - { - odometry_.clear(); - _stamps.clear(); - _models.clear(); - } - } - else - { - std::string opencv32warn; -#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && CV_MAJOR_VERSION < 2) - opencv32warn = " RTAB-Map is currently built with OpenCV < 3.2, only xml and yaml files are supported (not json)."; -#endif - UERROR("Parameter \"Config for each frame\" is true, but the " - "number of config files (%d) is not equal to number " - "of images (%d) in this directory \"%s\".%s", - (int)dirJson.getFileNames().size(), - (int)_dir->getFileNames().size(), - _path.c_str(), - opencv32warn.c_str()); - success = false; - } - } - if(_stamps.empty()) { if(_filenamesAreTimestamps) @@ -724,11 +713,96 @@ bool CameraImages::readPoses( bool CameraImages::isCalibrated() const { return (_dir && (_model.isValidForProjection() || - (_models.size() && _models.front().isValidForProjection()) || - (_multiModels.size() && !_multiModels.front().empty() && _multiModels.front().front().isValidForProjection()))) || + _modelFileNames.size() || // per-frame single-camera models loaded on demand (validated in init) + (_multiModels.size() && !_multiModels.front().empty() && _multiModels.front().front().isValidForProjection()) || + (_multiCameraCalib && _hasConfigForEachFrame && _multiCameraCount > 0))) || // per-frame multi-camera models loaded on demand _scanDir; } +std::vector CameraImages::loadMultiCameraModels(const std::string & baseName) const +{ + std::vector models(_multiCameraCount); + for(int i=0; i<_multiCameraCount; ++i) + { + std::string name = baseName + "_" + uNumber2Str(i); + if(!models[i].load(_calibrationFolder, name, _rectifyImages) || !models[i].isValidForProjection()) + { + UERROR("Failed to load a valid calibration \"%s/%s.yaml\" for multi-camera frame base \"%s\".", + _calibrationFolder.c_str(), name.c_str(), baseName.c_str()); + return std::vector(); + } + if(models[i].localTransform().isNull()) + { + // In multi-camera mode the rig extrinsics are required: each + // sub-camera calibration must provide a "local_transform". + UERROR("Calibration \"%s/%s.yaml\" has no \"local_transform\"; it is " + "required in multi-camera mode (rig extrinsics).", + _calibrationFolder.c_str(), name.c_str()); + return std::vector(); + } + if(_rectifyImages && !models[i].isValidForRectification()) + { + UERROR("Parameter \"rectifyImages\" is set, but calibration \"%s/%s.yaml\" is not valid for rectification.", + _calibrationFolder.c_str(), name.c_str()); + return std::vector(); + } + } + return models; +} + +CameraModel CameraImages::loadConfigModel(const std::string & filePath) +{ + cv::FileStorage fs(filePath, 0); + cv::FileNode poseNode = fs["cameraPoseARFrame"]; // Check if it is 3DScannerApp(iOS) format + CameraModel model; + if(poseNode.isNone()) + { + // RTAB-Map calibration format + bool hasLocalTransform = !fs["local_transform"].isNone(); + fs.release(); + model.load(filePath, _rectifyImages); + if(!hasLocalTransform) + { + if(!_configLocalTransformWarned) + { + UWARN("Loaded calibration file doesn't have local_transform field, " + "the global local_transform parameter is used by default (%s).", + this->getLocalTransform().prettyPrint().c_str()); + _configLocalTransformWarned = true; + } + model.setLocalTransform(this->getLocalTransform()); + } + } + else + { + // 3DScannerApp(iOS) format + cv::FileNode intrinsicsNode = fs["intrinsics"]; + if(intrinsicsNode.isNone() || intrinsicsNode.size()!=9) + { + UERROR("Failed reading \"intrinsics\" parameter (file=%s)", filePath.c_str()); + return CameraModel(); + } + model = CameraModel( + (double)intrinsicsNode[0], //fx + (double)intrinsicsNode[4], //fy + (double)intrinsicsNode[2], //cx + (double)intrinsicsNode[5], //cy + CameraModel::opticalRotation()); + } + if(!model.isValidForProjection()) + { + UERROR("Camera model loaded from \"%s\" is not valid for projection.", filePath.c_str()); + return CameraModel(); + } + if(_rectifyImages && !model.isValidForRectification()) + { + UERROR("Parameter \"rectifyImages\" is set, but camera model loaded from \"%s\" is not valid for rectification.", + filePath.c_str()); + return CameraModel(); + } + return model; +} + std::string CameraImages::getSerial() const { return _model.name(); @@ -862,7 +936,8 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info) { UERROR("groundTruth cannot be used when startAt < 0"); } - if((_models.size() && !models.back().isValidForProjection()) || _multiModels.size()) + if(_modelFileNames.size() || _multiModels.size() || + (_multiCameraCalib && _hasConfigForEachFrame)) { UERROR("models cannot be used when startAt < 0"); } @@ -904,19 +979,24 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info) groundTruthPose = groundTruth_.front(); groundTruth_.pop_front(); } - if(_models.size() && !models.back().isValidForProjection()) + if(_modelFileNames.size()) { - UASSERT_MSG(stampsSize==0 || stampsSize == _models.size(), - uFormat("Stamps=%ld models=%ld", _stamps.size(), _models.size()).c_str()); - models.back() = _models.front(); - _models.pop_front(); + UASSERT_MSG(stampsSize==0 || stampsSize == _modelFileNames.size(), + uFormat("Stamps=%ld models=%ld", _stamps.size(), _modelFileNames.size()).c_str()); + // per-frame calibration loaded on demand (not kept in memory) + models.back() = loadConfigModel(_modelFileNames.front()); + _modelFileNames.pop_front(); } - if(_multiModels.size()) + if(_multiCameraCalib && _hasConfigForEachFrame) { - UASSERT_MSG(stampsSize==0 || stampsSize == _multiModels.size(), - uFormat("Stamps=%ld multiModels=%ld", stampsSize, _multiModels.size()).c_str()); + // Per-frame calibration loaded on demand (not kept in memory), + // keyed by the current image's base name. + models = loadMultiCameraModels(imageFileName.substr(0, imageFileName.find_last_of('.'))); + } + else if(_multiModels.size()) + { + // calibration is shared across all frames models = _multiModels.front(); - _multiModels.pop_front(); } while(_count++ < _startAt) @@ -960,21 +1040,27 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info) groundTruthPose = groundTruth_.front(); groundTruth_.pop_front(); } - if(_models.size() && !models.back().isValidForProjection()) + if(_modelFileNames.size()) { - UASSERT_MSG(stampsSize==0 || stampsSize == _models.size(), - uFormat("Stamps=%ld models=%ld", _stamps.size(), _models.size()).c_str()); - models.back() = _models.front(); - _models.pop_front(); + UASSERT_MSG(stampsSize==0 || stampsSize == _modelFileNames.size(), + uFormat("Stamps=%ld models=%ld", _stamps.size(), _modelFileNames.size()).c_str()); + // per-frame calibration loaded on demand (not kept in memory) + models.back() = loadConfigModel(_modelFileNames.front()); + _modelFileNames.pop_front(); } - if(_multiModels.size()) + if(_multiCameraCalib && _hasConfigForEachFrame) { - UASSERT_MSG(stampsSize==0 || stampsSize == _multiModels.size(), - uFormat("Stamps=%ld multiModels=%ld", stampsSize, _multiModels.size()).c_str()); + // Per-frame calibration loaded on demand (not kept in memory), + // keyed by the current image's base name. + models = loadMultiCameraModels(imageFileName.substr(0, imageFileName.find_last_of('.'))); + } + else if(_multiModels.size()) + { + // calibration is shared across all frames models = _multiModels.front(); - _multiModels.pop_front(); } } + _lastImageFileName = imageFileName; // expose the current frame name to subclasses } } @@ -1053,6 +1139,7 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info) // into a new stacked image of the same layout. cv::Mat rectified; int offset = 0; + bool rectifyAborted = false; for(size_t i=0; i #include +#include +#include #include namespace rtabmap @@ -45,7 +47,9 @@ CameraStereoImages::CameraStereoImages( const Transform & localTransform) : CameraImages(pathLeftImages, imageRate, localTransform), camera2_(new CameraImages(pathRightImages)), - rightGrayScale_(true) + rightGrayScale_(true), + multiCameraCalib_(false), + multiCameraCount_(0) { this->setImagesRectified(rectifyImages); } @@ -57,7 +61,9 @@ CameraStereoImages::CameraStereoImages( const Transform & localTransform) : CameraImages("", imageRate, localTransform), camera2_(0), - rightGrayScale_(true) + rightGrayScale_(true), + multiCameraCalib_(false), + multiCameraCount_(0) { std::vector paths = uListToVector(uSplit(pathLeftRightImages, uStrContains(pathLeftRightImages, ":")?':':';')); if(paths.size() >= 1) @@ -87,10 +93,14 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std:: { UINFO("Calibration folder: \"%s\", name=\"%s\"", calibrationFolder.c_str(), cameraName.c_str()); + multiStereoModels_.clear(); + calibrationFolder_.clear(); + multiCameraCount_ = 0; + // look for calibration files - if(!calibrationFolder.empty() && !cameraName.empty()) + if(!multiCameraCalib_ && !calibrationFolder.empty() && !cameraName.empty()) { - if(!stereoModel_.load(calibrationFolder, cameraName, false) && !stereoModel_.isValidForProjection()) + if(!stereoModel_.load(calibrationFolder, cameraName, false, this->isImagesRectified()) && !stereoModel_.isValidForProjection()) { UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!", cameraName.c_str(), calibrationFolder.c_str()); @@ -105,16 +115,29 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std:: } } - stereoModel_.setLocalTransform(this->getLocalTransform()); - stereoModel_.setName(cameraName); - if(this->isImagesRectified() && !stereoModel_.isValidForRectification()) + if(!multiCameraCalib_) { - UWARN("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid for rectification. This can be ignored if input images are already rectified."); + stereoModel_.setLocalTransform(this->getLocalTransform()); + stereoModel_.setName(cameraName); + if(this->isImagesRectified() && !stereoModel_.isValidForRectification()) + { + UWARN("Parameter \"rectifyImages\" is set, but no stereo model is loaded or valid for rectification. This can be ignored if input images are already rectified."); + } } //desactivate before init as we will do it in this class instead for convenience bool rectify = this->isImagesRectified(); this->setImagesRectified(false); + // The base reader is used here only to enumerate/read the left images; in + // multi-camera mode this class loads the calibration itself, so prevent the + // base from triggering its own per-frame config loading (which would look for + // config files inside the image directory). The flag is restored below and + // still drives the per-frame vs. shared calibration loading in this class. + bool configForEachFrame = this->isConfigForEachFrame(); + if(multiCameraCalib_) + { + this->setConfigForEachFrame(false); + } bool success = false; if(CameraImages::init()) @@ -144,13 +167,140 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std:: success = true; } } + + // restore the flag: it drives the per-frame vs. shared calibration loading below + this->setConfigForEachFrame(configForEachFrame); + + if(success && multiCameraCalib_) + { + // Multi-camera stereo mode: each left/right image is a horizontal stack of N + // sub-camera images. Load one StereoCameraModel per sub-camera from calibration + // files named "__left.yaml" and "__right.yaml" + // (index starting at 0). + if(calibrationFolder.empty()) + { + UERROR("Multi-camera calibration is enabled but no calibration folder was provided."); + success = false; + } + else + { + const std::vector imageFiles = this->filenames(); + std::string firstBase = imageFiles.front().substr(0, imageFiles.front().find_last_of('.')); + + // In config-for-each-frame mode the prefix is each image's base name (one + // calibration set per frame). Otherwise a single calibration set is shared by all + // frames, using cameraName as the prefix when provided. cameraName may point to a + // specific sub-camera calibration "_" (e.g. when a single calibration + // file is selected); in that case strip the trailing "_" to recover the rig + // prefix shared by all sub-cameras. Fall back to the first image's base name if empty. + std::string leftSuffix = "_0_" + stereoModel_.getLeftSuffix() + ".yaml"; + std::string sharedBase = cameraName; + if(!sharedBase.empty() && !UFile::exists(calibrationFolder + "/" + sharedBase + leftSuffix)) + { + std::size_t us = sharedBase.find_last_of('_'); + if(us != std::string::npos && us+1 < sharedBase.size() && + sharedBase.find_first_not_of("0123456789", us+1) == std::string::npos && + UFile::exists(calibrationFolder + "/" + sharedBase.substr(0, us) + leftSuffix)) + { + sharedBase = sharedBase.substr(0, us); + } + } + if(sharedBase.empty()) + { + sharedBase = firstBase; + } + + // auto-detect the number of cameras + int numCameras = 0; + std::string detectBase = this->isConfigForEachFrame() ? firstBase : sharedBase; + while(UFile::exists(calibrationFolder + "/" + detectBase + "_" + uNumber2Str(numCameras) + "_" + stereoModel_.getLeftSuffix() + ".yaml")) + { + ++numCameras; + } + + if(numCameras == 0) + { + UERROR("Multi-camera calibration is enabled but no calibration file matching " + "\"%s/%s__%s.yaml\" was found.", + calibrationFolder.c_str(), detectBase.c_str(), stereoModel_.getLeftSuffix().c_str()); + success = false; + } + else + { + UINFO("Multi-camera stereo mode: %d sub-cameras detected from \"%s\" (%s).", numCameras, calibrationFolder.c_str(), + this->isConfigForEachFrame()?"one calibration per frame, loaded on demand": + uFormat("calibration \"%s_\" reused for all frames", sharedBase.c_str()).c_str()); + calibrationFolder_ = calibrationFolder; + multiCameraCount_ = numCameras; + if(this->isConfigForEachFrame()) + { + // Validate the first frame now; the per-frame sub-camera stereo models are + // loaded on demand in captureImage() (keyed by each image's base name) so we + // don't keep every frame's models - and their rectification maps - in memory. + if(loadStereoCameraModels(firstBase, rectify).empty()) + { + success = false; + } + } + else + { + // A single calibration set is shared by all frames: load it once. + std::vector models = loadStereoCameraModels(sharedBase, rectify); + if(models.empty()) + { + success = false; + } + else + { + multiStereoModels_.push_back(models); + } + } + } + } + } + this->setImagesRectified(rectify); // reset the flag return success; } bool CameraStereoImages::isCalibrated() const { - return stereoModel_.isValidForProjection(); + return stereoModel_.isValidForProjection() || + (multiStereoModels_.size() && !multiStereoModels_.front().empty() && multiStereoModels_.front().front().isValidForProjection()) || + (multiCameraCalib_ && this->isConfigForEachFrame() && multiCameraCount_ > 0); // per-frame models loaded on demand +} + +std::vector CameraStereoImages::loadStereoCameraModels(const std::string & baseName, bool rectify) const +{ + std::vector models(multiCameraCount_); + for(int i=0; i(); + } + if(models[i].localTransform().isNull()) + { + // In multi-camera mode the rig extrinsics are required: each + // sub-camera calibration must provide a "local_transform". + UERROR("Stereo calibration \"%s/%s_%s.yaml\" has no \"local_transform\"; it is " + "required in multi-camera mode (rig extrinsics).", + calibrationFolder_.c_str(), name.c_str(), models[i].getLeftSuffix().c_str()); + return std::vector(); + } + if(rectify && !models[i].isValidForRectification()) + { + UERROR("Parameter \"rectifyImages\" is set, but stereo calibration \"%s/%s_{%s,%s}.yaml\" is not valid for rectification.", + calibrationFolder_.c_str(), name.c_str(), + models[i].getLeftSuffix().c_str(), models[i].getRightSuffix().c_str()); + return std::vector(); + } + } + return models; } std::string CameraStereoImages::getSerial() const @@ -187,19 +337,86 @@ SensorData CameraStereoImages::captureImage(SensorCaptureInfo * info) cv::cvtColor(rightImage, tmp, CV_BGR2GRAY); rightImage = tmp; } - if(this->isImagesRectified() && stereoModel_.isValidForRectification()) - { - leftImage = stereoModel_.left().rectifyImage(leftImage); - rightImage = stereoModel_.right().rectifyImage(rightImage); - } - if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0) + if(multiCameraCalib_) { - stereoModel_.setImageSize(leftImage.size()); - } + // Multi-camera stereo mode: left and right images are horizontal stacks + // of N sub-images (one stereo pair per sub-camera). Each pair is rectified + // independently with its own model and written back into a stacked image of + // the same layout. Sub-images are split using a uniform width (cols / N), + // matching the convention used downstream to de-stack the images. + std::vector models; + if(this->isConfigForEachFrame()) + { + // Per-frame calibration loaded on demand (not kept in memory), + // keyed by the current image's base name. + std::string base = this->lastImageFileName(); + base = base.substr(0, base.find_last_of('.')); + models = loadStereoCameraModels(base, this->isImagesRectified()); + } + else + { + // a single calibration set is shared by all frames + UASSERT(multiStereoModels_.size()); + models = multiStereoModels_.front(); + } + if(models.empty()) + { + return data; + } + int n = (int)models.size(); - data = SensorData(left.laserScanRaw(), leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp()); - data.setGroundTruth(left.groundTruth()); + if(leftImage.cols % n != 0 || rightImage.cols % n != 0) + { + UERROR("Multi-camera stereo: stacked image width (left=%d, right=%d) is not " + "divisible by the number of cameras (%d).", leftImage.cols, rightImage.cols, n); + return data; + } + int subWidthLeft = leftImage.cols/n; + int subWidthRight = rightImage.cols/n; + + if(this->isImagesRectified()) + { + cv::Mat leftRect(leftImage.rows, leftImage.cols, leftImage.type()); + cv::Mat rightRect(rightImage.rows, rightImage.cols, rightImage.type()); + for(int i=0; iisImagesRectified() && stereoModel_.isValidForRectification()) + { + leftImage = stereoModel_.left().rectifyImage(leftImage); + rightImage = stereoModel_.right().rectifyImage(rightImage); + } + + if(stereoModel_.left().imageHeight() == 0 || stereoModel_.left().imageWidth() == 0) + { + stereoModel_.setImageSize(leftImage.size()); + } + + data = SensorData(left.laserScanRaw(), leftImage, rightImage, stereoModel_, left.id()/(camera2_?1:2), left.stamp()); + data.setGroundTruth(left.groundTruth()); + } } } return data; diff --git a/guilib/src/PreferencesDialog.cpp b/guilib/src/PreferencesDialog.cpp index 2770fdab..3a824ed4 100644 --- a/guilib/src/PreferencesDialog.cpp +++ b/guilib/src/PreferencesDialog.cpp @@ -4626,7 +4626,12 @@ void PreferencesDialog::selectCalibrationPath() { dir = getWorkingDirectory()+"/camera_info/"+dir; } - QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Calibration file (*.yaml)")); +#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && CV_MAJOR_VERSION < 2) + QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Calibration file (*.yaml *.xml)")); +#else + QString path = QFileDialog::getOpenFileName(this, tr("Select file"), dir, tr("Calibration file (*.yaml *.xml *.json)")); +#endif + if(path.size()) { _ui->lineEdit_calibrationFile->setText(path); @@ -7120,6 +7125,7 @@ Camera * PreferencesDialog::createCamera( _ui->lineEdit_cameraImages_timestamps->text().toStdString(), _ui->checkBox_cameraImages_syncTimeStamps->isChecked()); ((CameraStereoImages*)camera)->setConfigForEachFrame(_ui->checkBox_cameraImages_configForEachFrame->isChecked()); + ((CameraStereoImages*)camera)->setMultiCameraCalibration(_ui->checkBox_cameraImages_multiCameraCalibration->isChecked()); ((CameraStereoImages*)camera)->setRightGrayScale(_ui->checkBox_stereo_rightGrayScale->isChecked()); } else if (driver == kSrcStereoUsb) diff --git a/guilib/src/ui/preferencesDialog.ui b/guilib/src/ui/preferencesDialog.ui index eb58c431..c2fef56f 100644 --- a/guilib/src/ui/preferencesDialog.ui +++ b/guilib/src/ui/preferencesDialog.ui @@ -63,9 +63,9 @@ 0 - -557 + -1365 684 - 5360 + 5343 @@ -7661,7 +7661,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki - Load config file for each frame. For calibration, global calibration path above should be empty. Config files should be in the same directory than RGB frames and they should have the same name than the corresponding frame file. Currently supporting 3DScannerApp for iOS export config format (JSON, intrinsics, pose and stamp) and RTAB-Map calibration file format. + <html><head/><body><p>Load config file for each frame. Config files are read from the calibration folder and should have the same name than the corresponding frame file. Currently supporting 3DScannerApp for iOS export config format (JSON, intrinsics, pose and stamp) and RTAB-Map calibration file format. </p></body></html> true @@ -7801,8 +7801,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki + + Naming <prefix>_<index>.yaml, or <prefix>_<index>_left.yaml and <prefix>_<index>_right.yaml for stereo, index starting at 0 + - Multi-camera mode: each image is a horizontal stack of N sub-camera images. One calibration file per sub-camera (named <imageBaseName>_<index>.yaml, index starting at 0) must be in the calibration folder. The number of cameras is auto-detected and a local_transform (rig extrinsics) is required in each file. + <html><head/><body><p>Multi-camera mode: each image is a horizontal stack of N sub-camera images (for stereo, both left and right images are stacked the same way). One calibration file per sub-camera (see naming format on tooltip) must be in the calibration folder. A local_transform (rig extrinsics) is required in each file.</p></body></html> true