SensorData.h
Go to the documentation of this file.
00001 /*
00002 Copyright (c) 2010-2016, Mathieu Labbe - IntRoLab - Universite de Sherbrooke
00003 All rights reserved.
00004 
00005 Redistribution and use in source and binary forms, with or without
00006 modification, are permitted provided that the following conditions are met:
00007     * Redistributions of source code must retain the above copyright
00008       notice, this list of conditions and the following disclaimer.
00009     * Redistributions in binary form must reproduce the above copyright
00010       notice, this list of conditions and the following disclaimer in the
00011       documentation and/or other materials provided with the distribution.
00012     * Neither the name of the Universite de Sherbrooke nor the
00013       names of its contributors may be used to endorse or promote products
00014       derived from this software without specific prior written permission.
00015 
00016 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00017 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00018 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00019 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY
00020 DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00021 (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00022 LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00023 ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00024 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00025 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
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         // empty constructor
00052         SensorData();
00053 
00054         // Appearance-only constructor
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         // Mono constructor
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         // RGB-D constructor
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         // RGB-D constructor + laser scan
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         // Multi-cameras RGB-D constructor
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         // Multi-cameras RGB-D constructor + laser scan
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         // Stereo constructor
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         // Stereo constructor + laser scan
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         // IMU constructor
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         //for convenience
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); // only set raw
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         // detect automatically if raw or compressed. If raw, the data will be compressed.
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         // remove raw occupancy grids
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; // Return memory usage in Bytes
00251         void clearCompressedData() {_imageCompressed=cv::Mat(); _depthOrRightCompressed=cv::Mat(); _laserScanCompressed.clear(); _userDataCompressed=cv::Mat();}
00252 
00253         bool isPointVisibleFromCameras(const cv::Point3f & pt) const; // assuming point is in robot frame
00254 
00255 private:
00256         int _id;
00257         double _stamp;
00258 
00259         cv::Mat _imageCompressed;          // compressed image
00260         cv::Mat _depthOrRightCompressed;   // compressed image
00261         LaserScan _laserScanCompressed;      // compressed data
00262 
00263         cv::Mat _imageRaw;          // CV_8UC1 or CV_8UC3
00264         cv::Mat _depthOrRightRaw;   // depth CV_16UC1 or CV_32FC1, right image CV_8UC1
00265         LaserScan _laserScanRaw;
00266 
00267         std::vector<CameraModel> _cameraModels;
00268         StereoCameraModel _stereoCameraModel;
00269 
00270         // user data
00271         cv::Mat _userDataCompressed;      // compressed data
00272         cv::Mat _userDataRaw;
00273 
00274         // occupancy grid
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         // features
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_; // 6x6 double
00293 
00294         GPS gps_;
00295 
00296         IMU imu_;
00297 };
00298 
00299 }
00300 
00301 
00302 #endif /* SENSORDATA_H_ */


rtabmap
Author(s): Mathieu Labbe
autogenerated on Thu Jun 6 2019 21:59:22