mirror of
https://github.com/introlab/rtabmap.git
synced 2026-09-11 14:00:20 +08:00
Added rtabmap-kitti_dataset tool
This commit is contained in:
matlabbe
+180
-174
@@ -34,6 +34,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
#include "rtabmap/core/util3d_surface.h"
|
||||
#include "rtabmap/core/util3d_filtering.h"
|
||||
#include "rtabmap/core/StereoDense.h"
|
||||
#include "rtabmap/core/DBReader.h"
|
||||
#include "rtabmap/core/clams/discrete_depth_distortion_model.h"
|
||||
|
||||
#include <rtabmap/utilite/UTimer.h>
|
||||
@@ -129,181 +130,9 @@ void CameraThread::mainLoop()
|
||||
CameraInfo info;
|
||||
SensorData data = _camera->takeImage(&info);
|
||||
|
||||
if(!data.imageRaw().empty())
|
||||
if(!data.imageRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
|
||||
{
|
||||
if(_colorOnly && !data.depthRaw().empty())
|
||||
{
|
||||
data.setDepthOrRightRaw(cv::Mat());
|
||||
}
|
||||
|
||||
if(_distortionModel && !data.depthRaw().empty())
|
||||
{
|
||||
UTimer timer;
|
||||
if(_distortionModel->getWidth() == data.depthRaw().cols &&
|
||||
_distortionModel->getHeight() == data.depthRaw().rows )
|
||||
{
|
||||
cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures.
|
||||
_distortionModel->undistort(depth);
|
||||
data.setDepthOrRightRaw(depth);
|
||||
}
|
||||
else
|
||||
{
|
||||
UERROR("Distortion model size is %dx%d but dpeth image is %dx%d!",
|
||||
_distortionModel->getWidth(), _distortionModel->getHeight(),
|
||||
data.depthRaw().cols, data.depthRaw().rows);
|
||||
}
|
||||
info.timeUndistortDepth = timer.ticks();
|
||||
}
|
||||
|
||||
if(_bilateralFiltering && !data.depthRaw().empty())
|
||||
{
|
||||
UTimer timer;
|
||||
data.setDepthOrRightRaw(util2d::fastBilateralFiltering(data.depthRaw(), _bilateralSigmaS, _bilateralSigmaR));
|
||||
info.timeBilateralFiltering = timer.ticks();
|
||||
}
|
||||
|
||||
if(_imageDecimation>1 && !data.imageRaw().empty())
|
||||
{
|
||||
UDEBUG("");
|
||||
UTimer timer;
|
||||
if(!data.depthRaw().empty() &&
|
||||
!(data.depthRaw().rows % _imageDecimation == 0 && data.depthRaw().cols % _imageDecimation == 0))
|
||||
{
|
||||
UERROR("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
|
||||
"Images won't be resized.", _imageDecimation, data.depthRaw().cols, data.depthRaw().rows);
|
||||
}
|
||||
else
|
||||
{
|
||||
data.setImageRaw(util2d::decimate(data.imageRaw(), _imageDecimation));
|
||||
data.setDepthOrRightRaw(util2d::decimate(data.depthOrRightRaw(), _imageDecimation));
|
||||
std::vector<CameraModel> models = data.cameraModels();
|
||||
for(unsigned int i=0; i<models.size(); ++i)
|
||||
{
|
||||
if(models[i].isValidForProjection())
|
||||
{
|
||||
models[i] = models[i].scaled(1.0/double(_imageDecimation));
|
||||
}
|
||||
}
|
||||
data.setCameraModels(models);
|
||||
StereoCameraModel stereoModel = data.stereoCameraModel();
|
||||
if(stereoModel.isValidForProjection())
|
||||
{
|
||||
stereoModel.scale(1.0/double(_imageDecimation));
|
||||
data.setStereoCameraModel(stereoModel);
|
||||
}
|
||||
}
|
||||
info.timeImageDecimation = timer.ticks();
|
||||
}
|
||||
if(_mirroring && data.cameraModels().size() == 1)
|
||||
{
|
||||
UDEBUG("");
|
||||
UTimer timer;
|
||||
cv::Mat tmpRgb;
|
||||
cv::flip(data.imageRaw(), tmpRgb, 1);
|
||||
data.setImageRaw(tmpRgb);
|
||||
UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValidForProjection(), "Only single RGBD cameras are supported for mirroring.");
|
||||
if(data.cameraModels().size() && data.cameraModels()[0].cx())
|
||||
{
|
||||
CameraModel tmpModel(
|
||||
data.cameraModels()[0].fx(),
|
||||
data.cameraModels()[0].fy(),
|
||||
float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
|
||||
data.cameraModels()[0].cy(),
|
||||
data.cameraModels()[0].localTransform());
|
||||
data.setCameraModel(tmpModel);
|
||||
}
|
||||
if(!data.depthRaw().empty())
|
||||
{
|
||||
cv::Mat tmpDepth;
|
||||
cv::flip(data.depthRaw(), tmpDepth, 1);
|
||||
data.setDepthOrRightRaw(tmpDepth);
|
||||
}
|
||||
info.timeMirroring = timer.ticks();
|
||||
}
|
||||
if(_stereoToDepth && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
|
||||
{
|
||||
UDEBUG("");
|
||||
UTimer timer;
|
||||
cv::Mat depth = util2d::depthFromDisparity(
|
||||
_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
|
||||
data.stereoCameraModel().left().fx(),
|
||||
data.stereoCameraModel().baseline());
|
||||
// set Tx for stereo bundle adjustment (when used)
|
||||
CameraModel model = CameraModel(
|
||||
data.stereoCameraModel().left().fx(),
|
||||
data.stereoCameraModel().left().fy(),
|
||||
data.stereoCameraModel().left().cx(),
|
||||
data.stereoCameraModel().left().cy(),
|
||||
data.stereoCameraModel().localTransform(),
|
||||
-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx());
|
||||
data.setCameraModel(model);
|
||||
data.setDepthOrRightRaw(depth);
|
||||
data.setStereoCameraModel(StereoCameraModel());
|
||||
info.timeDisparity = timer.ticks();
|
||||
UDEBUG("Computing disparity = %f s", info.timeDisparity);
|
||||
}
|
||||
if(_scanFromDepth &&
|
||||
data.cameraModels().size() &&
|
||||
data.cameraModels().at(0).isValidForProjection() &&
|
||||
!data.depthRaw().empty())
|
||||
{
|
||||
UDEBUG("");
|
||||
if(data.laserScanRaw().empty())
|
||||
{
|
||||
UASSERT(_scanDecimation >= 1);
|
||||
UTimer timer;
|
||||
pcl::IndicesPtr validIndices(new std::vector<int>);
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
|
||||
data,
|
||||
_scanDecimation,
|
||||
_scanMaxDepth,
|
||||
_scanMinDepth,
|
||||
validIndices.get());
|
||||
float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
|
||||
cv::Mat scan;
|
||||
const Transform & baseToScan = data.cameraModels()[0].localTransform();
|
||||
if(validIndices->size())
|
||||
{
|
||||
if(_scanVoxelSize>0.0f)
|
||||
{
|
||||
cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
|
||||
float ratio = float(cloud->size()) / float(validIndices->size());
|
||||
maxPoints = ratio * maxPoints;
|
||||
}
|
||||
else if(!cloud->is_dense)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
|
||||
cloud = denseCloud;
|
||||
}
|
||||
|
||||
if(cloud->size())
|
||||
{
|
||||
if(_scanNormalsK>0)
|
||||
{
|
||||
Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
|
||||
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
|
||||
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
|
||||
scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
|
||||
}
|
||||
else
|
||||
{
|
||||
scan = util3d::laserScanFromPointCloud(*cloud, baseToScan.inverse());
|
||||
}
|
||||
}
|
||||
}
|
||||
data.setLaserScanRaw(scan, LaserScanInfo((int)maxPoints, _scanMaxDepth, baseToScan));
|
||||
info.timeScanFromDepth = timer.ticks();
|
||||
UDEBUG("Computing scan from depth = %f s", info.timeScanFromDepth);
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("Option to create laser scan from depth image is enabled, but "
|
||||
"there is already a laser scan in the captured sensor data. Scan from "
|
||||
"depth will not be created.");
|
||||
}
|
||||
}
|
||||
postUpdate(&data, &info);
|
||||
|
||||
info.cameraName = _camera->getSerial();
|
||||
info.timeTotal = totalTime.ticks();
|
||||
@@ -343,4 +172,181 @@ void CameraThread::mainLoopKill()
|
||||
}
|
||||
}
|
||||
|
||||
void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
|
||||
{
|
||||
UASSERT(dataPtr!=0);
|
||||
SensorData & data = *dataPtr;
|
||||
if(_colorOnly && !data.depthRaw().empty())
|
||||
{
|
||||
data.setDepthOrRightRaw(cv::Mat());
|
||||
}
|
||||
|
||||
if(_distortionModel && !data.depthRaw().empty())
|
||||
{
|
||||
UTimer timer;
|
||||
if(_distortionModel->getWidth() == data.depthRaw().cols &&
|
||||
_distortionModel->getHeight() == data.depthRaw().rows )
|
||||
{
|
||||
cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures.
|
||||
_distortionModel->undistort(depth);
|
||||
data.setDepthOrRightRaw(depth);
|
||||
}
|
||||
else
|
||||
{
|
||||
UERROR("Distortion model size is %dx%d but dpeth image is %dx%d!",
|
||||
_distortionModel->getWidth(), _distortionModel->getHeight(),
|
||||
data.depthRaw().cols, data.depthRaw().rows);
|
||||
}
|
||||
if(info) info->timeUndistortDepth = timer.ticks();
|
||||
}
|
||||
|
||||
if(_bilateralFiltering && !data.depthRaw().empty())
|
||||
{
|
||||
UTimer timer;
|
||||
data.setDepthOrRightRaw(util2d::fastBilateralFiltering(data.depthRaw(), _bilateralSigmaS, _bilateralSigmaR));
|
||||
if(info) info->timeBilateralFiltering = timer.ticks();
|
||||
}
|
||||
|
||||
if(_imageDecimation>1 && !data.imageRaw().empty())
|
||||
{
|
||||
UDEBUG("");
|
||||
UTimer timer;
|
||||
if(!data.depthRaw().empty() &&
|
||||
!(data.depthRaw().rows % _imageDecimation == 0 && data.depthRaw().cols % _imageDecimation == 0))
|
||||
{
|
||||
UERROR("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
|
||||
"Images won't be resized.", _imageDecimation, data.depthRaw().cols, data.depthRaw().rows);
|
||||
}
|
||||
else
|
||||
{
|
||||
data.setImageRaw(util2d::decimate(data.imageRaw(), _imageDecimation));
|
||||
data.setDepthOrRightRaw(util2d::decimate(data.depthOrRightRaw(), _imageDecimation));
|
||||
std::vector<CameraModel> models = data.cameraModels();
|
||||
for(unsigned int i=0; i<models.size(); ++i)
|
||||
{
|
||||
if(models[i].isValidForProjection())
|
||||
{
|
||||
models[i] = models[i].scaled(1.0/double(_imageDecimation));
|
||||
}
|
||||
}
|
||||
data.setCameraModels(models);
|
||||
StereoCameraModel stereoModel = data.stereoCameraModel();
|
||||
if(stereoModel.isValidForProjection())
|
||||
{
|
||||
stereoModel.scale(1.0/double(_imageDecimation));
|
||||
data.setStereoCameraModel(stereoModel);
|
||||
}
|
||||
}
|
||||
if(info) info->timeImageDecimation = timer.ticks();
|
||||
}
|
||||
if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size() == 1)
|
||||
{
|
||||
UDEBUG("");
|
||||
UTimer timer;
|
||||
cv::Mat tmpRgb;
|
||||
cv::flip(data.imageRaw(), tmpRgb, 1);
|
||||
data.setImageRaw(tmpRgb);
|
||||
UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValidForProjection(), "Only single RGBD cameras are supported for mirroring.");
|
||||
if(data.cameraModels().size() && data.cameraModels()[0].cx())
|
||||
{
|
||||
CameraModel tmpModel(
|
||||
data.cameraModels()[0].fx(),
|
||||
data.cameraModels()[0].fy(),
|
||||
float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
|
||||
data.cameraModels()[0].cy(),
|
||||
data.cameraModels()[0].localTransform());
|
||||
data.setCameraModel(tmpModel);
|
||||
}
|
||||
if(!data.depthRaw().empty())
|
||||
{
|
||||
cv::Mat tmpDepth;
|
||||
cv::flip(data.depthRaw(), tmpDepth, 1);
|
||||
data.setDepthOrRightRaw(tmpDepth);
|
||||
}
|
||||
if(info) info->timeMirroring = timer.ticks();
|
||||
}
|
||||
if(_stereoToDepth && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
|
||||
{
|
||||
UDEBUG("");
|
||||
UTimer timer;
|
||||
cv::Mat depth = util2d::depthFromDisparity(
|
||||
_stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
|
||||
data.stereoCameraModel().left().fx(),
|
||||
data.stereoCameraModel().baseline());
|
||||
// set Tx for stereo bundle adjustment (when used)
|
||||
CameraModel model = CameraModel(
|
||||
data.stereoCameraModel().left().fx(),
|
||||
data.stereoCameraModel().left().fy(),
|
||||
data.stereoCameraModel().left().cx(),
|
||||
data.stereoCameraModel().left().cy(),
|
||||
data.stereoCameraModel().localTransform(),
|
||||
-data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx());
|
||||
data.setCameraModel(model);
|
||||
data.setDepthOrRightRaw(depth);
|
||||
data.setStereoCameraModel(StereoCameraModel());
|
||||
if(info) info->timeDisparity = timer.ticks();
|
||||
}
|
||||
if(_scanFromDepth &&
|
||||
data.cameraModels().size() &&
|
||||
data.cameraModels().at(0).isValidForProjection() &&
|
||||
!data.depthRaw().empty())
|
||||
{
|
||||
UDEBUG("");
|
||||
if(data.laserScanRaw().empty())
|
||||
{
|
||||
UASSERT(_scanDecimation >= 1);
|
||||
UTimer timer;
|
||||
pcl::IndicesPtr validIndices(new std::vector<int>);
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(
|
||||
data,
|
||||
_scanDecimation,
|
||||
_scanMaxDepth,
|
||||
_scanMinDepth,
|
||||
validIndices.get());
|
||||
float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
|
||||
cv::Mat scan;
|
||||
const Transform & baseToScan = data.cameraModels()[0].localTransform();
|
||||
if(validIndices->size())
|
||||
{
|
||||
if(_scanVoxelSize>0.0f)
|
||||
{
|
||||
cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
|
||||
float ratio = float(cloud->size()) / float(validIndices->size());
|
||||
maxPoints = ratio * maxPoints;
|
||||
}
|
||||
else if(!cloud->is_dense)
|
||||
{
|
||||
pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
|
||||
pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
|
||||
cloud = denseCloud;
|
||||
}
|
||||
|
||||
if(cloud->size())
|
||||
{
|
||||
if(_scanNormalsK>0)
|
||||
{
|
||||
Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
|
||||
pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
|
||||
pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
|
||||
pcl::concatenateFields(*cloud, *normals, *cloudNormals);
|
||||
scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
|
||||
}
|
||||
else
|
||||
{
|
||||
scan = util3d::laserScanFromPointCloud(*cloud, baseToScan.inverse());
|
||||
}
|
||||
}
|
||||
}
|
||||
data.setLaserScanRaw(scan, LaserScanInfo((int)maxPoints, _scanMaxDepth, baseToScan));
|
||||
if(info) info->timeScanFromDepth = timer.ticks();
|
||||
}
|
||||
else
|
||||
{
|
||||
UWARN("Option to create laser scan from depth image is enabled, but "
|
||||
"there is already a laser scan in the captured sensor data. Scan from "
|
||||
"depth will not be created.");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace rtabmap
|
||||
|
||||
Reference in New Issue
Block a user