MRPT  1.9.9
CObservation3DRangeScan.h
Go to the documentation of this file.
1 /* +------------------------------------------------------------------------+
2  | Mobile Robot Programming Toolkit (MRPT) |
3  | https://www.mrpt.org/ |
4  | |
5  | Copyright (c) 2005-2019, Individual contributors, see AUTHORS file |
6  | See: https://www.mrpt.org/Authors - All rights reserved. |
7  | Released under BSD License. See: https://www.mrpt.org/License |
8  +------------------------------------------------------------------------+ */
9 #pragma once
10 
13 #include <mrpt/img/CImage.h>
14 #include <mrpt/math/CMatrixF.h>
15 #include <mrpt/math/CPolygon.h>
16 #include <mrpt/obs/CObservation.h>
20 #include <mrpt/poses/CPose2D.h>
21 #include <mrpt/poses/CPose3D.h>
25 
26 namespace mrpt
27 {
28 namespace obs
29 {
30 /** Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto() */
32 {
33  /** (Default: false) If false, local (sensor-centric) coordinates of points
34  * are generated. Otherwise, points are transformed with \a sensorPose.
35  * Furthermore, if provided, those coordinates are transformed with \a
36  * robotPoseInTheWorld */
38  /** (Default: nullptr) Read takeIntoAccountSensorPoseOnRobot */
40  /** (Default:true) [Only used when `range_is_depth`=true] Whether to use a
41  * Look-up-table (LUT) to speed up the conversion. It's thread safe in all
42  * situations <b>except</b> when you call this method from different threads
43  * <b>and</b> with different camera parameter matrices. In all other cases,
44  * it is a good idea to left it enabled. */
45  bool PROJ3D_USE_LUT{true};
46  /** (Default:true) If possible, use SSE2 optimized code. */
47  bool USE_SSE2{true};
48  /** (Default:false) set to true if you want an organized point cloud */
49  bool MAKE_ORGANIZED{false};
50 
51  /** (Default:1) If !=1, split the range image in blocks of DxD
52  * (D=decimation), and only generates one point per block, with the minimum
53  * valid range. */
55 
56  T3DPointsProjectionParams() = default;
57 };
58 /** Used in CObservation3DRangeScan::convertTo2DScan() */
60 {
61  /** The sensor label that will have the newly created observation. */
63  /** (Default=5 degrees) [Only if use_origin_sensor_pose=false] The upper &
64  * lower half-FOV angle (in radians). */
66  /** (Default:-inf, +inf) [Only if use_origin_sensor_pose=true] Only obstacle
67  * points with Z coordinates within the range [z_min,z_max] will be taken
68  * into account. */
69  double z_min, z_max;
70  /** (Default=1.2=120%) How many more laser scans rays to create (read docs
71  * for CObservation3DRangeScan::convertTo2DScan()). */
72  double oversampling_ratio{1.2};
73 
74  /** (Default:false) If `false`, the conversion will be such that the 2D
75  * observation pose on the robot coincides with that in the original 3D
76  * range scan.
77  * If `true`, the sensed points will be "reprojected" as seen from a sensor
78  * pose at the robot/vehicle frame origin (and angle_sup, angle_inf will be
79  * ignored) */
81 
83 };
84 
85 namespace detail
86 {
87 // Implemented in CObservation3DRangeScan_project3D_impl.h
88 template <class POINTMAP>
90  mrpt::obs::CObservation3DRangeScan& src_obs, POINTMAP& dest_pointcloud,
91  const mrpt::obs::T3DPointsProjectionParams& projectParams,
92  const mrpt::obs::TRangeImageFilterParams& filterParams);
93 } // namespace detail
94 
95 /** Declares a class derived from "CObservation" that encapsules a 3D range scan
96  *measurement, as from a time-of-flight range camera or any other RGBD sensor.
97  *
98  * This kind of observations can carry one or more of these data fields:
99  * - 3D point cloud (as float's).
100  * - Each 3D point has its associated (u,v) pixel coordinates in \a
101  *points3D_idxs_x & \a points3D_idxs_y (New in MRPT 1.4.0)
102  * - 2D range image (as a matrix): Each entry in the matrix
103  *"rangeImage(ROW,COLUMN)" contains a distance or a depth (in meters), depending
104  *on \a range_is_depth.
105  * - 2D intensity (grayscale or RGB) image (as a mrpt::img::CImage): For
106  *SwissRanger cameras, a logarithmic A-law compression is used to convert the
107  *original 16bit intensity to a more standard 8bit graylevel.
108  * - 2D confidence image (as a mrpt::img::CImage): For each pixel, a 0x00
109  *and a 0xFF mean the lowest and highest confidence levels, respectively.
110  * - Semantic labels: Stored as a matrix of bitfields, each bit having a
111  *user-defined meaning.
112  *
113  * The coordinates of the 3D point cloud are in meters with respect to the
114  *depth camera origin of coordinates
115  * (in SwissRanger, the front face of the camera: a small offset ~1cm in
116  *front of the physical focal point),
117  * with the +X axis pointing forward, +Y pointing left-hand and +Z pointing
118  *up. By convention, a 3D point with its coordinates set to (0,0,0), will be
119  *considered as invalid.
120  * The field CObservation3DRangeScan::relativePoseIntensityWRTDepth describes
121  *the change of coordinates from
122  * the depth camera to the intensity (RGB or grayscale) camera. In a
123  *SwissRanger camera both cameras coincide,
124  * so this pose is just a rotation (0,0,0,-90deg,0,-90deg). But in
125  * Microsoft Kinect there is also an offset, as shown in this figure:
126  *
127  * <div align=center>
128  * <img src="CObservation3DRangeScan_figRefSystem.png">
129  * </div>
130  *
131  * In any case, check the field \a relativePoseIntensityWRTDepth, or the method
132  *\a doDepthAndIntensityCamerasCoincide()
133  * to determine if both frames of reference coincide, since even for Kinect
134  *cameras both can coincide if the images
135  * have been rectified.
136  *
137  * The 2D images and matrices are stored as common images, with an up->down
138  *rows order and left->right, as usual.
139  * Optionally, the intensity and confidence channels can be set to
140  *delayed-load images for off-rawlog storage so it saves
141  * memory by having loaded in memory just the needed images. See the methods
142  *load() and unload().
143  * Due to the intensive storage requirements of this kind of observations, this
144  *observation is the only one in MRPT
145  * for which it's recommended to always call "load()" and "unload()" before
146  *and after using the observation, *ONLY* when
147  * the observation was read from a rawlog dataset, in order to make sure that
148  *all the externally stored data fields are
149  * loaded and ready in memory.
150  *
151  * Classes that grab observations of this type are:
152  * - mrpt::hwdrivers::CSwissRanger3DCamera
153  * - mrpt::hwdrivers::CKinect
154  * - mrpt::hwdrivers::COpenNI2Sensor
155  *
156  * There are two sets of calibration parameters (see
157  *mrpt::vision::checkerBoardStereoCalibration() or the ready-to-use GUI program
158  *<a href="http://www.mrpt.org/Application:kinect-calibrate"
159  *>kinect-calibrate</a>):
160  * - cameraParams: Projection parameters of the depth camera.
161  * - cameraParamsIntensity: Projection parameters of the intensity
162  *(gray-level or RGB) camera.
163  *
164  * In some cameras, like SwissRanger, both are the same. It is possible in
165  *Kinect to rectify the range images such both cameras
166  * seem to coincide and then both sets of camera parameters will be identical.
167  *
168  * Range data can be interpreted in two different ways depending on the 3D
169  *camera (this field is already set to the
170  * correct setting when grabbing observations from an mrpt::hwdrivers
171  *sensor):
172  * - range_is_depth=true -> Kinect-like ranges: entries of \a rangeImage
173  *are
174  *distances along the +X axis
175  * - range_is_depth=false -> Ranges in \a rangeImage are actual distances
176  *in
177  *3D.
178  *
179  * The "intensity" channel may come from different channels in sesnsors as
180  *Kinect. Look at field \a intensityImageChannel to
181  * find out if the image was grabbed from the visible (RGB) or IR channels.
182  *
183  * 3D point clouds can be generated at any moment after grabbing with
184  *CObservation3DRangeScan::project3DPointsFromDepthImage() and
185  *CObservation3DRangeScan::project3DPointsFromDepthImageInto(), provided the
186  *correct
187  * calibration parameters. Note that project3DPointsFromDepthImage() will
188  *store the point cloud in sensor-centric local coordinates. Use
189  *project3DPointsFromDepthImageInto() to directly obtain vehicle or world
190  *coordinates.
191  *
192  * Example of how to assign labels to pixels (for object segmentation, semantic
193  *information, etc.):
194  *
195  * \code
196  * // Assume obs of type CObservation3DRangeScan::Ptr
197  * obs->pixelLabels = CObservation3DRangeScan::TPixelLabelInfo::Ptr( new
198  *CObservation3DRangeScan::TPixelLabelInfo<NUM_BYTES>() );
199  * obs->pixelLabels->setSize(ROWS,COLS);
200  * obs->pixelLabels->setLabel(col,row, label_idx); // label_idxs =
201  *[0,2^NUM_BYTES-1]
202  * //...
203  * \endcode
204  *
205  * \note Starting at serialization version 2 (MRPT 0.9.1+), the confidence
206  *channel is stored as an image instead of a matrix to optimize memory and disk
207  *space.
208  * \note Starting at serialization version 3 (MRPT 0.9.1+), the 3D point cloud
209  *and the rangeImage can both be stored externally to save rawlog space.
210  * \note Starting at serialization version 5 (MRPT 0.9.5+), the new field \a
211  *range_is_depth
212  * \note Starting at serialization version 6 (MRPT 0.9.5+), the new field \a
213  *intensityImageChannel
214  * \note Starting at serialization version 7 (MRPT 1.3.1+), new fields for
215  *semantic labeling
216  * \note Since MRPT 1.5.0, external files format can be selected at runtime
217  *with `CObservation3DRangeScan::EXTERNALS_AS_TEXT`
218  *
219  * \sa mrpt::hwdrivers::CSwissRanger3DCamera, mrpt::hwdrivers::CKinect,
220  *CObservation
221  * \ingroup mrpt_obs_grp
222  */
224 {
226 
227  protected:
228  /** If set to true, m_points3D_external_file is valid. */
230  /** 3D points are in CImage::getImagesPathBase()+<this_file_name> */
232 
233  /** If set to true, m_rangeImage_external_file is valid. */
235  /** rangeImage is in CImage::getImagesPathBase()+<this_file_name> */
237 
238  public:
239  /** Default constructor */
241  /** Destructor */
242  ~CObservation3DRangeScan() override;
243 
244  /** @name Delayed-load manual control methods.
245  @{ */
246  /** Makes sure all images and other fields which may be externally stored
247  * are loaded in memory.
248  * Note that for all CImages, calling load() is not required since the
249  * images will be automatically loaded upon first access, so load()
250  * shouldn't be needed to be called in normal cases by the user.
251  * If all the data were alredy loaded or this object has no externally
252  * stored data fields, calling this method has no effects.
253  * \sa unload
254  */
255  void load() const override;
256  /** Unload all images, for the case they being delayed-load images stored in
257  * external files (othewise, has no effect).
258  * \sa load
259  */
260  void unload() override;
261  /** @} */
262 
263  /** Project the RGB+D images into a 3D point cloud (with color if the target
264  * map supports it) and optionally at a given 3D pose.
265  * The 3D point coordinates are computed from the depth image (\a
266  * rangeImage) and the depth camera camera parameters (\a cameraParams).
267  * There exist two set of formulas for projecting the i'th point,
268  * depending on the value of "range_is_depth".
269  * In all formulas below, "rangeImage" is the matrix of ranges and the
270  * pixel coordinates are (r,c).
271  *
272  * 1) [range_is_depth=true] With "range equals depth" or "Kinect-like
273  * depth mode": the range values
274  * are in fact distances along the "+X" axis, not real 3D ranges (this
275  * is the way Kinect reports ranges):
276  *
277  * \code
278  * x(i) = rangeImage(r,c)
279  * y(i) = (r_cx - c) * x(i) / r_fx
280  * z(i) = (r_cy - r) * x(i) / r_fy
281  * \endcode
282  *
283  *
284  * 2) [range_is_depth=false] With "normal ranges": range means distance in
285  * 3D. This must be set when
286  * processing data from the SwissRange 3D camera, among others.
287  *
288  * \code
289  * Ky = (r_cx - c)/r_fx
290  * Kz = (r_cy - r)/r_fy
291  *
292  * x(i) = rangeImage(r,c) / sqrt( 1 + Ky^2 + Kz^2 )
293  * y(i) = Ky * x(i)
294  * z(i) = Kz * x(i)
295  * \endcode
296  *
297  * The color of each point is determined by projecting the 3D local point
298  * into the RGB image using \a cameraParamsIntensity.
299  *
300  * By default the local (sensor-centric) coordinates of points are
301  * directly stored into the local map, but if indicated so in \a
302  * takeIntoAccountSensorPoseOnRobot
303  * the points are transformed with \a sensorPose. Furthermore, if
304  * provided, those coordinates are transformed with \a robotPoseInTheWorld
305  *
306  * \tparam POINTMAP Supported maps are all those covered by
307  * mrpt::opengl::PointCloudAdapter (mrpt::maps::CPointsMap and derived,
308  * mrpt::opengl::CPointCloudColoured, PCL point clouds,...)
309  *
310  * \note In MRPT < 0.9.5, this method always assumes that ranges were in
311  * Kinect-like format.
312  */
313  template <class POINTMAP>
315  POINTMAP& dest_pointcloud,
316  const T3DPointsProjectionParams& projectParams,
317  const TRangeImageFilterParams& filterParams = TRangeImageFilterParams())
318  {
319  detail::project3DPointsFromDepthImageInto<POINTMAP>(
320  *this, dest_pointcloud, projectParams, filterParams);
321  }
322 
323  /** This method is equivalent to \c project3DPointsFromDepthImageInto()
324  * storing the projected 3D points (without color, in local sensor-centric
325  * coordinates) in this same class.
326  * For new code it's recommended to use instead \c
327  * project3DPointsFromDepthImageInto() which is much more versatile. */
328  inline void project3DPointsFromDepthImage(const bool PROJ3D_USE_LUT = true)
329  {
331  p.takeIntoAccountSensorPoseOnRobot = false;
332  p.PROJ3D_USE_LUT = PROJ3D_USE_LUT;
333  this->project3DPointsFromDepthImageInto(*this, p);
334  }
335 
336  /** Convert this 3D observation into an "equivalent 2D fake laser scan",
337  * with a configurable vertical FOV.
338  *
339  * The result is a 2D laser scan with more "rays" (N) than columns has the
340  * 3D observation (W), exactly: N = W * oversampling_ratio.
341  * This oversampling is required since laser scans sample the space at
342  * evenly-separated angles, while
343  * a range camera follows a tangent-like distribution. By oversampling we
344  * make sure we don't leave "gaps" unseen by the virtual "2D laser".
345  *
346  * All obstacles within a frustum are considered and the minimum distance
347  * is kept in each direction.
348  * The horizontal FOV of the frustum is automatically computed from the
349  * intrinsic parameters, but the
350  * vertical FOV must be provided by the user, and can be set to be
351  * assymetric which may be useful
352  * depending on the zone of interest where to look for obstacles.
353  *
354  * All spatial transformations are riguorosly taken into account in this
355  * class, using the depth camera
356  * intrinsic calibration parameters.
357  *
358  * The timestamp of the new object is copied from the 3D object.
359  * Obviously, a requisite for calling this method is the 3D observation
360  * having range data,
361  * i.e. hasRangeImage must be true. It's not needed to have RGB data nor
362  * the raw 3D point clouds
363  * for this method to work.
364  *
365  * If `scanParams.use_origin_sensor_pose` is `true`, the points will be
366  * projected to 3D and then reprojected
367  * as seen from a different sensorPose at the vehicle frame origin.
368  * Otherwise (the default), the output 2D observation will share the
369  * sensorPose of the input 3D scan
370  * (using a more efficient algorithm that avoids trigonometric functions).
371  *
372  * \param[out] out_scan2d The resulting 2D equivalent scan.
373  *
374  * \sa The example in
375  * https://www.mrpt.org/tutorials/mrpt-examples/example-kinect-to-2d-laser-demo/
376  */
377  void convertTo2DScan(
379  const T3DPointsTo2DScanParams& scanParams,
380  const TRangeImageFilterParams& filterParams =
382 
383  /** Whether external files (3D points, range and confidence) are to be
384  * saved as `.txt` text files (MATLAB compatible) or `*.bin` binary
385  *(faster).
386  * Loading always will determine the type by inspecting the file extension.
387  * \note Default=false
388  **/
389  static void EXTERNALS_AS_TEXT(bool value);
390  static bool EXTERNALS_AS_TEXT();
391 
392  /** \name Point cloud
393  * @{ */
394  /** true means the field points3D contains valid data. */
395  bool hasPoints3D{false};
396  /** If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud
397  * detected by the camera. \sa resizePoints3DVectors */
398  std::vector<float> points3D_x, points3D_y, points3D_z;
399  /** If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point
400  * in \a points3D_x, points3D_y, points3D_z */
401  std::vector<uint16_t> points3D_idxs_x, points3D_idxs_y; //!<
402 
403  /** Use this method instead of resizing all three \a points3D_x, \a
404  * points3D_y & \a points3D_z to allow the usage of the internal memory
405  * pool. */
406  void resizePoints3DVectors(const size_t nPoints);
407 
408  /** Get the size of the scan pointcloud. \note Method is added for
409  * compatibility with its CObservation2DRangeScan counterpart */
410  size_t getScanSize() const;
411  /** @} */
412 
413  /** \name Point cloud external storage functions
414  * @{ */
415  inline bool points3D_isExternallyStored() const
416  {
418  }
420  {
422  }
424  std::string& out_path) const;
426  {
427  std::string tmp;
429  return tmp;
430  }
431  /** Users won't normally want to call this, it's only used from internal
432  * MRPT programs. \sa EXTERNALS_AS_TEXT */
434  const std::string& fileName, const std::string& use_this_base_dir);
435  /** Users normally won't need to use this */
436  inline void points3D_overrideExternalStoredFlag(bool isExternal)
437  {
438  m_points3D_external_stored = isExternal;
439  }
440  /** @} */
441 
442  /** \name Range (depth) image
443  * @{ */
444  /** true means the field rangeImage contains valid data */
445  bool hasRangeImage{false};
446  /** If hasRangeImage=true, a matrix of floats with the range data as
447  * captured by the camera (in meters) \sa range_is_depth */
449  /** true: Kinect-like ranges: entries of \a rangeImage are distances along
450  * the +X axis; false: Ranges in \a rangeImage are actual distances in 3D.
451  */
452  bool range_is_depth{true};
453 
454  /** Similar to calling "rangeImage.setSize(H,W)" but this method provides
455  * memory pooling to speed-up the memory allocation. */
456  void rangeImage_setSize(const int HEIGHT, const int WIDTH);
457  /** @} */
458 
459  /** \name Range Matrix external storage functions
460  * @{ */
461  inline bool rangeImage_isExternallyStored() const
462  {
464  }
466  {
468  }
470  std::string& out_path) const;
472  {
473  std::string tmp;
475  return tmp;
476  }
477  /** Users won't normally want to call this, it's only used from internal
478  * MRPT programs. \sa EXTERNALS_AS_TEXT */
480  const std::string& fileName, const std::string& use_this_base_dir);
481  /** Forces marking this observation as non-externally stored - it doesn't
482  * anything else apart from reseting the corresponding flag (Users won't
483  * normally want to call this, it's only used from internal MRPT programs)
484  */
486  {
488  }
489  /** @} */
490 
491  /** \name Intensity (RGB) channels
492  * @{ */
493  /** Enum type for intensityImageChannel */
495  {
496  /** Grayscale or RGB visible channel of the camera sensor. */
498  /** Infrarred (IR) channel */
499  CH_IR = 1
500  };
501 
502  /** true means the field intensityImage contains valid data */
503  bool hasIntensityImage{false};
504  /** If hasIntensityImage=true, a color or gray-level intensity image of the
505  * same size than "rangeImage" */
507  /** The source of the intensityImage; typically the visible channel \sa
508  * TIntensityChannelID */
510  /** @} */
511 
512  /** \name Confidence "channel"
513  * @{ */
514  /** true means the field confidenceImage contains valid data */
515  bool hasConfidenceImage{false};
516  /** If hasConfidenceImage=true, an image with the "confidence" value [range
517  * 0-255] as estimated by the capture drivers. */
519  /** @} */
520 
521  /** \name Pixel-wise classification labels (for semantic labeling, etc.)
522  * @{ */
523  /** Returns true if the field CObservation3DRangeScan::pixelLabels contains
524  * a non-NULL smart pointer.
525  * To enhance a 3D point cloud with labeling info, just assign an
526  * appropiate object to \a pixelLabels
527  */
528  bool hasPixelLabels() const { return pixelLabels ? true : false; }
529  /** Virtual interface to all pixel-label information structs. See
530  * CObservation3DRangeScan::pixelLabels */
532  {
533  /** Used in CObservation3DRangeScan::pixelLabels */
535  using TMapLabelID2Name = std::map<uint32_t, std::string>;
536 
537  /** The 'semantic' or human-friendly name of the i'th bit in
538  * pixelLabels(r,c) can be found in pixelLabelNames[i] as a std::string
539  */
541 
542  const std::string& getLabelName(unsigned int label_idx) const
543  {
544  auto it = pixelLabelNames.find(label_idx);
545  if (it == pixelLabelNames.end())
546  throw std::runtime_error(
547  "Error: label index has no defined name");
548  return it->second;
549  }
550  void setLabelName(unsigned int label_idx, const std::string& name)
551  {
552  pixelLabelNames[label_idx] = name;
553  }
554  /** Check the existence of a label by returning its associated index.
555  * -1 if it does not exist. */
557  {
558  std::map<uint32_t, std::string>::const_iterator it;
559  for (it = pixelLabelNames.begin(); it != pixelLabelNames.end();
560  it++)
561  if (it->second == name) return it->first;
562  return -1;
563  }
564 
565  /** Resizes the matrix pixelLabels to the given size, setting all
566  * bitfields to zero (that is, all pixels are assigned NONE category).
567  */
568  virtual void setSize(const int NROWS, const int NCOLS) = 0;
569  /** Mark the pixel(row,col) as classified in the category \a label_idx,
570  * which may be in the range 0 to MAX_NUM_LABELS-1
571  * Note that 0 is a valid label index, it does not mean "no label" \sa
572  * unsetLabel, unsetAll */
573  virtual void setLabel(
574  const int row, const int col, uint8_t label_idx) = 0;
575  virtual void getLabels(
576  const int row, const int col, uint8_t& labels) = 0;
577  /** For the pixel(row,col), removes its classification into the category
578  * \a label_idx, which may be in the range 0 to 7
579  * Note that 0 is a valid label index, it does not mean "no label" \sa
580  * setLabel, unsetAll */
581  virtual void unsetLabel(
582  const int row, const int col, uint8_t label_idx) = 0;
583  /** Removes all categories for pixel(row,col) \sa setLabel, unsetLabel
584  */
585  virtual void unsetAll(
586  const int row, const int col, uint8_t label_idx) = 0;
587  /** Checks whether pixel(row,col) has been clasified into category \a
588  * label_idx, which may be in the range 0 to 7
589  * \sa unsetLabel, unsetAll */
590  virtual bool checkLabel(
591  const int row, const int col, uint8_t label_idx) const = 0;
592 
596 
597  /// std stream interface
598  friend std::ostream& operator<<(
599  std::ostream& out, const TPixelLabelInfoBase& obj)
600  {
601  obj.Print(out);
602  return out;
603  }
604 
605  TPixelLabelInfoBase(unsigned int BITFIELD_BYTES_)
606  : BITFIELD_BYTES(BITFIELD_BYTES_)
607  {
608  }
609 
610  virtual ~TPixelLabelInfoBase() = default;
611  /** Minimum number of bytes required to hold MAX_NUM_DIFFERENT_LABELS
612  * bits. */
614 
615  protected:
616  virtual void internal_readFromStream(
618  virtual void internal_writeToStream(
619  mrpt::serialization::CArchive& out) const = 0;
620  virtual void Print(std::ostream&) const = 0;
621  };
622 
623  template <unsigned int BYTES_REQUIRED_>
625  {
627  enum
628  {
629  BYTES_REQUIRED = BYTES_REQUIRED_ // ((MAX_LABELS-1)/8)+1
630  };
631 
632  /** Automatically-determined integer type of the proper size such that
633  * all labels fit as one bit (max: 64) */
634  using bitmask_t =
636 
637  /** Each pixel may be assigned between 0 and MAX_NUM_LABELS-1 'labels'
638  * by
639  * setting to 1 the corresponding i'th bit [0,MAX_NUM_LABELS-1] in the
640  * byte in pixelLabels(r,c).
641  * That is, each pixel is assigned an 8*BITFIELD_BYTES bit-wide
642  * bitfield of possible categories.
643  * \sa hasPixelLabels
644  */
647 
648  void setSize(const int NROWS, const int NCOLS) override
649  {
650  pixelLabels = TPixelLabelMatrix::Zero(NROWS, NCOLS);
651  }
652  void setLabel(const int row, const int col, uint8_t label_idx) override
653  {
654  pixelLabels(row, col) |= static_cast<bitmask_t>(1) << label_idx;
655  }
656  void getLabels(const int row, const int col, uint8_t& labels) override
657  {
658  labels = pixelLabels(row, col);
659  }
660 
662  const int row, const int col, uint8_t label_idx) override
663  {
664  pixelLabels(row, col) &= ~(static_cast<bitmask_t>(1) << label_idx);
665  }
666  void unsetAll(const int row, const int col, uint8_t label_idx) override
667  {
668  pixelLabels(row, col) = 0;
669  }
671  const int row, const int col, uint8_t label_idx) const override
672  {
673  return (pixelLabels(row, col) &
674  (static_cast<bitmask_t>(1) << label_idx)) != 0;
675  }
676 
677  // Ctor: pass identification to parent for deserialization
678  TPixelLabelInfo() : TPixelLabelInfoBase(BYTES_REQUIRED_) {}
679 
680  protected:
684  mrpt::serialization::CArchive& out) const override;
685  void Print(std::ostream& out) const override
686  {
687  {
688  const auto nR = static_cast<uint32_t>(pixelLabels.rows());
689  const auto nC = static_cast<uint32_t>(pixelLabels.cols());
690  out << "Number of rows: " << nR << std::endl;
691  out << "Number of cols: " << nC << std::endl;
692  out << "Matrix of labels: " << std::endl;
693  for (uint32_t c = 0; c < nC; c++)
694  {
695  for (uint32_t r = 0; r < nR; r++)
696  out << pixelLabels.coeff(r, c) << " ";
697 
698  out << std::endl;
699  }
700  }
701  out << std::endl;
702  out << "Label indices and names: " << std::endl;
703  std::map<uint32_t, std::string>::const_iterator it;
704  for (it = pixelLabelNames.begin(); it != pixelLabelNames.end();
705  it++)
706  out << it->first << " " << it->second << std::endl;
707  }
708  }; // end TPixelLabelInfo
709 
710  /** All information about pixel labeling is stored in this (smart pointer
711  * to) structure; refer to TPixelLabelInfo for details on the contents
712  * User is responsible of creating a new object of the desired data type.
713  * It will be automatically (de)serialized no matter its specific type. */
715 
716  /** @} */
717 
718  /** \name Sensor parameters
719  * @{ */
720  /** Projection parameters of the depth camera. */
722  /** Projection parameters of the intensity (graylevel or RGB) camera. */
724 
725  /** Relative pose of the intensity camera wrt the depth camera (which is the
726  * coordinates origin for this observation).
727  * In a SwissRanger camera, this will be (0,0,0,-90deg,0,-90deg) since
728  * both cameras coincide.
729  * In a Kinect, this will include a small lateral displacement and a
730  * rotation, according to the drawing on the top of this page.
731  * \sa doDepthAndIntensityCamerasCoincide
732  */
734 
735  /** Return true if \a relativePoseIntensityWRTDepth equals the pure rotation
736  * (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
737  * \sa relativePoseIntensityWRTDepth
738  */
740 
741  /** The maximum range allowed by the device, in meters (e.g. 8.0m, 5.0m,...)
742  */
743  float maxRange{5.0f};
744  /** The 6D pose of the sensor on the robot. */
746  /** The "sigma" error of the device in meters, used while inserting the scan
747  * in an occupancy grid. */
748  float stdError{0.01f};
749 
750  // See base class docs
751  void getSensorPose(mrpt::poses::CPose3D& out_sensorPose) const override
752  {
753  out_sensorPose = sensorPose;
754  }
755  // See base class docs
756  void setSensorPose(const mrpt::poses::CPose3D& newSensorPose) override
757  {
758  sensorPose = newSensorPose;
759  }
760 
761  /** @} */ // end sensor params
762 
763  // See base class docs
764  void getDescriptionAsText(std::ostream& o) const override;
765 
766  /** Very efficient method to swap the contents of two observations. */
767  void swap(CObservation3DRangeScan& o);
768  /** Extract a ROI of the 3D observation as a new one. \note PixelLabels are
769  * *not* copied to the output subimage. */
770  void getZoneAsObs(
771  CObservation3DRangeScan& obs, const unsigned int& r1,
772  const unsigned int& r2, const unsigned int& c1, const unsigned int& c2);
773 
774  /** A Levenberg-Marquart-based optimizer to recover the calibration
775  * parameters of a 3D camera given a range (depth) image and the
776  * corresponding 3D point cloud.
777  * \param camera_offset The offset (in meters) in the +X direction of the
778  * point cloud. It's 1cm for SwissRanger SR4000.
779  * \return The final average reprojection error per pixel (typ <0.05 px)
780  */
782  const CObservation3DRangeScan& in_obs,
783  mrpt::img::TCamera& out_camParams, const double camera_offset = 0.01);
784 
785  /** Look-up-table struct for project3DPointsFromDepthImageInto() */
787  {
790  };
791  /** 3D point cloud projection look-up-table \sa
792  * project3DPointsFromDepthImage */
794 
795 }; // End of class def.
796 
797 } // namespace obs
798 namespace opengl
799 {
800 /** Specialization mrpt::opengl::PointCloudAdapter<CObservation3DRangeScan>
801  * \ingroup mrpt_adapters_grp */
802 template <>
804 {
805  private:
807 
808  public:
809  /** The type of each point XYZ coordinates */
810  using coords_t = float;
811  /** Has any color RGB info? */
812  static constexpr bool HAS_RGB = false;
813  /** Has native RGB info (as floats)? */
814  static constexpr bool HAS_RGBf = false;
815  /** Has native RGB info (as uint8_t)? */
816  static constexpr bool HAS_RGBu8 = false;
817 
818  /** Constructor (accept a const ref for convenience) */
820  : m_obj(*const_cast<mrpt::obs::CObservation3DRangeScan*>(&obj))
821  {
822  }
823  /** Get number of points */
824  inline size_t size() const { return m_obj.points3D_x.size(); }
825  /** Set number of points (to uninitialized values) */
826  inline void resize(const size_t N)
827  {
828  if (N) m_obj.hasPoints3D = true;
829  m_obj.resizePoints3DVectors(N);
830  }
831  /** Does nothing as of now */
832  inline void setDimensions(const size_t& height, const size_t& width) {}
833  /** Get XYZ coordinates of i'th point */
834  template <typename T>
835  inline void getPointXYZ(const size_t idx, T& x, T& y, T& z) const
836  {
837  x = m_obj.points3D_x[idx];
838  y = m_obj.points3D_y[idx];
839  z = m_obj.points3D_z[idx];
840  }
841  /** Set XYZ coordinates of i'th point */
842  inline void setPointXYZ(
843  const size_t idx, const coords_t x, const coords_t y, const coords_t z)
844  {
845  m_obj.points3D_x[idx] = x;
846  m_obj.points3D_y[idx] = y;
847  m_obj.points3D_z[idx] = z;
848  }
849  /** Set XYZ coordinates of i'th point */
850  inline void setInvalidPoint(const size_t idx)
851  {
853  "mrpt::obs::CObservation3DRangeScan requires needs to be dense");
854  }
855 
856 }; // end of PointCloudAdapter<CObservation3DRangeScan>
857 } // namespace opengl
858 } // namespace mrpt
863 
864 #include "CObservation3DRangeScan_project3D_impl.h"
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::getImagesPathBase()+<this_file_name>
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
void setDimensions(const size_t &height, const size_t &width)
Does nothing as of now.
const std::string & getLabelName(unsigned int label_idx) const
void writeToStream(mrpt::serialization::CArchive &out) const
GLdouble GLdouble z
Definition: glext.h:3879
void getZoneAsObs(CObservation3DRangeScan &obs, const unsigned int &r1, const unsigned int &r2, const unsigned int &c1, const unsigned int &c2)
Extract a ROI of the 3D observation as a new one.
TIntensityChannelID
Enum type for intensityImageChannel.
uint8_t decimation
(Default:1) If !=1, split the range image in blocks of DxD (D=decimation), and only generates one poi...
void resizePoints3DVectors(const size_t nPoints)
Use this method instead of resizing all three points3D_x, points3D_y & points3D_z to allow the usage ...
virtual void setSize(const int NROWS, const int NCOLS)=0
Resizes the matrix pixelLabels to the given size, setting all bitfields to zero (that is...
const mrpt::poses::CPose3D * robotPoseInTheWorld
(Default: nullptr) Read takeIntoAccountSensorPoseOnRobot
#define THROW_EXCEPTION(msg)
Definition: exceptions.h:67
MRPT_FILL_ENUM_MEMBER(mrpt::obs::CObservation3DRangeScan, CH_VISIBLE)
void setLabelName(unsigned int label_idx, const std::string &name)
static double recoverCameraCalibrationParameters(const CObservation3DRangeScan &in_obs, mrpt::img::TCamera &out_camParams, const double camera_offset=0.01)
A Levenberg-Marquart-based optimizer to recover the calibration parameters of a 3D camera given a ran...
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...
std::vector< uint16_t > points3D_idxs_x
If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point in points3D_x, points3D_y, points3D_z.
double oversampling_ratio
(Default=1.2=120%) How many more laser scans rays to create (read docs for CObservation3DRangeScan::c...
double angle_sup
(Default=5 degrees) [Only if use_origin_sensor_pose=false] The upper & lower half-FOV angle (in radia...
mrpt::math::CMatrixF rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) ...
bool m_rangeImage_external_stored
If set to true, m_rangeImage_external_file is valid.
virtual void internal_readFromStream(mrpt::serialization::CArchive &in)=0
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
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.
Look-up-table struct for project3DPointsFromDepthImageInto()
static TPixelLabelInfoBase * readAndBuildFromStream(mrpt::serialization::CArchive &in)
friend std::ostream & operator<<(std::ostream &out, const TPixelLabelInfoBase &obj)
std stream interface
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
double z_min
(Default:-inf, +inf) [Only if use_origin_sensor_pose=true] Only obstacle points with Z coordinates wi...
virtual void unsetLabel(const int row, const int col, uint8_t label_idx)=0
For the pixel(row,col), removes its classification into the category label_idx, which may be in the r...
GLsizei GLsizei GLuint * obj
Definition: glext.h:4085
virtual void setLabel(const int row, const int col, uint8_t label_idx)=0
Mark the pixel(row,col) as classified in the category label_idx, which may be in the range 0 to MAX_N...
void resize(const size_t N)
Set number of points (to uninitialized values)
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
GLenum GLsizei width
Definition: glext.h:3535
Usage: uint_select_by_bytecount<N>type var; allows defining var as a unsigned integer with...
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) override
A general method to change the sensor pose on the robot.
void rangeImage_convertToExternalStorage(const std::string &fileName, const std::string &use_this_base_dir)
Users won&#39;t normally want to call this, it&#39;s only used from internal MRPT programs.
unsigned char uint8_t
Definition: rptypes.h:44
const uint8_t BITFIELD_BYTES
Minimum number of bytes required to hold MAX_NUM_DIFFERENT_LABELS bits.
void internal_writeToStream(mrpt::serialization::CArchive &out) const override
PointCloudAdapter(const mrpt::obs::CObservation3DRangeScan &obj)
Constructor (accept a const ref for convenience)
std::string rangeImage_getExternalStorageFileAbsolutePath() const
void internal_readFromStream(mrpt::serialization::CArchive &in) override
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
An adapter to different kinds of point cloud object.
virtual bool checkLabel(const int row, const int col, uint8_t label_idx) const =0
Checks whether pixel(row,col) has been clasified into category label_idx, which may be in the range 0...
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...
const GLubyte * c
Definition: glext.h:6406
void unload() override
Unload all images, for the case they being delayed-load images stored in external files (othewise...
void swap(CObservation3DRangeScan &o)
Very efficient method to swap the contents of two observations.
void project3DPointsFromDepthImageInto(mrpt::obs::CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const mrpt::obs::T3DPointsProjectionParams &projectParams, const mrpt::obs::TRangeImageFilterParams &filterParams)
typename mrpt::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...
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
Used in CObservation3DRangeScan::convertTo2DScan()
int checkLabelNameExistence(const std::string &name) const
Check the existence of a label by returning its associated index.
std::string m_points3D_external_file
3D points are in CImage::getImagesPathBase()+<this_file_name>
bool hasRangeImage
true means the field rangeImage contains valid data
Structure to hold the parameters of a pinhole camera model.
Definition: TCamera.h:25
void convertTo2DScan(mrpt::obs::CObservation2DRangeScan &out_scan2d, const T3DPointsTo2DScanParams &scanParams, const TRangeImageFilterParams &filterParams=TRangeImageFilterParams())
Convert this 3D observation into an "equivalent 2D fake laser scan", with a configurable vertical FOV...
TIntensityChannelID intensityImageChannel
The source of the intensityImage; typically the visible channel.
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
void rangeImage_forceResetExternalStorage()
Forces marking this observation as non-externally stored - it doesn&#39;t anything else apart from reseti...
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.
mrpt::img::CImage confidenceImage
If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the ca...
std::string points3D_getExternalStorageFileAbsolutePath() const
bool hasPoints3D
true means the field points3D contains valid data.
GLsizei const GLchar ** string
Definition: glext.h:4116
size_type rows() const
Number of rows in the matrix.
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
size_type cols() const
Number of columns in the matrix.
const Scalar & coeff(int r, int c) const
void points3D_overrideExternalStoredFlag(bool isExternal)
Users normally won&#39;t need to use this.
bool USE_SSE2
(Default:true) If possible, use SSE2 optimized code.
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const override
A general method to retrieve the sensor pose on the robot.
This class is a "CSerializable" wrapper for "CMatrixFloat".
Definition: CMatrixF.h:22
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...
void setSize(const int NROWS, const int NCOLS) override
Resizes the matrix pixelLabels to the given size, setting all bitfields to zero (that is...
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Virtual base class for "archives": classes abstracting I/O streams.
Definition: CArchive.h:53
bool MAKE_ORGANIZED
(Default:false) set to true if you want an organized point cloud
GLdouble GLdouble GLdouble r
Definition: glext.h:3711
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
void getPointXYZ(const size_t idx, T &x, T &y, T &z) const
Get XYZ coordinates of i&#39;th point.
void getLabels(const int row, const int col, uint8_t &labels) override
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:84
Declares a class that represents any robot&#39;s observation.
Definition: CObservation.h:43
TPixelLabelInfoBase::Ptr pixelLabels
All information about pixel labeling is stored in this (smart pointer to) structure; refer to TPixelL...
static TCached3DProjTables & get_3dproj_lut()
3D point cloud projection look-up-table
GLenum GLenum GLvoid * row
Definition: glext.h:3580
Used in CObservation3DRangeScan::project3DPointsFromDepthImageInto()
virtual void internal_writeToStream(mrpt::serialization::CArchive &out) const =0
bool hasIntensityImage
true means the field intensityImage contains valid data
void setInvalidPoint(const size_t idx)
Set XYZ coordinates of i&#39;th point.
bool checkLabel(const int row, const int col, uint8_t label_idx) const override
Checks whether pixel(row,col) has been clasified into category label_idx, which may be in the range 0...
GLuint in
Definition: glext.h:7391
std::vector< float > points3D_x
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
GLuint const GLchar * name
Definition: glext.h:4068
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 ...
bool hasPixelLabels() const
Returns true if the field CObservation3DRangeScan::pixelLabels contains a non-NULL smart pointer...
void project3DPointsFromDepthImageInto(POINTMAP &dest_pointcloud, const T3DPointsProjectionParams &projectParams, const TRangeImageFilterParams &filterParams=TRangeImageFilterParams())
Project the RGB+D images into a 3D point cloud (with color if the target map supports it) and optiona...
virtual void Print(std::ostream &) const =0
GLenum GLint GLint y
Definition: glext.h:3542
void points3D_convertToExternalStorage(const std::string &fileName, const std::string &use_this_base_dir)
Users won&#39;t normally want to call this, it&#39;s only used from internal MRPT programs.
bool PROJ3D_USE_LUT
(Default:true) [Only used when range_is_depth=true] Whether to use a Look-up-table (LUT) to speed up ...
bool doDepthAndIntensityCamerasCoincide() const
Return true if relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
void unsetLabel(const int row, const int col, uint8_t label_idx) override
For the pixel(row,col), removes its classification into the category label_idx, which may be in the r...
GLsizei const GLfloat * value
Definition: glext.h:4134
virtual void unsetAll(const int row, const int col, uint8_t label_idx)=0
Removes all categories for pixel(row,col)
GLenum GLint x
Definition: glext.h:3542
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
GLenum GLsizei GLsizei height
Definition: glext.h:3558
size_t getScanSize() const
Get the size of the scan pointcloud.
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
unsigned __int32 uint32_t
Definition: rptypes.h:50
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
void setLabel(const int row, const int col, uint8_t label_idx) override
Mark the pixel(row,col) as classified in the category label_idx, which may be in the range 0 to MAX_N...
GLfloat GLfloat p
Definition: glext.h:6398
Virtual interface to all pixel-label information structs.
void unsetAll(const int row, const int col, uint8_t label_idx) override
Removes all categories for pixel(row,col)
Grayscale or RGB visible channel of the camera sensor.
bool takeIntoAccountSensorPoseOnRobot
(Default: false) If false, local (sensor-centric) coordinates of points are generated.
void load() const override
Makes sure all images and other fields which may be externally stored are loaded in memory...
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:147
virtual void getLabels(const int row, const int col, uint8_t &labels)=0
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.
bool hasConfidenceImage
true means the field confidenceImage contains valid data



Page generated by Doxygen 1.8.14 for MRPT 1.9.9 Git: 3316834cb Wed Aug 21 11:42:56 2019 +0200 at miƩ ago 21 11:50:11 CEST 2019