00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #include "rtabmap/core/CameraThread.h"
00029 #include "rtabmap/core/Camera.h"
00030 #include "rtabmap/core/CameraEvent.h"
00031 #include "rtabmap/core/CameraRGBD.h"
00032 #include "rtabmap/core/util2d.h"
00033 #include "rtabmap/core/util3d.h"
00034 #include "rtabmap/core/util3d_surface.h"
00035 #include "rtabmap/core/util3d_filtering.h"
00036 #include "rtabmap/core/StereoDense.h"
00037 #include "rtabmap/core/DBReader.h"
00038 #include "rtabmap/core/clams/discrete_depth_distortion_model.h"
00039 #include <opencv2/stitching/detail/exposure_compensate.hpp>
00040 #include <rtabmap/utilite/UTimer.h>
00041 #include <rtabmap/utilite/ULogger.h>
00042
00043 #include <pcl/io/io.h>
00044
00045 namespace rtabmap
00046 {
00047
00048
00049 CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
00050 _camera(camera),
00051 _mirroring(false),
00052 _stereoExposureCompensation(false),
00053 _colorOnly(false),
00054 _imageDecimation(1),
00055 _stereoToDepth(false),
00056 _scanFromDepth(false),
00057 _scanDecimation(4),
00058 _scanMaxDepth(4.0f),
00059 _scanMinDepth(0.0f),
00060 _scanVoxelSize(0.0f),
00061 _scanNormalsK(0),
00062 _scanNormalsRadius(0.0f),
00063 _stereoDense(new StereoBM(parameters)),
00064 _distortionModel(0),
00065 _bilateralFiltering(false),
00066 _bilateralSigmaS(10),
00067 _bilateralSigmaR(0.1)
00068 {
00069 UASSERT(_camera != 0);
00070 }
00071
00072 CameraThread::~CameraThread()
00073 {
00074 UDEBUG("");
00075 join(true);
00076 delete _camera;
00077 delete _distortionModel;
00078 delete _stereoDense;
00079 }
00080
00081 void CameraThread::setImageRate(float imageRate)
00082 {
00083 if(_camera)
00084 {
00085 _camera->setImageRate(imageRate);
00086 }
00087 }
00088
00089 void CameraThread::setDistortionModel(const std::string & path)
00090 {
00091 if(_distortionModel)
00092 {
00093 delete _distortionModel;
00094 _distortionModel = 0;
00095 }
00096 if(!path.empty())
00097 {
00098 _distortionModel = new clams::DiscreteDepthDistortionModel();
00099 _distortionModel->load(path);
00100 if(!_distortionModel->isValid())
00101 {
00102 UERROR("Loaded distortion model \"%s\" is not valid!", path.c_str());
00103 delete _distortionModel;
00104 _distortionModel = 0;
00105 }
00106 }
00107 }
00108
00109 void CameraThread::enableBilateralFiltering(float sigmaS, float sigmaR)
00110 {
00111 UASSERT(sigmaS > 0.0f && sigmaR > 0.0f);
00112 _bilateralFiltering = true;
00113 _bilateralSigmaS = sigmaS;
00114 _bilateralSigmaR = sigmaR;
00115 }
00116
00117 void CameraThread::mainLoopBegin()
00118 {
00119 ULogger::registerCurrentThread("Camera");
00120 _camera->resetTimer();
00121 }
00122
00123 void CameraThread::mainLoop()
00124 {
00125 UTimer totalTime;
00126 UDEBUG("");
00127 CameraInfo info;
00128 SensorData data = _camera->takeImage(&info);
00129
00130 if(!data.imageRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0))
00131 {
00132 postUpdate(&data, &info);
00133
00134 info.cameraName = _camera->getSerial();
00135 info.timeTotal = totalTime.ticks();
00136 this->post(new CameraEvent(data, info));
00137 }
00138 else if(!this->isKilled())
00139 {
00140 UWARN("no more images...");
00141 this->kill();
00142 this->post(new CameraEvent());
00143 }
00144 }
00145
00146 void CameraThread::mainLoopKill()
00147 {
00148 UDEBUG("");
00149 if(dynamic_cast<CameraFreenect2*>(_camera) != 0)
00150 {
00151 int i=20;
00152 while(i-->0)
00153 {
00154 uSleep(100);
00155 if(!this->isKilled())
00156 {
00157 break;
00158 }
00159 }
00160 if(this->isKilled())
00161 {
00162
00163 UERROR("CameraFreenect2: Failed to kill normally the Freenect2 driver! The thread is locked "
00164 "on waitForNewFrame() method of libfreenect2. This maybe caused by not linking on the right libusb. "
00165 "Note that rtabmap should link on libusb of libfreenect2. "
00166 "Tip before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
00167 }
00168
00169 }
00170 }
00171
00172 void CameraThread::postUpdate(SensorData * dataPtr, CameraInfo * info) const
00173 {
00174 UASSERT(dataPtr!=0);
00175 SensorData & data = *dataPtr;
00176 if(_colorOnly && !data.depthRaw().empty())
00177 {
00178 data.setDepthOrRightRaw(cv::Mat());
00179 }
00180
00181 if(_distortionModel && !data.depthRaw().empty())
00182 {
00183 UTimer timer;
00184 if(_distortionModel->getWidth() == data.depthRaw().cols &&
00185 _distortionModel->getHeight() == data.depthRaw().rows )
00186 {
00187 cv::Mat depth = data.depthRaw().clone();
00188 _distortionModel->undistort(depth);
00189 data.setDepthOrRightRaw(depth);
00190 }
00191 else
00192 {
00193 UERROR("Distortion model size is %dx%d but dpeth image is %dx%d!",
00194 _distortionModel->getWidth(), _distortionModel->getHeight(),
00195 data.depthRaw().cols, data.depthRaw().rows);
00196 }
00197 if(info) info->timeUndistortDepth = timer.ticks();
00198 }
00199
00200 if(_bilateralFiltering && !data.depthRaw().empty())
00201 {
00202 UTimer timer;
00203 data.setDepthOrRightRaw(util2d::fastBilateralFiltering(data.depthRaw(), _bilateralSigmaS, _bilateralSigmaR));
00204 if(info) info->timeBilateralFiltering = timer.ticks();
00205 }
00206
00207 if(_imageDecimation>1 && !data.imageRaw().empty())
00208 {
00209 UDEBUG("");
00210 UTimer timer;
00211 if(!data.depthRaw().empty() &&
00212 !(data.depthRaw().rows % _imageDecimation == 0 && data.depthRaw().cols % _imageDecimation == 0))
00213 {
00214 UERROR("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
00215 "Images won't be resized.", _imageDecimation, data.depthRaw().cols, data.depthRaw().rows);
00216 }
00217 else
00218 {
00219 data.setImageRaw(util2d::decimate(data.imageRaw(), _imageDecimation));
00220 data.setDepthOrRightRaw(util2d::decimate(data.depthOrRightRaw(), _imageDecimation));
00221 std::vector<CameraModel> models = data.cameraModels();
00222 for(unsigned int i=0; i<models.size(); ++i)
00223 {
00224 if(models[i].isValidForProjection())
00225 {
00226 models[i] = models[i].scaled(1.0/double(_imageDecimation));
00227 }
00228 }
00229 data.setCameraModels(models);
00230 StereoCameraModel stereoModel = data.stereoCameraModel();
00231 if(stereoModel.isValidForProjection())
00232 {
00233 stereoModel.scale(1.0/double(_imageDecimation));
00234 data.setStereoCameraModel(stereoModel);
00235 }
00236 }
00237 if(info) info->timeImageDecimation = timer.ticks();
00238 }
00239 if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size() == 1)
00240 {
00241 UDEBUG("");
00242 UTimer timer;
00243 cv::Mat tmpRgb;
00244 cv::flip(data.imageRaw(), tmpRgb, 1);
00245 data.setImageRaw(tmpRgb);
00246 UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValidForProjection(), "Only single RGBD cameras are supported for mirroring.");
00247 if(data.cameraModels().size() && data.cameraModels()[0].cx())
00248 {
00249 CameraModel tmpModel(
00250 data.cameraModels()[0].fx(),
00251 data.cameraModels()[0].fy(),
00252 float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
00253 data.cameraModels()[0].cy(),
00254 data.cameraModels()[0].localTransform(),
00255 data.cameraModels()[0].Tx(),
00256 data.cameraModels()[0].imageSize());
00257 data.setCameraModel(tmpModel);
00258 }
00259 if(!data.depthRaw().empty())
00260 {
00261 cv::Mat tmpDepth;
00262 cv::flip(data.depthRaw(), tmpDepth, 1);
00263 data.setDepthOrRightRaw(tmpDepth);
00264 }
00265 if(info) info->timeMirroring = timer.ticks();
00266 }
00267
00268 if(_stereoExposureCompensation && !data.imageRaw().empty() && !data.rightRaw().empty())
00269 {
00270 #if CV_MAJOR_VERSION < 3
00271 UWARN("Stereo exposure compensation not implemented for OpenCV version under 3.");
00272 #else
00273 UDEBUG("");
00274 UTimer timer;
00275 cv::Ptr<cv::detail::ExposureCompensator> compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN);
00276 std::vector<cv::Point> topLeftCorners(2, cv::Point(0,0));
00277 std::vector<cv::UMat> images;
00278 std::vector<cv::UMat> masks(2, cv::UMat(data.imageRaw().size(), CV_8UC1, cv::Scalar(255)));
00279 images.push_back(data.imageRaw().getUMat(cv::ACCESS_READ));
00280 images.push_back(data.rightRaw().getUMat(cv::ACCESS_READ));
00281 compensator->feed(topLeftCorners, images, masks);
00282 cv::Mat img = data.imageRaw().clone();
00283 compensator->apply(0, cv::Point(0,0), img, masks[0]);
00284 data.setImageRaw(img);
00285 img = data.rightRaw().clone();
00286 compensator->apply(1, cv::Point(0,0), img, masks[1]);
00287 data.setDepthOrRightRaw(img);
00288 cv::detail::GainCompensator * gainCompensator = (cv::detail::GainCompensator*)compensator.get();
00289 UDEBUG("gains = %f %f ", gainCompensator->gains()[0], gainCompensator->gains()[1]);
00290 if(info) info->timeStereoExposureCompensation = timer.ticks();
00291 #endif
00292 }
00293
00294 if(_stereoToDepth && !data.imageRaw().empty() && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
00295 {
00296 UDEBUG("");
00297 UTimer timer;
00298 cv::Mat depth = util2d::depthFromDisparity(
00299 _stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
00300 data.stereoCameraModel().left().fx(),
00301 data.stereoCameraModel().baseline());
00302
00303 CameraModel model = CameraModel(
00304 data.stereoCameraModel().left().fx(),
00305 data.stereoCameraModel().left().fy(),
00306 data.stereoCameraModel().left().cx(),
00307 data.stereoCameraModel().left().cy(),
00308 data.stereoCameraModel().localTransform(),
00309 -data.stereoCameraModel().baseline()*data.stereoCameraModel().left().fx(),
00310 data.stereoCameraModel().left().imageSize());
00311 data.setCameraModel(model);
00312 data.setDepthOrRightRaw(depth);
00313 data.setStereoCameraModel(StereoCameraModel());
00314 if(info) info->timeDisparity = timer.ticks();
00315 }
00316 if(_scanFromDepth &&
00317 data.cameraModels().size() &&
00318 data.cameraModels().at(0).isValidForProjection() &&
00319 !data.depthRaw().empty())
00320 {
00321 UDEBUG("");
00322 if(data.laserScanRaw().isEmpty())
00323 {
00324 UASSERT(_scanDecimation >= 1);
00325 UTimer timer;
00326 pcl::IndicesPtr validIndices(new std::vector<int>);
00327 pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
00328 data,
00329 _scanDecimation,
00330 _scanMaxDepth,
00331 _scanMinDepth,
00332 validIndices.get());
00333 float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
00334 cv::Mat scan;
00335 const Transform & baseToScan = data.cameraModels()[0].localTransform();
00336 LaserScan::Format format = LaserScan::kXYZRGB;
00337 if(validIndices->size())
00338 {
00339 if(_scanVoxelSize>0.0f)
00340 {
00341 cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
00342 float ratio = float(cloud->size()) / float(validIndices->size());
00343 maxPoints = ratio * maxPoints;
00344 }
00345 else if(!cloud->is_dense)
00346 {
00347 pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
00348 pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
00349 cloud = denseCloud;
00350 }
00351
00352 if(cloud->size())
00353 {
00354 if(_scanNormalsK>0 || _scanNormalsRadius>0.0f)
00355 {
00356 Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
00357 pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius, viewPoint);
00358 pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
00359 pcl::concatenateFields(*cloud, *normals, *cloudNormals);
00360 scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
00361 format = LaserScan::kXYZRGBNormal;
00362 }
00363 else
00364 {
00365 scan = util3d::laserScanFromPointCloud(*cloud, baseToScan.inverse());
00366 }
00367 }
00368 }
00369 data.setLaserScanRaw(LaserScan(scan, (int)maxPoints, _scanMaxDepth, format, baseToScan));
00370 if(info) info->timeScanFromDepth = timer.ticks();
00371 }
00372 else
00373 {
00374 UWARN("Option to create laser scan from depth image is enabled, but "
00375 "there is already a laser scan in the captured sensor data. Scan from "
00376 "depth will not be created.");
00377 }
00378 }
00379 }
00380
00381 }