00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028 #ifndef SENSORDATA_H_
00029 #define SENSORDATA_H_
00030
00031 #include <rtabmap/core/RtabmapExp.h>
00032 #include <rtabmap/core/Transform.h>
00033 #include <rtabmap/core/CameraModel.h>
00034 #include <rtabmap/core/StereoCameraModel.h>
00035 #include <rtabmap/core/Transform.h>
00036 #include <rtabmap/core/GeodeticCoords.h>
00037 #include <opencv2/core/core.hpp>
00038 #include <opencv2/features2d/features2d.hpp>
00039 #include <rtabmap/core/LaserScan.h>
00040 #include <rtabmap/core/IMU.h>
00041
00042 namespace rtabmap
00043 {
00044
00048 class RTABMAP_EXP SensorData
00049 {
00050 public:
00051
00052 SensorData();
00053
00054
00055 SensorData(
00056 const cv::Mat & image,
00057 int id = 0,
00058 double stamp = 0.0,
00059 const cv::Mat & userData = cv::Mat());
00060
00061
00062 SensorData(
00063 const cv::Mat & image,
00064 const CameraModel & cameraModel,
00065 int id = 0,
00066 double stamp = 0.0,
00067 const cv::Mat & userData = cv::Mat());
00068
00069
00070 SensorData(
00071 const cv::Mat & rgb,
00072 const cv::Mat & depth,
00073 const CameraModel & cameraModel,
00074 int id = 0,
00075 double stamp = 0.0,
00076 const cv::Mat & userData = cv::Mat());
00077
00078
00079 SensorData(
00080 const LaserScan & laserScan,
00081 const cv::Mat & rgb,
00082 const cv::Mat & depth,
00083 const CameraModel & cameraModel,
00084 int id = 0,
00085 double stamp = 0.0,
00086 const cv::Mat & userData = cv::Mat());
00087
00088
00089 SensorData(
00090 const cv::Mat & rgb,
00091 const cv::Mat & depth,
00092 const std::vector<CameraModel> & cameraModels,
00093 int id = 0,
00094 double stamp = 0.0,
00095 const cv::Mat & userData = cv::Mat());
00096
00097
00098 SensorData(
00099 const LaserScan & laserScan,
00100 const cv::Mat & rgb,
00101 const cv::Mat & depth,
00102 const std::vector<CameraModel> & cameraModels,
00103 int id = 0,
00104 double stamp = 0.0,
00105 const cv::Mat & userData = cv::Mat());
00106
00107
00108 SensorData(
00109 const cv::Mat & left,
00110 const cv::Mat & right,
00111 const StereoCameraModel & cameraModel,
00112 int id = 0,
00113 double stamp = 0.0,
00114 const cv::Mat & userData = cv::Mat());
00115
00116
00117 SensorData(
00118 const LaserScan & laserScan,
00119 const cv::Mat & left,
00120 const cv::Mat & right,
00121 const StereoCameraModel & cameraModel,
00122 int id = 0,
00123 double stamp = 0.0,
00124 const cv::Mat & userData = cv::Mat());
00125
00126
00127 SensorData(
00128 const IMU & imu,
00129 int id = 0,
00130 double stamp = 0.0);
00131
00132 virtual ~SensorData();
00133
00134 bool isValid() const {
00135 return !(_id == 0 &&
00136 _stamp == 0.0 &&
00137 _imageRaw.empty() &&
00138 _imageCompressed.empty() &&
00139 _depthOrRightRaw.empty() &&
00140 _depthOrRightCompressed.empty() &&
00141 _laserScanRaw.isEmpty() &&
00142 _laserScanCompressed.isEmpty() &&
00143 _cameraModels.size() == 0 &&
00144 !_stereoCameraModel.isValidForProjection() &&
00145 _userDataRaw.empty() &&
00146 _userDataCompressed.empty() &&
00147 _keypoints.size() == 0 &&
00148 _descriptors.empty() &&
00149 imu_.empty());
00150 }
00151
00152 int id() const {return _id;}
00153 void setId(int id) {_id = id;}
00154 double stamp() const {return _stamp;}
00155 void setStamp(double stamp) {_stamp = stamp;}
00156
00157 const cv::Mat & imageCompressed() const {return _imageCompressed;}
00158 const cv::Mat & depthOrRightCompressed() const {return _depthOrRightCompressed;}
00159 const LaserScan & laserScanCompressed() const {return _laserScanCompressed;}
00160
00161 const cv::Mat & imageRaw() const {return _imageRaw;}
00162 const cv::Mat & depthOrRightRaw() const {return _depthOrRightRaw;}
00163 const LaserScan & laserScanRaw() const {return _laserScanRaw;}
00164 void setImageRaw(const cv::Mat & imageRaw) {_imageRaw = imageRaw;}
00165 void setDepthOrRightRaw(const cv::Mat & depthOrImageRaw) {_depthOrRightRaw =depthOrImageRaw;}
00166 void setLaserScanRaw(const LaserScan & laserScanRaw) {_laserScanRaw =laserScanRaw;}
00167 void setCameraModel(const CameraModel & model) {_cameraModels.clear(); _cameraModels.push_back(model);}
00168 void setCameraModels(const std::vector<CameraModel> & models) {_cameraModels = models;}
00169 void setStereoCameraModel(const StereoCameraModel & stereoCameraModel) {_stereoCameraModel = stereoCameraModel;}
00170
00171
00172 cv::Mat depthRaw() const {return _depthOrRightRaw.type()!=CV_8UC1?_depthOrRightRaw:cv::Mat();}
00173 cv::Mat rightRaw() const {return _depthOrRightRaw.type()==CV_8UC1?_depthOrRightRaw:cv::Mat();}
00174
00175 void uncompressData();
00176 void uncompressData(
00177 cv::Mat * imageRaw,
00178 cv::Mat * depthOrRightRaw,
00179 LaserScan * laserScanRaw = 0,
00180 cv::Mat * userDataRaw = 0,
00181 cv::Mat * groundCellsRaw = 0,
00182 cv::Mat * obstacleCellsRaw = 0,
00183 cv::Mat * emptyCellsRaw = 0);
00184 void uncompressDataConst(
00185 cv::Mat * imageRaw,
00186 cv::Mat * depthOrRightRaw,
00187 LaserScan * laserScanRaw = 0,
00188 cv::Mat * userDataRaw = 0,
00189 cv::Mat * groundCellsRaw = 0,
00190 cv::Mat * obstacleCellsRaw = 0,
00191 cv::Mat * emptyCellsRaw = 0) const;
00192
00193 const std::vector<CameraModel> & cameraModels() const {return _cameraModels;}
00194 const StereoCameraModel & stereoCameraModel() const {return _stereoCameraModel;}
00195
00196 void setUserDataRaw(const cv::Mat & userDataRaw);
00204 void setUserData(const cv::Mat & userData);
00205 const cv::Mat & userDataRaw() const {return _userDataRaw;}
00206 const cv::Mat & userDataCompressed() const {return _userDataCompressed;}
00207
00208
00209 void setOccupancyGrid(
00210 const cv::Mat & ground,
00211 const cv::Mat & obstacles,
00212 const cv::Mat & empty,
00213 float cellSize,
00214 const cv::Point3f & viewPoint);
00215
00216 void clearOccupancyGridRaw() {_groundCellsRaw = cv::Mat(); _obstacleCellsRaw = cv::Mat();}
00217 const cv::Mat & gridGroundCellsRaw() const {return _groundCellsRaw;}
00218 const cv::Mat & gridGroundCellsCompressed() const {return _groundCellsCompressed;}
00219 const cv::Mat & gridObstacleCellsRaw() const {return _obstacleCellsRaw;}
00220 const cv::Mat & gridObstacleCellsCompressed() const {return _obstacleCellsCompressed;}
00221 const cv::Mat & gridEmptyCellsRaw() const {return _emptyCellsRaw;}
00222 const cv::Mat & gridEmptyCellsCompressed() const {return _emptyCellsCompressed;}
00223 float gridCellSize() const {return _cellSize;}
00224 const cv::Point3f & gridViewPoint() const {return _viewPoint;}
00225
00226 void setFeatures(const std::vector<cv::KeyPoint> & keypoints, const std::vector<cv::Point3f> & keypoints3D, const cv::Mat & descriptors);
00227 const std::vector<cv::KeyPoint> & keypoints() const {return _keypoints;}
00228 const std::vector<cv::Point3f> & keypoints3D() const {return _keypoints3D;}
00229 const cv::Mat & descriptors() const {return _descriptors;}
00230
00231 void setGroundTruth(const Transform & pose) {groundTruth_ = pose;}
00232 const Transform & groundTruth() const {return groundTruth_;}
00233
00234 void setGlobalPose(const Transform & pose, const cv::Mat & covariance) {globalPose_ = pose; globalPoseCovariance_ = covariance;}
00235 const Transform & globalPose() const {return globalPose_;}
00236 const cv::Mat & globalPoseCovariance() const {return globalPoseCovariance_;}
00237
00238 void setGPS(const GPS & gps)
00239 {
00240 gps_ = gps;
00241 }
00242 const GPS & gps() const {return gps_;}
00243
00244 void setIMU(const IMU & imu)
00245 {
00246 imu_ = imu;
00247 }
00248 const IMU & imu() const {return imu_;}
00249
00250 long getMemoryUsed() const;
00251 void clearCompressedData() {_imageCompressed=cv::Mat(); _depthOrRightCompressed=cv::Mat(); _laserScanCompressed.clear(); _userDataCompressed=cv::Mat();}
00252
00253 bool isPointVisibleFromCameras(const cv::Point3f & pt) const;
00254
00255 private:
00256 int _id;
00257 double _stamp;
00258
00259 cv::Mat _imageCompressed;
00260 cv::Mat _depthOrRightCompressed;
00261 LaserScan _laserScanCompressed;
00262
00263 cv::Mat _imageRaw;
00264 cv::Mat _depthOrRightRaw;
00265 LaserScan _laserScanRaw;
00266
00267 std::vector<CameraModel> _cameraModels;
00268 StereoCameraModel _stereoCameraModel;
00269
00270
00271 cv::Mat _userDataCompressed;
00272 cv::Mat _userDataRaw;
00273
00274
00275 cv::Mat _groundCellsCompressed;
00276 cv::Mat _obstacleCellsCompressed;
00277 cv::Mat _emptyCellsCompressed;
00278 cv::Mat _groundCellsRaw;
00279 cv::Mat _obstacleCellsRaw;
00280 cv::Mat _emptyCellsRaw;
00281 float _cellSize;
00282 cv::Point3f _viewPoint;
00283
00284
00285 std::vector<cv::KeyPoint> _keypoints;
00286 std::vector<cv::Point3f> _keypoints3D;
00287 cv::Mat _descriptors;
00288
00289 Transform groundTruth_;
00290
00291 Transform globalPose_;
00292 cv::Mat globalPoseCovariance_;
00293
00294 GPS gps_;
00295
00296 IMU imu_;
00297 };
00298
00299 }
00300
00301
00302 #endif