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,
117 const cv::Mat & userData) :
130 const cv::Mat & depth,
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) :
195 const cv::Mat & depth,
197 bool clearPreviousData)
199 std::vector<CameraModel> models;
200 models.push_back(model);
205 const cv::Mat & depth,
206 const std::vector<CameraModel> & models,
207 bool clearPreviousData)
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!");
222 UASSERT(rgb.type() == CV_8UC1);
229 else if(!rgb.empty())
231 UASSERT(rgb.type() == CV_8UC1 ||
232 rgb.type() == CV_8UC3);
247 UASSERT(depth.type() == CV_8UC1);
254 else if(!depth.empty())
256 UASSERT(depth.type() == CV_32FC1 ||
257 depth.type() == CV_16UC1);
271 const cv::Mat & left,
272 const cv::Mat & right,
274 bool clearPreviousData)
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!");
283 bool clearData = clearPreviousData || !
_cameraModels.empty();
290 UASSERT(left.type() == CV_8UC1);
297 else if(!left.empty())
299 UASSERT(left.type() == CV_8UC1 ||
300 left.type() == CV_8UC3);
315 UASSERT(right.type() == CV_8UC1);
322 else if(!right.empty())
324 UASSERT(right.type() == CV_8UC1);
342 if(clearPreviousData)
350 if(clearPreviousData)
357 void SensorData::setImageRaw(
const cv::Mat & image)
359 UASSERT(image.empty() || image.rows > 1);
362 void SensorData::setDepthOrRightRaw(
const cv::Mat & image)
364 UASSERT(image.empty() || image.rows > 1);
367 void SensorData::setLaserScanRaw(
const LaserScan & scan)
373 void SensorData::setUserDataRaw(
const cv::Mat &
userDataRaw)
380 if(clearPreviousData)
386 if(userData.type() == CV_8UC1 && userData.rows == 1 && userData.cols > int(3*
sizeof(
int)))
393 if(!userData.empty())
401 const cv::Mat & ground,
402 const cv::Mat & obstacles,
403 const cv::Mat & empty,
405 const cv::Point3f & viewPoint)
407 UDEBUG(
"ground=%d obstacles=%d empty=%d", ground.cols, obstacles.cols, empty.cols);
412 UWARN(
"Occupancy grid cannot be overwritten! id=%d, Set occupancy grid of %d to null " 413 "before setting a new one.", this->
id());
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))
435 else if(ground.type() == CV_8UC1)
440 if(!obstacles.empty())
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))
447 else if(obstacles.type() == CV_8UC1)
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))
459 else if(empty.type() == CV_8UC1)
486 cv::Mat tmpA, tmpB, tmpD, tmpE, tmpF, tmpG;
501 cv::Mat * userDataRaw,
502 cv::Mat * groundCellsRaw,
503 cv::Mat * obstacleCellsRaw,
504 cv::Mat * emptyCellsRaw)
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);
511 groundCellsRaw == 0 &&
512 obstacleCellsRaw == 0 &&
526 if(imageRaw && !imageRaw->empty() &&
_imageRaw.empty())
561 if(userDataRaw && !userDataRaw->empty() &&
_userDataRaw.empty())
565 if(groundCellsRaw && !groundCellsRaw->empty() &&
_groundCellsRaw.empty())
573 if(emptyCellsRaw && !emptyCellsRaw->empty() &&
_emptyCellsRaw.empty())
583 cv::Mat * userDataRaw,
584 cv::Mat * groundCellsRaw,
585 cv::Mat * obstacleCellsRaw,
586 cv::Mat * emptyCellsRaw)
const 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()))
654 ctGroundCells.
start();
659 ctObstacleCells.
start();
664 ctEmptyCells.
start();
670 ctGroundCells.
join();
671 ctObstacleCells.
join();
674 if(imageRaw && imageRaw->empty())
677 if(imageRaw->empty())
681 UWARN(
"Requested raw image data, but the sensor data (%d) doesn't have image.", this->
id());
685 UERROR(
"Requested image data, but failed to uncompress (%d).", this->
id());
689 if(depthRaw && depthRaw->empty())
692 if(depthRaw->empty())
696 UWARN(
"Requested depth/right image data, but the sensor data (%d) doesn't have depth/right image.", this->
id());
700 UERROR(
"Requested depth/right image data, but failed to uncompress (%d).", this->
id());
704 if(laserScanRaw && laserScanRaw->
isEmpty())
718 UWARN(
"Requested laser scan data, but the sensor data (%d) doesn't have laser scan.", this->
id());
722 UERROR(
"Requested laser scan data, but failed to uncompress (%d).", this->
id());
726 if(userDataRaw && userDataRaw->empty())
728 *userDataRaw = ctUserData.getUncompressedData();
730 if(userDataRaw->empty())
734 UWARN(
"Requested user data, but the sensor data (%d) doesn't have user data.", this->
id());
738 UERROR(
"Requested user data, but failed to uncompress (%d).", this->
id());
742 if(groundCellsRaw && groundCellsRaw->empty())
746 if(obstacleCellsRaw && obstacleCellsRaw->empty())
750 if(emptyCellsRaw && emptyCellsRaw->empty())
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());
831 if(ptInCameraFrame.z > 0.0f)
833 int borderWidth = int(
float(
_cameraModels[i].imageWidth())* 0.2);
835 _cameraModels[i].reproject(ptInCameraFrame.x, ptInCameraFrame.y, ptInCameraFrame.z, u, v);
848 if(ptInCameraFrame.z > 0.0f)
858 UERROR(
"no valid camera model!");
void clearCompressedData(bool images=true, bool scan=true, bool userData=true)
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
void clearRawData(bool images=true, bool scan=true, bool userData=true)
void setOccupancyGrid(const cv::Mat &ground, const cv::Mat &obstacles, const cv::Mat &empty, float cellSize, const cv::Point3f &viewPoint)
cv::Mat _userDataCompressed
cv::Point3f RTABMAP_EXP transformPoint(const cv::Point3f &pt, const Transform &transform)
const cv::Mat & data() const
std::vector< cv::Point3f > _keypoints3D
void setLaserScan(const LaserScan &laserScan, bool clearPreviousData=true)
LaserScan _laserScanCompressed
bool uIsInBounds(const T &value, const T &low, const T &high)
cv::Mat _obstacleCellsRaw
const cv::Mat & descriptors() const
void setRGBDImage(const cv::Mat &rgb, const cv::Mat &depth, const CameraModel &model, bool clearPreviousData=true)
cv::Mat _groundCellsCompressed
const std::vector< cv::KeyPoint > & keypoints() const
Basic mathematics functions.
Some conversion functions.
unsigned long getMemoryUsed() const
const LaserScan & laserScanRaw() const
cv::Mat _emptyCellsCompressed
const cv::Mat & imageRaw() const
std::vector< cv::KeyPoint > _keypoints
#define UASSERT(condition)
bool isValidForProjection() const
const CameraModel & left() const
#define UASSERT_MSG(condition, msg_str)
cv::Mat _obstacleCellsCompressed
void reproject(float x, float y, float z, float &u, float &v) const
bool isCompressed() const
Transform localTransform() const
float angleIncrement() const
void setStereoImage(const cv::Mat &left, const cv::Mat &right, const StereoCameraModel &stereoCameraModel, bool clearPreviousData=true)
cv::Mat RTABMAP_EXP compressData2(const cv::Mat &data)
const cv::Mat & userDataRaw() const
void setUserData(const cv::Mat &userData, bool clearPreviousData=true)
bool isPointVisibleFromCameras(const cv::Point3f &pt) const
cv::Mat _depthOrRightCompressed
const std::vector< CameraModel > & cameraModels() const
const cv::Mat & getCompressedData() const
StereoCameraModel _stereoCameraModel
ULogger class and convenient macros.
const StereoCameraModel & stereoCameraModel() const
std::vector< CameraModel > _cameraModels
void join(bool killFirst=false)
const Transform & localTransform() const
std::string UTILITE_EXP uFormat(const char *fmt,...)
cv::Mat & getUncompressedData()
void setFeatures(const std::vector< cv::KeyPoint > &keypoints, const std::vector< cv::Point3f > &keypoints3D, const cv::Mat &descriptors)
const std::vector< cv::Point3f > & keypoints3D() const
GLM_FUNC_DECL matType< T, P > inverse(matType< T, P > const &m)