Main MRPT website > C++ reference for MRPT 1.5.3
obs/CObservation3DRangeScan.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | http://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2017, Individual contributors, see AUTHORS file |
6  | See: http://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See details in http://www.mrpt.org/License |
8  +---------------------------------------------------------------------------+ */
9 #ifndef CObservation3DRangeScan_H
10 #define CObservation3DRangeScan_H
11 
13 #include <mrpt/utils/CImage.h>
14 #include <mrpt/obs/CObservation.h>
17 #include <mrpt/poses/CPose3D.h>
18 #include <mrpt/poses/CPose2D.h>
19 #include <mrpt/math/CPolygon.h>
20 #include <mrpt/math/CMatrix.h>
21 #include <mrpt/utils/TEnumType.h>
22 #include <mrpt/utils/adapters.h>
25 
26 namespace mrpt
27 {
28 namespace obs
29 {
30  /** Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto() */
32  {
33  bool takeIntoAccountSensorPoseOnRobot; //!< (Default: false) If false, local (sensor-centric) coordinates of points are generated. Otherwise, points are transformed with \a sensorPose. Furthermore, if provided, those coordinates are transformed with \a robotPoseInTheWorld
34  const mrpt::poses::CPose3D *robotPoseInTheWorld; //!< (Default: NULL) Read takeIntoAccountSensorPoseOnRobot
35  bool PROJ3D_USE_LUT; //!< (Default:true) [Only used when `range_is_depth`=true] Whether to use a Look-up-table (LUT) to speed up the conversion. It's thread safe in all situations <b>except</b> when you call this method from different threads <b>and</b> with different camera parameter matrices. In all other cases, it is a good idea to left it enabled.
36  bool USE_SSE2; //!< (Default:true) If possible, use SSE2 optimized code.
37  bool MAKE_DENSE; //!< (Default:true) set to false if you want to preserve the organization of the point cloud
38  T3DPointsProjectionParams() : takeIntoAccountSensorPoseOnRobot(false), robotPoseInTheWorld(NULL), PROJ3D_USE_LUT(true),USE_SSE2(true), MAKE_DENSE(true)
39  {}
40  };
41  /** Used in CObservation3DRangeScan::convertTo2DScan() */
43  {
44  std::string sensorLabel; //!< The sensor label that will have the newly created observation.
45  double angle_sup, angle_inf; //!< (Default=5 degrees) [Only if use_origin_sensor_pose=false] The upper & lower half-FOV angle (in radians).
46  double z_min,z_max; //!< (Default:-inf, +inf) [Only if use_origin_sensor_pose=true] Only obstacle points with Z coordinates within the range [z_min,z_max] will be taken into account.
47  double oversampling_ratio; //!< (Default=1.2=120%) How many more laser scans rays to create (read docs for CObservation3DRangeScan::convertTo2DScan()).
48 
49  /** (Default:false) If `false`, the conversion will be such that the 2D observation pose on the robot coincides with that in the original 3D range scan.
50  * If `true`, the sensed points will be "reprojected" as seen from a sensor pose at the robot/vehicle frame origin (and angle_sup, angle_inf will be ignored) */
52 
54  };
55 
56  namespace detail {
57  // Implemented in CObservation3DRangeScan_project3D_impl.h
58  template <class POINTMAP>
59  void project3DPointsFromDepthImageInto(mrpt::obs::CObservation3DRangeScan & src_obs,POINTMAP & dest_pointcloud, const mrpt::obs::T3DPointsProjectionParams & projectParams, const mrpt::obs::TRangeImageFilterParams &filterParams);
60  }
61 
63 
64  /** Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
65  *
66  * This kind of observations can carry one or more of these data fields:
67  * - 3D point cloud (as float's).
68  * - Each 3D point has its associated (u,v) pixel coordinates in \a points3D_idxs_x & \a points3D_idxs_y (New in MRPT 1.4.0)
69  * - 2D range image (as a matrix): Each entry in the matrix "rangeImage(ROW,COLUMN)" contains a distance or a depth (in meters), depending on \a range_is_depth.
70  * - 2D intensity (grayscale or RGB) image (as a mrpt::utils::CImage): For SwissRanger cameras, a logarithmic A-law compression is used to convert the original 16bit intensity to a more standard 8bit graylevel.
71  * - 2D confidence image (as a mrpt::utils::CImage): For each pixel, a 0x00 and a 0xFF mean the lowest and highest confidence levels, respectively.
72  * - Semantic labels: Stored as a matrix of bitfields, each bit having a user-defined meaning.
73  *
74  * The coordinates of the 3D point cloud are in meters with respect to the depth camera origin of coordinates
75  * (in SwissRanger, the front face of the camera: a small offset ~1cm in front of the physical focal point),
76  * with the +X axis pointing forward, +Y pointing left-hand and +Z pointing up. By convention, a 3D point with its coordinates set to (0,0,0), will be considered as invalid.
77  * The field CObservation3DRangeScan::relativePoseIntensityWRTDepth describes the change of coordinates from
78  * the depth camera to the intensity (RGB or grayscale) camera. In a SwissRanger camera both cameras coincide,
79  * so this pose is just a rotation (0,0,0,-90deg,0,-90deg). But in
80  * Microsoft Kinect there is also an offset, as shown in this figure:
81  *
82  * <div align=center>
83  * <img src="CObservation3DRangeScan_figRefSystem.png">
84  * </div>
85  *
86  * In any case, check the field \a relativePoseIntensityWRTDepth, or the method \a doDepthAndIntensityCamerasCoincide()
87  * to determine if both frames of reference coincide, since even for Kinect cameras both can coincide if the images
88  * have been rectified.
89  *
90  * The 2D images and matrices are stored as common images, with an up->down rows order and left->right, as usual.
91  * Optionally, the intensity and confidence channels can be set to delayed-load images for off-rawlog storage so it saves
92  * memory by having loaded in memory just the needed images. See the methods load() and unload().
93  * Due to the intensive storage requirements of this kind of observations, this observation is the only one in MRPT
94  * for which it's recommended to always call "load()" and "unload()" before and after using the observation, *ONLY* when
95  * the observation was read from a rawlog dataset, in order to make sure that all the externally stored data fields are
96  * loaded and ready in memory.
97  *
98  * Classes that grab observations of this type are:
99  * - mrpt::hwdrivers::CSwissRanger3DCamera
100  * - mrpt::hwdrivers::CKinect
101  *
102  * There are two sets of calibration parameters (see mrpt::vision::checkerBoardStereoCalibration() or the ready-to-use GUI program <a href="http://www.mrpt.org/Application:kinect-calibrate" >kinect-calibrate</a>):
103  * - cameraParams: Projection parameters of the depth camera.
104  * - cameraParamsIntensity: Projection parameters of the intensity (gray-level or RGB) camera.
105  *
106  * In some cameras, like SwissRanger, both are the same. It is possible in Kinect to rectify the range images such both cameras
107  * seem to coincide and then both sets of camera parameters will be identical.
108  *
109  * Range data can be interpreted in two different ways depending on the 3D camera (this field is already set to the
110  * correct setting when grabbing observations from an mrpt::hwdrivers sensor):
111  * - range_is_depth=true -> Kinect-like ranges: entries of \a rangeImage are distances along the +X axis
112  * - range_is_depth=false -> Ranges in \a rangeImage are actual distances in 3D.
113  *
114  * The "intensity" channel may come from different channels in sesnsors as Kinect. Look at field \a intensityImageChannel to
115  * find out if the image was grabbed from the visible (RGB) or IR channels.
116  *
117  * 3D point clouds can be generated at any moment after grabbing with CObservation3DRangeScan::project3DPointsFromDepthImage() and CObservation3DRangeScan::project3DPointsFromDepthImageInto(), provided the correct
118  * calibration parameters. Note that project3DPointsFromDepthImage() will store the point cloud in sensor-centric local coordinates. Use project3DPointsFromDepthImageInto() to directly obtain vehicle or world coordinates.
119  *
120  * Example of how to assign labels to pixels (for object segmentation, semantic information, etc.):
121  *
122  * \code
123  * // Assume obs of type CObservation3DRangeScanPtr
124  * obs->pixelLabels = CObservation3DRangeScan::TPixelLabelInfoPtr( new CObservation3DRangeScan::TPixelLabelInfo<NUM_BYTES>() );
125  * obs->pixelLabels->setSize(ROWS,COLS);
126  * obs->pixelLabels->setLabel(col,row, label_idx); // label_idxs = [0,2^NUM_BYTES-1]
127  * //...
128  * \endcode
129  *
130  * \note Starting at serialization version 2 (MRPT 0.9.1+), the confidence channel is stored as an image instead of a matrix to optimize memory and disk space.
131  * \note Starting at serialization version 3 (MRPT 0.9.1+), the 3D point cloud and the rangeImage can both be stored externally to save rawlog space.
132  * \note Starting at serialization version 5 (MRPT 0.9.5+), the new field \a range_is_depth
133  * \note Starting at serialization version 6 (MRPT 0.9.5+), the new field \a intensityImageChannel
134  * \note Starting at serialization version 7 (MRPT 1.3.1+), new fields for semantic labeling
135  * \note Since MRPT 1.5.0, external files format can be selected at runtime with `CObservation3DRangeScan::EXTERNALS_AS_TEXT`
136  *
137  * \sa mrpt::hwdrivers::CSwissRanger3DCamera, mrpt::hwdrivers::CKinect, CObservation
138  * \ingroup mrpt_obs_grp
139  */
141  {
142  // This must be added to any CSerializable derived class:
144 
145  protected:
146  bool m_points3D_external_stored; //!< If set to true, m_points3D_external_file is valid.
147  std::string m_points3D_external_file; //!< 3D points are in CImage::IMAGES_PATH_BASE+<this_file_name>
148 
149  bool m_rangeImage_external_stored; //!< If set to true, m_rangeImage_external_file is valid.
150  std::string m_rangeImage_external_file; //!< rangeImage is in CImage::IMAGES_PATH_BASE+<this_file_name>
151 
152  public:
153  CObservation3DRangeScan( ); //!< Default constructor
154  virtual ~CObservation3DRangeScan( ); //!< Destructor
155 
156  /** @name Delayed-load manual control methods.
157  @{ */
158  /** Makes sure all images and other fields which may be externally stored are loaded in memory.
159  * Note that for all CImages, calling load() is not required since the images will be automatically loaded upon first access, so load() shouldn't be needed to be called in normal cases by the user.
160  * If all the data were alredy loaded or this object has no externally stored data fields, calling this method has no effects.
161  * \sa unload
162  */
163  virtual void load() const MRPT_OVERRIDE;
164  /** Unload all images, for the case they being delayed-load images stored in external files (othewise, has no effect).
165  * \sa load
166  */
167  virtual void unload() MRPT_OVERRIDE;
168  /** @} */
169 
170  /** Project the RGB+D images into a 3D point cloud (with color if the target map supports it) and optionally at a given 3D pose.
171  * The 3D point coordinates are computed from the depth image (\a rangeImage) and the depth camera camera parameters (\a cameraParams).
172  * There exist two set of formulas for projecting the i'th point, depending on the value of "range_is_depth".
173  * In all formulas below, "rangeImage" is the matrix of ranges and the pixel coordinates are (r,c).
174  *
175  * 1) [range_is_depth=true] With "range equals depth" or "Kinect-like depth mode": the range values
176  * are in fact distances along the "+X" axis, not real 3D ranges (this is the way Kinect reports ranges):
177  *
178  * \code
179  * x(i) = rangeImage(r,c)
180  * y(i) = (r_cx - c) * x(i) / r_fx
181  * z(i) = (r_cy - r) * x(i) / r_fy
182  * \endcode
183  *
184  *
185  * 2) [range_is_depth=false] With "normal ranges": range means distance in 3D. This must be set when
186  * processing data from the SwissRange 3D camera, among others.
187  *
188  * \code
189  * Ky = (r_cx - c)/r_fx
190  * Kz = (r_cy - r)/r_fy
191  *
192  * x(i) = rangeImage(r,c) / sqrt( 1 + Ky^2 + Kz^2 )
193  * y(i) = Ky * x(i)
194  * z(i) = Kz * x(i)
195  * \endcode
196  *
197  * The color of each point is determined by projecting the 3D local point into the RGB image using \a cameraParamsIntensity.
198  *
199  * By default the local (sensor-centric) coordinates of points are directly stored into the local map, but if indicated so in \a takeIntoAccountSensorPoseOnRobot
200  * the points are transformed with \a sensorPose. Furthermore, if provided, those coordinates are transformed with \a robotPoseInTheWorld
201  *
202  * \tparam POINTMAP Supported maps are all those covered by mrpt::utils::PointCloudAdapter (mrpt::maps::CPointsMap and derived, mrpt::opengl::CPointCloudColoured, PCL point clouds,...)
203  *
204  * \note In MRPT < 0.9.5, this method always assumes that ranges were in Kinect-like format.
205  */
206  template <class POINTMAP>
207  inline void project3DPointsFromDepthImageInto(POINTMAP & dest_pointcloud, const T3DPointsProjectionParams & projectParams, const TRangeImageFilterParams &filterParams = TRangeImageFilterParams() ) {
208  detail::project3DPointsFromDepthImageInto<POINTMAP>(*this,dest_pointcloud,projectParams, filterParams);
209  }
210 
211  template <class POINTMAP>
212  MRPT_DEPRECATED("DEPRECATED: Use the other method signature with structured parameters instead.")
214  POINTMAP & dest_pointcloud,
215  const bool takeIntoAccountSensorPoseOnRobot,
216  const mrpt::poses::CPose3D *robotPoseInTheWorld=NULL,
217  const bool PROJ3D_USE_LUT=true,
218  const mrpt::math::CMatrix * rangeMask_min = NULL
219  )
220  {
222  pp.takeIntoAccountSensorPoseOnRobot = takeIntoAccountSensorPoseOnRobot;
223  pp.robotPoseInTheWorld = robotPoseInTheWorld;
224  pp.PROJ3D_USE_LUT = PROJ3D_USE_LUT;
226  fp.rangeMask_min=rangeMask_min;
227  detail::project3DPointsFromDepthImageInto<POINTMAP>(*this,dest_pointcloud,pp,fp);
228  }
229 
230  /** This method is equivalent to \c project3DPointsFromDepthImageInto() storing the projected 3D points (without color, in local sensor-centric coordinates) in this same class.
231  * For new code it's recommended to use instead \c project3DPointsFromDepthImageInto() which is much more versatile. */
232  inline void project3DPointsFromDepthImage(const bool PROJ3D_USE_LUT=true) {
235  p.PROJ3D_USE_LUT = PROJ3D_USE_LUT;
236  this->project3DPointsFromDepthImageInto(*this,p);
237  }
238 
239  /** Convert this 3D observation into an "equivalent 2D fake laser scan", with a configurable vertical FOV.
240  *
241  * The result is a 2D laser scan with more "rays" (N) than columns has the 3D observation (W), exactly: N = W * oversampling_ratio.
242  * This oversampling is required since laser scans sample the space at evenly-separated angles, while
243  * a range camera follows a tangent-like distribution. By oversampling we make sure we don't leave "gaps" unseen by the virtual "2D laser".
244  *
245  * All obstacles within a frustum are considered and the minimum distance is kept in each direction.
246  * The horizontal FOV of the frustum is automatically computed from the intrinsic parameters, but the
247  * vertical FOV must be provided by the user, and can be set to be assymetric which may be useful
248  * depending on the zone of interest where to look for obstacles.
249  *
250  * All spatial transformations are riguorosly taken into account in this class, using the depth camera
251  * intrinsic calibration parameters.
252  *
253  * The timestamp of the new object is copied from the 3D object.
254  * Obviously, a requisite for calling this method is the 3D observation having range data,
255  * i.e. hasRangeImage must be true. It's not needed to have RGB data nor the raw 3D point clouds
256  * for this method to work.
257  *
258  * If `scanParams.use_origin_sensor_pose` is `true`, the points will be projected to 3D and then reprojected
259  * as seen from a different sensorPose at the vehicle frame origin. Otherwise (the default), the output 2D observation will share the sensorPose of the input 3D scan
260  * (using a more efficient algorithm that avoids trigonometric functions).
261  *
262  * \param[out] out_scan2d The resulting 2D equivalent scan.
263  *
264  * \sa The example in http://www.mrpt.org/tutorials/mrpt-examples/example-kinect-to-2d-laser-demo/
265  */
266  void convertTo2DScan(mrpt::obs::CObservation2DRangeScan & out_scan2d, const T3DPointsTo2DScanParams &scanParams, const TRangeImageFilterParams &filterParams = TRangeImageFilterParams() );
267 
268  MRPT_DEPRECATED("DEPRECATED: Use the other method signature with structured parameters instead.")
269  void convertTo2DScan(
271  const std::string & sensorLabel,
272  const double angle_sup = mrpt::utils::DEG2RAD(5),
273  const double angle_inf = mrpt::utils::DEG2RAD(5),
274  const double oversampling_ratio = 1.2,
275  const mrpt::math::CMatrix * rangeMask_min = NULL
276  );
277 
278  /** Whether external files (3D points, range and confidence) are to be
279  * saved as `.txt` text files (MATLAB compatible) or `*.bin` binary (faster).
280  * Loading always will determine the type by inspecting the file extension.
281  * \note Default=false
282  **/
283  static bool EXTERNALS_AS_TEXT;
284 
285  /** \name Point cloud
286  * @{ */
287  bool hasPoints3D; //!< true means the field points3D contains valid data.
288  std::vector<float> points3D_x,points3D_y,points3D_z; //!< If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera. \sa resizePoints3DVectors
289  std::vector<uint16_t> points3D_idxs_x, points3D_idxs_y; //!< //!< If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point in \a points3D_x, points3D_y, points3D_z
290 
291  /** Use this method instead of resizing all three \a points3D_x, \a points3D_y & \a points3D_z to allow the usage of the internal memory pool. */
292  void resizePoints3DVectors(const size_t nPoints);
293  /** @} */
294 
295  /** \name Point cloud external storage functions
296  * @{ */
297  inline bool points3D_isExternallyStored() const { return m_points3D_external_stored; }
298  inline std::string points3D_getExternalStorageFile() const { return m_points3D_external_file; }
299  void points3D_getExternalStorageFileAbsolutePath(std::string &out_path) const;
300  inline std::string points3D_getExternalStorageFileAbsolutePath() const {
301  std::string tmp;
302  points3D_getExternalStorageFileAbsolutePath(tmp);
303  return tmp;
304  }
305  void points3D_convertToExternalStorage( const std::string &fileName, const std::string &use_this_base_dir ); //!< Users won't normally want to call this, it's only used from internal MRPT programs. \sa EXTERNALS_AS_TEXT
306  /** @} */
307 
308  /** \name Range (depth) image
309  * @{ */
310  bool hasRangeImage; //!< true means the field rangeImage contains valid data
311  mrpt::math::CMatrix rangeImage; //!< If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) \sa range_is_depth
312  bool range_is_depth; //!< true: Kinect-like ranges: entries of \a rangeImage are distances along the +X axis; false: Ranges in \a rangeImage are actual distances in 3D.
313 
314  void rangeImage_setSize(const int HEIGHT, const int WIDTH); //!< Similar to calling "rangeImage.setSize(H,W)" but this method provides memory pooling to speed-up the memory allocation.
315  /** @} */
316 
317  /** \name Range Matrix external storage functions
318  * @{ */
319  inline bool rangeImage_isExternallyStored() const { return m_rangeImage_external_stored; }
320  inline std::string rangeImage_getExternalStorageFile() const { return m_rangeImage_external_file; }
321  void rangeImage_getExternalStorageFileAbsolutePath(std::string &out_path) const;
323  std::string tmp;
324  rangeImage_getExternalStorageFileAbsolutePath(tmp);
325  return tmp;
326  }
327  void rangeImage_convertToExternalStorage( const std::string &fileName, const std::string &use_this_base_dir ); //!< Users won't normally want to call this, it's only used from internal MRPT programs. \sa EXTERNALS_AS_TEXT
328  /** Forces marking this observation as non-externally stored - it doesn't anything else apart from reseting the corresponding flag (Users won't normally want to call this, it's only used from internal MRPT programs) */
329  void rangeImage_forceResetExternalStorage() { m_rangeImage_external_stored=false; }
330  /** @} */
331 
332 
333  /** \name Intensity (RGB) channels
334  * @{ */
335  /** Enum type for intensityImageChannel */
337  {
338  CH_VISIBLE = 0, //!< Grayscale or RGB visible channel of the camera sensor.
339  CH_IR = 1 //!< Infrarred (IR) channel
340  };
341 
342  bool hasIntensityImage; //!< true means the field intensityImage contains valid data
343  mrpt::utils::CImage intensityImage; //!< If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"
344  TIntensityChannelID intensityImageChannel; //!< The source of the intensityImage; typically the visible channel \sa TIntensityChannelID
345  /** @} */
346 
347  /** \name Confidence "channel"
348  * @{ */
349  bool hasConfidenceImage; //!< true means the field confidenceImage contains valid data
350  mrpt::utils::CImage confidenceImage; //!< If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the capture drivers.
351  /** @} */
352 
353  /** \name Pixel-wise classification labels (for semantic labeling, etc.)
354  * @{ */
355  /** Returns true if the field CObservation3DRangeScan::pixelLabels contains a non-NULL smart pointer.
356  * To enhance a 3D point cloud with labeling info, just assign an appropiate object to \a pixelLabels
357  */
358  bool hasPixelLabels() const { return pixelLabels.present(); }
359 
360  /** Virtual interface to all pixel-label information structs. See CObservation3DRangeScan::pixelLabels */
362  {
363  typedef std::map<uint32_t,std::string> TMapLabelID2Name;
364 
365  /** The 'semantic' or human-friendly name of the i'th bit in pixelLabels(r,c) can be found in pixelLabelNames[i] as a std::string */
366  TMapLabelID2Name pixelLabelNames;
367 
368  const std::string & getLabelName(unsigned int label_idx) const {
369  std::map<uint32_t,std::string>::const_iterator it = pixelLabelNames.find(label_idx);
370  if (it==pixelLabelNames.end()) throw std::runtime_error("Error: label index has no defined name");
371  return it->second;
372  }
373  void setLabelName(unsigned int label_idx, const std::string &name) { pixelLabelNames[label_idx]=name; }
374  /** Check the existence of a label by returning its associated index.
375  * -1 if it does not exist. */
376  int checkLabelNameExistence(const std::string &name) const {
378  for ( it = pixelLabelNames.begin() ; it != pixelLabelNames.end(); it++ )
379  if ( it->second == name )
380  return it->first;
381  return -1;
382  }
383 
384  /** Resizes the matrix pixelLabels to the given size, setting all bitfields to zero (that is, all pixels are assigned NONE category). */
385  virtual void setSize(const int NROWS, const int NCOLS) =0;
386  /** Mark the pixel(row,col) as classified in the category \a label_idx, which may be in the range 0 to MAX_NUM_LABELS-1
387  * Note that 0 is a valid label index, it does not mean "no label" \sa unsetLabel, unsetAll */
388  virtual void setLabel(const int row, const int col, uint8_t label_idx) =0;
389  virtual void getLabels( const int row, const int col, uint8_t &labels ) =0;
390  /** For the pixel(row,col), removes its classification into the category \a label_idx, which may be in the range 0 to 7
391  * Note that 0 is a valid label index, it does not mean "no label" \sa setLabel, unsetAll */
392  virtual void unsetLabel(const int row, const int col, uint8_t label_idx)=0;
393  /** Removes all categories for pixel(row,col) \sa setLabel, unsetLabel */
394  virtual void unsetAll(const int row, const int col, uint8_t label_idx) =0;
395  /** Checks whether pixel(row,col) has been clasified into category \a label_idx, which may be in the range 0 to 7
396  * \sa unsetLabel, unsetAll */
397  virtual bool checkLabel(const int row, const int col, uint8_t label_idx) const =0;
398 
399  void writeToStream(mrpt::utils::CStream &out) const;
400  static TPixelLabelInfoBase* readAndBuildFromStream(mrpt::utils::CStream &in);
401 
402  /// std stream interface
403  friend std::ostream& operator<<( std::ostream& out, const TPixelLabelInfoBase& obj ){
404  obj.Print( out );
405  return out;
406  }
407 
408  TPixelLabelInfoBase(unsigned int BITFIELD_BYTES_) :
409  BITFIELD_BYTES (BITFIELD_BYTES_)
410  {
411  }
412 
413  virtual ~TPixelLabelInfoBase() {}
414 
415  const uint8_t BITFIELD_BYTES; //!< Minimum number of bytes required to hold MAX_NUM_DIFFERENT_LABELS bits.
416 
417  protected:
418  virtual void internal_readFromStream(mrpt::utils::CStream &in) = 0;
419  virtual void internal_writeToStream(mrpt::utils::CStream &out) const = 0;
420  virtual void Print( std::ostream& ) const =0;
421  };
422  typedef stlplus::smart_ptr<TPixelLabelInfoBase> TPixelLabelInfoPtr; //!< Used in CObservation3DRangeScan::pixelLabels
423 
424  template <unsigned int BYTES_REQUIRED_>
426  {
427  enum {
428  BYTES_REQUIRED = BYTES_REQUIRED_ // ((MAX_LABELS-1)/8)+1
429  };
430 
431  /** Automatically-determined integer type of the proper size such that all labels fit as one bit (max: 64) */
433 
434  /** Each pixel may be assigned between 0 and MAX_NUM_LABELS-1 'labels' by
435  * setting to 1 the corresponding i'th bit [0,MAX_NUM_LABELS-1] in the byte in pixelLabels(r,c).
436  * That is, each pixel is assigned an 8*BITFIELD_BYTES bit-wide bitfield of possible categories.
437  * \sa hasPixelLabels
438  */
439  typedef Eigen::Matrix<bitmask_t,Eigen::Dynamic,Eigen::Dynamic> TPixelLabelMatrix;
440  TPixelLabelMatrix pixelLabels;
441 
442  void setSize(const int NROWS, const int NCOLS) MRPT_OVERRIDE {
443  pixelLabels = TPixelLabelMatrix::Zero(NROWS,NCOLS);
444  }
445  void setLabel(const int row, const int col, uint8_t label_idx) MRPT_OVERRIDE {
446  pixelLabels(row,col) |= static_cast<bitmask_t>(1) << label_idx;
447  }
448  void getLabels( const int row, const int col, uint8_t &labels ) MRPT_OVERRIDE
449  {
450  labels = pixelLabels(row,col);
451  }
452 
453  void unsetLabel(const int row, const int col, uint8_t label_idx) MRPT_OVERRIDE {
454  pixelLabels(row,col) &= ~(static_cast<bitmask_t>(1) << label_idx);
455  }
456  void unsetAll(const int row, const int col, uint8_t label_idx) MRPT_OVERRIDE {
457  pixelLabels(row,col) = 0;
458  }
459  bool checkLabel(const int row, const int col, uint8_t label_idx) const MRPT_OVERRIDE {
460  return (pixelLabels(row,col) & (static_cast<bitmask_t>(1) << label_idx)) != 0;
461  }
462 
463  // Ctor: pass identification to parent for deserialization
465  {
466  }
467 
468  protected:
470  {
471  {
472  uint32_t nR,nC;
473  in >> nR >> nC;
474  pixelLabels.resize(nR,nC);
475  for (uint32_t c=0;c<nC;c++)
476  for (uint32_t r=0;r<nR;r++)
477  in >> pixelLabels.coeffRef(r,c);
478  }
479  in >> pixelLabelNames;
480  }
482  {
483  {
484  const uint32_t nR=static_cast<uint32_t>(pixelLabels.rows());
485  const uint32_t nC=static_cast<uint32_t>(pixelLabels.cols());
486  out << nR << nC;
487  for (uint32_t c=0;c<nC;c++)
488  for (uint32_t r=0;r<nR;r++)
489  out << pixelLabels.coeff(r,c);
490  }
491  out << pixelLabelNames;
492  }
493  void Print( std::ostream& out ) const MRPT_OVERRIDE
494  {
495  {
496  const uint32_t nR=static_cast<uint32_t>(pixelLabels.rows());
497  const uint32_t nC=static_cast<uint32_t>(pixelLabels.cols());
498  out << "Number of rows: " << nR << std::endl;
499  out << "Number of cols: " << nC << std::endl;
500  out << "Matrix of labels: " << std::endl;
501  for (uint32_t c=0;c<nC;c++)
502  {
503  for (uint32_t r=0;r<nR;r++)
504  out << pixelLabels.coeff(r,c) << " ";
505 
506  out << std::endl;
507  }
508  }
509  out << std::endl;
510  out << "Label indices and names: " << std::endl;
512  for ( it = pixelLabelNames.begin(); it != pixelLabelNames.end(); it++)
513  out << it->first << " " << it->second << std::endl;
514  }
515  }; // end TPixelLabelInfo
516 
517  /** All information about pixel labeling is stored in this (smart pointer to) structure; refer to TPixelLabelInfo for details on the contents
518  * User is responsible of creating a new object of the desired data type. It will be automatically (de)serialized no matter its specific type. */
519  TPixelLabelInfoPtr pixelLabels;
520 
521  /** @} */
522 
523  /** \name Sensor parameters
524  * @{ */
525  mrpt::utils::TCamera cameraParams; //!< Projection parameters of the depth camera.
526  mrpt::utils::TCamera cameraParamsIntensity; //!< Projection parameters of the intensity (graylevel or RGB) camera.
527 
528  /** Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this observation).
529  * In a SwissRanger camera, this will be (0,0,0,-90deg,0,-90deg) since both cameras coincide.
530  * In a Kinect, this will include a small lateral displacement and a rotation, according to the drawing on the top of this page.
531  * \sa doDepthAndIntensityCamerasCoincide
532  */
534 
535  /** Return true if \a relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
536  * \sa relativePoseIntensityWRTDepth
537  */
538  bool doDepthAndIntensityCamerasCoincide() const;
539 
540  float maxRange; //!< The maximum range allowed by the device, in meters (e.g. 8.0m, 5.0m,...)
541  mrpt::poses::CPose3D sensorPose; //!< The 6D pose of the sensor on the robot.
542  float stdError; //!< The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid.
543 
544  // See base class docs
545  void getSensorPose( mrpt::poses::CPose3D &out_sensorPose ) const MRPT_OVERRIDE { out_sensorPose = sensorPose; }
546  // See base class docs
547  void setSensorPose( const mrpt::poses::CPose3D &newSensorPose ) MRPT_OVERRIDE { sensorPose = newSensorPose; }
548 
549  /** @} */ // end sensor params
550 
551  // See base class docs
552  void getDescriptionAsText(std::ostream &o) const MRPT_OVERRIDE;
553 
554  void swap(CObservation3DRangeScan &o); //!< Very efficient method to swap the contents of two observations.
555  /** Extract a ROI of the 3D observation as a new one. \note PixelLabels are *not* copied to the output subimage. */
556  void getZoneAsObs( CObservation3DRangeScan &obs, const unsigned int &r1, const unsigned int &r2, const unsigned int &c1, const unsigned int &c2 );
557 
558  /** A Levenberg-Marquart-based optimizer to recover the calibration parameters of a 3D camera given a range (depth) image and the corresponding 3D point cloud.
559  * \param camera_offset The offset (in meters) in the +X direction of the point cloud. It's 1cm for SwissRanger SR4000.
560  * \return The final average reprojection error per pixel (typ <0.05 px)
561  */
562  static double recoverCameraCalibrationParameters(
563  const CObservation3DRangeScan &in_obs,
564  mrpt::utils::TCamera &out_camParams,
565  const double camera_offset = 0.01 );
566 
567  /** Look-up-table struct for project3DPointsFromDepthImageInto() */
569  {
572  };
573  static TCached3DProjTables m_3dproj_lut; //!< 3D point cloud projection look-up-table \sa project3DPointsFromDepthImage
574 
575  }; // End of class def.
577 
578 
579  } // End of namespace
580 
581  namespace utils
582  {
583  // Specialization must occur in the same namespace
585 
586  // Enum <-> string converter:
587  template <>
589  {
591  static void fill(bimap<enum_t,std::string> &m_map)
592  {
595  }
596  };
597  }
598 
599  namespace utils
600  {
601  /** Specialization mrpt::utils::PointCloudAdapter<CObservation3DRangeScan> \ingroup mrpt_adapters_grp */
602  template <>
603  class PointCloudAdapter< mrpt::obs::CObservation3DRangeScan> : public detail::PointCloudAdapterHelperNoRGB<mrpt::obs::CObservation3DRangeScan,float>
604  {
605  private:
607  public:
608  typedef float coords_t; //!< The type of each point XYZ coordinates
609  static const int HAS_RGB = 0; //!< Has any color RGB info?
610  static const int HAS_RGBf = 0; //!< Has native RGB info (as floats)?
611  static const int HAS_RGBu8 = 0; //!< Has native RGB info (as uint8_t)?
612 
613  /** Constructor (accept a const ref for convenience) */
614  inline PointCloudAdapter(const mrpt::obs::CObservation3DRangeScan &obj) : m_obj(*const_cast<mrpt::obs::CObservation3DRangeScan*>(&obj)) { }
615  /** Get number of points */
616  inline size_t size() const { return m_obj.points3D_x.size(); }
617  /** Set number of points (to uninitialized values) */
618  inline void resize(const size_t N) {
619  if (N) m_obj.hasPoints3D = true;
620  m_obj.resizePoints3DVectors(N);
621  }
622 
623  /** Get XYZ coordinates of i'th point */
624  template <typename T>
625  inline void getPointXYZ(const size_t idx, T &x,T &y, T &z) const {
626  x=m_obj.points3D_x[idx];
627  y=m_obj.points3D_y[idx];
628  z=m_obj.points3D_z[idx];
629  }
630  /** Set XYZ coordinates of i'th point */
631  inline void setPointXYZ(const size_t idx, const coords_t x,const coords_t y, const coords_t z) {
632  m_obj.points3D_x[idx]=x;
633  m_obj.points3D_y[idx]=y;
634  m_obj.points3D_z[idx]=z;
635  }
636  /** Set XYZ coordinates of i'th point */
637  inline void setInvalidPoint(const size_t idx)
638  {
639  THROW_EXCEPTION("mrpt::obs::CObservation3DRangeScan requires needs to be dense");
640  }
641 
642  }; // end of PointCloudAdapter<CObservation3DRangeScan>
643  }
644 } // End of namespace
645 
647 
648 #endif
bool m_points3D_external_stored
If set to true, m_points3D_external_file is valid.
std::string m_rangeImage_external_file
rangeImage is in CImage::IMAGES_PATH_BASE+<this_file_name>
void setInvalidPoint(const size_t idx)
Set XYZ coordinates of i&#39;th point.
const std::string & getLabelName(unsigned int label_idx) const
TIntensityChannelID
Enum type for intensityImageChannel.
double DEG2RAD(const double x)
Degrees to radians.
const mrpt::poses::CPose3D * robotPoseInTheWorld
(Default: NULL) Read takeIntoAccountSensorPoseOnRobot
void resize(const size_t N)
Set number of points (to uninitialized values)
void setPointXYZ(const size_t idx, const coords_t x, const coords_t y, const coords_t z)
Set XYZ coordinates of i&#39;th point.
stlplus::smart_ptr< TPixelLabelInfoBase > TPixelLabelInfoPtr
Used in CObservation3DRangeScan::pixelLabels.
#define MRPT_OVERRIDE
C++11 "override" for virtuals:
mrpt::utils::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const MRPT_OVERRIDE
A general method to retrieve the sensor pose on the robot.
A helper base class for those PointCloudAdapter<> which do not handle RGB data; it declares needed in...
Definition: adapters.h:48
void setLabelName(unsigned int label_idx, const std::string &name)
A class for storing images as grayscale or RGB bitmaps.
Definition: CImage.h:101
TPixelLabelInfoPtr pixelLabels
All information about pixel labeling is stored in this (smart pointer to) structure; refer to TPixelL...
#define THROW_EXCEPTION(msg)
std::string sensorLabel
The sensor label that will have the newly created observation.
void project3DPointsFromDepthImage(const bool PROJ3D_USE_LUT=true)
This method is equivalent to project3DPointsFromDepthImageInto() storing the projected 3D points (wit...
void unsetLabel(const int row, const int col, uint8_t label_idx) MRPT_OVERRIDE
For the pixel(row,col), removes its classification into the category label_idx, which may be in the r...
double oversampling_ratio
(Default=1.2=120%) How many more laser scans rays to create (read docs for CObservation3DRangeScan::c...
bool m_rangeImage_external_stored
If set to true, m_rangeImage_external_file is valid.
Declares a class derived from "CObservation" that encapsules a 3D range scan measurement, as from a time-of-flight range camera or any other RGBD sensor.
const mrpt::math::CMatrix * rangeMask_min
(Default: NULL) If provided, each data range will be tested to be greater-than (rangeMask_min) or les...
Look-up-table struct for project3DPointsFromDepthImageInto()
Column vector, like Eigen::MatrixX*, but automatically initialized to zeros since construction...
Definition: eigen_frwds.h:35
void resizePoints3DVectors(const size_t nPoints)
Use this method instead of resizing all three points3D_x, points3D_y & points3D_z to allow the usage ...
Usage: uint_select_by_bytecount<N>::type var; allows defining var as a unsigned integer with...
PointCloudAdapter(const mrpt::obs::CObservation3DRangeScan &obj)
Constructor (accept a const ref for convenience)
Only specializations of this class are defined for each enum type of interest.
Definition: TEnumType.h:23
friend std::ostream & operator<<(std::ostream &out, const TPixelLabelInfoBase &obj)
std stream interface
const Scalar * const_iterator
Definition: eigen_plugins.h:24
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
std::vector< float > points3D_z
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
mrpt::math::CMatrix rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
mrpt::utils::TCamera cameraParams
Projection parameters of the depth camera.
const uint8_t BITFIELD_BYTES
Minimum number of bytes required to hold MAX_NUM_DIFFERENT_LABELS bits.
void internal_readFromStream(mrpt::utils::CStream &in) MRPT_OVERRIDE
This base class is used to provide a unified interface to files,memory buffers,..Please see the deriv...
Definition: CStream.h:38
std::string rangeImage_getExternalStorageFileAbsolutePath() const
void setLabel(const int row, const int col, uint8_t label_idx) MRPT_OVERRIDE
Mark the pixel(row,col) as classified in the category label_idx, which may be in the range 0 to MAX_N...
#define DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
This declaration must be inserted in all CSerializable classes definition, before the class declarati...
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
std::vector< uint16_t > points3D_idxs_y
//!< If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point in points3D_x, points3D_y, points3D_z
TMapLabelID2Name pixelLabelNames
The &#39;semantic&#39; or human-friendly name of the i&#39;th bit in pixelLabels(r,c) can be found in pixelLabelN...
void project3DPointsFromDepthImageInto(mrpt::obs::CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const mrpt::obs::T3DPointsProjectionParams &projectParams, const mrpt::obs::TRangeImageFilterParams &filterParams)
Used in CObservation3DRangeScan::convertTo2DScan()
int checkLabelNameExistence(const std::string &name) const
Check the existence of a label by returning its associated index.
A bidirectional version of std::map, declared as bimap<KEY,VALUE> and which actually contains two std...
Definition: bimap.h:28
std::string m_points3D_external_file
3D points are in CImage::IMAGES_PATH_BASE+<this_file_name>
This namespace contains representation of robot actions and observations.
EIGEN_STRONG_INLINE void setSize(size_t row, size_t col)
Changes the size of matrix, maintaining its previous content as possible and padding with zeros where...
bool hasRangeImage
true means the field rangeImage contains valid data
TIntensityChannelID intensityImageChannel
The source of the intensityImage; typically the visible channel.
void rangeImage_forceResetExternalStorage()
Forces marking this observation as non-externally stored - it doesn&#39;t anything else apart from reseti...
std::string points3D_getExternalStorageFileAbsolutePath() const
bool hasPoints3D
true means the field points3D contains valid data.
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
void internal_writeToStream(mrpt::utils::CStream &out) const MRPT_OVERRIDE
Eigen::Matrix< bitmask_t, Eigen::Dynamic, Eigen::Dynamic > TPixelLabelMatrix
Each pixel may be assigned between 0 and MAX_NUM_LABELS-1 &#39;labels&#39; by setting to 1 the corresponding ...
#define MRPT_DECLARE_TTYPENAME_PTR_NAMESPACE(_TYPE, __NS)
Definition: TTypeName.h:72
bool USE_SSE2
(Default:true) If possible, use SSE2 optimized code.
void Print(std::ostream &out) const MRPT_OVERRIDE
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
#define DEFINE_SERIALIZABLE(class_name)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:72
Declares a class that represents any robot&#39;s observation.
bool checkLabel(const int row, const int col, uint8_t label_idx) const MRPT_OVERRIDE
Checks whether pixel(row,col) has been clasified into category label_idx, which may be in the range 0...
static bool EXTERNALS_AS_TEXT
Whether external files (3D points, range and confidence) are to be saved as .txt text files (MATLAB c...
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
bool hasIntensityImage
true means the field intensityImage contains valid data
#define MRPT_DEPRECATED(msg)
Usage: MRPT_DEPRECATED("Use XX instead") void myFunc(double);.
x y t t *t x y t t t x y t t t x *y t *t t x *y t *t t x y t t t x y t t t x(y+z)
bool hasPixelLabels() const
Returns true if the field CObservation3DRangeScan::pixelLabels contains a non-NULL smart pointer...
virtual void Print(std::ostream &) const =0
mrpt::utils::uint_select_by_bytecount< BYTES_REQUIRED >::type bitmask_t
Automatically-determined integer type of the proper size such that all labels fit as one bit (max: 64...
bool PROJ3D_USE_LUT
(Default:true) [Only used when range_is_depth=true] Whether to use a Look-up-table (LUT) to speed up ...
mrpt::utils::CImage confidenceImage
If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the ca...
An adapter to different kinds of point cloud object.
Definition: adapters.h:38
void insert(const KEY &k, const VALUE &v)
Insert a new pair KEY<->VALUE in the bi-map.
Definition: bimap.h:69
void getPointXYZ(const size_t idx, T &x, T &y, T &z) const
Get XYZ coordinates of i&#39;th point.
bool MAKE_DENSE
(Default:true) set to false if you want to preserve the organization of the point cloud ...
mrpt::utils::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrix.h:30
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
#define DEFINE_SERIALIZABLE_POST_CUSTOM_BASE_LINKAGE(class_name, base_name, _LINKAGE_)
void getLabels(const int row, const int col, uint8_t &labels) MRPT_OVERRIDE
void unsetAll(const int row, const int col, uint8_t label_idx) MRPT_OVERRIDE
Removes all categories for pixel(row,col)
static TCached3DProjTables m_3dproj_lut
3D point cloud projection look-up-table
Virtual interface to all pixel-label information structs.
void setSize(const int NROWS, const int NCOLS) MRPT_OVERRIDE
Resizes the matrix pixelLabels to the given size, setting all bitfields to zero (that is...
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) MRPT_OVERRIDE
A general method to change the sensor pose on the robot.
Grayscale or RGB visible channel of the camera sensor.
bool takeIntoAccountSensorPoseOnRobot
(Default: false) If false, local (sensor-centric) coordinates of points are generated. Otherwise, points are transformed with sensorPose. Furthermore, if provided, those coordinates are transformed with robotPoseInTheWorld
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:31
bool use_origin_sensor_pose
(Default:false) If false, the conversion will be such that the 2D observation pose on the robot coinc...
float maxRange
The maximum range allowed by the device, in meters (e.g. 8.0m, 5.0m,...)
bool hasConfidenceImage
true means the field confidenceImage contains valid data



Page generated by Doxygen 1.8.13 for MRPT 1.5.3 at Tue Oct 31 07:27:35 UTC 2017