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