CameraImages: support reading multicamera image and calibration files (#1721)

* CameraImages: support reading multicamera image and calibration files * Added multi camera support for CameraStereoImages. Refactored single calib per frame option. * refactored * refactor * bump 0.23.8
2026-09-02 09:30:25 +08:00 · 2026-06-23 19:13:18 -07:00
parent bc724a2125
commit c55c05a97e
13 changed files with 1125 additions and 545 deletions
--- 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
--- 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)
--- a/corelib/src/camera/CameraImages.cpp
+++ b/corelib/src/camera/CameraImages.cpp
@@ -41,6 +41,8 @@ namespace rtabmap
 {

 CameraImages::CameraImages() :
+		_multiCameraCalib(false),
+		_multiCameraCount(0),
 		_startAt(0),
 		_maxFrames(0),
 		_refreshDir(false),
@@ -71,6 +73,8 @@ CameraImages::CameraImages(const std::string & path,
 					 float imageRate,
 					 const Transform & localTransform) :
 	Camera(imageRate, localTransform),
+	_multiCameraCalib(false),
+	_multiCameraCount(0),
 	_path(path),
 	_startAt(0),
 	_maxFrames(0),
@@ -111,14 +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)
@@ -199,107 +211,117 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 		return false;
 	}

+	bool success = _dir|| _scanDir;
+
 	if(_dir)
 	{
-		// look for calibration files
-		UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
-		if(!calibrationFolder.empty() && !cameraName.empty())
+		if(_multiCameraCalib)
 		{
-			if(!_model.load(calibrationFolder, cameraName))
+			// Multi-camera mode: each image is a horizontal stack of N sub-camera
+			// images. Load one CameraModel per sub-camera from calibration files
+			// named "<prefix>_<index>.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())
 			{
-				UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
-						cameraName.c_str(), calibrationFolder.c_str());
+				UERROR("Multi-camera calibration is enabled but no calibration folder was provided.");
+				return false;
+			}
+			const std::list<std::string> & imageFiles = _dir->getFileNames();
+			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. Otherwise
+			// the shared prefix is cameraName when provided. cameraName may point to a specific
+			// sub-camera calibration "<rig>_<index>" (e.g. when a single calibration file is
+			// selected); in that case strip the trailing "_<index>" 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;
+			}
+
+			if(numCameras == 0)
+			{
+				UERROR("Multi-camera calibration is enabled but no calibration file matching "
+						"\"%s/%s_<index>.yaml\" was found.",
+						calibrationFolder.c_str(), detectBase.c_str());
+				return false;
+			}
+
+			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_<index>\" reused for all frames", sharedBase.c_str()).c_str());
+			_calibrationFolder = calibrationFolder;
+			_multiCameraCount = numCameras;
+			if(_hasConfigForEachFrame)
+			{
+				// Validate the first frame now; the per-frame sub-camera 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(loadMultiCameraModels(firstBase).empty())
+				{
+					return false;
+				}
 			}
 			else
 			{
-				UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
-						_model.fx(),
-						_model.fy(),
-						_model.cx(),
-						_model.cy());
-
-				cv::FileStorage fs(calibrationFolder+"/"+cameraName+".yaml", 0);
-				cv::FileNode poseNode = fs["local_transform"];
-				if(!poseNode.isNone())
+				// A single calibration set is shared by all frames: load it once.
+				std::vector<CameraModel> models = loadMultiCameraModels(sharedBase);
+				if(models.empty())
 				{
-					UWARN("Using local transform from calibration file (%s) instead of the parameter one (%s).",
-							_model.localTransform().prettyPrint().c_str(),
-							this->getLocalTransform().prettyPrint().c_str());
-					this->setLocalTransform(_model.localTransform());
+					return false;
 				}
+				_multiModels = models;
 			}
 		}
-		_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)
+		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(_path, "yaml xml");
+			UDirectory dirJson(calibrationFolder, "yaml xml");
 #else
-			UDirectory dirJson(_path, "yaml xml json");
+			UDirectory dirJson(calibrationFolder, "yaml xml json");
 #endif
 			if(dirJson.getFileNames().size() == _dir->getFileNames().size())
 			{
-				bool modelsWarned = false;
-				bool localTWarned = false;
+				// 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<std::string>::const_iterator iter=dirJson.getFileNames().begin(); iter!=dirJson.getFileNames().end() && success; ++iter)
 				{
-					std::string filePath = _path+"/"+*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 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
+					if(!poseNode.isNone())
 					{
 						cv::FileNode timeNode = fs["time"];
 						cv::FileNode intrinsicsNode = fs["intrinsics"];
-						if(poseNode.isNone() || poseNode.size() != 16)
+						if(poseNode.size() != 16)
 						{
 							UERROR("Failed reading \"cameraPoseARFrame\" parameter, it should have 16 values (file=%s)", filePath.c_str());
 							success = false;
@@ -320,23 +342,6 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 						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],
@@ -346,12 +351,18 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 							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();
-					_models.clear();
+					_modelFileNames.clear();
 				}
 			}
 			else
@@ -365,12 +376,60 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
 						"of images (%d) in this directory \"%s\".%s",
 						(int)dirJson.getFileNames().size(),
 						(int)_dir->getFileNames().size(),
-						_path.c_str(),
+						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, _rectifyImages))
+				{
+					UWARN("Missing calibration files for camera \"%s\" in \"%s\" folder, you should calibrate the camera!",
+							cameraName.c_str(), calibrationFolder.c_str());
+				}
+				else
+				{
+					UINFO("Camera parameters: fx=%f fy=%f cx=%f cy=%f",
+							_model.fx(),
+							_model.fy(),
+							_model.cx(),
+							_model.cy());

+					cv::FileStorage fs(calibrationFolder+"/"+cameraName+".yaml", 0);
+					cv::FileNode poseNode = fs["local_transform"];
+					if(!poseNode.isNone())
+					{
+						UWARN("Using local transform from calibration file (%s) instead of the parameter one (%s).",
+								_model.localTransform().prettyPrint().c_str(),
+								this->getLocalTransform().prettyPrint().c_str());
+						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(success)
+	{
 		if(_stamps.empty())
 		{
 			if(_filenamesAreTimestamps)
@@ -653,10 +712,97 @@ bool CameraImages::readPoses(

 bool CameraImages::isCalibrated() const
 {
-	return (_dir && (_model.isValidForProjection() || (_models.size() && _models.front().isValidForProjection()))) ||
+	return (_dir && (_model.isValidForProjection() ||
+			_modelFileNames.size() || // per-frame single-camera models loaded on demand (validated in init)
+			(!_multiModels.empty() && _multiModels.front().isValidForProjection()) ||
+			(_multiCameraCalib && _hasConfigForEachFrame && _multiCameraCount > 0))) || // per-frame multi-camera models loaded on demand
 			_scanDir;
 }

+std::vector<CameraModel> CameraImages::loadMultiCameraModels(const std::string & baseName) const
+{
+	std::vector<CameraModel> 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<CameraModel>();
+		}
+		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<CameraModel>();
+		}
+		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<CameraModel>();
+		}
+	}
+	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();
@@ -728,7 +874,13 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 	cv::Mat covariance;
 	Transform groundTruthPose;
 	cv::Mat depthFromScan;
-	CameraModel model = _model;
+	std::vector<CameraModel> models; // one entry per camera (single entry in single-camera mode)
+	if(!_multiCameraCalib)
+	{
+		// Single-camera mode keeps exactly one model in the vector; in multi-camera
+		// mode the per-frame vector is taken from _multiModels below.
+		models.push_back(_model);
+	}
 	UDEBUG("");
 	if(_dir || _scanDir)
 	{
@@ -784,7 +936,8 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 			{
 				UERROR("groundTruth cannot be used when startAt < 0");
 			}
-			if(_models.size() && !model.isValidForProjection())
+			if(_modelFileNames.size() || !_multiModels.empty() ||
+				(_multiCameraCalib && _hasConfigForEachFrame))
 			{
 				UERROR("models cannot be used when startAt < 0");
 			}
@@ -826,12 +979,24 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 					groundTruthPose = groundTruth_.front();
 					groundTruth_.pop_front();
 				}
-				if(_models.size() && !model.isValidForProjection())
+				if(_modelFileNames.size())
 				{
-					UASSERT_MSG(stampsSize==0 || stampsSize == _models.size(), 
-						uFormat("Stamps=%ld models=%ld", _stamps.size(), _models.size()).c_str());
-					model = _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(_multiCameraCalib && _hasConfigForEachFrame)
+				{
+					// 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.empty())
+				{
+					// calibration is shared across all frames
+					models = _multiModels;
 				}

 				while(_count++ < _startAt)
@@ -875,14 +1040,27 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 						groundTruthPose = groundTruth_.front();
 						groundTruth_.pop_front();
 					}
-					if(_models.size() && !model.isValidForProjection())
+					if(_modelFileNames.size())
 					{
-						UASSERT_MSG(stampsSize==0 || stampsSize == _models.size(), 
-							uFormat("Stamps=%ld models=%ld", _stamps.size(), _models.size()).c_str());
-						model = _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(_multiCameraCalib && _hasConfigForEachFrame)
+					{
+						// 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.empty())
+					{
+						// calibration is shared across all frames
+						models = _multiModels;
 					}
 				}
+				_lastImageFileName = imageFileName; // expose the current frame name to subclasses
 			}
 		}

@@ -951,9 +1129,67 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 					}
 				}

-				if(!img.empty() && model.isValidForRectification() && _rectifyImages)
+				if(!img.empty() && !models.empty())
 				{
-					img = model.rectifyImage(img);
+					// The image is a horizontal stack of N sub-images (N==1 in
+					// single-camera mode). Each sub-camera's width comes from its
+					// calibrated image size; if a model has no size (==0), assume a
+					// uniform split (stackedWidth / N) and set it. If requested, each
+					// sub-image is rectified independently with its model and written
+					// into a new stacked image of the same layout.
+					cv::Mat rectified;
+					int offset = 0;
+					bool rectifyAborted = false;
+					for(size_t i=0; i<models.size(); ++i)
+					{
+						if(models[i].imageWidth()==0 || models[i].imageHeight()==0)
+						{
+							models[i].setImageSize(cv::Size(img.cols/(int)models.size(), img.rows));
+						}
+						int subWidth = models[i].imageWidth();
+						if(offset+subWidth > img.cols)
+						{
+							UERROR("Multi-camera: sum of sub-image widths (%d) exceeds the stacked "
+									"image width (%d) at camera %d.", offset+subWidth, img.cols, (int)i);
+							rectified = cv::Mat();
+							rectifyAborted = true;
+							break;
+						}
+						if(_rectifyImages)
+						{
+							if(!models[i].isValidForRectification())
+							{
+								// In multi-camera mode a valid calibration is expected for each
+								// sub-camera. In single-camera mode this is a passthrough (e.g. the
+								// base reader of a stereo/RGBD subclass that rectifies itself, or
+								// images that are already rectified): skip silently to keep the
+								// backward-compatible behavior.
+								if(_multiCameraCalib)
+								{
+									UERROR("Parameter \"rectifyImages\" is set, but camera model %d is not valid for rectification.", (int)i);
+								}
+								rectified = cv::Mat();
+								rectifyAborted = true;
+								break;
+							}
+							if(rectified.empty())
+							{
+								rectified = cv::Mat::zeros(img.rows, img.cols, img.type());
+							}
+							cv::Rect roi(offset, 0, subWidth, img.rows);
+							models[i].rectifyImage(img(roi)).copyTo(rectified(roi));
+						}
+						offset += subWidth;
+					}
+					if(!rectifyAborted && offset != img.cols)
+					{
+						UWARN("Multi-camera: sum of sub-image widths (%d) does not match the "
+								"stacked image width (%d).", offset, img.cols);
+					}
+					if(!rectified.empty())
+					{
+						img = rectified;
+					}
 				}
 			}

@@ -966,7 +1202,7 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 				if(_depthFromScan && !img.empty())
 				{
 					UDEBUG("Computing depth from scan...");
-					if(!model.isValidForProjection())
+					if(models.empty() || !models.front().isValidForProjection())
 					{
 						UWARN("Depth from laser scan: Camera model should be valid.");
 					}
@@ -977,10 +1213,23 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 					else
 					{
 						pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
-						depthFromScan = util3d::projectCloudToCamera(img.size(), model.K(), cloud, model.localTransform());
-						if(_depthFromScanFillHoles!=0)
+						// There is no multi-camera version of projectCloudToCamera(): project
+						// the scan into each (sub-)camera and stack the registered depth images
+						// horizontally, matching the RGB layout (single iteration in single-camera
+						// mode). Sub-image widths come from each model's (already set) image size.
+						// Holes are filled per sub-image so they don't bleed across cameras.
+						depthFromScan = cv::Mat::zeros(img.rows, img.cols, CV_32FC1);
+						int offset = 0;
+						for(size_t i=0; i<models.size() && offset+models[i].imageWidth()<=img.cols; ++i)
 						{
-							util3d::fillProjectedCloudHoles(depthFromScan, _depthFromScanFillHoles>0, _depthFromScanFillHolesFromBorder);
+							int subWidth = models[i].imageWidth();
+							cv::Mat subDepth = util3d::projectCloudToCamera(cv::Size(subWidth, img.rows), models[i].K(), cloud, models[i].localTransform());
+							if(_depthFromScanFillHoles!=0)
+							{
+								util3d::fillProjectedCloudHoles(subDepth, _depthFromScanFillHoles>0, _depthFromScanFillHolesFromBorder);
+							}
+							subDepth.copyTo(depthFromScan(cv::Rect(offset, 0, subWidth, img.rows)));
+							offset += subWidth;
 						}
 					}
 				}
@@ -992,15 +1241,10 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
 		UWARN("Directory is not set, camera must be initialized.");
 	}

-	if(model.imageHeight() == 0 || model.imageWidth() == 0)
-	{
-		model.setImageSize(img.size());
-	}
-
 	SensorData data;
 	if(!img.empty() || !scan.empty())
 	{
-		data = SensorData(scan, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, model, this->getNextSeqID(), stamp);
+		data = SensorData(scan, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, models, this->getNextSeqID(), stamp);
 		data.setGroundTruth(groundTruthPose);
 	}

--- a/corelib/src/camera/CameraStereoImages.cpp
+++ b/corelib/src/camera/CameraStereoImages.cpp
@@ -27,6 +27,8 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.

 #include <rtabmap/core/camera/CameraStereoImages.h>
 #include <rtabmap/utilite/UStl.h>
+#include <rtabmap/utilite/UFile.h>
+#include <rtabmap/utilite/UConversion.h>
 #include <opencv2/imgproc/types_c.h>

 namespace rtabmap
@@ -87,10 +89,12 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
 {
 	UINFO("Calibration folder: \"%s\", name=\"%s\"", calibrationFolder.c_str(), cameraName.c_str());

+	multiStereoModels_.clear();
+
 	// 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 +109,32 @@ 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 and splits the images
+	// itself, so prevent the base from doing its own multi-camera handling and
+	// per-frame config loading (which would look for config files inside the image
+	// directory). Both flags are restored below: _multiCameraCalib still drives the
+	// per-frame vs. shared calibration loading in this class.
+	bool configForEachFrame = this->isConfigForEachFrame();
+	bool multiCameraCalib = _multiCameraCalib;
+	if(multiCameraCalib)
+	{
+		this->setConfigForEachFrame(false);
+	}
+	_multiCameraCalib = false;

 	bool success = false;
 	if(CameraImages::init())
@@ -144,13 +164,141 @@ bool CameraStereoImages::init(const std::string & calibrationFolder, const std::
 			success = true;
 		}
 	}
+
+	// restore the flags: _multiCameraCalib drives the per-frame vs. shared calibration loading below
+	this->setConfigForEachFrame(configForEachFrame);
+	_multiCameraCalib = multiCameraCalib;
+
+	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 "<imageBaseName>_<index>_left.yaml" and "<imageBaseName>_<index>_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<std::string> 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 "<rig>_<index>" (e.g. when a single calibration
+			// file is selected); in that case strip the trailing "_<index>" 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_<index>_%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_<index>\" 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<StereoCameraModel> models = loadStereoCameraModels(sharedBase, rectify);
+					if(models.empty())
+					{
+						success = false;
+					}
+					else
+					{
+						multiStereoModels_ = models;
+					}
+				}
+			}
+		}
+	}
+
 	this->setImagesRectified(rectify); // reset the flag
 	return success;
 }

 bool CameraStereoImages::isCalibrated() const
 {
-	return stereoModel_.isValidForProjection();
+	return stereoModel_.isValidForProjection() ||
+			(!multiStereoModels_.empty() && multiStereoModels_.front().isValidForProjection()) ||
+			(_multiCameraCalib && this->isConfigForEachFrame() && _multiCameraCount > 0); // per-frame models loaded on demand
+}
+
+std::vector<StereoCameraModel> CameraStereoImages::loadStereoCameraModels(const std::string & baseName, bool rectify) const
+{
+	std::vector<StereoCameraModel> models(_multiCameraCount);
+	for(int i=0; i<_multiCameraCount; ++i)
+	{
+		std::string name = baseName + "_" + uNumber2Str(i);
+		if(!models[i].load(_calibrationFolder, name, true /*ignoreStereoTransform*/, rectify) || !models[i].isValidForProjection())
+		{
+			UERROR("Failed to load a valid stereo calibration \"%s/%s_{%s,%s}.yaml\" for multi-camera frame base \"%s\".",
+					_calibrationFolder.c_str(), name.c_str(),
+					models[i].getLeftSuffix().c_str(), models[i].getRightSuffix().c_str(), baseName.c_str());
+			return std::vector<StereoCameraModel>();
+		}
+		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<StereoCameraModel>();
+		}
+		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<StereoCameraModel>();
+		}
+	}
+	return models;
 }

 std::string CameraStereoImages::getSerial() const
@@ -163,6 +311,11 @@ SensorData CameraStereoImages::captureImage(SensorCaptureInfo * info)
 	SensorData data;

 	SensorData left, right;
+	// Disable the base reader's own multi-camera handling while reading the stacked
+	// left/right images: this class splits and rectifies them itself below. Restored
+	// before the multi-camera branch, which relies on the real flag value.
+	bool multiCameraCalib = _multiCameraCalib;
+	_multiCameraCalib = false;
 	left = CameraImages::captureImage(info);
 	if(!left.imageRaw().empty())
 	{
@@ -175,18 +328,86 @@ SensorData CameraStereoImages::captureImage(SensorCaptureInfo * info)
 		{
 			right = this->takeImage(info);
 		}
+	}
+	_multiCameraCalib = multiCameraCalib;

-		if(!right.imageRaw().empty())
+	if(!left.imageRaw().empty() && !right.imageRaw().empty())
+	{
+		// Rectification
+		cv::Mat leftImage = left.imageRaw();
+		cv::Mat rightImage = right.imageRaw();
+		if(rightImage.type() != CV_8UC1 && rightGrayScale_)
 		{
-			// Rectification
-			cv::Mat leftImage = left.imageRaw();
-			cv::Mat rightImage = right.imageRaw();
-			if(rightImage.type() != CV_8UC1 && rightGrayScale_)
+			cv::Mat tmp;
+			cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
+			rightImage = tmp;
+		}
+
+		if(multiCameraCalib)
+		{
+			// 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<StereoCameraModel> models;
+			if(this->isConfigForEachFrame())
 			{
-				cv::Mat tmp;
-				cv::cvtColor(rightImage, tmp, CV_BGR2GRAY);
-				rightImage = tmp;
+				// 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_.empty());
+				models = multiStereoModels_;
+			}
+			if(models.empty())
+			{
+				return data;
+			}
+			int n = (int)models.size();
+
+			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; i<n; ++i)
+				{
+					cv::Rect roiL(subWidthLeft*i, 0, subWidthLeft, leftImage.rows);
+					cv::Rect roiR(subWidthRight*i, 0, subWidthRight, rightImage.rows);
+					models[i].left().rectifyImage(leftImage(roiL)).copyTo(leftRect(roiL));
+					models[i].right().rectifyImage(rightImage(roiR)).copyTo(rightRect(roiR));
+				}
+				leftImage = leftRect;
+				rightImage = rightRect;
+			}
+
+			for(int i=0; i<n; ++i)
+			{
+				if(models[i].left().imageHeight() == 0 || models[i].left().imageWidth() == 0)
+				{
+					models[i].setImageSize(cv::Size(subWidthLeft, leftImage.rows));
+				}
+			}
+
+			data = SensorData(left.laserScanRaw(), leftImage, rightImage, models, left.id()/(camera2_?1:2), left.stamp());
+			data.setGroundTruth(left.groundTruth());
+		}
+		else
+		{
 			if(this->isImagesRectified() && stereoModel_.isValidForRectification())
 			{
 				leftImage = stereoModel_.left().rectifyImage(leftImage);