void CameraThread::mainLoop()
{
UTimer timer;
UDEBUG("");
SensorData data = _camera->takeImage();
if(!data.imageRaw().empty())
{
if(_colorOnly && !data.depthRaw().empty())
{
data.setDepthOrRightRaw(cv::Mat());
}
if(_mirroring && data.cameraModels().size() == 1)
{
cv::Mat tmpRgb;
cv::flip(data.imageRaw(), tmpRgb, 1);
data.setImageRaw(tmpRgb);
if(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(_stereoToDepth && data.stereoCameraModel().isValid() && !data.rightRaw().empty())
{
cv::Mat depth = util2d::depthFromDisparity(
util2d::disparityFromStereoImages(data.imageRaw(), data.rightRaw()),
data.stereoCameraModel().left().fx(),
data.stereoCameraModel().baseline());
data.setCameraModel(data.stereoCameraModel().left());
data.setDepthOrRightRaw(depth);
data.setStereoCameraModel(StereoCameraModel());
}
this->post(new CameraEvent(data, _camera->getSerial()));
}
else if(!this->isKilled())
{
UWARN("no more images...");
this->kill();
this->post(new CameraEvent());
}
}
void CameraThread::mainLoop()
{
UDEBUG("");
CameraInfo info;
SensorData data = _camera->takeImage(&info);
if(!data.imageRaw().empty())
{
if(_colorOnly && !data.depthRaw().empty())
{
data.setDepthOrRightRaw(cv::Mat());
}
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());
data.setCameraModel(data.stereoCameraModel().left());
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);
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;
}
cv::Mat scan;
if(_scanNormalsK>0)
{
scan = util3d::laserScanFromPointCloud(*util3d::computeNormals(cloud, _scanNormalsK));
}
else
{
scan = util3d::laserScanFromPointCloud(*cloud);
}
data.setLaserScanRaw(scan, (int)maxPoints, _scanMaxDepth);
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.");
}
}
info.cameraName = _camera->getSerial();
this->post(new CameraEvent(data, info));
}
else if(!this->isKilled())
{
UWARN("no more images...");
this->kill();
this->post(new CameraEvent());
}
}
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.");
}
}
}