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

rtabmap
Author(s): Mathieu Labbe
autogenerated on Wed Jun 5 2019 22:41:32