49 const cv::Mat & image,
52 const cv::Mat & userData) :
63 const cv::Mat & image,
67 const cv::Mat & userData) :
79 const cv::Mat & depth,
83 const cv::Mat & userData) :
96 const cv::Mat & depth,
100 const cv::Mat & userData) :
113 const cv::Mat & depth,
114 const std::vector<CameraModel> & cameraModels,
117 const cv::Mat & userData) :
130 const cv::Mat & depth,
131 const std::vector<CameraModel> & cameraModels,
134 const cv::Mat & userData) :
146 const cv::Mat & left,
147 const cv::Mat & right,
151 const cv::Mat & userData):
163 const cv::Mat & left,
164 const cv::Mat & right,
168 const cv::Mat & userData) :
180 const cv::Mat & left,
181 const cv::Mat & right,
182 const std::vector<StereoCameraModel> & cameraModels,
185 const cv::Mat & userData):
197 const cv::Mat & left,
198 const cv::Mat & right,
199 const std::vector<StereoCameraModel> & cameraModels,
202 const cv::Mat & userData) :
229 const cv::Mat & depth,
231 bool clearPreviousData)
233 std::vector<CameraModel> models;
234 models.push_back(
model);
239 const cv::Mat & depth,
240 const std::vector<CameraModel> & models,
241 bool clearPreviousData)
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!");
256 UASSERT(rgb.type() == CV_8UC1);
263 else if(!rgb.empty())
265 UASSERT(rgb.type() == CV_8UC1 ||
266 rgb.type() == CV_8UC3);
281 UASSERT(depth.type() == CV_8UC1);
288 else if(!depth.empty())
290 UASSERT(depth.type() == CV_32FC1 ||
291 depth.type() == CV_16UC1);
305 const cv::Mat & left,
306 const cv::Mat & right,
308 bool clearPreviousData)
310 std::vector<StereoCameraModel> models;
311 models.push_back(stereoCameraModel);
315 const cv::Mat & left,
316 const cv::Mat & right,
317 const std::vector<StereoCameraModel> & stereoCameraModels,
318 bool clearPreviousData)
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!");
327 bool clearData = clearPreviousData || !
_cameraModels.empty();
334 UASSERT(left.type() == CV_8UC1);
341 else if(!left.empty())
343 UASSERT(left.type() == CV_8UC1 ||
344 left.type() == CV_8UC3);
359 UASSERT(right.type() == CV_8UC1);
366 else if(!right.empty())
368 UASSERT(right.type() == CV_8UC1);
386 if(clearPreviousData)
394 if(clearPreviousData)
403 UASSERT(image.empty() || image.rows > 1);
408 UASSERT(image.empty() || image.rows > 1);
424 if(clearPreviousData)
430 if(userData.type() == CV_8UC1 && userData.rows == 1 && userData.cols >
int(3*
sizeof(
int)))
437 if(!userData.empty())
445 const cv::Mat & ground,
446 const cv::Mat & obstacles,
447 const cv::Mat &
empty,
449 const cv::Point3f & viewPoint)
451 UDEBUG(
"ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols,
empty.cols);
456 UWARN(
"Occupancy grid cannot be overwritten! id=%d, Set occupancy grid of %d to null "
457 "before setting a new one.", this->
id());
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))
479 else if(ground.type() == CV_8UC1)
484 if(!obstacles.empty())
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))
491 else if(obstacles.type() == CV_8UC1)
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))
503 else if(
empty.type() == CV_8UC1)
530 cv::Mat tmpA, tmpB, tmpD, tmpE, tmpF, tmpG;
545 cv::Mat * userDataRaw,
546 cv::Mat * groundCellsRaw,
547 cv::Mat * obstacleCellsRaw,
548 cv::Mat * emptyCellsRaw)
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);
555 groundCellsRaw == 0 &&
556 obstacleCellsRaw == 0 &&
609 if(groundCellsRaw && !groundCellsRaw->empty() &&
_groundCellsRaw.empty())
617 if(emptyCellsRaw && !emptyCellsRaw->empty() &&
_emptyCellsRaw.empty())
627 cv::Mat * userDataRaw,
628 cv::Mat * groundCellsRaw,
629 cv::Mat * obstacleCellsRaw,
630 cv::Mat * emptyCellsRaw)
const
664 (groundCellsRaw && groundCellsRaw->empty()) ||
665 (obstacleCellsRaw && obstacleCellsRaw->empty()) ||
666 (emptyCellsRaw && emptyCellsRaw->empty()))
698 ctGroundCells.
start();
703 ctObstacleCells.
start();
708 ctEmptyCells.
start();
714 ctGroundCells.
join();
715 ctObstacleCells.
join();
725 UWARN(
"Requested raw image data, but the sensor data (%d) doesn't have image.", this->
id());
729 UERROR(
"Requested image data, but failed to uncompress (%d).", this->
id());
740 UWARN(
"Requested depth/right image data, but the sensor data (%d) doesn't have depth/right image.", this->
id());
744 UERROR(
"Requested depth/right image data, but failed to uncompress (%d).", this->
id());
762 UWARN(
"Requested laser scan data, but the sensor data (%d) doesn't have laser scan.", this->
id());
766 UERROR(
"Requested laser scan data, but failed to uncompress (%d).", this->
id());
778 UWARN(
"Requested user data, but the sensor data (%d) doesn't have user data.", this->
id());
782 UERROR(
"Requested user data, but failed to uncompress (%d).", this->
id());
786 if(groundCellsRaw && groundCellsRaw->empty())
790 if(obstacleCellsRaw && obstacleCellsRaw->empty())
794 if(emptyCellsRaw && emptyCellsRaw->empty())
801 void SensorData::setFeatures(
const std::vector<cv::KeyPoint> & keypoints,
const std::vector<cv::Point3f> & keypoints3D,
const cv::Mat & descriptors)
875 if(ptInCameraFrame.z > 0.0f)
879 _cameraModels[
i].reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u,
v);
896 if(ptInCameraFrame.z > 0.0f)
899 _stereoCameraModels[
i].left().reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u,
v);
911 UERROR(
"no valid camera model!");