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 
178 // Multi-Stereo constructor
180  const cv::Mat & left,
181  const cv::Mat & right,
182  const std::vector<StereoCameraModel> & cameraModels,
183  int id,
184  double stamp,
185  const cv::Mat & userData):
186  _id(id),
187  _stamp(stamp),
188  _cellSize(0.0f)
189 {
190  setStereoImage(left, right, cameraModels);
191  setUserData(userData);
192 }
193 
194 // Multi-Stereo constructor + 2d laser scan
196  const LaserScan & laserScan,
197  const cv::Mat & left,
198  const cv::Mat & right,
199  const std::vector<StereoCameraModel> & cameraModels,
200  int id,
201  double stamp,
202  const cv::Mat & userData) :
203  _id(id),
204  _stamp(stamp),
205  _cellSize(0.0f)
206 {
207  setStereoImage(left, right, cameraModels);
208  setLaserScan(laserScan);
209  setUserData(userData);
210 }
211 
213  const IMU & imu,
214  int id,
215  double stamp) :
216  _id(id),
217  _stamp(stamp),
218  _cellSize(0.0f)
219 {
220  imu_ = imu;
221 }
222 
224 {
225 }
226 
228  const cv::Mat & rgb,
229  const cv::Mat & depth,
230  const CameraModel & model,
231  bool clearPreviousData)
232 {
233  std::vector<CameraModel> models;
234  models.push_back(model);
235  setRGBDImage(rgb, depth, models, clearPreviousData);
236 }
238  const cv::Mat & rgb,
239  const cv::Mat & depth,
240  const std::vector<CameraModel> & models,
241  bool clearPreviousData)
242 {
243  if(!clearPreviousData && !_stereoCameraModels.empty())
244  {
245  UERROR("Sensor data has previously stereo images "
246  "but clearPreviousData parameter is false. We "
247  "will still clear previous data to avoid incompatibilities "
248  "between raw and compressed data!");
249  }
250  bool clearData = clearPreviousData || !_stereoCameraModels.empty();
251 
252  _stereoCameraModels.clear();
253  _cameraModels = models;
254  if(rgb.rows == 1)
255  {
256  UASSERT(rgb.type() == CV_8UC1); // Bytes
257  _imageCompressed = rgb;
258  if(clearData)
259  {
260  _imageRaw = cv::Mat();
261  }
262  }
263  else if(!rgb.empty())
264  {
265  UASSERT(rgb.type() == CV_8UC1 || // Mono
266  rgb.type() == CV_8UC3); // RGB
267  _imageRaw = rgb;
268  if(clearData)
269  {
270  _imageCompressed = cv::Mat();
271  }
272  }
273  else if(clearData)
274  {
275  _imageRaw = cv::Mat();
276  _imageCompressed = cv::Mat();
277  }
278 
279  if(depth.rows == 1)
280  {
281  UASSERT(depth.type() == CV_8UC1); // Bytes
282  _depthOrRightCompressed = depth;
283  if(clearData)
284  {
285  _depthOrRightRaw = cv::Mat();
286  }
287  }
288  else if(!depth.empty())
289  {
290  UASSERT(depth.type() == CV_32FC1 || // Depth in meter
291  depth.type() == CV_16UC1); // Depth in millimetre
292  _depthOrRightRaw = depth;
293  if(clearData)
294  {
295  _depthOrRightCompressed = cv::Mat();
296  }
297  }
298  else if(clearData)
299  {
300  _depthOrRightRaw = cv::Mat();
301  _depthOrRightCompressed = cv::Mat();
302  }
303 }
305  const cv::Mat & left,
306  const cv::Mat & right,
307  const StereoCameraModel & stereoCameraModel,
308  bool clearPreviousData)
309 {
310  std::vector<StereoCameraModel> models;
311  models.push_back(stereoCameraModel);
312  setStereoImage(left, right, models, clearPreviousData);
313 }
315  const cv::Mat & left,
316  const cv::Mat & right,
317  const std::vector<StereoCameraModel> & stereoCameraModels,
318  bool clearPreviousData)
319 {
320  if(!clearPreviousData && !_cameraModels.empty())
321  {
322  UERROR("Sensor data has previously RGB-D/RGB images "
323  "but clearPreviousData parameter is false. We "
324  "will still clear previous data to avoid incompatibilities "
325  "between raw and compressed data!");
326  }
327  bool clearData = clearPreviousData || !_cameraModels.empty();
328 
329  _cameraModels.clear();
331 
332  if(left.rows == 1)
333  {
334  UASSERT(left.type() == CV_8UC1); // Bytes
335  _imageCompressed = left;
336  if(clearData)
337  {
338  _imageRaw = cv::Mat();
339  }
340  }
341  else if(!left.empty())
342  {
343  UASSERT(left.type() == CV_8UC1 || // Mono
344  left.type() == CV_8UC3); // RGB
345  _imageRaw = left;
346  if(clearData)
347  {
348  _imageCompressed = cv::Mat();
349  }
350  }
351  else if(clearData)
352  {
353  _imageRaw = cv::Mat();
354  _imageCompressed = cv::Mat();
355  }
356 
357  if(right.rows == 1)
358  {
359  UASSERT(right.type() == CV_8UC1); // Bytes
360  _depthOrRightCompressed = right;
361  if(clearData)
362  {
363  _depthOrRightRaw = cv::Mat();
364  }
365  }
366  else if(!right.empty())
367  {
368  UASSERT(right.type() == CV_8UC1); // Mono
369  _depthOrRightRaw = right;
370  if(clearData)
371  {
372  _depthOrRightCompressed = cv::Mat();
373  }
374  }
375  else if(clearData)
376  {
377  _depthOrRightRaw = cv::Mat();
378  _depthOrRightCompressed = cv::Mat();
379  }
380 }
381 void SensorData::setLaserScan(const LaserScan & laserScan, bool clearPreviousData)
382 {
383  if(!laserScan.isCompressed())
384  {
385  _laserScanRaw = laserScan;
386  if(clearPreviousData)
387  {
389  }
390  }
391  else
392  {
393  _laserScanCompressed = laserScan;
394  if(clearPreviousData)
395  {
397  }
398  }
399 }
400 
401 void SensorData::setImageRaw(const cv::Mat & image)
402 {
403  UASSERT(image.empty() || image.rows > 1);
404  _imageRaw = image;
405 }
406 void SensorData::setDepthOrRightRaw(const cv::Mat & image)
407 {
408  UASSERT(image.empty() || image.rows > 1);
409  _depthOrRightRaw = image;
410 }
412 {
413  UASSERT(scan.isEmpty() || !scan.isCompressed());
414  _laserScanRaw = scan;
415 }
416 
417 void SensorData::setUserDataRaw(const cv::Mat & userDataRaw)
418 {
420 }
421 
422 void SensorData::setUserData(const cv::Mat & userData, bool clearPreviousData)
423 {
424  if(clearPreviousData)
425  {
426  _userDataRaw = cv::Mat();
427  _userDataCompressed = cv::Mat();
428  }
429 
430  if(userData.type() == CV_8UC1 && userData.rows == 1 && userData.cols > int(3*sizeof(int))) // Bytes
431  {
432  _userDataCompressed = userData; // assume compressed
433  }
434  else
435  {
436  _userDataRaw = userData;
437  if(!userData.empty())
438  {
440  }
441  }
442 }
443 
445  const cv::Mat & ground,
446  const cv::Mat & obstacles,
447  const cv::Mat & empty,
448  float cellSize,
449  const cv::Point3f & viewPoint)
450 {
451  UDEBUG("ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols, empty.cols);
452  if((!ground.empty() && (!_groundCellsCompressed.empty() || !_groundCellsRaw.empty())) ||
453  (!obstacles.empty() && (!_obstacleCellsCompressed.empty() || !_obstacleCellsRaw.empty())) ||
454  (!empty.empty() && (!_emptyCellsCompressed.empty() || !_emptyCellsRaw.empty())))
455  {
456  UWARN("Occupancy grid cannot be overwritten! id=%d, Set occupancy grid of %d to null "
457  "before setting a new one.", this->id());
458  return;
459  }
460 
461  _groundCellsRaw = cv::Mat();
462  _groundCellsCompressed = cv::Mat();
463  _obstacleCellsRaw = cv::Mat();
464  _obstacleCellsCompressed = cv::Mat();
465  _emptyCellsRaw = cv::Mat();
466  _emptyCellsCompressed = cv::Mat();
467 
468  CompressionThread ctGround(ground);
469  CompressionThread ctObstacles(obstacles);
470  CompressionThread ctEmpty(empty);
471 
472  if(!ground.empty())
473  {
474  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))
475  {
476  _groundCellsRaw = ground;
477  ctGround.start();
478  }
479  else if(ground.type() == CV_8UC1)
480  {
481  _groundCellsCompressed = ground;
482  }
483  }
484  if(!obstacles.empty())
485  {
486  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))
487  {
488  _obstacleCellsRaw = obstacles;
489  ctObstacles.start();
490  }
491  else if(obstacles.type() == CV_8UC1)
492  {
493  _obstacleCellsCompressed = obstacles;
494  }
495  }
496  if(!empty.empty())
497  {
498  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))
499  {
501  ctEmpty.start();
502  }
503  else if(empty.type() == CV_8UC1)
504  {
506  }
507  }
508  ctGround.join();
509  ctObstacles.join();
510  ctEmpty.join();
511  if(!_groundCellsRaw.empty())
512  {
514  }
515  if(!_obstacleCellsRaw.empty())
516  {
518  }
519  if(!_emptyCellsRaw.empty())
520  {
522  }
523 
524  _cellSize = cellSize;
525  _viewPoint = viewPoint;
526 }
527 
529 {
530  cv::Mat tmpA, tmpB, tmpD, tmpE, tmpF, tmpG;
531  LaserScan tmpC;
532  uncompressData(_imageCompressed.empty()?0:&tmpA,
533  _depthOrRightCompressed.empty()?0:&tmpB,
534  _laserScanCompressed.isEmpty()?0:&tmpC,
535  _userDataCompressed.empty()?0:&tmpD,
536  _groundCellsCompressed.empty()?0:&tmpE,
537  _obstacleCellsCompressed.empty()?0:&tmpF,
538  _emptyCellsCompressed.empty()?0:&tmpG);
539 }
540 
542  cv::Mat * imageRaw,
543  cv::Mat * depthRaw,
544  LaserScan * laserScanRaw,
545  cv::Mat * userDataRaw,
546  cv::Mat * groundCellsRaw,
547  cv::Mat * obstacleCellsRaw,
548  cv::Mat * emptyCellsRaw)
549 {
550  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);
551  if(imageRaw == 0 &&
552  depthRaw == 0 &&
553  laserScanRaw == 0 &&
554  userDataRaw == 0 &&
555  groundCellsRaw == 0 &&
556  obstacleCellsRaw == 0 &&
557  emptyCellsRaw == 0)
558  {
559  return;
560  }
562  imageRaw,
563  depthRaw,
564  laserScanRaw,
565  userDataRaw,
566  groundCellsRaw,
567  obstacleCellsRaw,
568  emptyCellsRaw);
569 
570  if(imageRaw && !imageRaw->empty() && _imageRaw.empty())
571  {
572  _imageRaw = *imageRaw;
573  //backward compatibility, set image size in camera model if not set
574  if(!_imageRaw.empty() && _cameraModels.size())
575  {
576  cv::Size size(_imageRaw.cols/_cameraModels.size(), _imageRaw.rows);
577  for(unsigned int i=0; i<_cameraModels.size(); ++i)
578  {
579  if(_cameraModels[i].fx() && _cameraModels[i].fy() && _cameraModels[i].imageWidth() == 0)
580  {
581  _cameraModels[i].setImageSize(size);
582  }
583  }
584  }
585  }
586  if(depthRaw && !depthRaw->empty() && _depthOrRightRaw.empty())
587  {
589  }
591  {
594  {
596  {
598  }
599  else
600  {
602  }
603  }
604  }
605  if(userDataRaw && !userDataRaw->empty() && _userDataRaw.empty())
606  {
608  }
609  if(groundCellsRaw && !groundCellsRaw->empty() && _groundCellsRaw.empty())
610  {
611  _groundCellsRaw = *groundCellsRaw;
612  }
613  if(obstacleCellsRaw && !obstacleCellsRaw->empty() && _obstacleCellsRaw.empty())
614  {
615  _obstacleCellsRaw = *obstacleCellsRaw;
616  }
617  if(emptyCellsRaw && !emptyCellsRaw->empty() && _emptyCellsRaw.empty())
618  {
619  _emptyCellsRaw = *emptyCellsRaw;
620  }
621 }
622 
624  cv::Mat * imageRaw,
625  cv::Mat * depthRaw,
626  LaserScan * laserScanRaw,
627  cv::Mat * userDataRaw,
628  cv::Mat * groundCellsRaw,
629  cv::Mat * obstacleCellsRaw,
630  cv::Mat * emptyCellsRaw) const
631 {
632  if(imageRaw)
633  {
634  *imageRaw = _imageRaw;
635  }
636  if(depthRaw)
637  {
639  }
640  if(laserScanRaw)
641  {
643  }
644  if(userDataRaw)
645  {
647  }
648  if(groundCellsRaw)
649  {
650  *groundCellsRaw = _groundCellsRaw;
651  }
652  if(obstacleCellsRaw)
653  {
654  *obstacleCellsRaw = _obstacleCellsRaw;
655  }
656  if(emptyCellsRaw)
657  {
658  *emptyCellsRaw = _emptyCellsRaw;
659  }
660  if( (imageRaw && imageRaw->empty()) ||
661  (depthRaw && depthRaw->empty()) ||
663  (userDataRaw && userDataRaw->empty()) ||
664  (groundCellsRaw && groundCellsRaw->empty()) ||
665  (obstacleCellsRaw && obstacleCellsRaw->empty()) ||
666  (emptyCellsRaw && emptyCellsRaw->empty()))
667  {
675  if(imageRaw && imageRaw->empty() && !_imageCompressed.empty())
676  {
677  UASSERT(_imageCompressed.type() == CV_8UC1);
678  ctImage.start();
679  }
680  if(depthRaw && depthRaw->empty() && !_depthOrRightCompressed.empty())
681  {
682  UASSERT(_depthOrRightCompressed.type() == CV_8UC1);
683  ctDepth.start();
684  }
686  {
688  ctLaserScan.start();
689  }
690  if(userDataRaw && userDataRaw->empty() && !_userDataCompressed.empty())
691  {
692  UASSERT(_userDataCompressed.type() == CV_8UC1);
693  ctUserData.start();
694  }
695  if(groundCellsRaw && groundCellsRaw->empty() && !_groundCellsCompressed.empty())
696  {
697  UASSERT(_groundCellsCompressed.type() == CV_8UC1);
698  ctGroundCells.start();
699  }
700  if(obstacleCellsRaw && obstacleCellsRaw->empty() && !_obstacleCellsCompressed.empty())
701  {
702  UASSERT(_obstacleCellsCompressed.type() == CV_8UC1);
703  ctObstacleCells.start();
704  }
705  if(emptyCellsRaw && emptyCellsRaw->empty() && !_emptyCellsCompressed.empty())
706  {
707  UASSERT(_emptyCellsCompressed.type() == CV_8UC1);
708  ctEmptyCells.start();
709  }
710  ctImage.join();
711  ctDepth.join();
712  ctLaserScan.join();
713  ctUserData.join();
714  ctGroundCells.join();
715  ctObstacleCells.join();
716  ctEmptyCells.join();
717 
718  if(imageRaw && imageRaw->empty())
719  {
720  *imageRaw = ctImage.getUncompressedData();
721  if(imageRaw->empty())
722  {
723  if(_imageCompressed.empty())
724  {
725  UWARN("Requested raw image data, but the sensor data (%d) doesn't have image.", this->id());
726  }
727  else
728  {
729  UERROR("Requested image data, but failed to uncompress (%d).", this->id());
730  }
731  }
732  }
733  if(depthRaw && depthRaw->empty())
734  {
735  *depthRaw = ctDepth.getUncompressedData();
736  if(depthRaw->empty())
737  {
738  if(_depthOrRightCompressed.empty())
739  {
740  UWARN("Requested depth/right image data, but the sensor data (%d) doesn't have depth/right image.", this->id());
741  }
742  else
743  {
744  UERROR("Requested depth/right image data, but failed to uncompress (%d).", this->id());
745  }
746  }
747  }
748  if(laserScanRaw && laserScanRaw->isEmpty())
749  {
751  {
753  }
754  else
755  {
757  }
758  if(laserScanRaw->isEmpty())
759  {
761  {
762  UWARN("Requested laser scan data, but the sensor data (%d) doesn't have laser scan.", this->id());
763  }
764  else
765  {
766  UERROR("Requested laser scan data, but failed to uncompress (%d).", this->id());
767  }
768  }
769  }
770  if(userDataRaw && userDataRaw->empty())
771  {
772  *userDataRaw = ctUserData.getUncompressedData();
773 
774  if(userDataRaw->empty())
775  {
776  if(_userDataCompressed.empty())
777  {
778  UWARN("Requested user data, but the sensor data (%d) doesn't have user data.", this->id());
779  }
780  else
781  {
782  UERROR("Requested user data, but failed to uncompress (%d).", this->id());
783  }
784  }
785  }
786  if(groundCellsRaw && groundCellsRaw->empty())
787  {
788  *groundCellsRaw = ctGroundCells.getUncompressedData();
789  }
790  if(obstacleCellsRaw && obstacleCellsRaw->empty())
791  {
792  *obstacleCellsRaw = ctObstacleCells.getUncompressedData();
793  }
794  if(emptyCellsRaw && emptyCellsRaw->empty())
795  {
796  *emptyCellsRaw = ctEmptyCells.getUncompressedData();
797  }
798  }
799 }
800 
801 void SensorData::setFeatures(const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> & keypoints3D, const cv::Mat & descriptors)
802 {
803  UASSERT_MSG(keypoints3D.empty() || keypoints.size() == keypoints3D.size(), uFormat("keypoints=%d keypoints3D=%d", (int)keypoints.size(), (int)keypoints3D.size()).c_str());
804  UASSERT_MSG(descriptors.empty() || (int)keypoints.size() == descriptors.rows, uFormat("keypoints=%d descriptors=%d", (int)keypoints.size(), descriptors.rows).c_str());
808 }
809 
810 unsigned long SensorData::getMemoryUsed() const // Return memory usage in Bytes
811 {
812  return sizeof(SensorData) +
813  (_imageCompressed.empty()?0:_imageCompressed.total()*_imageCompressed.elemSize()) +
814  (_imageRaw.empty()?0:_imageRaw.total()*_imageRaw.elemSize()) +
816  (_depthOrRightRaw.empty()?0:_depthOrRightRaw.total()*_depthOrRightRaw.elemSize()) +
817  (_userDataCompressed.empty()?0:_userDataCompressed.total()*_userDataCompressed.elemSize()) +
818  (_userDataRaw.empty()?0:_userDataRaw.total()*_userDataRaw.elemSize()) +
820  (_laserScanRaw.empty()?0:_laserScanRaw.data().total()*_laserScanRaw.data().elemSize()) +
822  (_groundCellsRaw.empty()?0:_groundCellsRaw.total()*_groundCellsRaw.elemSize()) +
824  (_obstacleCellsRaw.empty()?0:_obstacleCellsRaw.total()*_obstacleCellsRaw.elemSize())+
825  (_emptyCellsCompressed.empty()?0:_emptyCellsCompressed.total()*_emptyCellsCompressed.elemSize()) +
826  (_emptyCellsRaw.empty()?0:_emptyCellsRaw.total()*_emptyCellsRaw.elemSize())+
827  _keypoints.size() * sizeof(cv::KeyPoint) +
828  _keypoints3D.size() * sizeof(cv::Point3f) +
829  (_descriptors.empty()?0:_descriptors.total()*_descriptors.elemSize());
830 }
831 
832 void SensorData::clearCompressedData(bool images, bool scan, bool userData)
833 {
834  if(images)
835  {
836  _imageCompressed=cv::Mat();
837  _depthOrRightCompressed=cv::Mat();
838  }
839  if(scan)
840  {
842  }
843  if(userData)
844  {
845  _userDataCompressed=cv::Mat();
846  }
847 }
848 void SensorData::clearRawData(bool images, bool scan, bool userData)
849 {
850  if(images)
851  {
852  _imageRaw=cv::Mat();
853  _depthOrRightRaw=cv::Mat();
854  }
855  if(scan)
856  {
858  }
859  if(userData)
860  {
861  _userDataRaw=cv::Mat();
862  }
863 }
864 
865 
866 bool SensorData::isPointVisibleFromCameras(const cv::Point3f & pt) const
867 {
868  if(_cameraModels.size() >= 1)
869  {
870  for(unsigned int i=0; i<_cameraModels.size(); ++i)
871  {
872  if(_cameraModels[i].isValidForProjection() && !_cameraModels[i].localTransform().isNull())
873  {
874  cv::Point3f ptInCameraFrame = util3d::transformPoint(pt, _cameraModels[i].localTransform().inverse());
875  if(ptInCameraFrame.z > 0.0f)
876  {
877  int borderWidth = int(float(_cameraModels[i].imageWidth())* 0.2);
878  int u, v;
879  _cameraModels[i].reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
880  if(uIsInBounds(u, borderWidth, _cameraModels[i].imageWidth()-2*borderWidth) &&
881  uIsInBounds(v, borderWidth, _cameraModels[i].imageHeight()-2*borderWidth))
882  {
883  return true;
884  }
885  }
886  }
887  }
888  }
889  else if(_stereoCameraModels.size() >= 1)
890  {
891  for(unsigned int i=0; i<_stereoCameraModels.size(); ++i)
892  {
893  if(_stereoCameraModels[i].isValidForProjection() && !_stereoCameraModels[i].localTransform().isNull())
894  {
895  cv::Point3f ptInCameraFrame = util3d::transformPoint(pt, _stereoCameraModels[i].localTransform().inverse());
896  if(ptInCameraFrame.z > 0.0f)
897  {
898  int u, v;
899  _stereoCameraModels[i].left().reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
900  if(uIsInBounds(u, 0, _stereoCameraModels[i].left().imageWidth()) &&
901  uIsInBounds(v, 0, _stereoCameraModels[i].left().imageHeight()))
902  {
903  return true;
904  }
905  }
906  }
907  }
908  }
909  else
910  {
911  UERROR("no valid camera model!");
912  }
913  return false;
914 }
915 
916 } // namespace rtabmap
917 
int
int
rtabmap::SensorData::_emptyCellsRaw
cv::Mat _emptyCellsRaw
Definition: SensorData.h:342
rtabmap::LaserScan::localTransform
Transform localTransform() const
Definition: LaserScan.h:127
Compression.h
rtabmap::SensorData::setLaserScan
void setLaserScan(const LaserScan &laserScan, bool clearPreviousData=true)
Definition: SensorData.cpp:381
rtabmap::StereoCameraModel
Definition: StereoCameraModel.h:35
rtabmap::SensorData::uncompressData
void uncompressData()
Definition: SensorData.cpp:528
rtabmap::SensorData::isPointVisibleFromCameras
bool isPointVisibleFromCameras(const cv::Point3f &pt) const
Definition: SensorData.cpp:866
rtabmap::LaserScan::maxPoints
int maxPoints() const
Definition: LaserScan.h:120
rtabmap::SensorData::uncompressDataConst
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:623
rtabmap::LaserScan::rangeMax
float rangeMax() const
Definition: LaserScan.h:122
rtabmap::SensorData::_cellSize
float _cellSize
Definition: SensorData.h:343
rtabmap::SensorData::_viewPoint
cv::Point3f _viewPoint
Definition: SensorData.h:344
rtabmap::SensorData::setImageRaw
RTABMAP_DEPRECATED void setImageRaw(const cv::Mat &image)
Definition: SensorData.cpp:401
fx
const double fx
size
Index size
rtabmap::CameraModel
Definition: CameraModel.h:38
rtabmap::CompressionThread::getCompressedData
const cv::Mat & getCompressedData() const
Definition: Compression.h:61
rtabmap::SensorData::_stereoCameraModels
std::vector< StereoCameraModel > _stereoCameraModels
Definition: SensorData.h:330
rtabmap::LaserScan::data
const cv::Mat & data() const
Definition: LaserScan.h:116
rtabmap::SensorData::_obstacleCellsCompressed
cv::Mat _obstacleCellsCompressed
Definition: SensorData.h:338
SensorData.h
UThread::join
void join(bool killFirst=false)
Definition: UThread.cpp:85
rtabmap::SensorData::_groundCellsCompressed
cv::Mat _groundCellsCompressed
Definition: SensorData.h:337
rtabmap::CompressionThread::getUncompressedData
cv::Mat & getUncompressedData()
Definition: Compression.h:62
rtabmap::SensorData::_imageRaw
cv::Mat _imageRaw
Definition: SensorData.h:325
rtabmap::LaserScan::kUnknown
@ kUnknown
Definition: LaserScan.h:40
rtabmap::SensorData::_laserScanCompressed
LaserScan _laserScanCompressed
Definition: SensorData.h:323
rtabmap::LaserScan
Definition: LaserScan.h:37
rtabmap::SensorData::cameraModels
const std::vector< CameraModel > & cameraModels() const
Definition: SensorData.h:240
rtabmap::LaserScan::isEmpty
bool isEmpty() const
Definition: LaserScan.h:130
UMath.h
Basic mathematics functions.
rtabmap::SensorData::stereoCameraModels
const std::vector< StereoCameraModel > & stereoCameraModels() const
Definition: SensorData.h:241
rtabmap::SensorData::_userDataCompressed
cv::Mat _userDataCompressed
Definition: SensorData.h:333
rtabmap::SensorData::_cameraModels
std::vector< CameraModel > _cameraModels
Definition: SensorData.h:329
rtabmap::SensorData::SensorData
SensorData()
Definition: SensorData.cpp:40
glm::isNull
GLM_FUNC_DECL bool isNull(detail::tmat2x2< T, P > const &m, T const &epsilon)
rtabmap::LaserScan::format
Format format() const
Definition: LaserScan.h:117
rtabmap::SensorData::setRGBDImage
void setRGBDImage(const cv::Mat &rgb, const cv::Mat &depth, const CameraModel &model, bool clearPreviousData=true)
Definition: SensorData.cpp:227
rtabmap::util3d::transformPoint
cv::Point3f RTABMAP_CORE_EXPORT transformPoint(const cv::Point3f &pt, const Transform &transform)
Definition: util3d_transforms.cpp:211
rtabmap::IMU
Definition: IMU.h:19
rtabmap::SensorData::laserScanRaw
const LaserScan & laserScanRaw() const
Definition: SensorData.h:185
util3d_transforms.h
rtabmap::SensorData::_keypoints3D
std::vector< cv::Point3f > _keypoints3D
Definition: SensorData.h:354
rtabmap::SensorData::descriptors
const cv::Mat & descriptors() const
Definition: SensorData.h:276
rtabmap::SensorData::_obstacleCellsRaw
cv::Mat _obstacleCellsRaw
Definition: SensorData.h:341
rtabmap::SensorData::_imageCompressed
cv::Mat _imageCompressed
Definition: SensorData.h:321
UConversion.h
Some conversion functions.
rtabmap::SensorData::_emptyCellsCompressed
cv::Mat _emptyCellsCompressed
Definition: SensorData.h:339
rtabmap::SensorData::_laserScanRaw
LaserScan _laserScanRaw
Definition: SensorData.h:327
rtabmap::SensorData::setUserDataRaw
RTABMAP_DEPRECATED void setUserDataRaw(const cv::Mat &data)
Definition: SensorData.cpp:417
rtabmap::SensorData::_depthOrRightRaw
cv::Mat _depthOrRightRaw
Definition: SensorData.h:326
rtabmap::CompressionThread
Definition: Compression.h:55
rtabmap::SensorData::clearCompressedData
void clearCompressedData(bool images=true, bool scan=true, bool userData=true)
Definition: SensorData.cpp:832
rtabmap::SensorData::imu_
IMU imu_
Definition: SensorData.h:367
rtabmap::SensorData::_depthOrRightCompressed
cv::Mat _depthOrRightCompressed
Definition: SensorData.h:322
UASSERT
#define UASSERT(condition)
rtabmap::SensorData::getMemoryUsed
unsigned long getMemoryUsed() const
Definition: SensorData.cpp:810
UThread::start
void start()
Definition: UThread.cpp:122
uIsInBounds
bool uIsInBounds(const T &value, const T &low, const T &high)
Definition: UMath.h:932
rtabmap::LaserScan::angleMax
float angleMax() const
Definition: LaserScan.h:124
rtabmap::SensorData::userDataRaw
const cv::Mat & userDataRaw() const
Definition: SensorData.h:252
rtabmap::SensorData::setFeatures
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
Definition: SensorData.cpp:801
UASSERT_MSG
#define UASSERT_MSG(condition, msg_str)
Definition: ULogger.h:67
rtabmap::SensorData::_keypoints
std::vector< cv::KeyPoint > _keypoints
Definition: SensorData.h:353
UWARN
#define UWARN(...)
rtabmap::compressData2
cv::Mat RTABMAP_CORE_EXPORT compressData2(const cv::Mat &data)
Definition: Compression.cpp:201
f
Point2(* f)(const Point3 &, OptionalJacobian< 2, 3 >)
uFormat
std::string UTILITE_EXPORT uFormat(const char *fmt,...)
Definition: UConversion.cpp:365
rtabmap::SensorData::_groundCellsRaw
cv::Mat _groundCellsRaw
Definition: SensorData.h:340
rtabmap::LaserScan::clear
void clear()
Definition: LaserScan.h:150
rtabmap::SensorData::imageRaw
const cv::Mat & imageRaw() const
Definition: SensorData.h:183
ULogger.h
ULogger class and convenient macros.
empty
rtabmap::SensorData::~SensorData
virtual ~SensorData()
Definition: SensorData.cpp:223
rtabmap::SensorData::depthRaw
cv::Mat depthRaw() const
Definition: SensorData.h:210
rtabmap::SensorData::imu
const IMU & imu() const
Definition: SensorData.h:294
id
id
rtabmap::LaserScan::empty
bool empty() const
Definition: LaserScan.h:129
rtabmap::SensorData::setUserData
void setUserData(const cv::Mat &userData, bool clearPreviousData=true)
Definition: SensorData.cpp:422
v
Array< int, Dynamic, 1 > v
rtabmap::SensorData::_userDataRaw
cv::Mat _userDataRaw
Definition: SensorData.h:334
UDEBUG
#define UDEBUG(...)
rtabmap::LaserScan::angleIncrement
float angleIncrement() const
Definition: LaserScan.h:125
rtabmap::SensorData::setOccupancyGrid
void setOccupancyGrid(const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty, float cellSize, const cv::Point3f &viewPoint)
Definition: SensorData.cpp:444
rtabmap::SensorData::keypoints
const std::vector< cv::KeyPoint > & keypoints() const
Definition: SensorData.h:274
rtabmap::SensorData::_descriptors
cv::Mat _descriptors
Definition: SensorData.h:355
rtabmap::SensorData::setStereoImage
void setStereoImage(const cv::Mat &left, const cv::Mat &right, const StereoCameraModel &stereoCameraModel, bool clearPreviousData=true)
Definition: SensorData.cpp:304
rtabmap::SensorData::clearRawData
void clearRawData(bool images=true, bool scan=true, bool userData=true)
Definition: SensorData.cpp:848
rtabmap
Definition: CameraARCore.cpp:35
rtabmap::LaserScan::isCompressed
bool isCompressed() const
Definition: LaserScan.h:138
UERROR
#define UERROR(...)
rtabmap::LaserScan::angleMin
float angleMin() const
Definition: LaserScan.h:123
rtabmap::SensorData::keypoints3D
const std::vector< cv::Point3f > & keypoints3D() const
Definition: SensorData.h:275
trace.model
model
Definition: trace.py:4
glm::inverse
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)
i
int i
rtabmap::SensorData::setLaserScanRaw
RTABMAP_DEPRECATED void setLaserScanRaw(const LaserScan &scan)
Definition: SensorData.cpp:411
rtabmap::SensorData::setDepthOrRightRaw
RTABMAP_DEPRECATED void setDepthOrRightRaw(const cv::Mat &image)
Definition: SensorData.cpp:406
rtabmap::LaserScan::rangeMin
float rangeMin() const
Definition: LaserScan.h:121
fy
const double fy


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jul 25 2024 02:50:15