SensorData.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 
33 #include <rtabmap/utilite/UMath.h>
35 
36 namespace rtabmap
37 {
38 
39 // empty constructor
41  _id(0),
42  _stamp(0.0),
43  _cellSize(0.0f)
44 {
45 }
46 
47 // Appearance-only constructor
49  const cv::Mat & image,
50  int id,
51  double stamp,
52  const cv::Mat & userData) :
53  _id(id),
54  _stamp(stamp),
55  _cellSize(0.0f)
56 {
57  setRGBDImage(image, cv::Mat(), CameraModel());
58  setUserData(userData);
59 }
60 
61 // Mono constructor
63  const cv::Mat & image,
64  const CameraModel & cameraModel,
65  int id,
66  double stamp,
67  const cv::Mat & userData) :
68  _id(id),
69  _stamp(stamp),
70  _cellSize(0.0f)
71 {
72  setRGBDImage(image, cv::Mat(), cameraModel);
73  setUserData(userData);
74 }
75 
76 // RGB-D constructor
78  const cv::Mat & rgb,
79  const cv::Mat & depth,
80  const CameraModel & cameraModel,
81  int id,
82  double stamp,
83  const cv::Mat & userData) :
84  _id(id),
85  _stamp(stamp),
86  _cellSize(0.0f)
87 {
88  setRGBDImage(rgb, depth, cameraModel);
89  setUserData(userData);
90 }
91 
92 // RGB-D constructor + laser scan
94  const LaserScan & laserScan,
95  const cv::Mat & rgb,
96  const cv::Mat & depth,
97  const CameraModel & cameraModel,
98  int id,
99  double stamp,
100  const cv::Mat & userData) :
101  _id(id),
102  _stamp(stamp),
103  _cellSize(0.0f)
104 {
105  setRGBDImage(rgb, depth, cameraModel);
106  setLaserScan(laserScan);
107  setUserData(userData);
108 }
109 
110 // Multi-cameras RGB-D constructor
112  const cv::Mat & rgb,
113  const cv::Mat & depth,
114  const std::vector<CameraModel> & cameraModels,
115  int id,
116  double stamp,
117  const cv::Mat & userData) :
118  _id(id),
119  _stamp(stamp),
120  _cellSize(0.0f)
121 {
122  setRGBDImage(rgb, depth, cameraModels);
123  setUserData(userData);
124 }
125 
126 // Multi-cameras RGB-D constructor + laser scan
128  const LaserScan & laserScan,
129  const cv::Mat & rgb,
130  const cv::Mat & depth,
131  const std::vector<CameraModel> & cameraModels,
132  int id,
133  double stamp,
134  const cv::Mat & userData) :
135  _id(id),
136  _stamp(stamp),
137  _cellSize(0.0f)
138 {
139  setRGBDImage(rgb, depth, cameraModels);
140  setLaserScan(laserScan);
141  setUserData(userData);
142 }
143 
144 // Stereo constructor
146  const cv::Mat & left,
147  const cv::Mat & right,
148  const StereoCameraModel & cameraModel,
149  int id,
150  double stamp,
151  const cv::Mat & userData):
152  _id(id),
153  _stamp(stamp),
154  _cellSize(0.0f)
155 {
156  setStereoImage(left, right, cameraModel);
157  setUserData(userData);
158 }
159 
160 // Stereo constructor + 2d laser scan
162  const LaserScan & laserScan,
163  const cv::Mat & left,
164  const cv::Mat & right,
165  const StereoCameraModel & cameraModel,
166  int id,
167  double stamp,
168  const cv::Mat & userData) :
169  _id(id),
170  _stamp(stamp),
171  _cellSize(0.0f)
172 {
173  setStereoImage(left, right, cameraModel);
174  setLaserScan(laserScan);
175  setUserData(userData);
176 }
177 
179  const IMU & imu,
180  int id,
181  double stamp) :
182  _id(id),
183  _stamp(stamp),
184  _cellSize(0.0f)
185 {
186  imu_ = imu;
187 }
188 
190 {
191 }
192 
194  const cv::Mat & rgb,
195  const cv::Mat & depth,
196  const CameraModel & model,
197  bool clearPreviousData)
198 {
199  std::vector<CameraModel> models;
200  models.push_back(model);
201  setRGBDImage(rgb, depth, models, clearPreviousData);
202 }
204  const cv::Mat & rgb,
205  const cv::Mat & depth,
206  const std::vector<CameraModel> & models,
207  bool clearPreviousData)
208 {
209  if(!clearPreviousData && _stereoCameraModel.isValidForProjection())
210  {
211  UERROR("Sensor data has previously stereo images "
212  "but clearPreviousData parameter is false. We "
213  "will still clear previous data to avoid incompatibilities "
214  "between raw and compressed data!");
215  }
216  bool clearData = clearPreviousData || _stereoCameraModel.isValidForProjection();
217 
219  _cameraModels = models;
220  if(rgb.rows == 1)
221  {
222  UASSERT(rgb.type() == CV_8UC1); // Bytes
223  _imageCompressed = rgb;
224  if(clearData)
225  {
226  _imageRaw = cv::Mat();
227  }
228  }
229  else if(!rgb.empty())
230  {
231  UASSERT(rgb.type() == CV_8UC1 || // Mono
232  rgb.type() == CV_8UC3); // RGB
233  _imageRaw = rgb;
234  if(clearData)
235  {
236  _imageCompressed = cv::Mat();
237  }
238  }
239  else if(clearData)
240  {
241  _imageRaw = cv::Mat();
242  _imageCompressed = cv::Mat();
243  }
244 
245  if(depth.rows == 1)
246  {
247  UASSERT(depth.type() == CV_8UC1); // Bytes
248  _depthOrRightCompressed = depth;
249  if(clearData)
250  {
251  _depthOrRightRaw = cv::Mat();
252  }
253  }
254  else if(!depth.empty())
255  {
256  UASSERT(depth.type() == CV_32FC1 || // Depth in meter
257  depth.type() == CV_16UC1); // Depth in millimetre
258  _depthOrRightRaw = depth;
259  if(clearData)
260  {
261  _depthOrRightCompressed = cv::Mat();
262  }
263  }
264  else if(clearData)
265  {
266  _depthOrRightRaw = cv::Mat();
267  _depthOrRightCompressed = cv::Mat();
268  }
269 }
271  const cv::Mat & left,
272  const cv::Mat & right,
274  bool clearPreviousData)
275 {
276  if(!clearPreviousData && !_cameraModels.empty())
277  {
278  UERROR("Sensor data has previously RGB-D/RGB images "
279  "but clearPreviousData parameter is false. We "
280  "will still clear previous data to avoid incompatibilities "
281  "between raw and compressed data!");
282  }
283  bool clearData = clearPreviousData || !_cameraModels.empty();
284 
285  _cameraModels.clear();
287 
288  if(left.rows == 1)
289  {
290  UASSERT(left.type() == CV_8UC1); // Bytes
291  _imageCompressed = left;
292  if(clearData)
293  {
294  _imageRaw = cv::Mat();
295  }
296  }
297  else if(!left.empty())
298  {
299  UASSERT(left.type() == CV_8UC1 || // Mono
300  left.type() == CV_8UC3); // RGB
301  _imageRaw = left;
302  if(clearData)
303  {
304  _imageCompressed = cv::Mat();
305  }
306  }
307  else if(clearData)
308  {
309  _imageRaw = cv::Mat();
310  _imageCompressed = cv::Mat();
311  }
312 
313  if(right.rows == 1)
314  {
315  UASSERT(right.type() == CV_8UC1); // Bytes
316  _depthOrRightCompressed = right;
317  if(clearData)
318  {
319  _depthOrRightRaw = cv::Mat();
320  }
321  }
322  else if(!right.empty())
323  {
324  UASSERT(right.type() == CV_8UC1); // Mono
325  _depthOrRightRaw = right;
326  if(clearData)
327  {
328  _depthOrRightCompressed = cv::Mat();
329  }
330  }
331  else if(clearData)
332  {
333  _depthOrRightRaw = cv::Mat();
334  _depthOrRightCompressed = cv::Mat();
335  }
336 }
337 void SensorData::setLaserScan(const LaserScan & laserScan, bool clearPreviousData)
338 {
339  if(!laserScan.isCompressed())
340  {
341  _laserScanRaw = laserScan;
342  if(clearPreviousData)
343  {
345  }
346  }
347  else
348  {
349  _laserScanCompressed = laserScan;
350  if(clearPreviousData)
351  {
353  }
354  }
355 }
356 
357 void SensorData::setImageRaw(const cv::Mat & image)
358 {
359  UASSERT(image.empty() || image.rows > 1);
360  _imageRaw = image;
361 }
362 void SensorData::setDepthOrRightRaw(const cv::Mat & image)
363 {
364  UASSERT(image.empty() || image.rows > 1);
365  _depthOrRightRaw = image;
366 }
367 void SensorData::setLaserScanRaw(const LaserScan & scan)
368 {
369  UASSERT(scan.isEmpty() || !scan.isCompressed());
370  _laserScanRaw = scan;
371 }
372 
373 void SensorData::setUserDataRaw(const cv::Mat & userDataRaw)
374 {
376 }
377 
378 void SensorData::setUserData(const cv::Mat & userData, bool clearPreviousData)
379 {
380  if(clearPreviousData)
381  {
382  _userDataRaw = cv::Mat();
383  _userDataCompressed = cv::Mat();
384  }
385 
386  if(userData.type() == CV_8UC1 && userData.rows == 1 && userData.cols > int(3*sizeof(int))) // Bytes
387  {
388  _userDataCompressed = userData; // assume compressed
389  }
390  else
391  {
392  _userDataRaw = userData;
393  if(!userData.empty())
394  {
396  }
397  }
398 }
399 
401  const cv::Mat & ground,
402  const cv::Mat & obstacles,
403  const cv::Mat & empty,
404  float cellSize,
405  const cv::Point3f & viewPoint)
406 {
407  UDEBUG("ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols, empty.cols);
408  if((!ground.empty() && (!_groundCellsCompressed.empty() || !_groundCellsRaw.empty())) ||
409  (!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())) ||
410  (!empty.empty() && (!_emptyCellsCompressed.empty() || !_emptyCellsRaw.empty())))
411  {
412  UWARN("Occupancy grid cannot be overwritten! id=%d, Set occupancy grid of %d to null "
413  "before setting a new one.", this->id());
414  return;
415  }
416 
417  _groundCellsRaw = cv::Mat();
418  _groundCellsCompressed = cv::Mat();
419  _obstacleCellsRaw = cv::Mat();
420  _obstacleCellsCompressed = cv::Mat();
421  _emptyCellsRaw = cv::Mat();
422  _emptyCellsCompressed = cv::Mat();
423 
424  CompressionThread ctGround(ground);
425  CompressionThread ctObstacles(obstacles);
426  CompressionThread ctEmpty(empty);
427 
428  if(!ground.empty())
429  {
430  if(ground.type() == CV_32FC2 || ground.type() == CV_32FC3 || ground.type() == CV_32FC(4) || ground.type() == CV_32FC(5) || ground.type() == CV_32FC(6) || ground.type() == CV_32FC(7))
431  {
432  _groundCellsRaw = ground;
433  ctGround.start();
434  }
435  else if(ground.type() == CV_8UC1)
436  {
437  _groundCellsCompressed = ground;
438  }
439  }
440  if(!obstacles.empty())
441  {
442  if(obstacles.type() == CV_32FC2 || obstacles.type() == CV_32FC3 || obstacles.type() == CV_32FC(4) || obstacles.type() == CV_32FC(5) || obstacles.type() == CV_32FC(6) || obstacles.type() == CV_32FC(7))
443  {
444  _obstacleCellsRaw = obstacles;
445  ctObstacles.start();
446  }
447  else if(obstacles.type() == CV_8UC1)
448  {
449  _obstacleCellsCompressed = obstacles;
450  }
451  }
452  if(!empty.empty())
453  {
454  if(empty.type() == CV_32FC2 || empty.type() == CV_32FC3 || empty.type() == CV_32FC(4) || empty.type() == CV_32FC(5) || empty.type() == CV_32FC(6) || empty.type() == CV_32FC(7))
455  {
456  _emptyCellsRaw = empty;
457  ctEmpty.start();
458  }
459  else if(empty.type() == CV_8UC1)
460  {
461  _emptyCellsCompressed = empty;
462  }
463  }
464  ctGround.join();
465  ctObstacles.join();
466  ctEmpty.join();
467  if(!_groundCellsRaw.empty())
468  {
470  }
471  if(!_obstacleCellsRaw.empty())
472  {
474  }
475  if(!_emptyCellsRaw.empty())
476  {
478  }
479 
480  _cellSize = cellSize;
481  _viewPoint = viewPoint;
482 }
483 
485 {
486  cv::Mat tmpA, tmpB, tmpD, tmpE, tmpF, tmpG;
487  LaserScan tmpC;
488  uncompressData(_imageCompressed.empty()?0:&tmpA,
489  _depthOrRightCompressed.empty()?0:&tmpB,
490  _laserScanCompressed.isEmpty()?0:&tmpC,
491  _userDataCompressed.empty()?0:&tmpD,
492  _groundCellsCompressed.empty()?0:&tmpE,
493  _obstacleCellsCompressed.empty()?0:&tmpF,
494  _emptyCellsCompressed.empty()?0:&tmpG);
495 }
496 
498  cv::Mat * imageRaw,
499  cv::Mat * depthRaw,
501  cv::Mat * userDataRaw,
502  cv::Mat * groundCellsRaw,
503  cv::Mat * obstacleCellsRaw,
504  cv::Mat * emptyCellsRaw)
505 {
506  UDEBUG("%d data(%d,%d,%d,%d,%d,%d,%d)", this->id(), imageRaw?1:0, depthRaw?1:0, laserScanRaw?1:0, userDataRaw?1:0, groundCellsRaw?1:0, obstacleCellsRaw?1:0, emptyCellsRaw?1:0);
507  if(imageRaw == 0 &&
508  depthRaw == 0 &&
509  laserScanRaw == 0 &&
510  userDataRaw == 0 &&
511  groundCellsRaw == 0 &&
512  obstacleCellsRaw == 0 &&
513  emptyCellsRaw == 0)
514  {
515  return;
516  }
518  imageRaw,
519  depthRaw,
520  laserScanRaw,
521  userDataRaw,
522  groundCellsRaw,
523  obstacleCellsRaw,
524  emptyCellsRaw);
525 
526  if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
527  {
528  _imageRaw = *imageRaw;
529  //backward compatibility, set image size in camera model if not set
530  if(!_imageRaw.empty() && _cameraModels.size())
531  {
532  cv::Size size(_imageRaw.cols/_cameraModels.size(), _imageRaw.rows);
533  for(unsigned int i=0; i<_cameraModels.size(); ++i)
534  {
535  if(_cameraModels[i].fx() && _cameraModels[i].fy() && _cameraModels[i].imageWidth() == 0)
536  {
537  _cameraModels[i].setImageSize(size);
538  }
539  }
540  }
541  }
542  if(depthRaw && !depthRaw->empty() && _depthOrRightRaw.empty())
543  {
545  }
546  if(laserScanRaw && !laserScanRaw->isEmpty() && _laserScanRaw.isEmpty())
547  {
550  {
552  {
554  }
555  else
556  {
558  }
559  }
560  }
561  if(userDataRaw && !userDataRaw->empty() && _userDataRaw.empty())
562  {
564  }
565  if(groundCellsRaw && !groundCellsRaw->empty() && _groundCellsRaw.empty())
566  {
567  _groundCellsRaw = *groundCellsRaw;
568  }
569  if(obstacleCellsRaw && !obstacleCellsRaw->empty() && _obstacleCellsRaw.empty())
570  {
571  _obstacleCellsRaw = *obstacleCellsRaw;
572  }
573  if(emptyCellsRaw && !emptyCellsRaw->empty() && _emptyCellsRaw.empty())
574  {
575  _emptyCellsRaw = *emptyCellsRaw;
576  }
577 }
578 
580  cv::Mat * imageRaw,
581  cv::Mat * depthRaw,
583  cv::Mat * userDataRaw,
584  cv::Mat * groundCellsRaw,
585  cv::Mat * obstacleCellsRaw,
586  cv::Mat * emptyCellsRaw) const
587 {
588  if(imageRaw)
589  {
590  *imageRaw = _imageRaw;
591  }
592  if(depthRaw)
593  {
594  *depthRaw = _depthOrRightRaw;
595  }
596  if(laserScanRaw)
597  {
598  *laserScanRaw = _laserScanRaw;
599  }
600  if(userDataRaw)
601  {
602  *userDataRaw = _userDataRaw;
603  }
604  if(groundCellsRaw)
605  {
606  *groundCellsRaw = _groundCellsRaw;
607  }
608  if(obstacleCellsRaw)
609  {
610  *obstacleCellsRaw = _obstacleCellsRaw;
611  }
612  if(emptyCellsRaw)
613  {
614  *emptyCellsRaw = _emptyCellsRaw;
615  }
616  if( (imageRaw && imageRaw->empty()) ||
617  (depthRaw && depthRaw->empty()) ||
618  (laserScanRaw && laserScanRaw->isEmpty()) ||
619  (userDataRaw && userDataRaw->empty()) ||
620  (groundCellsRaw && groundCellsRaw->empty()) ||
621  (obstacleCellsRaw && obstacleCellsRaw->empty()) ||
622  (emptyCellsRaw && emptyCellsRaw->empty()))
623  {
631  if(imageRaw && imageRaw->empty() && !_imageCompressed.empty())
632  {
633  UASSERT(_imageCompressed.type() == CV_8UC1);
634  ctImage.start();
635  }
636  if(depthRaw && depthRaw->empty() && !_depthOrRightCompressed.empty())
637  {
638  UASSERT(_depthOrRightCompressed.type() == CV_8UC1);
639  ctDepth.start();
640  }
641  if(laserScanRaw && laserScanRaw->isEmpty() && !_laserScanCompressed.isEmpty())
642  {
644  ctLaserScan.start();
645  }
646  if(userDataRaw && userDataRaw->empty() && !_userDataCompressed.empty())
647  {
648  UASSERT(_userDataCompressed.type() == CV_8UC1);
649  ctUserData.start();
650  }
651  if(groundCellsRaw && groundCellsRaw->empty() && !_groundCellsCompressed.empty())
652  {
653  UASSERT(_groundCellsCompressed.type() == CV_8UC1);
654  ctGroundCells.start();
655  }
656  if(obstacleCellsRaw && obstacleCellsRaw->empty() && !_obstacleCellsCompressed.empty())
657  {
658  UASSERT(_obstacleCellsCompressed.type() == CV_8UC1);
659  ctObstacleCells.start();
660  }
661  if(emptyCellsRaw && emptyCellsRaw->empty() && !_emptyCellsCompressed.empty())
662  {
663  UASSERT(_emptyCellsCompressed.type() == CV_8UC1);
664  ctEmptyCells.start();
665  }
666  ctImage.join();
667  ctDepth.join();
668  ctLaserScan.join();
669  ctUserData.join();
670  ctGroundCells.join();
671  ctObstacleCells.join();
672  ctEmptyCells.join();
673 
674  if(imageRaw && imageRaw->empty())
675  {
676  *imageRaw = ctImage.getUncompressedData();
677  if(imageRaw->empty())
678  {
679  if(_imageCompressed.empty())
680  {
681  UWARN("Requested raw image data, but the sensor data (%d) doesn't have image.", this->id());
682  }
683  else
684  {
685  UERROR("Requested image data, but failed to uncompress (%d).", this->id());
686  }
687  }
688  }
689  if(depthRaw && depthRaw->empty())
690  {
691  *depthRaw = ctDepth.getUncompressedData();
692  if(depthRaw->empty())
693  {
694  if(_depthOrRightCompressed.empty())
695  {
696  UWARN("Requested depth/right image data, but the sensor data (%d) doesn't have depth/right image.", this->id());
697  }
698  else
699  {
700  UERROR("Requested depth/right image data, but failed to uncompress (%d).", this->id());
701  }
702  }
703  }
704  if(laserScanRaw && laserScanRaw->isEmpty())
705  {
707  {
709  }
710  else
711  {
713  }
714  if(laserScanRaw->isEmpty())
715  {
717  {
718  UWARN("Requested laser scan data, but the sensor data (%d) doesn't have laser scan.", this->id());
719  }
720  else
721  {
722  UERROR("Requested laser scan data, but failed to uncompress (%d).", this->id());
723  }
724  }
725  }
726  if(userDataRaw && userDataRaw->empty())
727  {
728  *userDataRaw = ctUserData.getUncompressedData();
729 
730  if(userDataRaw->empty())
731  {
732  if(_userDataCompressed.empty())
733  {
734  UWARN("Requested user data, but the sensor data (%d) doesn't have user data.", this->id());
735  }
736  else
737  {
738  UERROR("Requested user data, but failed to uncompress (%d).", this->id());
739  }
740  }
741  }
742  if(groundCellsRaw && groundCellsRaw->empty())
743  {
744  *groundCellsRaw = ctGroundCells.getUncompressedData();
745  }
746  if(obstacleCellsRaw && obstacleCellsRaw->empty())
747  {
748  *obstacleCellsRaw = ctObstacleCells.getUncompressedData();
749  }
750  if(emptyCellsRaw && emptyCellsRaw->empty())
751  {
752  *emptyCellsRaw = ctEmptyCells.getUncompressedData();
753  }
754  }
755 }
756 
757 void SensorData::setFeatures(const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> & keypoints3D, const cv::Mat & descriptors)
758 {
759  UASSERT_MSG(keypoints3D.empty() || keypoints.size() == keypoints3D.size(), uFormat("keypoints=%d keypoints3D=%d", (int)keypoints.size(), (int)keypoints3D.size()).c_str());
760  UASSERT_MSG(descriptors.empty() || (int)keypoints.size() == descriptors.rows, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors.rows).c_str());
764 }
765 
766 unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
767 {
768  return sizeof(SensorData) +
769  _imageCompressed.total()*_imageCompressed.elemSize() +
770  _imageRaw.total()*_imageRaw.elemSize() +
772  _depthOrRightRaw.total()*_depthOrRightRaw.elemSize() +
773  _userDataCompressed.total()*_userDataCompressed.elemSize() +
774  _userDataRaw.total()*_userDataRaw.elemSize() +
775  _laserScanCompressed.data().total()*_laserScanCompressed.data().elemSize() +
776  _laserScanRaw.data().total()*_laserScanRaw.data().elemSize() +
778  _groundCellsRaw.total()*_groundCellsRaw.elemSize() +
780  _obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize()+
781  _emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize() +
782  _emptyCellsRaw.total()*_emptyCellsRaw.elemSize()+
783  _keypoints.size() * sizeof(cv::KeyPoint) +
784  _keypoints3D.size() * sizeof(cv::Point3f) +
785  _descriptors.total()*_descriptors.elemSize();
786 }
787 
788 void SensorData::clearCompressedData(bool images, bool scan, bool userData)
789 {
790  if(images)
791  {
792  _imageCompressed=cv::Mat();
793  _depthOrRightCompressed=cv::Mat();
794  }
795  if(scan)
796  {
798  }
799  if(userData)
800  {
801  _userDataCompressed=cv::Mat();
802  }
803 }
804 void SensorData::clearRawData(bool images, bool scan, bool userData)
805 {
806  if(images)
807  {
808  _imageRaw=cv::Mat();
809  _depthOrRightRaw=cv::Mat();
810  }
811  if(scan)
812  {
814  }
815  if(userData)
816  {
817  _userDataRaw=cv::Mat();
818  }
819 }
820 
821 
822 bool SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
823 {
824  if(_cameraModels.size() >= 1)
825  {
826  for(unsigned int i=0; i<_cameraModels.size(); ++i)
827  {
828  if(_cameraModels[i].isValidForProjection() && !_cameraModels[i].localTransform().isNull())
829  {
830  cv::Point3f ptInCameraFrame = util3d::transformPoint(pt, _cameraModels[i].localTransform().inverse());
831  if(ptInCameraFrame.z > 0.0f)
832  {
833  int borderWidth = int(float(_cameraModels[i].imageWidth())* 0.2);
834  int u, v;
835  _cameraModels[i].reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
836  if(uIsInBounds(u, borderWidth, _cameraModels[i].imageWidth()-2*borderWidth) &&
837  uIsInBounds(v, borderWidth, _cameraModels[i].imageHeight()-2*borderWidth))
838  {
839  return true;
840  }
841  }
842  }
843  }
844  }
846  {
847  cv::Point3f ptInCameraFrame = util3d::transformPoint(pt, _stereoCameraModel.localTransform().inverse());
848  if(ptInCameraFrame.z > 0.0f)
849  {
850  int u, v;
851  _stereoCameraModel.left().reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
852  return uIsInBounds(u, 0, _stereoCameraModel.left().imageWidth()) &&
854  }
855  }
856  else
857  {
858  UERROR("no valid camera model!");
859  }
860  return false;
861 }
862 
863 } // namespace rtabmap
864 
int imageWidth() const
Definition: CameraModel.h:120
float rangeMin() const
Definition: LaserScan.h:93
void clearCompressedData(bool images=true, bool scan=true, bool userData=true)
Definition: SensorData.cpp:788
void uncompressDataConst(cv::Mat *imageRaw, cv::Mat *depthOrRightRaw, LaserScan *laserScanRaw=0, cv::Mat *userDataRaw=0, cv::Mat *groundCellsRaw=0, cv::Mat *obstacleCellsRaw=0, cv::Mat *emptyCellsRaw=0) const
Definition: SensorData.cpp:579
void clearRawData(bool images=true, bool scan=true, bool userData=true)
Definition: SensorData.cpp:804
void start()
Definition: UThread.cpp:122
void setOccupancyGrid(const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty, float cellSize, const cv::Point3f &viewPoint)
Definition: SensorData.cpp:400
cv::Mat _userDataCompressed
Definition: SensorData.h:308
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
const cv::Mat & data() const
Definition: LaserScan.h:88
std::vector< cv::Point3f > _keypoints3D
Definition: SensorData.h:329
Format format() const
Definition: LaserScan.h:89
void setLaserScan(const LaserScan &laserScan, bool clearPreviousData=true)
Definition: SensorData.cpp:337
LaserScan _laserScanCompressed
Definition: SensorData.h:298
cv::Mat _emptyCellsRaw
Definition: SensorData.h:317
f
bool uIsInBounds(const T &value, const T &low, const T &high)
Definition: UMath.h:934
cv::Mat _depthOrRightRaw
Definition: SensorData.h:301
cv::Mat _obstacleCellsRaw
Definition: SensorData.h:316
float rangeMax() const
Definition: LaserScan.h:94
bool isEmpty() const
Definition: LaserScan.h:101
const cv::Mat & descriptors() const
Definition: SensorData.h:251
void setRGBDImage(const cv::Mat &rgb, const cv::Mat &depth, const CameraModel &model, bool clearPreviousData=true)
Definition: SensorData.cpp:193
cv::Mat _groundCellsCompressed
Definition: SensorData.h:312
int maxPoints() const
Definition: LaserScan.h:92
const std::vector< cv::KeyPoint > & keypoints() const
Definition: SensorData.h:249
Basic mathematics functions.
Some conversion functions.
unsigned long getMemoryUsed() const
Definition: SensorData.cpp:766
const LaserScan & laserScanRaw() const
Definition: SensorData.h:166
cv::Mat _emptyCellsCompressed
Definition: SensorData.h:314
const cv::Mat & imageRaw() const
Definition: SensorData.h:164
std::vector< cv::KeyPoint > _keypoints
Definition: SensorData.h:328
#define UASSERT(condition)
const CameraModel & left() const
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
cv::Mat _obstacleCellsCompressed
Definition: SensorData.h:313
float angleMin() const
Definition: LaserScan.h:95
void reproject(float x, float y, float z, float &u, float &v) const
bool isCompressed() const
Definition: LaserScan.h:108
float angleMax() const
Definition: LaserScan.h:96
Transform localTransform() const
Definition: LaserScan.h:98
LaserScan _laserScanRaw
Definition: SensorData.h:302
float angleIncrement() const
Definition: LaserScan.h:97
void setStereoImage(const cv::Mat &left, const cv::Mat &right, const StereoCameraModel &stereoCameraModel, bool clearPreviousData=true)
Definition: SensorData.cpp:270
cv::Mat RTABMAP_EXP compressData2(const cv::Mat &data)
const cv::Mat & userDataRaw() const
Definition: SensorData.h:227
double stamp() const
Definition: SensorData.h:157
void setUserData(const cv::Mat &userData, bool clearPreviousData=true)
Definition: SensorData.cpp:378
bool isPointVisibleFromCameras(const cv::Point3f &pt) const
Definition: SensorData.cpp:822
const IMU & imu() const
Definition: SensorData.h:269
cv::Mat _depthOrRightCompressed
Definition: SensorData.h:297
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:215
cv::Mat _imageCompressed
Definition: SensorData.h:296
cv::Point3f _viewPoint
Definition: SensorData.h:319
#define UDEBUG(...)
const cv::Mat & getCompressedData() const
Definition: Compression.h:61
#define UERROR(...)
StereoCameraModel _stereoCameraModel
Definition: SensorData.h:305
ULogger class and convenient macros.
const StereoCameraModel & stereoCameraModel() const
Definition: SensorData.h:216
#define UWARN(...)
std::vector< CameraModel > _cameraModels
Definition: SensorData.h:304
Transform inverse() const
Definition: Transform.cpp:178
void join(bool killFirst=false)
Definition: UThread.cpp:85
const Transform & localTransform() const
std::string UTILITE_EXP uFormat(const char *fmt,...)
int imageHeight() const
Definition: CameraModel.h:121
cv::Mat & getUncompressedData()
Definition: Compression.h:62
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:757
const std::vector< cv::Point3f > & keypoints3D() const
Definition: SensorData.h:250
cv::Mat depthRaw() const
Definition: SensorData.h:189
cv::Mat _groundCellsRaw
Definition: SensorData.h:315
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)


rtabmap
Author(s): Mathieu Labbe
autogenerated on Mon Dec 14 2020 03:35:00