SensorCaptureThread.cpp
Go to the documentation of this file.
1 /*
2 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions are met:
7  * Redistributions of source code must retain the above copyright
8  notice, this list of conditions and the following disclaimer.
9  * Redistributions in binary form must reproduce the above copyright
10  notice, this list of conditions and the following disclaimer in the
11  documentation and/or other materials provided with the distribution.
12  * Neither the name of the Universite de Sherbrooke nor the
13  names of its contributors may be used to endorse or promote products
14  derived from this software without specific prior written permission.
15 
16 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
17 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
20 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27 
28 #include "rtabmap/core/SensorCaptureThread.h"
29 #include "rtabmap/core/Camera.h"
30 #include "rtabmap/core/Lidar.h"
31 #include "rtabmap/core/SensorEvent.h"
32 #include "rtabmap/core/CameraRGBD.h"
33 #include "rtabmap/core/util2d.h"
34 #include "rtabmap/core/util3d.h"
35 #include "rtabmap/core/util3d_surface.h"
36 #include "rtabmap/core/util3d_filtering.h"
37 #include "rtabmap/core/StereoDense.h"
38 #include "rtabmap/core/DBReader.h"
39 #include "rtabmap/core/IMUFilter.h"
40 #include "rtabmap/core/Features2d.h"
41 #include "rtabmap/core/clams/discrete_depth_distortion_model.h"
42 #include <opencv2/imgproc/types_c.h>
43 #include <opencv2/stitching/detail/exposure_compensate.hpp>
44 #include <rtabmap/utilite/UTimer.h>
45 #include <rtabmap/utilite/ULogger.h>
46 #include <rtabmap/utilite/UStl.h>
47 
48 #include <pcl/io/io.h>
49 
50 namespace rtabmap
51 {
52 
53 // ownership transferred
54 SensorCaptureThread::SensorCaptureThread(
55  Camera * camera,
56  const ParametersMap & parameters) :
57  SensorCaptureThread(0, camera, 0, Transform(), 0.0, 1.0f, 0.1, parameters)
58 {
59  UASSERT(camera != 0);
60 }
61 
62 // ownership transferred
63 SensorCaptureThread::SensorCaptureThread(
64  Camera * camera,
65  SensorCapture * odomSensor,
66  const Transform & extrinsics,
67  double poseTimeOffset,
68  float poseScaleFactor,
69  double poseWaitTime,
70  const ParametersMap & parameters) :
71  SensorCaptureThread(0, camera, odomSensor, extrinsics, poseTimeOffset, poseScaleFactor, poseWaitTime, parameters)
72 {
73  UASSERT(camera != 0 && odomSensor != 0 && !extrinsics.isNull());
74 }
75 
76 SensorCaptureThread::SensorCaptureThread(
77  Lidar * lidar,
78  const ParametersMap & parameters) :
79  SensorCaptureThread(lidar, 0, 0, Transform(), 0.0, 1.0f, 0.1, parameters)
80 {
81  UASSERT(lidar != 0);
82 }
83 
84 SensorCaptureThread::SensorCaptureThread(
85  Lidar * lidar,
86  Camera * camera,
87  const ParametersMap & parameters) :
88  SensorCaptureThread(lidar, camera, 0, Transform(), 0.0, 1.0f, 0.1, parameters)
89 {
90  UASSERT(lidar != 0 && camera != 0);
91 }
92 
93 SensorCaptureThread::SensorCaptureThread(
94  Lidar * lidar,
95  SensorCapture * odomSensor,
96  double poseTimeOffset,
97  float poseScaleFactor,
98  double poseWaitTime,
99  const ParametersMap & parameters) :
100  SensorCaptureThread(lidar, 0, odomSensor, Transform(), poseTimeOffset, poseScaleFactor, poseWaitTime, parameters)
101 {
102  UASSERT(lidar != 0 && odomSensor != 0);
103 }
104 
105 SensorCaptureThread::SensorCaptureThread(
106  Lidar * lidar,
107  Camera * camera,
108  SensorCapture * odomSensor,
109  const Transform & extrinsics,
110  double poseTimeOffset,
111  float poseScaleFactor,
112  double poseWaitTime,
113  const ParametersMap & parameters) :
114  _camera(camera),
115  _odomSensor(odomSensor),
116  _lidar(lidar),
117  _extrinsicsOdomToCamera(extrinsics * CameraModel::opticalRotation()),
118  _odomAsGt(false),
119  _poseTimeOffset(poseTimeOffset),
120  _poseScaleFactor(poseScaleFactor),
121  _poseWaitTime(poseWaitTime),
122  _mirroring(false),
123  _stereoExposureCompensation(false),
124  _colorOnly(false),
125  _imageDecimation(1),
126  _histogramMethod(0),
127  _stereoToDepth(false),
128  _scanDeskewing(false),
129  _scanFromDepth(false),
130  _scanDownsampleStep(1),
131  _scanRangeMin(0.0f),
132  _scanRangeMax(0.0f),
133  _scanVoxelSize(0.0f),
134  _scanNormalsK(0),
135  _scanNormalsRadius(0.0f),
136  _scanForceGroundNormalsUp(false),
137  _stereoDense(StereoDense::create(parameters)),
138  _distortionModel(0),
139  _bilateralFiltering(false),
140  _bilateralSigmaS(10),
141  _bilateralSigmaR(0.1),
142  _imuFilter(0),
143  _imuBaseFrameConversion(false),
144  _featureDetector(0),
145  _depthAsMask(Parameters::defaultVisDepthAsMask())
146 {
147  UASSERT(_camera != 0 || _lidar != 0);
148  if(_lidar && _camera)
149  {
150  _camera->setFrameRate(0);
151  }
152  if(_odomSensor)
153  {
154  if(_camera)
155  {
156  if(_odomSensor == _camera && _extrinsicsOdomToCamera.isNull())
157  {
158  _extrinsicsOdomToCamera.setIdentity();
159  }
160  UASSERT(!_extrinsicsOdomToCamera.isNull());
161  UDEBUG("_extrinsicsOdomToCamera=%s", _extrinsicsOdomToCamera.prettyPrint().c_str());
162  }
163  UDEBUG("_poseTimeOffset =%f", _poseTimeOffset);
164  UDEBUG("_poseScaleFactor =%f", _poseScaleFactor);
165  UDEBUG("_poseWaitTime =%f", _poseWaitTime);
166  }
167 }
168 
169 SensorCaptureThread::~SensorCaptureThread()
170 {
171  join(true);
172  if(_odomSensor != _camera && _odomSensor != _lidar)
173  {
174  delete _odomSensor;
175  }
176  delete _camera;
177  delete _lidar;
178  delete _distortionModel;
179  delete _stereoDense;
180  delete _imuFilter;
181  delete _featureDetector;
182 }
183 
184 void SensorCaptureThread::setFrameRate(float frameRate)
185 {
186  if(_lidar)
187  {
188  _lidar->setFrameRate(frameRate);
189  }
190  else if(_camera)
191  {
192  _camera->setFrameRate(frameRate);
193  }
194 }
195 
196 void SensorCaptureThread::setDistortionModel(const std::string & path)
197 {
198  if(_distortionModel)
199  {
200  delete _distortionModel;
201  _distortionModel = 0;
202  }
203  if(!path.empty())
204  {
205  _distortionModel = new clams::DiscreteDepthDistortionModel();
206  _distortionModel->load(path);
207  if(!_distortionModel->isValid())
208  {
209  UERROR("Loaded distortion model \"%s\" is not valid!", path.c_str());
210  delete _distortionModel;
211  _distortionModel = 0;
212  }
213  }
214 }
215 
216 void SensorCaptureThread::enableBilateralFiltering(float sigmaS, float sigmaR)
217 {
218  UASSERT(sigmaS > 0.0f && sigmaR > 0.0f);
219  _bilateralFiltering = true;
220  _bilateralSigmaS = sigmaS;
221  _bilateralSigmaR = sigmaR;
222 }
223 
224 void SensorCaptureThread::enableIMUFiltering(int filteringStrategy, const ParametersMap & parameters, bool baseFrameConversion)
225 {
226  delete _imuFilter;
227  _imuFilter = IMUFilter::create((IMUFilter::Type)filteringStrategy, parameters);
228  _imuBaseFrameConversion = baseFrameConversion;
229 }
230 
231 void SensorCaptureThread::disableIMUFiltering()
232 {
233  delete _imuFilter;
234  _imuFilter = 0;
235 }
236 
237 void SensorCaptureThread::enableFeatureDetection(const ParametersMap & parameters)
238 {
239  delete _featureDetector;
240  ParametersMap params = parameters;
241  ParametersMap defaultParams = Parameters::getDefaultParameters("Vis");
242  uInsert(params, ParametersPair(Parameters::kKpDetectorStrategy(), uValue(params, Parameters::kVisFeatureType(), defaultParams.at(Parameters::kVisFeatureType()))));
243  uInsert(params, ParametersPair(Parameters::kKpMaxFeatures(), uValue(params, Parameters::kVisMaxFeatures(), defaultParams.at(Parameters::kVisMaxFeatures()))));
244  uInsert(params, ParametersPair(Parameters::kKpSSC(), uValue(params, Parameters::kVisSSC(), defaultParams.at(Parameters::kVisSSC()))));
245  uInsert(params, ParametersPair(Parameters::kKpMaxDepth(), uValue(params, Parameters::kVisMaxDepth(), defaultParams.at(Parameters::kVisMaxDepth()))));
246  uInsert(params, ParametersPair(Parameters::kKpMinDepth(), uValue(params, Parameters::kVisMinDepth(), defaultParams.at(Parameters::kVisMinDepth()))));
247  uInsert(params, ParametersPair(Parameters::kKpRoiRatios(), uValue(params, Parameters::kVisRoiRatios(), defaultParams.at(Parameters::kVisRoiRatios()))));
248  uInsert(params, ParametersPair(Parameters::kKpSubPixEps(), uValue(params, Parameters::kVisSubPixEps(), defaultParams.at(Parameters::kVisSubPixEps()))));
249  uInsert(params, ParametersPair(Parameters::kKpSubPixIterations(), uValue(params, Parameters::kVisSubPixIterations(), defaultParams.at(Parameters::kVisSubPixIterations()))));
250  uInsert(params, ParametersPair(Parameters::kKpSubPixWinSize(), uValue(params, Parameters::kVisSubPixWinSize(), defaultParams.at(Parameters::kVisSubPixWinSize()))));
251  uInsert(params, ParametersPair(Parameters::kKpGridRows(), uValue(params, Parameters::kVisGridRows(), defaultParams.at(Parameters::kVisGridRows()))));
252  uInsert(params, ParametersPair(Parameters::kKpGridCols(), uValue(params, Parameters::kVisGridCols(), defaultParams.at(Parameters::kVisGridCols()))));
253  _featureDetector = Feature2D::create(params);
254  _depthAsMask = Parameters::parse(params, Parameters::kVisDepthAsMask(), _depthAsMask);
255 }
256 void SensorCaptureThread::disableFeatureDetection()
257 {
258  delete _featureDetector;
259  _featureDetector = 0;
260 }
261 
262 void SensorCaptureThread::setScanParameters(
263  bool fromDepth,
264  int downsampleStep,
265  float rangeMin,
266  float rangeMax,
267  float voxelSize,
268  int normalsK,
269  float normalsRadius,
270  bool forceGroundNormalsUp,
271  bool deskewing)
272 {
273  setScanParameters(fromDepth, downsampleStep, rangeMin, rangeMax, voxelSize, normalsK, normalsRadius, forceGroundNormalsUp?0.8f:0.0f, deskewing);
274 }
275 
276 void SensorCaptureThread::setScanParameters(
277  bool fromDepth,
278  int downsampleStep, // decimation of the depth image in case the scan is from depth image
279  float rangeMin,
280  float rangeMax,
281  float voxelSize,
282  int normalsK,
283  float normalsRadius,
284  float groundNormalsUp,
285  bool deskewing)
286 {
287  _scanDeskewing = deskewing;
288  _scanFromDepth = fromDepth;
289  _scanDownsampleStep=downsampleStep;
290  _scanRangeMin = rangeMin;
291  _scanRangeMax = rangeMax;
292  _scanVoxelSize = voxelSize;
293  _scanNormalsK = normalsK;
294  _scanNormalsRadius = normalsRadius;
295  _scanForceGroundNormalsUp = groundNormalsUp;
296 }
297 
298 bool SensorCaptureThread::odomProvided() const
299 {
300  if(_odomAsGt)
301  {
302  return false;
303  }
304  return _odomSensor != 0;
305 }
306 
307 void SensorCaptureThread::mainLoopBegin()
308 {
309  ULogger::registerCurrentThread("Camera");
310  if(_lidar)
311  {
312  _lidar->resetTimer();
313  }
314  else if(_camera)
315  {
316  _camera->resetTimer();
317  }
318  if(_imuFilter)
319  {
320  // In case we paused the camera and moved somewhere else, restart filtering.
321  _imuFilter->reset();
322  }
323 }
324 
325 void SensorCaptureThread::mainLoop()
326 {
327  UASSERT(_lidar || _camera);
328  UTimer totalTime;
329  SensorCaptureInfo info;
330  SensorData data;
331  SensorData cameraData;
332  double lidarStamp = 0.0;
333  double cameraStamp = 0.0;
334  if(_lidar)
335  {
336  data = _lidar->takeData(&info);
337  if(data.stamp() == 0.0)
338  {
339  UWARN("Could not capture scan!");
340  }
341  else
342  {
343  lidarStamp = data.stamp();
344  if(_camera)
345  {
346  cameraData = _camera->takeData();
347  if(cameraData.stamp() == 0.0)
348  {
349  UWARN("Could not capture image!");
350  }
351  else
352  {
353  double stampStart = UTimer::now();
354  while(cameraData.stamp() < data.stamp() &&
355  !isKilled() &&
356  UTimer::now() - stampStart < _poseWaitTime &&
357  !cameraData.imageRaw().empty())
358  {
359  // Make sure the camera frame is newer than lidar frame so
360  // that if there are imus published by the cameras, we can get
361  // them all in odometry before deskewing.
362  cameraData = _camera->takeData();
363  }
364 
365  cameraStamp = cameraData.stamp();
366  if(cameraData.stamp() < data.stamp())
367  {
368  UWARN("Could not get camera frame (%f) with stamp more recent than lidar frame (%f) after waiting for %f seconds.",
369  cameraData.stamp(),
370  data.stamp(),
371  _poseWaitTime);
372  }
373 
374  if(!cameraData.stereoCameraModels().empty())
375  {
376  data.setStereoImage(cameraData.imageRaw(), cameraData.depthOrRightRaw(), cameraData.stereoCameraModels(), true);
377  }
378  else
379  {
380  data.setRGBDImage(cameraData.imageRaw(), cameraData.depthOrRightRaw(), cameraData.cameraModels(), true);
381  }
382  }
383  }
384  }
385  }
386  else if(_camera)
387  {
388  data = _camera->takeData(&info);
389  if(data.stamp() == 0.0)
390  {
391  UWARN("Could not capture image!");
392  }
393  else
394  {
395  cameraStamp = cameraData.stamp();
396  }
397  }
398 
399  if(_odomSensor && data.stamp() != 0.0)
400  {
401  if(lidarStamp!=0.0 && _scanDeskewing)
402  {
403  UDEBUG("Deskewing begin");
404  if(!data.laserScanRaw().empty() && data.laserScanRaw().hasTime())
405  {
406  float scanTime =
407  data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()] -
408  data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
409 
410  Transform poseFirstScan;
411  Transform poseLastScan;
412  cv::Mat cov;
413  double firstStamp = data.stamp() + data.laserScanRaw().data().ptr<float>(0, 0)[data.laserScanRaw().getTimeOffset()];
414  double lastStamp = data.stamp() + data.laserScanRaw().data().ptr<float>(0, data.laserScanRaw().size()-1)[data.laserScanRaw().getTimeOffset()];
415  if(_odomSensor->getPose(firstStamp+_poseTimeOffset, poseFirstScan, cov, _poseWaitTime>0?_poseWaitTime:0) &&
416  _odomSensor->getPose(lastStamp+_poseTimeOffset, poseLastScan, cov, _poseWaitTime>0?_poseWaitTime:0))
417  {
418  if(_poseScaleFactor>0 && _poseScaleFactor!=1.0f)
419  {
420  poseFirstScan.x() *= _poseScaleFactor;
421  poseFirstScan.y() *= _poseScaleFactor;
422  poseFirstScan.z() *= _poseScaleFactor;
423  poseLastScan.x() *= _poseScaleFactor;
424  poseLastScan.y() *= _poseScaleFactor;
425  poseLastScan.z() *= _poseScaleFactor;
426  }
427 
428  UASSERT(!poseFirstScan.isNull() && !poseLastScan.isNull());
429 
430  Transform transform = poseFirstScan.inverse() * poseLastScan;
431 
432  // convert to velocity
433  float x,y,z,roll,pitch,yaw;
434  transform.getTranslationAndEulerAngles(x, y, z, roll, pitch, yaw);
435  x/=scanTime;
436  y/=scanTime;
437  z/=scanTime;
438  roll /= scanTime;
439  pitch /= scanTime;
440  yaw /= scanTime;
441 
442  Transform velocity(x,y,z,roll,pitch,yaw);
443  UTimer timeDeskewing;
444  LaserScan scanDeskewed = util3d::deskew(data.laserScanRaw(), data.stamp(), velocity);
445  info.timeDeskewing = timeDeskewing.ticks();
446  if(!scanDeskewed.isEmpty())
447  {
448  data.setLaserScan(scanDeskewed);
449  }
450  }
451  else if(!data.laserScanRaw().empty())
452  {
453  UWARN("Failed to get poses for stamps %f and %f! Lidar won't be deskewed!", firstStamp+_poseTimeOffset, lastStamp+_poseTimeOffset);
454  }
455  }
456  else if(!data.laserScanRaw().empty())
457  {
458  UWARN("The input scan doesn't have time channel (scan format received=%s)!. Lidar won't be deskewed!", data.laserScanRaw().formatName().c_str());
459  }
460  UDEBUG("Deskewing end");
461  }
462 
463  Transform pose;
464  cv::Mat covariance;
465  if(!info.odomPose.isNull() && _lidar == 0 && _odomSensor == _camera)
466  {
467  // Do nothing, we have already the pose
468  }
469  else if(_odomSensor->getPose(data.stamp()+_poseTimeOffset, pose, covariance, _poseWaitTime>0?_poseWaitTime:0))
470  {
471  info.odomPose = pose;
472  info.odomCovariance = covariance;
473  if(_poseScaleFactor>0 && _poseScaleFactor!=1.0f)
474  {
475  info.odomPose.x() *= _poseScaleFactor;
476  info.odomPose.y() *= _poseScaleFactor;
477  info.odomPose.z() *= _poseScaleFactor;
478  }
479 
480  if(cameraStamp != 0.0)
481  {
482  Transform cameraCorrection = Transform::getIdentity();
483  if(lidarStamp > 0.0 && lidarStamp != cameraStamp)
484  {
485  if(_odomSensor->getPose(cameraStamp+_poseTimeOffset, pose, covariance, _poseWaitTime>0?_poseWaitTime:0))
486  {
487  cameraCorrection = info.odomPose.inverse() * pose;
488  }
489  else
490  {
491  UWARN("Could not get pose at stamp %f, the camera local motion against lidar won't be adjusted.", cameraStamp);
492  }
493  }
494 
495  // Adjust local transform of the camera based on the pose frame
496  if(!data.cameraModels().empty())
497  {
498  UASSERT(data.cameraModels().size()==1);
499  CameraModel model = data.cameraModels()[0];
500  model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera);
501  data.setCameraModel(model);
502  }
503  else if(!data.stereoCameraModels().empty())
504  {
505  UASSERT(data.stereoCameraModels().size()==1);
506  StereoCameraModel model = data.stereoCameraModels()[0];
507  model.setLocalTransform(cameraCorrection*_extrinsicsOdomToCamera);
508  data.setStereoCameraModel(model);
509  }
510  }
511 
512  // Fake IMU to intialize gravity (assuming pose is aligned with gravity!)
513  Eigen::Quaterniond q = info.odomPose.getQuaterniond();
514  data.setIMU(IMU(
515  cv::Vec4d(q.x(), q.y(), q.z(), q.w()), cv::Mat(),
516  cv::Vec3d(), cv::Mat(),
517  cv::Vec3d(), cv::Mat(),
518  Transform::getIdentity()));
519  this->disableIMUFiltering();
520  }
521  else
522  {
523  UWARN("Could not get pose at stamp %f", data.stamp());
524  }
525  }
526 
527  if(_odomAsGt && !info.odomPose.isNull())
528  {
529  data.setGroundTruth(info.odomPose);
530  info.odomPose.setNull();
531  }
532 
533  if(!data.imageRaw().empty() || !data.laserScanRaw().empty() || (dynamic_cast<DBReader*>(_camera) != 0 && data.id()>0)) // intermediate nodes could not have image set
534  {
535  postUpdate(&data, &info);
536  info.cameraName = _lidar?_lidar->getSerial():_camera->getSerial();
537  info.timeTotal = totalTime.ticks();
538  this->post(new SensorEvent(data, info));
539  }
540  else if(!this->isKilled())
541  {
542  UWARN("no more data...");
543  this->kill();
544  this->post(new SensorEvent());
545  }
546 }
547 
548 void SensorCaptureThread::mainLoopKill()
549 {
550  if(dynamic_cast<CameraFreenect2*>(_camera) != 0)
551  {
552  int i=20;
553  while(i-->0)
554  {
555  uSleep(100);
556  if(!this->isKilled())
557  {
558  break;
559  }
560  }
561  if(this->isKilled())
562  {
563  //still in killed state, maybe a deadlock
564  UERROR("CameraFreenect2: Failed to kill normally the Freenect2 driver! The thread is locked "
565  "on waitForNewFrame() method of libfreenect2. This maybe caused by not linking on the right libusb. "
566  "Note that rtabmap should link on libusb of libfreenect2. "
567  "Tip before starting rtabmap: \"$ export LD_LIBRARY_PATH=~/libfreenect2/depends/libusb/lib:$LD_LIBRARY_PATH\"");
568  }
569 
570  }
571 }
572 
573 void SensorCaptureThread::postUpdate(SensorData * dataPtr, SensorCaptureInfo * info) const
574 {
575  UASSERT(dataPtr!=0);
576  SensorData & data = *dataPtr;
577  if(_colorOnly)
578  {
579  if(!data.depthRaw().empty())
580  {
581  data.setRGBDImage(data.imageRaw(), cv::Mat(), data.cameraModels());
582  }
583  else if(!data.rightRaw().empty())
584  {
585  std::vector<CameraModel> models;
586  for(size_t i=0; i<data.stereoCameraModels().size(); ++i)
587  {
588  models.push_back(data.stereoCameraModels()[i].left());
589  }
590  data.setRGBDImage(data.imageRaw(), cv::Mat(), models);
591  }
592  }
593 
594  if(_distortionModel && !data.depthRaw().empty())
595  {
596  UTimer timer;
597  if(_distortionModel->getWidth() >= data.depthRaw().cols &&
598  _distortionModel->getHeight() >= data.depthRaw().rows &&
599  _distortionModel->getWidth() % data.depthRaw().cols == 0 &&
600  _distortionModel->getHeight() % data.depthRaw().rows == 0)
601  {
602  cv::Mat depth = data.depthRaw().clone();// make sure we are not modifying data in cached signatures.
603  _distortionModel->undistort(depth);
604  data.setRGBDImage(data.imageRaw(), depth, data.cameraModels());
605  }
606  else
607  {
608  UERROR("Distortion model size is %dx%d but depth image is %dx%d!",
609  _distortionModel->getWidth(), _distortionModel->getHeight(),
610  data.depthRaw().cols, data.depthRaw().rows);
611  }
612  if(info) info->timeUndistortDepth = timer.ticks();
613  }
614 
615  if(_bilateralFiltering && !data.depthRaw().empty())
616  {
617  UTimer timer;
618  data.setRGBDImage(data.imageRaw(), util2d::fastBilateralFiltering(data.depthRaw(), _bilateralSigmaS, _bilateralSigmaR), data.cameraModels());
619  if(info) info->timeBilateralFiltering = timer.ticks();
620  }
621 
622  if(_imageDecimation>1 && !data.imageRaw().empty())
623  {
624  UDEBUG("");
625  UTimer timer;
626  if(!data.depthRaw().empty() &&
627  !(data.depthRaw().rows % _imageDecimation == 0 && data.depthRaw().cols % _imageDecimation == 0))
628  {
629  UERROR("Decimation of depth images should be exact (decimation=%d, size=(%d,%d))! "
630  "Images won't be resized.", _imageDecimation, data.depthRaw().cols, data.depthRaw().rows);
631  }
632  else
633  {
634  cv::Mat image = util2d::decimate(data.imageRaw(), _imageDecimation);
635 
636  int depthDecimation = _imageDecimation;
637  if(data.depthOrRightRaw().rows <= image.rows || data.depthOrRightRaw().cols <= image.cols)
638  {
639  depthDecimation = 1;
640  }
641  else
642  {
643  depthDecimation = 2;
644  while(data.depthOrRightRaw().rows / depthDecimation > image.rows ||
645  data.depthOrRightRaw().cols / depthDecimation > image.cols ||
646  data.depthOrRightRaw().rows % depthDecimation != 0 ||
647  data.depthOrRightRaw().cols % depthDecimation != 0)
648  {
649  ++depthDecimation;
650  }
651  UDEBUG("depthDecimation=%d", depthDecimation);
652  }
653  cv::Mat depthOrRight = util2d::decimate(data.depthOrRightRaw(), depthDecimation);
654 
655  std::vector<CameraModel> models = data.cameraModels();
656  for(unsigned int i=0; i<models.size(); ++i)
657  {
658  if(models[i].isValidForProjection())
659  {
660  models[i] = models[i].scaled(1.0/double(_imageDecimation));
661  }
662  }
663  if(!models.empty())
664  {
665  data.setRGBDImage(image, depthOrRight, models);
666  }
667 
668 
669  std::vector<StereoCameraModel> stereoModels = data.stereoCameraModels();
670  for(unsigned int i=0; i<stereoModels.size(); ++i)
671  {
672  if(stereoModels[i].isValidForProjection())
673  {
674  stereoModels[i].scale(1.0/double(_imageDecimation));
675  }
676  }
677  if(!stereoModels.empty())
678  {
679  data.setStereoImage(image, depthOrRight, stereoModels);
680  }
681 
682  std::vector<cv::KeyPoint> kpts = data.keypoints();
683  double log2value = log(double(_imageDecimation))/log(2.0);
684  for(unsigned int i=0; i<kpts.size(); ++i)
685  {
686  kpts[i].pt.x /= _imageDecimation;
687  kpts[i].pt.y /= _imageDecimation;
688  kpts[i].size /= _imageDecimation;
689  kpts[i].octave -= log2value;
690  }
691  data.setFeatures(kpts, data.keypoints3D(), data.descriptors());
692  }
693  if(info) info->timeImageDecimation = timer.ticks();
694  }
695 
696  if(_mirroring && !data.imageRaw().empty() && data.cameraModels().size()>=1)
697  {
698  if(data.cameraModels().size() == 1)
699  {
700  UDEBUG("");
701  UTimer timer;
702  cv::Mat tmpRgb;
703  cv::flip(data.imageRaw(), tmpRgb, 1);
704 
705  CameraModel tmpModel = data.cameraModels()[0];
706  if(data.cameraModels()[0].cx())
707  {
708  tmpModel = CameraModel(
709  data.cameraModels()[0].fx(),
710  data.cameraModels()[0].fy(),
711  float(data.imageRaw().cols) - data.cameraModels()[0].cx(),
712  data.cameraModels()[0].cy(),
713  data.cameraModels()[0].localTransform(),
714  data.cameraModels()[0].Tx(),
715  data.cameraModels()[0].imageSize());
716  }
717  cv::Mat tmpDepth = data.depthOrRightRaw();
718  if(!data.depthRaw().empty())
719  {
720  cv::flip(data.depthRaw(), tmpDepth, 1);
721  }
722  data.setRGBDImage(tmpRgb, tmpDepth, tmpModel);
723  if(info) info->timeMirroring = timer.ticks();
724  }
725  else
726  {
727  UWARN("Mirroring is not implemented for multiple cameras or stereo...");
728  }
729  }
730 
731  if(_histogramMethod && !data.imageRaw().empty())
732  {
733  UDEBUG("");
734  UTimer timer;
735  cv::Mat image;
736  if(_histogramMethod == 1)
737  {
738  if(data.imageRaw().type() == CV_8UC1)
739  {
740  cv::equalizeHist(data.imageRaw(), image);
741  }
742  else if(data.imageRaw().type() == CV_8UC3)
743  {
744  cv::Mat channels[3];
745  cv::cvtColor(data.imageRaw(), image, CV_BGR2YCrCb);
746  cv::split(image, channels);
747  cv::equalizeHist(channels[0], channels[0]);
748  cv::merge(channels, 3, image);
749  cv::cvtColor(image, image, CV_YCrCb2BGR);
750  }
751  if(!data.depthRaw().empty())
752  {
753  data.setRGBDImage(image, data.depthRaw(), data.cameraModels());
754  }
755  else if(!data.rightRaw().empty())
756  {
757  cv::Mat right;
758  if(data.rightRaw().type() == CV_8UC1)
759  {
760  cv::equalizeHist(data.rightRaw(), right);
761  }
762  else if(data.rightRaw().type() == CV_8UC3)
763  {
764  cv::Mat channels[3];
765  cv::cvtColor(data.rightRaw(), right, CV_BGR2YCrCb);
766  cv::split(right, channels);
767  cv::equalizeHist(channels[0], channels[0]);
768  cv::merge(channels, 3, right);
769  cv::cvtColor(right, right, CV_YCrCb2BGR);
770  }
771  data.setStereoImage(image, right, data.stereoCameraModels()[0]);
772  }
773  }
774  else if(_histogramMethod == 2)
775  {
776  cv::Ptr<cv::CLAHE> clahe = cv::createCLAHE(3.0);
777  if(data.imageRaw().type() == CV_8UC1)
778  {
779  clahe->apply(data.imageRaw(), image);
780  }
781  else if(data.imageRaw().type() == CV_8UC3)
782  {
783  cv::Mat channels[3];
784  cv::cvtColor(data.imageRaw(), image, CV_BGR2YCrCb);
785  cv::split(image, channels);
786  clahe->apply(channels[0], channels[0]);
787  cv::merge(channels, 3, image);
788  cv::cvtColor(image, image, CV_YCrCb2BGR);
789  }
790  if(!data.depthRaw().empty())
791  {
792  data.setRGBDImage(image, data.depthRaw(), data.cameraModels());
793  }
794  else if(!data.rightRaw().empty())
795  {
796  cv::Mat right;
797  if(data.rightRaw().type() == CV_8UC1)
798  {
799  clahe->apply(data.rightRaw(), right);
800  }
801  else if(data.rightRaw().type() == CV_8UC3)
802  {
803  cv::Mat channels[3];
804  cv::cvtColor(data.rightRaw(), right, CV_BGR2YCrCb);
805  cv::split(right, channels);
806  clahe->apply(channels[0], channels[0]);
807  cv::merge(channels, 3, right);
808  cv::cvtColor(right, right, CV_YCrCb2BGR);
809  }
810  data.setStereoImage(image, right, data.stereoCameraModels()[0]);
811  }
812  }
813  if(info) info->timeHistogramEqualization = timer.ticks();
814  }
815 
816  if(_stereoExposureCompensation && !data.imageRaw().empty() && !data.rightRaw().empty())
817  {
818  if(data.stereoCameraModels().size()==1)
819  {
820 #if CV_MAJOR_VERSION < 3
821  UWARN("Stereo exposure compensation not implemented for OpenCV version under 3.");
822 #else
823  UDEBUG("");
824  UTimer timer;
825  cv::Ptr<cv::detail::ExposureCompensator> compensator = cv::detail::ExposureCompensator::createDefault(cv::detail::ExposureCompensator::GAIN);
826  std::vector<cv::Point> topLeftCorners(2, cv::Point(0,0));
827  std::vector<cv::UMat> images;
828  std::vector<cv::UMat> masks(2, cv::UMat(data.imageRaw().size(), CV_8UC1, cv::Scalar(255)));
829  images.push_back(data.imageRaw().getUMat(cv::ACCESS_READ));
830  images.push_back(data.rightRaw().getUMat(cv::ACCESS_READ));
831  compensator->feed(topLeftCorners, images, masks);
832  cv::Mat imgLeft = data.imageRaw().clone();
833  compensator->apply(0, cv::Point(0,0), imgLeft, masks[0]);
834  cv::Mat imgRight = data.rightRaw().clone();
835  compensator->apply(1, cv::Point(0,0), imgRight, masks[1]);
836  data.setStereoImage(imgLeft, imgRight, data.stereoCameraModels()[0]);
837  cv::detail::GainCompensator * gainCompensator = (cv::detail::GainCompensator*)compensator.get();
838  UDEBUG("gains = %f %f ", gainCompensator->gains()[0], gainCompensator->gains()[1]);
839  if(info) info->timeStereoExposureCompensation = timer.ticks();
840 #endif
841  }
842  else
843  {
844  UWARN("Stereo exposure compensation only is not implemented to multiple stereo cameras...");
845  }
846  }
847 
848  if(_stereoToDepth && !data.imageRaw().empty() && !data.stereoCameraModels().empty() && data.stereoCameraModels()[0].isValidForProjection() && !data.rightRaw().empty())
849  {
850  if(data.stereoCameraModels().size()==1)
851  {
852  UDEBUG("");
853  UTimer timer;
854  cv::Mat depth = util2d::depthFromDisparity(
855  _stereoDense->computeDisparity(data.imageRaw(), data.rightRaw()),
856  data.stereoCameraModels()[0].left().fx(),
857  data.stereoCameraModels()[0].baseline());
858  // set Tx for stereo bundle adjustment (when used)
859  CameraModel model = CameraModel(
860  data.stereoCameraModels()[0].left().fx(),
861  data.stereoCameraModels()[0].left().fy(),
862  data.stereoCameraModels()[0].left().cx(),
863  data.stereoCameraModels()[0].left().cy(),
864  data.stereoCameraModels()[0].localTransform(),
865  -data.stereoCameraModels()[0].baseline()*data.stereoCameraModels()[0].left().fx(),
866  data.stereoCameraModels()[0].left().imageSize());
867  data.setRGBDImage(data.imageRaw(), depth, model);
868  if(info) info->timeDisparity = timer.ticks();
869  }
870  else
871  {
872  UWARN("Stereo to depth is not implemented for multiple stereo cameras...");
873  }
874  }
875  if(_scanFromDepth &&
876  data.cameraModels().size() &&
877  data.cameraModels().at(0).isValidForProjection() &&
878  !data.depthRaw().empty())
879  {
880  UDEBUG("");
881  if(data.laserScanRaw().isEmpty())
882  {
883  UASSERT(_scanDownsampleStep >= 1);
884  UTimer timer;
885  pcl::IndicesPtr validIndices(new std::vector<int>);
886  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud = util3d::cloudRGBFromSensorData(
887  data,
888  _scanDownsampleStep,
889  _scanRangeMax,
890  _scanRangeMin,
891  validIndices.get());
892  float maxPoints = (data.depthRaw().rows/_scanDownsampleStep)*(data.depthRaw().cols/_scanDownsampleStep);
893  LaserScan scan;
894  const Transform & baseToScan = data.cameraModels()[0].localTransform();
895  if(validIndices->size())
896  {
897  if(_scanVoxelSize>0.0f)
898  {
899  cloud = util3d::voxelize(cloud, validIndices, _scanVoxelSize);
900  float ratio = float(cloud->size()) / float(validIndices->size());
901  maxPoints = ratio * maxPoints;
902  }
903  else if(!cloud->is_dense)
904  {
905  pcl::PointCloud<pcl::PointXYZRGB>::Ptr denseCloud(new pcl::PointCloud<pcl::PointXYZRGB>);
906  pcl::copyPointCloud(*cloud, *validIndices, *denseCloud);
907  cloud = denseCloud;
908  }
909 
910  if(cloud->size())
911  {
912  if(_scanNormalsK>0 || _scanNormalsRadius>0.0f)
913  {
914  Eigen::Vector3f viewPoint(baseToScan.x(), baseToScan.y(), baseToScan.z());
915  pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(cloud, _scanNormalsK, _scanNormalsRadius, viewPoint);
916  pcl::PointCloud<pcl::PointXYZRGBNormal>::Ptr cloudNormals(new pcl::PointCloud<pcl::PointXYZRGBNormal>);
917  pcl::concatenateFields(*cloud, *normals, *cloudNormals);
918  scan = util3d::laserScanFromPointCloud(*cloudNormals, baseToScan.inverse());
919  }
920  else
921  {
922  scan = util3d::laserScanFromPointCloud(*cloud, baseToScan.inverse());
923  }
924  }
925  }
926  data.setLaserScan(LaserScan(scan, (int)maxPoints, _scanRangeMax, baseToScan));
927  if(info) info->timeScanFromDepth = timer.ticks();
928  }
929  else
930  {
931  UWARN("Option to create laser scan from depth image is enabled, but "
932  "there is already a laser scan in the captured sensor data. Scan from "
933  "depth will not be created.");
934  }
935  }
936  else if(!data.laserScanRaw().isEmpty())
937  {
938  UDEBUG("");
939  // filter the scan after registration
940  data.setLaserScan(util3d::commonFiltering(data.laserScanRaw(), _scanDownsampleStep, _scanRangeMin, _scanRangeMax, _scanVoxelSize, _scanNormalsK, _scanNormalsRadius, _scanForceGroundNormalsUp));
941  }
942 
943  // IMU filtering
944  if(_imuFilter && !data.imu().empty())
945  {
946  if(data.imu().angularVelocity()[0] == 0 &&
947  data.imu().angularVelocity()[1] == 0 &&
948  data.imu().angularVelocity()[2] == 0 &&
949  data.imu().linearAcceleration()[0] == 0 &&
950  data.imu().linearAcceleration()[1] == 0 &&
951  data.imu().linearAcceleration()[2] == 0)
952  {
953  UWARN("IMU's acc and gyr values are null! Please disable IMU filtering.");
954  }
955  else
956  {
957  // Transform IMU data in base_link to correctly initialize yaw
958  IMU imu = data.imu();
959  if(_imuBaseFrameConversion)
960  {
961  UASSERT(!data.imu().localTransform().isNull());
962  imu.convertToBaseFrame();
963 
964  }
965  _imuFilter->update(
966  imu.angularVelocity()[0],
967  imu.angularVelocity()[1],
968  imu.angularVelocity()[2],
969  imu.linearAcceleration()[0],
970  imu.linearAcceleration()[1],
971  imu.linearAcceleration()[2],
972  data.stamp());
973  double qx,qy,qz,qw;
974  _imuFilter->getOrientation(qx,qy,qz,qw);
975 
976  data.setIMU(IMU(
977  cv::Vec4d(qx,qy,qz,qw), cv::Mat::eye(3,3,CV_64FC1),
978  imu.angularVelocity(), imu.angularVelocityCovariance(),
979  imu.linearAcceleration(), imu.linearAccelerationCovariance(),
980  imu.localTransform()));
981 
982  UDEBUG("%f %f %f %f (gyro=%f %f %f, acc=%f %f %f, %fs)",
983  data.imu().orientation()[0],
984  data.imu().orientation()[1],
985  data.imu().orientation()[2],
986  data.imu().orientation()[3],
987  data.imu().angularVelocity()[0],
988  data.imu().angularVelocity()[1],
989  data.imu().angularVelocity()[2],
990  data.imu().linearAcceleration()[0],
991  data.imu().linearAcceleration()[1],
992  data.imu().linearAcceleration()[2],
993  data.stamp());
994  }
995  }
996 
997  if(_featureDetector && !data.imageRaw().empty())
998  {
999  UDEBUG("Detecting features");
1000  cv::Mat grayScaleImg = data.imageRaw();
1001  if(data.imageRaw().channels() > 1)
1002  {
1003  cv::Mat tmp;
1004  cv::cvtColor(grayScaleImg, tmp, cv::COLOR_BGR2GRAY);
1005  grayScaleImg = tmp;
1006  }
1007 
1008  cv::Mat depthMask;
1009  if(!data.depthRaw().empty() && _depthAsMask)
1010  {
1011  if( data.imageRaw().rows % data.depthRaw().rows == 0 &&
1012  data.imageRaw().cols % data.depthRaw().cols == 0 &&
1013  data.imageRaw().rows/data.depthRaw().rows == data.imageRaw().cols/data.depthRaw().cols)
1014  {
1015  depthMask = util2d::interpolate(data.depthRaw(), data.imageRaw().rows/data.depthRaw().rows, 0.1f);
1016  }
1017  else
1018  {
1019  UWARN("%s is true, but RGB size (%dx%d) modulo depth size (%dx%d) is not 0. Ignoring depth mask for feature detection.",
1020  Parameters::kVisDepthAsMask().c_str(),
1021  data.imageRaw().rows, data.imageRaw().cols,
1022  data.depthRaw().rows, data.depthRaw().cols);
1023  }
1024  }
1025 
1026  std::vector<cv::KeyPoint> keypoints = _featureDetector->generateKeypoints(grayScaleImg, depthMask);
1027  cv::Mat descriptors;
1028  std::vector<cv::Point3f> keypoints3D;
1029  if(!keypoints.empty())
1030  {
1031  descriptors = _featureDetector->generateDescriptors(grayScaleImg, keypoints);
1032  if(!keypoints.empty())
1033  {
1034  keypoints3D = _featureDetector->generateKeypoints3D(data, keypoints);
1035  }
1036  }
1037 
1038  data.setFeatures(keypoints, keypoints3D, descriptors);
1039  }
1040 }
1041 
1042 } // namespace rtabmap
rtabmap::SensorData
Definition: SensorData.h:51
rtabmap::SensorCaptureThread
Definition: SensorCaptureThread.h:58
rtabmap::ParametersPair
std::pair< std::string, std::string > ParametersPair
Definition: Parameters.h:44
create
ADT create(const Signature &signature)
rtabmap::SensorCaptureThread::_odomSensor
SensorCapture * _odomSensor
Definition: SensorCaptureThread.h:180
rtabmap::SensorCapture::getSerial
virtual std::string getSerial() const =0
rtabmap::IMU::linearAcceleration
const cv::Vec3d linearAcceleration() const
Definition: IMU.h:59
rtabmap::Feature2D::create
static Feature2D * create(const ParametersMap &parameters=ParametersMap())
Definition: Features2d.cpp:569
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
rtabmap::SensorCaptureThread::_histogramMethod
int _histogramMethod
Definition: SensorCaptureThread.h:191
clams::DiscreteDepthDistortionModel
Definition: discrete_depth_distortion_model.h:68
rtabmap::Feature2D::generateKeypoints
std::vector< cv::KeyPoint > generateKeypoints(const cv::Mat &image, const cv::Mat &mask=cv::Mat())
Definition: Features2d.cpp:728
SensorCaptureThread.h
glm::yaw
GLM_FUNC_DECL T yaw(detail::tquat< T, P > const &x)
util3d_surface.h
rtabmap::SensorCaptureThread::_scanRangeMax
float _scanRangeMax
Definition: SensorCaptureThread.h:197
rtabmap::SensorCaptureThread::setScanParameters
RTABMAP_DEPRECATED void setScanParameters(bool fromDepth, int downsampleStep, float rangeMin, float rangeMax, float voxelSize, int normalsK, float normalsRadius, bool forceGroundNormalsUp, bool deskewing)
Definition: SensorCaptureThread.cpp:262
rtabmap::SensorCaptureThread::_colorOnly
bool _colorOnly
Definition: SensorCaptureThread.h:189
discrete_depth_distortion_model.h
rtabmap::SensorCaptureThread::SensorCaptureThread
SensorCaptureThread(Camera *camera, const ParametersMap &parameters=ParametersMap())
Definition: SensorCaptureThread.cpp:54
rtabmap::util2d::interpolate
cv::Mat RTABMAP_CORE_EXPORT interpolate(const cv::Mat &image, int factor, float depthErrorRatio=0.02f)
Definition: util2d.cpp:1250
UTimer::now
static double now()
Definition: UTimer.cpp:80
timer
rtabmap::SensorCaptureThread::_scanRangeMin
float _scanRangeMin
Definition: SensorCaptureThread.h:196
rtabmap::SensorCaptureThread::odomSensor
SensorCapture * odomSensor()
Definition: SensorCaptureThread.h:170
rtabmap::SensorCaptureThread::disableFeatureDetection
void disableFeatureDetection()
Definition: SensorCaptureThread.cpp:256
clams::DiscreteDepthDistortionModel::load
void load(const std::string &path)
Definition: discrete_depth_distortion_model.cpp:352
rtabmap::SensorCaptureThread::_bilateralSigmaS
float _bilateralSigmaS
Definition: SensorCaptureThread.h:205
rtabmap::Transform::getIdentity
static Transform getIdentity()
Definition: Transform.cpp:411
size
Index size
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::SensorCaptureThread::_scanForceGroundNormalsUp
float _scanForceGroundNormalsUp
Definition: SensorCaptureThread.h:201
rtabmap::util3d::voxelize
pcl::PointCloud< pcl::PointXYZ >::Ptr RTABMAP_CORE_EXPORT voxelize(const pcl::PointCloud< pcl::PointXYZ >::Ptr &cloud, const pcl::IndicesPtr &indices, float voxelSize)
Definition: util3d_filtering.cpp:613
UThread::join
void join(bool killFirst=false)
Definition: UThread.cpp:85
rtabmap::SensorCaptureThread::_poseScaleFactor
float _poseScaleFactor
Definition: SensorCaptureThread.h:185
clams::DiscreteDepthDistortionModel::getWidth
int getWidth() const
Definition: discrete_depth_distortion_model.h:106
rtabmap::IMU::angularVelocityCovariance
const cv::Mat & angularVelocityCovariance() const
Definition: IMU.h:57
CameraRGBD.h
rtabmap::SensorCaptureThread::mainLoop
virtual void mainLoop()
Definition: SensorCaptureThread.cpp:325
rtabmap::SensorCapture::resetTimer
void resetTimer()
Definition: SensorCapture.cpp:59
rtabmap::SensorCaptureThread::_poseTimeOffset
double _poseTimeOffset
Definition: SensorCaptureThread.h:184
DBReader.h
rtabmap::util3d::commonFiltering
LaserScan RTABMAP_CORE_EXPORT commonFiltering(const LaserScan &scan, int downsamplingStep, float rangeMin=0.0f, float rangeMax=0.0f, float voxelSize=0.0f, int normalK=0, float normalRadius=0.0f, float groundNormalsUp=0.0f)
Definition: util3d_filtering.cpp:74
rtabmap::SensorData::stamp
double stamp() const
Definition: SensorData.h:176
y
Matrix3f y
rtabmap::SensorCapture::setFrameRate
void setFrameRate(float frameRate)
Definition: SensorCapture.h:65
Lidar.h
rtabmap::SensorCaptureThread::mainLoopBegin
virtual void mainLoopBegin()
Definition: SensorCaptureThread.cpp:307
util3d.h
rtabmap::IMU::localTransform
const Transform & localTransform() const
Definition: IMU.h:62
rtabmap::LaserScan
Definition: LaserScan.h:37
rtabmap::ParametersMap
std::map< std::string, std::string > ParametersMap
Definition: Parameters.h:43
rtabmap::SensorCaptureThread::_featureDetector
Feature2D * _featureDetector
Definition: SensorCaptureThread.h:209
rtabmap::util3d::laserScanFromPointCloud
LaserScan laserScanFromPointCloud(const PointCloud2T &cloud, bool filterNaNs, bool is2D, const Transform &transform)
Definition: util3d.hpp:37
rtabmap::SensorCaptureThread::_stereoToDepth
bool _stereoToDepth
Definition: SensorCaptureThread.h:192
UTimer.h
rtabmap::util2d::decimate
cv::Mat RTABMAP_CORE_EXPORT decimate(const cv::Mat &image, int d)
Definition: util2d.cpp:1201
rtabmap::Transform::isNull
bool isNull() const
Definition: Transform.cpp:107
rtabmap::Feature2D::generateDescriptors
cv::Mat generateDescriptors(const cv::Mat &image, std::vector< cv::KeyPoint > &keypoints) const
Definition: Features2d.cpp:833
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:240
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:130
rtabmap::IMUFilter::create
static IMUFilter * create(const ParametersMap &parameters=ParametersMap())
Definition: IMUFilter.cpp:38
rtabmap::IMUFilter::update
void update(double gx, double gy, double gz, double ax, double ay, double az, double stamp)
Definition: IMUFilter.cpp:72
rtabmap::IMUFilter::getOrientation
virtual void getOrientation(double &qx, double &qy, double &qz, double &qw) const =0
rtabmap::IMUFilter::Type
Type
Definition: IMUFilter.h:40
rtabmap::Parameters::parse
static bool parse(const ParametersMap &parameters, const std::string &key, bool &value)
Definition: Parameters.cpp:500
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:241
rtabmap::SensorCaptureThread::setDistortionModel
void setDistortionModel(const std::string &path)
Definition: SensorCaptureThread.cpp:196
rtabmap::SensorCaptureThread::disableIMUFiltering
void disableIMUFiltering()
Definition: SensorCaptureThread.cpp:231
rtabmap::IMU::linearAccelerationCovariance
const cv::Mat & linearAccelerationCovariance() const
Definition: IMU.h:60
rtabmap::SensorCaptureThread::_imuBaseFrameConversion
bool _imuBaseFrameConversion
Definition: SensorCaptureThread.h:208
rtabmap::SensorCaptureThread::_camera
Camera * _camera
Definition: SensorCaptureThread.h:179
rtabmap::SensorCaptureInfo
Definition: SensorCaptureInfo.h:36
rtabmap::SensorCaptureThread::lidar
Lidar * lidar()
Definition: SensorCaptureThread.h:171
rtabmap::SensorCaptureThread::postUpdate
void postUpdate(SensorData *data, SensorCaptureInfo *info=0) const
Definition: SensorCaptureThread.cpp:573
rtabmap::SensorCaptureThread::_depthAsMask
bool _depthAsMask
Definition: SensorCaptureThread.h:210
uInsert
void uInsert(std::map< K, V > &map, const std::pair< K, V > &pair)
Definition: UStl.h:441
rtabmap::Transform::y
float & y()
Definition: Transform.h:93
rtabmap::SensorCaptureThread::_mirroring
bool _mirroring
Definition: SensorCaptureThread.h:187
rtabmap::SensorCaptureThread::enableIMUFiltering
void enableIMUFiltering(int filteringStrategy=1, const ParametersMap &parameters=ParametersMap(), bool baseFrameConversion=false)
Definition: SensorCaptureThread.cpp:224
rtabmap::IMU
Definition: IMU.h:19
rtabmap::Parameters::getDefaultParameters
static const ParametersMap & getDefaultParameters()
Definition: Parameters.h:896
rtabmap::Transform::z
float & z()
Definition: Transform.h:94
rtabmap::util3d::cloudRGBFromSensorData
pcl::PointCloud< pcl::PointXYZRGB >::Ptr RTABMAP_CORE_EXPORT cloudRGBFromSensorData(const SensorData &sensorData, int decimation=1, float maxDepth=0.0f, float minDepth=0.0f, std::vector< int > *validIndices=0, const ParametersMap &stereoParameters=ParametersMap(), const std::vector< float > &roiRatios=std::vector< float >())
Definition: util3d.cpp:1268
UThread::isKilled
bool isKilled() const
Definition: UThread.cpp:255
rtabmap::Transform::prettyPrint
std::string prettyPrint() const
Definition: Transform.cpp:326
data
int data[]
rtabmap::CameraFreenect2
Definition: CameraFreenect2.h:46
rtabmap::SensorCaptureThread::_imageDecimation
int _imageDecimation
Definition: SensorCaptureThread.h:190
Features2d.h
rtabmap::SensorCaptureThread::_stereoExposureCompensation
bool _stereoExposureCompensation
Definition: SensorCaptureThread.h:188
rtabmap::util2d::fastBilateralFiltering
cv::Mat RTABMAP_CORE_EXPORT fastBilateralFiltering(const cv::Mat &depth, float sigmaS=15.0f, float sigmaR=0.05f, bool earlyDivision=false)
Definition: util2d.cpp:1814
q
EIGEN_DEVICE_FUNC const Scalar & q
rtabmap::StereoDense
Definition: StereoDense.h:38
util3d_filtering.h
rtabmap::util3d::deskew
LaserScan RTABMAP_CORE_EXPORT deskew(const LaserScan &input, double inputStamp, const rtabmap::Transform &velocity)
Lidar deskewing.
Definition: util3d.cpp:3553
rtabmap::Transform::x
float & x()
Definition: Transform.h:92
clams::DiscreteDepthDistortionModel::undistort
void undistort(cv::Mat &depth) const
Definition: discrete_depth_distortion_model.cpp:276
velocity
Velocity3 velocity(const NavState &X, OptionalJacobian< 3, 9 > H)
rtabmap::SensorCaptureThread::odomProvided
bool odomProvided() const
Definition: SensorCaptureThread.cpp:298
UThread::kill
void kill()
Definition: UThread.cpp:48
rtabmap::SensorCaptureThread::_scanNormalsRadius
float _scanNormalsRadius
Definition: SensorCaptureThread.h:200
SensorEvent.h
rtabmap::SensorCaptureThread::enableFeatureDetection
void enableFeatureDetection(const ParametersMap &parameters=ParametersMap())
Definition: SensorCaptureThread.cpp:237
rtabmap::SensorCapture::takeData
SensorData takeData(SensorCaptureInfo *info=0)
Definition: SensorCapture.cpp:64
rtabmap::IMUFilter::reset
virtual void reset(double qx=0.0, double qy=0.0, double qz=0.0, double qw=1.0)=0
info
else if n * info
UASSERT
#define UASSERT(condition)
params
SmartProjectionParams params(gtsam::HESSIAN, gtsam::ZERO_ON_DEGENERACY)
z
z
uValue
V uValue(const std::map< K, V > &m, const K &key, const V &defaultValue=V())
Definition: UStl.h:238
x
x
rtabmap::SensorCaptureThread::_imuFilter
IMUFilter * _imuFilter
Definition: SensorCaptureThread.h:207
rtabmap::Parameters
Definition: Parameters.h:170
rtabmap::Transform::setIdentity
void setIdentity()
Definition: Transform.cpp:157
rtabmap::SensorCaptureThread::_bilateralFiltering
bool _bilateralFiltering
Definition: SensorCaptureThread.h:204
rtabmap::util2d::depthFromDisparity
cv::Mat RTABMAP_CORE_EXPORT depthFromDisparity(const cv::Mat &disparity, float fx, float baseline, int type=CV_32FC1)
Definition: util2d.cpp:763
rtabmap::Camera
Definition: Camera.h:43
rtabmap::SensorCaptureThread::_scanFromDepth
bool _scanFromDepth
Definition: SensorCaptureThread.h:194
glm::pitch
GLM_FUNC_DECL T pitch(detail::tquat< T, P > const &x)
rtabmap::SensorCapture
Definition: SensorCapture.h:49
rtabmap::Transform::inverse
Transform inverse() const
Definition: Transform.cpp:178
rtabmap::SensorCaptureThread::_odomAsGt
bool _odomAsGt
Definition: SensorCaptureThread.h:183
path
path
UWARN
#define UWARN(...)
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
glm::log
GLM_FUNC_DECL genType log(genType const &x)
ULogger::registerCurrentThread
static void registerCurrentThread(const std::string &name)
Definition: ULogger.cpp:218
qz
RealQZ< MatrixXf > qz(4)
rtabmap::Feature2D::generateKeypoints3D
std::vector< cv::Point3f > generateKeypoints3D(const SensorData &data, const std::vector< cv::KeyPoint > &keypoints) const
Definition: Features2d.cpp:849
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
Eigen::Quaternion
ULogger.h
ULogger class and convenient macros.
rtabmap::Transform
Definition: Transform.h:41
UTimer::ticks
double ticks()
Definition: UTimer.cpp:117
util2d.h
rtabmap::SensorCaptureThread::~SensorCaptureThread
virtual ~SensorCaptureThread()
Definition: SensorCaptureThread.cpp:169
rtabmap::StereoDense::computeDisparity
virtual cv::Mat computeDisparity(const cv::Mat &leftImage, const cv::Mat &rightImage) const =0
rtabmap::SensorCaptureThread::_scanVoxelSize
float _scanVoxelSize
Definition: SensorCaptureThread.h:198
rtabmap::SensorCaptureThread::_scanDeskewing
bool _scanDeskewing
Definition: SensorCaptureThread.h:193
rtabmap::SensorCapture::getPose
virtual bool getPose(double stamp, Transform &pose, cv::Mat &covariance, double maxWaitTime=0.06)
Definition: SensorCapture.h:58
rtabmap::SensorCaptureThread::_scanDownsampleStep
int _scanDownsampleStep
Definition: SensorCaptureThread.h:195
clams::DiscreteDepthDistortionModel::isValid
bool isValid() const
Definition: discrete_depth_distortion_model.h:109
rtabmap::SensorCaptureThread::_lidar
Lidar * _lidar
Definition: SensorCaptureThread.h:181
rtabmap::SensorCaptureThread::_scanNormalsK
int _scanNormalsK
Definition: SensorCaptureThread.h:199
IMUFilter.h
rtabmap::SensorCaptureThread::_stereoDense
StereoDense * _stereoDense
Definition: SensorCaptureThread.h:202
rtabmap::SensorCaptureThread::_extrinsicsOdomToCamera
Transform _extrinsicsOdomToCamera
Definition: SensorCaptureThread.h:182
camera
Camera camera(Pose3(Rot3().retract(Vector3(0.1, 0.2, 0.3)), Point3(0, 5, 0)), Cal3Bundler0(1, 0, 0))
c_str
const char * c_str(Args &&...args)
rtabmap::IMU::convertToBaseFrame
void convertToBaseFrame()
Definition: IMU.cpp:33
ratio
set noclip points set clip one set noclip two set bar set border lt lw set xdata set ydata set zdata set x2data set y2data set boxwidth set dummy y set format x g set format y g set format x2 g set format y2 g set format z g set angles radians set nogrid set key title set key left top Right noreverse box linetype linewidth samplen spacing width set nolabel set noarrow set nologscale set logscale x set set pointsize set encoding default set nopolar set noparametric set set set set surface set nocontour set clabel set mapping cartesian set nohidden3d set cntrparam order set cntrparam linear set cntrparam levels auto set cntrparam points set size ratio
rtabmap::SensorCaptureThread::mainLoopKill
virtual void mainLoopKill()
Definition: SensorCaptureThread.cpp:548
UStl.h
Wrappers of STL for convenient functions.
UDEBUG
#define UDEBUG(...)
rtabmap::SensorCaptureThread::_poseWaitTime
double _poseWaitTime
Definition: SensorCaptureThread.h:186
uSleep
void uSleep(unsigned int ms)
Definition: Posix/UThreadC.h:23
Camera.h
UTimer
Definition: UTimer.h:46
clams::DiscreteDepthDistortionModel::getHeight
int getHeight() const
Definition: discrete_depth_distortion_model.h:107
rtabmap::DBReader
Definition: DBReader.h:46
float
float
rtabmap::IMU::angularVelocity
const cv::Vec3d & angularVelocity() const
Definition: IMU.h:56
rtabmap::Lidar
Definition: Lidar.h:40
rtabmap::SensorCaptureThread::_distortionModel
clams::DiscreteDepthDistortionModel * _distortionModel
Definition: SensorCaptureThread.h:203
false
#define false
Definition: ConvertUTF.c:56
transform
EIGEN_DONT_INLINE void transform(const Quaternion< Scalar > &t, Data &data)
rtabmap::util3d::computeNormals
cv::Mat RTABMAP_CORE_EXPORT computeNormals(const cv::Mat &laserScan, int searchK, float searchRadius)
rtabmap::SensorData::depthOrRightRaw
const cv::Mat & depthOrRightRaw() const
Definition: SensorData.h:184
rtabmap::SensorCaptureThread::camera
Camera * camera()
Definition: SensorCaptureThread.h:169
rtabmap
Definition: CameraARCore.cpp:35
UERROR
#define UERROR(...)
trace.model
model
Definition: trace.py:4
i
int i
rtabmap::SensorEvent
Definition: SensorEvent.h:37
rtabmap::SensorCaptureThread::enableBilateralFiltering
void enableBilateralFiltering(float sigmaS, float sigmaR)
Definition: SensorCaptureThread.cpp:216
StereoDense.h
rtabmap::SensorCaptureThread::_bilateralSigmaR
float _bilateralSigmaR
Definition: SensorCaptureThread.h:206
glm::roll
GLM_FUNC_DECL T roll(detail::tquat< T, P > const &x)
rtabmap::SensorCaptureThread::setFrameRate
void setFrameRate(float frameRate)
Definition: SensorCaptureThread.cpp:184
UEventsSender::post
void post(UEvent *event, bool async=true) const
Definition: UEventsSender.cpp:28

rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Jul 1 2024 02:42:33