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

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<unsigned char> serialize() const;
 	unsigned int deserialize(const std::vector<unsigned char>& data);

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<unsigned char> serialize() const;

corelib/include/rtabmap/core/camera/CameraImages.h

@@ -76,6 +76,28 @@ 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 "<prefix>_<index>.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.
+	// Note that it is not recommended to use setConfigForEachFrame() if images have
+	// to be rectified, a rectification matrix would need to be re-initilaized for
+	// each frame.
+	void setMultiCameraCalibration(bool enabled)
+	{
+		_multiCameraCalib = enabled;
+	}
 
 	void setScanPath(
 			const std::string & dir,
@@ -121,6 +143,22 @@ 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;}
+
+	// Calibration folder passed to init(), kept to load per-frame calibration on
+	// demand. Shared with subclasses (e.g. stereo) that load calibration themselves.
+	std::string _calibrationFolder;
+
+	// Multi-camera mode state, shared with subclasses (e.g. stereo) that layer their
+	// own multi-camera handling over the base reader. Such subclasses temporarily set
+	// _multiCameraCalib to false while delegating to base init()/captureImage() so the
+	// base returns the raw stacked image instead of doing its own split, then restore it.
+	bool _multiCameraCalib;
+	int _multiCameraCount; // number of sub-cameras detected in multi-camera mode
+
 private:
 	bool readPoses(
 		std::list<Transform> & outputPoses,
@@ -129,6 +167,16 @@ private:
 		int format,
 		double maxTimeDiff) const;
 
+	// Load the sub-camera models of a multi-camera rig from calibration files named
+	// "<baseName>_<index>.yaml" (index 0.._multiCameraCount-1) in _calibrationFolder.
+	// Returns an empty vector (and logs an error) if any model is missing or invalid.
+	std::vector<CameraModel> 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.
+	CameraModel loadConfigModel(const std::string & filePath);
+
 private:
 	std::string _path;
 	int _startAt;
@@ -144,6 +192,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,7 +222,9 @@ private:
 	std::list<cv::Mat> covariances_;
 	std::list<Transform> groundTruth_;
 	CameraModel _model;
-	std::list<CameraModel> _models;
+	std::list<std::string> _modelFileNames; // per-frame single-camera config file paths (config-for-each-frame)
+	bool _configLocalTransformWarned; // warn only once when a per-frame config has no local_transform
+	std::vector<CameraModel> _multiModels; // sub-camera models, shared by all frames (multi-camera mode); empty when loaded per-frame
 
 	UTimer _captureTimer;
 	double _captureDelay;

corelib/include/rtabmap/core/camera/CameraStereoImages.h

@@ -58,6 +58,24 @@ 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 "<prefix>_<index>_left.yaml"
+	// and "<prefix>_<index>_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.
+	// Note that it is not recommended to use setConfigForEachFrame() if images have
+	// to be rectified, a rectification matrix would need to be re-initilaized for each
+	// frame.
+	void setMultiCameraCalibration(bool enabled) {CameraImages::setMultiCameraCalibration(enabled);}
+
 	virtual bool init(const std::string & calibrationFolder = ".", const std::string & cameraName = "");
 	virtual bool isCalibrated() const;
 	virtual std::string getSerial() const;
@@ -68,10 +86,17 @@ public:
 protected:
 	virtual SensorData captureImage(SensorCaptureInfo * info = 0);
 
+private:
+	// Load the sub-camera stereo models of a frame from calibration files named
+	// "<baseName>_<index>_{left,right}.yaml" (index 0.._multiCameraCount-1) in
+	// _calibrationFolder. Returns an empty vector (and logs an error) on failure.
+	std::vector<StereoCameraModel> loadStereoCameraModels(const std::string & baseName, bool rectify) const;
+
 private:
 	CameraImages * camera2_;
 	StereoCameraModel stereoModel_;
 	bool rightGrayScale_;
+	std::vector<StereoCameraModel> multiStereoModels_; // sub-camera stereo models shared by all frames (multi-camera mode); empty when loaded per-frame
 };