i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-02 09:30:25 +08:00
Code Issues Packages Projects Releases Wiki Activity

CameraImages: added configForEachFrame option (added to GUI too). CameraThread: for decimation, if depth is smaller than RGB, RGB is decimated first and if the resulting RGB image is smaller than the original depth, we then decimate the depth.

Browse Source
This commit is contained in:
matlabbe
2020-11-14 13:39:12 -05:00
parent 7be22d1b67
commit 01eb57f293
9 changed files with 285 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files

21
corelib/src/CameraThread.cpp
View File

@@ -237,7 +237,26 @@ void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
else
{
cv::Mat image = util2d::decimate(data.imageRaw(), _imageDecimation);
cv::Mat depthOrRight = util2d::decimate(data.depthOrRightRaw(), _imageDecimation);
int depthDecimation = _imageDecimation;
if(data.depthOrRightRaw().rows <= image.rows || data.depthOrRightRaw().cols <= image.cols)
{
depthDecimation = 1;
}
else
{
depthDecimation = 2;
while(data.depthOrRightRaw().rows / depthDecimation > image.rows ||
data.depthOrRightRaw().cols / depthDecimation > image.cols ||
data.depthOrRightRaw().rows % depthDecimation != 0 ||
data.depthOrRightRaw().cols % depthDecimation != 0)
{
++depthDecimation;
}
UDEBUG("depthDecimation=%d", depthDecimation);
}
cv::Mat depthOrRight = util2d::decimate(data.depthOrRightRaw(), depthDecimation);
std::vector<CameraModel> models = data.cameraModels();
for(unsigned int i=0; i<models.size(); ++i)
{

2
corelib/src/Graph.cpp
View File

@@ -169,7 +169,7 @@ bool exportPoses(
bool importPoses(
const std::string & filePath,
int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAC
int format, // 0=Raw, 1=RGBD-SLAM motion capture (10=without change of coordinate frame), 2=KITTI, 3=TORO, 4=g2o, 5=NewCollege(t,x,y), 6=Malaga Urban GPS, 7=St Lucia INS, 8=Karlsruhe, 9=EuRoC MAV
std::map<int, Transform> & poses,
std::multimap<int, Link> * constraints, // optional for formats 3 and 4
std::map<int, double> * stamps) // optional for format 1 and 9

208
corelib/src/camera/CameraImages.cpp
View File

@@ -29,6 +29,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
#include <rtabmap/utilite/UConversion.h>
#include <rtabmap/utilite/UDirectory.h>
#include <rtabmap/utilite/UStl.h>
#include <rtabmap/utilite/UFile.h>
#include <rtabmap/utilite/UThreadC.h>
#include <rtabmap/core/util3d.h>
#include <rtabmap/core/util3d_filtering.h>
@@ -58,6 +59,7 @@ CameraImages::CameraImages() :
_depthFromScanFillHoles(1),
_depthFromScanFillHolesFromBorder(false),
_filenamesAreTimestamps(false),
_hasConfigForEachFrame(false),
_syncImageRateWithStamps(true),
_odometryFormat(0),
_groundTruthFormat(0),
@@ -87,6 +89,7 @@ CameraImages::CameraImages(const std::string & path,
_depthFromScanFillHoles(1),
_depthFromScanFillHolesFromBorder(false),
_filenamesAreTimestamps(false),
_hasConfigForEachFrame(false),
_syncImageRateWithStamps(true),
_odometryFormat(0),
_groundTruthFormat(0),
@@ -111,6 +114,9 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
_countScan = 0;
_captureDelay = 0.0;
_framesPublished=0;
_model = cameraModel();
_models.clear();
covariances_.clear();
UDEBUG("");
if(_dir)
@@ -213,7 +219,99 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
groundTruth_.clear();
if(success)
{
if(_filenamesAreTimestamps)
if(_hasConfigForEachFrame)
{
UDirectory dirJson(_path, "json");
if(dirJson.getFileNames().size() == _dir->getFileNames().size())
{
bool modelsWarned = false;
bool firstFrame = true;
for(std::list<std::string>::const_iterator iter=dirJson.getFileNames().begin(); iter!=dirJson.getFileNames().end() && success; ++iter)
{
// Assuming 3DScannerApp(iOS) format (only this one supported...)
std::string filePath = _path+"/"+*iter;
cv::FileStorage fs(filePath, 0);
cv::FileNode poseNode = fs["cameraPoseARFrame"];
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(timeNode.real());
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(
intrinsicsNode[0].real(), //fx
intrinsicsNode[4].real(), //fy
intrinsicsNode[2].real(), //cx
intrinsicsNode[5].real(), //cy
CameraModel::opticalRotation()));
}
// we need to rotate from opengl world to rtabmap world
Transform pose(
poseNode[0].real(), poseNode[1].real(), poseNode[2].real(), poseNode[3].real(),
poseNode[4].real(), poseNode[5].real(), poseNode[6].real(), poseNode[7].real(),
poseNode[8].real(), poseNode[9].real(), poseNode[10].real(), poseNode[11].real());
pose = Transform::rtabmap_T_opengl() * pose * Transform::opengl_T_rtabmap();
odometry_.push_back(pose);
// linear cov = 0.0001
cv::Mat covariance = cv::Mat::eye(6,6,CV_64FC1) * (firstFrame?9999.0:0.0001);
if(!firstFrame)
{
// angular cov = 0.000001
covariance.at<double>(3,3) *= 0.01;
covariance.at<double>(4,4) *= 0.01;
covariance.at<double>(5,5) *= 0.01;
}
firstFrame = false;
covariances_.push_back(covariance);
}
}
if(!success)
{
odometry_.clear();
_stamps.clear();
_models.clear();
covariances_.clear();
}
}
else
{
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\"",
(int)dirJson.getFileNames().size(),
(int)_dir->getFileNames().size(),
_path.c_str());
success = false;
}
}
else if(_filenamesAreTimestamps)
{
const std::list<std::string> & filenames = _dir->getFileNames();
for(std::list<std::string>::const_iterator iter=filenames.begin(); iter!=filenames.end(); ++iter)
@@ -316,7 +414,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
}
}
if(success && _odometryPath.size())
if(success && _odometryPath.size() && odometry_.empty())
{
success = readPoses(odometry_, _stamps, _odometryPath, _odometryFormat, _maxPoseTimeDiff);
}
@@ -332,7 +430,12 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
return success;
}
bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<double> & inOutStamps, const std::string & filePath, int format, double maxTimeDiff) const
bool CameraImages::readPoses(
std::list<Transform> & outputPoses,
std::list<double> & inOutStamps,
const std::string & filePath,
int format,
double maxTimeDiff) const
{
outputPoses.clear();
std::map<int, Transform> poses;
@@ -448,7 +551,7 @@ bool CameraImages::readPoses(std::list<Transform> & outputPoses, std::list<doubl
bool CameraImages::isCalibrated() const
{
return _model.isValidForProjection();
return _model.isValidForProjection() || (_models.size() && _models.front().isValidForProjection());
}
std::string CameraImages::getSerial() const
@@ -511,8 +614,10 @@ SensorData CameraImages::captureImage(CameraInfo * info)
LaserScan scan(cv::Mat(), _scanMaxPts, 0, LaserScan::kUnknown, _scanLocalTransform);
double stamp = UTimer::now();
Transform odometryPose;
cv::Mat covariance;
Transform groundTruthPose;
cv::Mat depthFromScan;
CameraModel model = _model;
UDEBUG("");
if(_dir->isValid())
{
@@ -558,17 +663,27 @@ SensorData CameraImages::captureImage(CameraInfo * info)
{
_captureDelay = _stamps.front() - stamp;
}
if(odometry_.size())
}
if(odometry_.size())
{
odometryPose = odometry_.front();
odometry_.pop_front();
if(covariances_.size())
{
odometryPose = odometry_.front();
odometry_.pop_front();
}
if(groundTruth_.size())
{
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
covariance = covariances_.front();
covariances_.pop_front();
}
}
if(groundTruth_.size())
{
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
}
if(_models.size() && !model.isValidForProjection())
{
model = _models.front();
_models.pop_front();
}
}
else
{
@@ -585,17 +700,27 @@ SensorData CameraImages::captureImage(CameraInfo * info)
{
_captureDelay = _stamps.front() - stamp;
}
if(odometry_.size())
}
if(odometry_.size())
{
odometryPose = odometry_.front();
odometry_.pop_front();
if(covariances_.size())
{
odometryPose = odometry_.front();
odometry_.pop_front();
}
if(groundTruth_.size())
{
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
covariance = covariances_.front();
covariances_.pop_front();
}
}
if(groundTruth_.size())
{
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
}
if(_models.size() && !model.isValidForProjection())
{
model = _models.front();
_models.pop_front();
}
while(_count++ < _startAt && (fileName = _dir->getNextFileName()).size())
{
@@ -608,17 +733,27 @@ SensorData CameraImages::captureImage(CameraInfo * info)
{
_captureDelay = _stamps.front() - stamp;
}
if(odometry_.size())
}
if(odometry_.size())
{
odometryPose = odometry_.front();
odometry_.pop_front();
if(covariances_.size())
{
odometryPose = odometry_.front();
odometry_.pop_front();
}
if(groundTruth_.size())
{
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
covariance = covariances_.front();
covariances_.pop_front();
}
}
if(groundTruth_.size())
{
groundTruthPose = groundTruth_.front();
groundTruth_.pop_front();
}
if(_models.size() && !model.isValidForProjection())
{
model = _models.front();
_models.pop_front();
}
}
}
if(_scanDir)
@@ -698,12 +833,11 @@ SensorData CameraImages::captureImage(CameraInfo * info)
UWARN("Error debayering images: \"%s\". Please set bayer mode to -1 if images are not bayered!", e.what());
}
}
}
if(!img.empty() && _model.isValidForRectification() && _rectifyImages)
if(!img.empty() && model.isValidForRectification() && _rectifyImages)
{
img = _model.rectifyImage(img);
img = model.rectifyImage(img);
}
}
@@ -716,7 +850,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
if(_depthFromScan && !img.empty())
{
UDEBUG("Computing depth from scan...");
if(!_model.isValidForProjection())
if(!model.isValidForProjection())
{
UWARN("Depth from laser scan: Camera model should be valid.");
}
@@ -727,7 +861,7 @@ SensorData CameraImages::captureImage(CameraInfo * info)
else
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(scan, scan.localTransform());
depthFromScan = util3d::projectCloudToCamera(img.size(), _model.K(), cloud, _model.localTransform());
depthFromScan = util3d::projectCloudToCamera(img.size(), model.K(), cloud, model.localTransform());
if(_depthFromScanFillHoles!=0)
{
util3d::fillProjectedCloudHoles(depthFromScan, _depthFromScanFillHoles>0, _depthFromScanFillHolesFromBorder);
@@ -742,18 +876,18 @@ SensorData CameraImages::captureImage(CameraInfo * info)
UWARN("Directory is not set, camera must be initialized.");
}
if(_model.imageHeight() == 0 || _model.imageWidth() == 0)
if(model.imageHeight() == 0 || model.imageWidth() == 0)
{
_model.setImageSize(img.size());
model.setImageSize(img.size());
}
SensorData data(scan, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, _model, this->getNextSeqID(), stamp);
SensorData data(scan, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, model, this->getNextSeqID(), stamp);
data.setGroundTruth(groundTruthPose);
if(info && !odometryPose.isNull())
{
info->odomPose = odometryPose;
info->odomCovariance = cv::Mat::eye(6,6,CV_64FC1); // Note that with TORO and g2o file formats, we could get the covariance
info->odomCovariance = covariance.empty()?cv::Mat::eye(6,6,CV_64FC1):covariance; // Note that with TORO and g2o file formats, we could get the covariance
}
return data;

10
corelib/src/camera/CameraRGBDImages.cpp
View File

@@ -70,16 +70,6 @@ bool CameraRGBDImages::init(const std::string & calibrationFolder, const std::st
return success;
}
bool CameraRGBDImages::isCalibrated() const
{
return this->cameraModel().isValidForProjection();
}
std::string CameraRGBDImages::getSerial() const
{
return this->cameraModel().name();
}
SensorData CameraRGBDImages::captureImage(CameraInfo * info)
{
SensorData data;