Added multi camera support for CameraStereoImages. Refactored single calib per frame option.

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,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 "<imageBaseName>_<index>.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 "<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.
 	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<Transform> & 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
+	// "<baseName>_<index>.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<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. 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<cv::Mat> covariances_;
 	std::list<Transform> groundTruth_;
 	CameraModel _model;
-	std::list<CameraModel> _models;
-	std::list<std::vector<CameraModel> > _multiModels; // one vector of sub-camera models per frame (multi-camera mode)
+	std::list<std::string> _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<std::vector<CameraModel> > _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;

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 "<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.
+	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
+	// "<baseName>_<index>_{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<StereoCameraModel> loadStereoCameraModels(const std::string & baseName, bool rectify) const;
+
 private:
 	CameraImages * camera2_;
 	StereoCameraModel stereoModel_;
 	bool rightGrayScale_;
+	bool multiCameraCalib_;
+	std::list<std::vector<StereoCameraModel> > 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
 };