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: support scan only dataset.

Browse Source
This commit is contained in:
matlabbe
2021-01-24 13:21:30 -05:00
parent e99c658276
commit 4d75361fe0
7 changed files with 180 additions and 98 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
corelib/src/CameraThread.cpp
View File

@@ -174,7 +174,7 @@ void CameraThread::mainLoop()
CameraInfo info;
SensorData data = _camera->takeImage(&info);
if(!data.imageRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
{
postUpdate(&data, &info);

3
corelib/src/Memory.cpp
View File

@@ -5247,7 +5247,8 @@ Signature * Memory::createSignature(const SensorData & inputData, const Transfor
// Occupancy grid map stuff
if(_createOccupancyGrid && !isIntermediateNode)
{
if(!data.depthOrRightRaw().empty())
if( (_occupancy->isGridFromDepth() && !data.depthOrRightRaw().empty()) ||
(!_occupancy->isGridFromDepth() && !data.laserScanRaw().empty()))
{
cv::Mat ground, obstacles, empty;
float cellSize = 0.0f;

7
corelib/src/OdometryThread.cpp
View File

@@ -119,7 +119,7 @@ void OdometryThread::mainLoop()
OdometryInfo info;
UDEBUG("Processing data...");
Transform pose = _odometry->process(data, &info);
if(!data.imageRaw().empty() || (pose.isNull() && data.imu().empty()))
if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (pose.isNull() && data.imu().empty()))
{
UDEBUG("Odom pose = %s", pose.prettyPrint().c_str());
// a null pose notify that odometry could not be computed
@@ -134,9 +134,10 @@ void OdometryThread::addData(const SensorData & data)
{
if(dynamic_cast<OdometryMono*>(_odometry) == 0)
{
if(data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection()))
if((data.imageRaw().empty() || data.depthOrRightRaw().empty() || (data.cameraModels().size()==0 && !data.stereoCameraModel().isValidForProjection())) &&
data.laserScanRaw().empty())
{
ULOGGER_ERROR("Missing some information (images empty or missing calibration)!?");
ULOGGER_ERROR("Missing some information (images/scans empty or missing calibration)!?");
return;
}
}

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

@@ -121,27 +121,32 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
UDEBUG("");
if(_dir)
{
_dir->setPath(_path, "jpg ppm png bmp pnm tiff pgm");
delete _dir;
_dir = 0;
}
else
if(!_path.empty())
{
_dir = new UDirectory(_path, "jpg ppm png bmp pnm tiff pgm");
}
if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
{
_path.append("/");
}
if(!_dir->isValid())
{
ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str());
}
else if(_dir->getFileNames().size() == 0)
{
UWARN("Directory is empty \"%s\"", _path.c_str());
}
else
{
UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount());
if(_path[_path.size()-1] != '\\' && _path[_path.size()-1] != '/')
{
_path.append("/");
}
if(!_dir->isValid())
{
ULOGGER_ERROR("Directory path is not valid \"%s\"", _path.c_str());
delete _dir;
_dir = 0;
}
else if(_dir->getFileNames().size() == 0)
{
UWARN("Directory is empty \"%s\"", _path.c_str());
delete _dir;
_dir = 0;
}
else
{
UINFO("path=%s images=%d", _path.c_str(), (int)this->imagesCount());
}
}
// check for scan directory
@@ -170,7 +175,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
delete _scanDir;
_scanDir = 0;
}
else if(_scanDir->getFileNames().size() != _dir->getFileNames().size())
else if(_dir && _scanDir->getFileNames().size() != _dir->getFileNames().size())
{
UERROR("Scan and image directories should be the same size \"%s\"(%d) vs \"%s\"(%d)",
_scanPath.c_str(),
@@ -186,40 +191,49 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
}
}
// look for calibration files
UINFO("calibration folder=%s name=%s", calibrationFolder.c_str(), cameraName.c_str());
if(!calibrationFolder.empty() && !cameraName.empty())
if(_dir==0 && _scanDir == 0)
{
if(!_model.load(calibrationFolder, cameraName))
{
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());
}
}
_model.setName(cameraName);
_model.setLocalTransform(this->getLocalTransform());
if(_rectifyImages && !_model.isValidForRectification())
{
UERROR("Parameter \"rectifyImages\" is set, but no camera model is loaded or valid.");
ULOGGER_ERROR("Images path or scans path should be set!");
return false;
}
bool success = _dir->isValid();
if(_dir)
{
// 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))
{
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());
}
}
_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(_hasConfigForEachFrame)
if(_dir && _hasConfigForEachFrame)
{
#if CV_MAJOR_VERSION < 3 || (CV_MAJOR_VERSION == 3 && CV_MAJOR_VERSION < 2)
UDirectory dirJson(_path, "yaml xml");
@@ -322,7 +336,7 @@ bool CameraImages::init(const std::string & calibrationFolder, const std::string
}
else if(_filenamesAreTimestamps)
{
const std::list<std::string> & filenames = _dir->getFileNames();
std::list<std::string> filenames = _dir?_dir->getFileNames():_scanDir->getFileNames();
for(std::list<std::string>::const_iterator iter=filenames.begin(); iter!=filenames.end(); ++iter)
{
// format is text_1223445645.12334_text.png or text_122344564512334_text.png
@@ -560,7 +574,8 @@ bool CameraImages::readPoses(
bool CameraImages::isCalibrated() const
{
return _model.isValidForProjection() || (_models.size() && _models.front().isValidForProjection());
return (_dir && (_model.isValidForProjection() || (_models.size() && _models.front().isValidForProjection()))) ||
_scanDir;
}
std::string CameraImages::getSerial() const
@@ -574,6 +589,10 @@ unsigned int CameraImages::imagesCount() const
{
return (unsigned int)_dir->getFileNames().size();
}
else if(_scanDir)
{
return (unsigned int)_scanDir->getFileNames().size();
}
return 0;
}
@@ -583,6 +602,10 @@ std::vector<std::string> CameraImages::filenames() const
{
return uListToVector(_dir->getFileNames());
}
else if(_scanDir)
{
return uListToVector(_scanDir->getFileNames());
}
return std::vector<std::string>();
}
@@ -628,11 +651,14 @@ SensorData CameraImages::captureImage(CameraInfo * info)
cv::Mat depthFromScan;
CameraModel model = _model;
UDEBUG("");
if(_dir->isValid())
if(_dir || _scanDir)
{
if(_refreshDir)
{
_dir->update();
if(_dir)
{
_dir->update();
}
if(_scanDir)
{
_scanDir->update();
@@ -642,13 +668,16 @@ SensorData CameraImages::captureImage(CameraInfo * info)
std::string scanFilePath;
if(_startAt < 0)
{
const std::list<std::string> & fileNames = _dir->getFileNames();
if(fileNames.size())
if(_dir)
{
if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
const std::list<std::string> & fileNames = _dir->getFileNames();
if(fileNames.size())
{
_lastFileName = *fileNames.rbegin();
imageFilePath = _path + _lastFileName;
if(_lastFileName.empty() || uStrNumCmp(_lastFileName,*fileNames.rbegin()) < 0)
{
_lastFileName = *fileNames.rbegin();
imageFilePath = _path + _lastFileName;
}
}
}
if(_scanDir)
@@ -696,11 +725,12 @@ SensorData CameraImages::captureImage(CameraInfo * info)
}
else
{
std::string fileName;
fileName = _dir->getNextFileName();
if(!fileName.empty())
std::string imageFileName = _dir?_dir->getNextFileName():"";
std::string scanFileName = _scanDir?_scanDir->getNextFileName():"";
if((_dir && !imageFileName.empty()) || (!_dir && !scanFileName.empty()))
{
imageFilePath = _path + fileName;
imageFilePath = _path + imageFileName;
scanFilePath = _scanPath + scanFileName;
if(_stamps.size())
{
stamp = _stamps.front();
@@ -731,9 +761,18 @@ SensorData CameraImages::captureImage(CameraInfo * info)
_models.pop_front();
}
while(_count++ < _startAt && (fileName = _dir->getNextFileName()).size())
while(_count++ < _startAt)
{
imageFilePath = _path + fileName;
imageFileName = _dir?_dir->getNextFileName():"";
scanFileName = _scanDir?_scanDir->getNextFileName():"";
if((_dir && imageFileName.empty()) || (!_dir && scanFileName.empty()))
{
break;
}
imageFilePath = _path + imageFileName;
scanFilePath = _scanPath + scanFileName;
if(_stamps.size())
{
stamp = _stamps.front();
@@ -765,18 +804,6 @@ SensorData CameraImages::captureImage(CameraInfo * info)
}
}
}
if(_scanDir)
{
fileName = _scanDir->getNextFileName();
if(!fileName.empty())
{
scanFilePath = _scanPath + fileName;
while(_countScan++ < _startAt && (fileName = _scanDir->getNextFileName()).size())
{
scanFilePath = _scanPath + fileName;
}
}
}
}
if(_maxFrames <=0 || ++_framesPublished <= _maxFrames)
@@ -890,8 +917,12 @@ SensorData CameraImages::captureImage(CameraInfo * info)
model.setImageSize(img.size());
}
SensorData data(scan, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, model, this->getNextSeqID(), stamp);
data.setGroundTruth(groundTruthPose);
SensorData data;
if(!img.empty() || !scan.empty())
{
data = SensorData(scan, _isDepth?cv::Mat():img, _isDepth?img:depthFromScan, model, this->getNextSeqID(), stamp);
data.setGroundTruth(groundTruthPose);
}
if(info && !odometryPose.isNull())
{

51
corelib/src/util3d.cpp
View File

@@ -3239,17 +3239,52 @@ LaserScan loadScan(const std::string & path)
{
return LaserScan(loadBINScan(path), 0, 0, LaserScan::kXYZI);
}
else if(UFile::getExtension(fileName).compare("pcd") == 0)
{
pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
pcl::io::loadPCDFile(path, *cloud);
return laserScanFromPointCloud(*cloud);
}
else
{
pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
pcl::io::loadPLYFile(path, *cloud);
return laserScanFromPointCloud(*cloud);
if(UFile::getExtension(fileName).compare("pcd") == 0)
{
pcl::io::loadPCDFile(path, *cloud);
}
else // PLY
{
pcl::io::loadPLYFile(path, *cloud);
}
bool is2D = false;
if(!cloud->data.empty())
{
// If all z values are zeros, we assume it is a 2D scan
int zOffset = -1;
for(unsigned int i=0; i<cloud->fields.size(); ++i)
{
if(cloud->fields[i].name.compare("z") == 0)
{
zOffset = cloud->fields[i].offset;
}
}
if(zOffset>=0)
{
is2D = true;
for (uint32_t row = 0; row < (uint32_t)cloud->height; ++row)
{
const uint8_t* row_data = &cloud->data[row * cloud->row_step];
for (uint32_t col = 0; col < (uint32_t)cloud->width; ++col)
{
const uint8_t* msg_data = row_data + col * cloud->point_step;
float z = *(float*)(msg_data + zOffset);
is2D = z == 0.0f;
if(!is2D)
{
break;
}
}
}
}
}
return laserScanFromPointCloud(*cloud, true, is2D);
}
return LaserScan();
}