mirror of
https://github.com/introlab/rtabmap.git
synced 2026-10-07 02:27:47 +08:00
Added multi camera support for CameraStereoImages. Refactored single calib per frame option.
This commit is contained in:
@@ -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);
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
@@ -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
|
||||
};
|
||||
|
||||
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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)
|
||||
|
||||
+294
-197
@@ -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 "<imageBaseName>_<index>.yaml" (index starting at 0).
|
||||
// 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())
|
||||
{
|
||||
UERROR("Multi-camera calibration is enabled but no calibration folder was provided.");
|
||||
return false;
|
||||
}
|
||||
const std::list<std::string> & 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 "<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;
|
||||
}
|
||||
@@ -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_<index>.yaml\" was found for the first image \"%s\".",
|
||||
calibrationFolder.c_str(), firstBase.c_str(), imageFiles.front().c_str());
|
||||
"\"%s/%s_<index>.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<std::string>::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_<index>\" 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<CameraModel> models(numCameras);
|
||||
for(int i=0; i<numCameras; ++i)
|
||||
// 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())
|
||||
{
|
||||
std::string name = base + "_" + uNumber2Str(i);
|
||||
if(!models[i].load(calibrationFolder, name) || !models[i].isValidForProjection())
|
||||
{
|
||||
UERROR("Failed to load a valid calibration \"%s/%s.yaml\" for multi-camera frame \"%s\".",
|
||||
calibrationFolder.c_str(), name.c_str(), iter->c_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<CameraModel> 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<std::string>::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<std::string>::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<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();
|
||||
@@ -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<models.size(); ++i)
|
||||
{
|
||||
if(models[i].imageWidth()==0 || models[i].imageHeight()==0)
|
||||
@@ -1065,14 +1152,24 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
|
||||
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())
|
||||
{
|
||||
UERROR("Parameter \"rectifyImages\" is set, but camera model %d is not valid for rectification.", (int)i);
|
||||
// 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())
|
||||
@@ -1084,7 +1181,7 @@ SensorData CameraImages::captureImage(SensorCaptureInfo * info)
|
||||
}
|
||||
offset += subWidth;
|
||||
}
|
||||
if(offset != img.cols)
|
||||
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);
|
||||
|
||||
@@ -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
|
||||
@@ -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<std::string> 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 "<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_.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<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
|
||||
@@ -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<StereoCameraModel> 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; 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);
|
||||
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;
|
||||
|
||||
@@ -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)
|
||||
|
||||
@@ -63,9 +63,9 @@
|
||||
<property name="geometry">
|
||||
<rect>
|
||||
<x>0</x>
|
||||
<y>-557</y>
|
||||
<y>-1365</y>
|
||||
<width>684</width>
|
||||
<height>5360</height>
|
||||
<height>5343</height>
|
||||
</rect>
|
||||
</property>
|
||||
<layout class="QVBoxLayout" name="verticalLayout_16">
|
||||
@@ -7661,7 +7661,7 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
||||
<item row="1" column="2">
|
||||
<widget class="QLabel" name="label_605">
|
||||
<property name="text">
|
||||
<string>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.</string>
|
||||
<string><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></string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
@@ -7801,8 +7801,11 @@ when using the file type, logs are saved in LogRtabmap.txt (located in the worki
|
||||
</item>
|
||||
<item row="2" column="2">
|
||||
<widget class="QLabel" name="label_798">
|
||||
<property name="toolTip">
|
||||
<string>Naming <prefix>_<index>.yaml, or <prefix>_<index>_left.yaml and <prefix>_<index>_right.yaml for stereo, index starting at 0</string>
|
||||
</property>
|
||||
<property name="text">
|
||||
<string>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.</string>
|
||||
<string><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></string>
|
||||
</property>
|
||||
<property name="wordWrap">
|
||||
<bool>true</bool>
|
||||
|
||||
Reference in New Issue
Block a user