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
00038 #include <rtabmap/utilite/UTimer.h>
00039 #include <rtabmap/utilite/ULogger.h>
00040
00041 #include <pcl/io/io.h>
00042
00043 namespace rtabmap
00044 {
00045
00046
00047 CameraThread::CameraThread(Camera * camera, const ParametersMap & parameters) :
00048 _camera(camera),
00049 _mirroring(false),
00050 _colorOnly(false),
00051 _imageDecimation(1),
00052 _stereoToDepth(false),
00053 _scanFromDepth(false),
00054 _scanDecimation(4),
00055 _scanMaxDepth(4.0f),
00056 _scanMinDepth(0.0f),
00057 _scanVoxelSize(0.0f),
00058 _scanNormalsK(0),
00059 _stereoDense(new StereoBM(parameters))
00060 {
00061 UASSERT(_camera != 0);
00062 }
00063
00064 CameraThread::~CameraThread()
00065 {
00066 UDEBUG("");
00067 join(true);
00068 if(_camera)
00069 {
00070 delete _camera;
00071 }
00072 delete _stereoDense;
00073 }
00074
00075 void CameraThread::setImageRate(float imageRate)
00076 {
00077 if(_camera)
00078 {
00079 _camera->setImageRate(imageRate);
00080 }
00081 }
00082
00083 void CameraThread::mainLoop()
00084 {
00085 UDEBUG("");
00086 CameraInfo info;
00087 SensorData data = _camera->takeImage(&info);
00088
00089 if(!data.imageRaw().empty())
00090 {
00091 if(_colorOnly && !data.depthRaw().empty())
00092 {
00093 data.setDepthOrRightRaw(cv::Mat());
00094 }
00095 if(_imageDecimation>1 && !data.imageRaw().empty())
00096 {
00097 UDEBUG("");
00098 UTimer timer;
00099 if(!data.depthRaw().empty() &&
00100 !(data.depthRaw().rows % _imageDecimation == 0 && data.depthRaw().cols % _imageDecimation == 0))
00101 {
00102 UERROR("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
00103 "Images won't be resized.", _imageDecimation, data.depthRaw().cols, data.depthRaw().rows);
00104 }
00105 else
00106 {
00107 data.setImageRaw(util2d::decimate(data.imageRaw(), _imageDecimation));
00108 data.setDepthOrRightRaw(util2d::decimate(data.depthOrRightRaw(), _imageDecimation));
00109 std::vector<CameraModel> models = data.cameraModels();
00110 for(unsigned int i=0; i<models.size(); ++i)
00111 {
00112 if(models[i].isValidForProjection())
00113 {
00114 models[i] = models[i].scaled(1.0/double(_imageDecimation));
00115 }
00116 }
00117 data.setCameraModels(models);
00118 StereoCameraModel stereoModel = data.stereoCameraModel();
00119 if(stereoModel.isValidForProjection())
00120 {
00121 stereoModel.scale(1.0/double(_imageDecimation));
00122 data.setStereoCameraModel(stereoModel);
00123 }
00124 }
00125 info.timeImageDecimation = timer.ticks();
00126 }
00127 if(_mirroring && data.cameraModels().size() == 1)
00128 {
00129 UDEBUG("");
00130 UTimer timer;
00131 cv::Mat tmpRgb;
00132 cv::flip(data.imageRaw(), tmpRgb, 1);
00133 data.setImageRaw(tmpRgb);
00134 UASSERT_MSG(data.cameraModels().size() <= 1 && !data.stereoCameraModel().isValidForProjection(), "Only single RGBD cameras are supported for mirroring.");
00135 if(data.cameraModels().size() && data.cameraModels()[0].cx())
00136 {
00137 CameraModel tmpModel(
00138 data.cameraModels()[0].fx(),
00139 data.cameraModels()[0].fy(),
00140 float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
00141 data.cameraModels()[0].cy(),
00142 data.cameraModels()[0].localTransform());
00143 data.setCameraModel(tmpModel);
00144 }
00145 if(!data.depthRaw().empty())
00146 {
00147 cv::Mat tmpDepth;
00148 cv::flip(data.depthRaw(), tmpDepth, 1);
00149 data.setDepthOrRightRaw(tmpDepth);
00150 }
00151 info.timeMirroring = timer.ticks();
00152 }
00153 if(_stereoToDepth && data.stereoCameraModel().isValidForProjection() && !data.rightRaw().empty())
00154 {
00155 UDEBUG("");
00156 UTimer timer;
00157 cv::Mat depth = util2d::depthFromDisparity(
00158 _stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
00159 data.stereoCameraModel().left().fx(),
00160 data.stereoCameraModel().baseline());
00161 data.setCameraModel(data.stereoCameraModel().left());
00162 data.setDepthOrRightRaw(depth);
00163 data.setStereoCameraModel(StereoCameraModel());
00164 info.timeDisparity = timer.ticks();
00165 UDEBUG("Computing disparity = %f s", info.timeDisparity);
00166 }
00167 if(_scanFromDepth &&
00168 data.cameraModels().size() &&
00169 data.cameraModels().at(0).isValidForProjection() &&
00170 !data.depthRaw().empty())
00171 {
00172 UDEBUG("");
00173 if(data.laserScanRaw().empty())
00174 {
00175 UASSERT(_scanDecimation >= 1);
00176 UTimer timer;
00177 pcl::IndicesPtr validIndices(new std::vector<int>);
00178 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::cloudFromSensorData(data, _scanDecimation, _scanMaxDepth, _scanMinDepth, validIndices.get());
00179 float maxPoints = (data.depthRaw().rows/_scanDecimation)*(data.depthRaw().cols/_scanDecimation);
00180 cv::Mat scan;
00181 if(validIndices->size())
00182 {
00183 if(_scanVoxelSize>0.0f)
00184 {
00185 cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
00186 float ratio = float(cloud->size()) / float(validIndices->size());
00187 maxPoints = ratio * maxPoints;
00188 }
00189 else if(!cloud->is_dense)
00190 {
00191 pcl::PointCloud<pcl::PointXYZ>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZ>);
00192 pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
00193 cloud = denseCloud;
00194 }
00195
00196 if(cloud->size())
00197 {
00198 if(_scanNormalsK>0)
00199 {
00200
00201 Eigen::Vector3f viewPoint(0.0f,0.0f,0.0f);
00202 if(data.cameraModels().size() && !data.cameraModels()[0].localTransform().isNull())
00203 {
00204 viewPoint[0] = data.cameraModels()[0].localTransform().x();
00205 viewPoint[1] = data.cameraModels()[0].localTransform().y();
00206 viewPoint[2] = data.cameraModels()[0].localTransform().z();
00207 }
00208 else if(!data.stereoCameraModel().localTransform().isNull())
00209 {
00210 viewPoint[0] = data.stereoCameraModel().localTransform().x();
00211 viewPoint[1] = data.stereoCameraModel().localTransform().y();
00212 viewPoint[2] = data.stereoCameraModel().localTransform().z();
00213 }
00214 pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, viewPoint);
00215 pcl::PointCloud<pcl::PointNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointNormal>);
00216 pcl::concatenateFields(*cloud, *normals, *cloudNormals);
00217 scan = util3d::laserScanFromPointCloud(*cloudNormals);
00218 }
00219 else
00220 {
00221 scan = util3d::laserScanFromPointCloud(*cloud);
00222 }
00223 }
00224 }
00225 data.setLaserScanRaw(scan, (int)maxPoints, _scanMaxDepth);
00226 info.timeScanFromDepth = timer.ticks();
00227 UDEBUG("Computing scan from depth = %f s", info.timeScanFromDepth);
00228 }
00229 else
00230 {
00231 UWARN("Option to create laser scan from depth image is enabled, but "
00232 "there is already a laser scan in the captured sensor data. Scan from "
00233 "depth will not be created.");
00234 }
00235 }
00236
00237 info.cameraName = _camera->getSerial();
00238 this->post(new CameraEvent(data, info));
00239 }
00240 else if(!this->isKilled())
00241 {
00242 UWARN("no more images...");
00243 this->kill();
00244 this->post(new CameraEvent());
00245 }
00246 }
00247
00248 void CameraThread::mainLoopKill()
00249 {
00250 UDEBUG("");
00251 if(dynamic_cast<CameraFreenect2*>(_camera) != 0)
00252 {
00253 int i=20;
00254 while(i-->0)
00255 {
00256 uSleep(100);
00257 if(!this->isKilled())
00258 {
00259 break;
00260 }
00261 }
00262 if(this->isKilled())
00263 {
00264
00265 UERROR("CameraFreenect2: Failed to kill normally the Freenect2 driver! The thread is locked "
00266 "on waitForNewFrame() method of libfreenect2. This maybe caused by not linking on the right libusb. "
00267 "Note that rtabmap should link on libusb of libfreenect2. "
00268 "Tip before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
00269 }
00270
00271 }
00272 }
00273
00274 }