i/rtabmap
1
0
Fork 0
mirror of https://github.com/introlab/rtabmap.git synced 2026-09-08 04:20:20 +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
corelib/src/CameraThread.cpp
+1 -1
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);
corelib/src/Memory.cpp
+2 -1
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;
corelib/src/OdometryThread.cpp
+4 -3
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;
}
}
corelib/src/camera/CameraImages.cpp
+104 -73
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())
{
corelib/src/util3d.cpp
+43 -8
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();
}
docker/jfr2018/run_kitti_datasets.sh
+1 -1
View File
@@ -30,7 +30,7 @@ then
STRATEGY=$2
REG=$3
else
echo "Usage: run_tum_datasets.sh \"output name\" \"odom strategy: 0=f2m 1=f2f 11=f2f_optflow 2=fovis 3=viso2 4=dvo 5=orbslam2 7=LOAM 9=VINS\" \"reg strategy: 0=vis 1=icp-point2point 11=icp-point2plane\" [sequence]"
echo "Usage: run_kitti_datasets.sh \"output name\" \"odom strategy: 0=f2m 1=f2f 11=f2f_optflow 2=fovis 3=viso2 4=dvo 5=orbslam2 7=LOAM 9=VINS\" \"reg strategy: 0=vis 1=icp-point2point 11=icp-point2plane\" [sequence]"
exit
fi
tools/KittiDataset/main.cpp
+25 -11
View File
@@ -61,7 +61,7 @@ void showUsage()
" --height Add car's height to camera local transform (1.67m).\n"
" --disp Generate full disparity.\n"
" --exposure_comp Do exposure compensation between left and right images.\n"
" --scan Include velodyne scan in node's data.\n"
" --scan Include velodyne scan in node's data (use --scan_only to ignore image data).\n"
" --scan_step # Scan downsample step (default=1).\n"
" --scan_voxel #.# Scan voxel size (default 0.5 m).\n"
" --scan_k Scan normal K (default 0).\n"
@@ -115,6 +115,7 @@ int main(int argc, char * argv[])
float scanVoxel = 0.5f;
int scanNormalK = 0;
float scanNormalRadius = 0.0f;
bool scanOnly = false;
std::string gtPath;
bool quiet = false;
if(argc < 2)
@@ -173,6 +174,10 @@ int main(int argc, char * argv[])
showUsage();
}
}
else if(std::strcmp(argv[i], "--scan_only") == 0)
{
scan = scanOnly = true;
}
else if(std::strcmp(argv[i], "--gt") == 0)
{
gtPath = argv[++i];
@@ -264,6 +269,7 @@ int main(int argc, char * argv[])
{
pathScan = path+"/velodyne";
printf(" Scan: %s\n", pathScan.c_str());
printf(" Scan only: %s\n", scanOnly?"true":"false");
printf(" Scan step: %d\n", scanStep);
printf(" Scan voxel: %fm\n", scanVoxel);
printf(" Scan normal k: %d\n", scanNormalK);
@@ -354,14 +360,22 @@ int main(int argc, char * argv[])
// We use CameraThread only to use postUpdate() method
Transform opticalRotation(0,0,1,0, -1,0,0,color?-0.06:0, 0,-1,0,height?1.67:0.0);
CameraThread cameraThread(new
CameraStereoImages(
pathLeftImages,
pathRightImages,
false, // assume that images are already rectified
0.0f,
opticalRotation), parameters);
((CameraStereoImages*)cameraThread.camera())->setTimestamps(false, pathTimes, false);
Camera * camera = 0;
if(scanOnly)
{
camera = new CameraImages(""); // Scan path is set below
}
else
{
camera = new CameraStereoImages(
pathLeftImages,
pathRightImages,
false, // assume that images are already rectified
0.0f,
opticalRotation);
}
CameraThread cameraThread(camera, parameters);
((CameraImages*)cameraThread.camera())->setTimestamps(false, pathTimes, false);
if(exposureCompensation)
{
cameraThread.setStereoExposureCompensation(true);
@@ -372,11 +386,11 @@ int main(int argc, char * argv[])
}
if(!gtPath.empty())
{
((CameraStereoImages*)cameraThread.camera())->setGroundTruthPath(gtPath, 2);
((CameraImages*)cameraThread.camera())->setGroundTruthPath(gtPath, 2);
}
if(!pathScan.empty())
{
((CameraStereoImages*)cameraThread.camera())->setScanPath(
((CameraImages*)cameraThread.camera())->setScanPath(
pathScan,
130000,
Transform(-0.27f, 0.0f, 0.08+(height?1.67f:0.0f), 0.0f, 0.0f, 0.0f));