MRPT  2.0.5
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-2020, 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 
12 #include <mrpt/img/CImage.h>
13 #include <mrpt/img/color_maps.h>
14 #include <mrpt/math/CMatrixF.h>
15 #include <mrpt/math/CPolygon.h>
16 #include <mrpt/obs/CObservation.h>
23 #include <mrpt/poses/CPose2D.h>
24 #include <mrpt/poses/CPose3D.h>
28 #include <optional>
29 
30 namespace mrpt::obs
31 {
32 namespace detail
33 {
34 // Implemented in CObservation3DRangeScan_project3D_impl.h
35 template <class POINTMAP>
36 void unprojectInto(
37  mrpt::obs::CObservation3DRangeScan& src_obs, POINTMAP& dest_pointcloud,
38  const mrpt::obs::T3DPointsProjectionParams& projectParams,
39  const mrpt::obs::TRangeImageFilterParams& filterParams);
40 } // namespace detail
41 
42 /** A range or depth 3D scan measurement, as from a time-of-flight range camera
43  *or a structured-light depth RGBD sensor.
44  *
45  * This kind of observations can carry one or more of these data fields:
46  * - 3D point cloud (as float's).
47  * - Each 3D point has its associated (u,v) pixel coordinates in \a
48  *points3D_idxs_x & \a points3D_idxs_y (New in MRPT 1.4.0)
49  * - 2D range image (as a matrix): Each entry in the matrix
50  *"rangeImage(ROW,COLUMN)" contains a distance or a depth, depending
51  *on \a range_is_depth. Ranges are stored as uint16_t for efficiency. The units
52  *of ranges are stored separately in rangeUnits.
53  * - 2D intensity (grayscale or RGB) image (as a mrpt::img::CImage): For
54  *SwissRanger cameras, a logarithmic A-law compression is used to convert the
55  *original 16bit intensity to a more standard 8bit graylevel.
56  * - 2D confidence image (as a mrpt::img::CImage): For each pixel, a 0x00
57  *and a 0xFF mean the lowest and highest confidence levels, respectively.
58  * - Semantic labels: Stored as a matrix of bitfields, each bit having a
59  *user-defined meaning.
60  * - For cameras supporting multiple returns per pixels, different layers of
61  *range images are available in the map \a rangeImageOtherLayers.
62  *
63  * The coordinates of the 3D point cloud are in meters with respect to the
64  *depth camera origin of coordinates
65  * (in SwissRanger, the front face of the camera: a small offset ~1cm in
66  *front of the physical focal point),
67  * with the +X axis pointing forward, +Y pointing left-hand and +Z pointing
68  *up. By convention, a 3D point with its coordinates set to (0,0,0), will be
69  *considered as invalid.
70  * The field CObservation3DRangeScan::relativePoseIntensityWRTDepth describes
71  *the change of coordinates from
72  * the depth camera to the intensity (RGB or grayscale) camera. In a
73  *SwissRanger camera both cameras coincide,
74  * so this pose is just a rotation (0,0,0,-90deg,0,-90deg). But in
75  * Microsoft Kinect there is also an offset, as shown in this figure:
76  *
77  * <div align=center>
78  * <img src="CObservation3DRangeScan_figRefSystem.png">
79  * </div>
80  *
81  * In any case, check the field \a relativePoseIntensityWRTDepth, or the method
82  *\a doDepthAndIntensityCamerasCoincide()
83  * to determine if both frames of reference coincide, since even for Kinect
84  *cameras both can coincide if the images
85  * have been rectified.
86  *
87  * The 2D images and matrices are stored as common images, with an up->down
88  *rows order and left->right, as usual.
89  * Optionally, the intensity and confidence channels can be set to
90  *delayed-load images for off-rawlog storage so it saves
91  * memory by having loaded in memory just the needed images. See the methods
92  *load() and unload().
93  * Due to the intensive storage requirements of this kind of observations, this
94  *observation is the only one in MRPT
95  * for which it's recommended to always call "load()" and "unload()" before
96  *and after using the observation, *ONLY* when
97  * the observation was read from a rawlog dataset, in order to make sure that
98  *all the externally stored data fields are
99  * loaded and ready in memory.
100  *
101  * Classes that grab observations of this type are:
102  * - mrpt::hwdrivers::CSwissRanger3DCamera
103  * - mrpt::hwdrivers::CKinect
104  * - mrpt::hwdrivers::COpenNI2Sensor
105  *
106  * There are two sets of calibration parameters (see
107  *mrpt::vision::checkerBoardStereoCalibration() or the ready-to-use GUI program
108  *<a href="http://www.mrpt.org/Application:kinect-calibrate"
109  *>kinect-calibrate</a>):
110  * - cameraParams: Projection parameters of the depth camera.
111  * - cameraParamsIntensity: Projection parameters of the intensity
112  *(gray-level or RGB) camera.
113  *
114  * In some cameras, like SwissRanger, both are the same. It is possible in
115  *Kinect to rectify the range images such both cameras
116  * seem to coincide and then both sets of camera parameters will be identical.
117  *
118  * Range data can be interpreted in two different ways depending on the 3D
119  *camera (this field is already set to the
120  * correct setting when grabbing observations from an mrpt::hwdrivers
121  *sensor):
122  * - range_is_depth=true -> Kinect-like ranges: entries of \a rangeImage
123  * are distances along the +X (front-facing) axis.
124  * - range_is_depth=false -> Ranges in \a rangeImage are actual distances
125  * in 3D.
126  *
127  * The "intensity" channel may come from different channels in sesnsors as
128  * Kinect. Look at field \a intensityImageChannel to find out if the image was
129  * grabbed from the visible (RGB) or IR channels.
130  *
131  * 3D point clouds can be generated at any moment after grabbing with
132  * CObservation3DRangeScan::unprojectInto(), provided the correct calibration
133  *parameters. Note that unprojectInto() will store the point cloud in
134  *sensor-centric local coordinates. Use unprojectInto() to directly obtain
135  *vehicle or world coordinates.
136  *
137  * Example of how to assign labels to pixels (for object segmentation, semantic
138  *information, etc.):
139  *
140  * \code
141  * // Assume obs of type CObservation3DRangeScan::Ptr
142  * obs->pixelLabels =TPixelLabelInfo::Ptr( new
143  *CObservation3DRangeScan::TPixelLabelInfo<NUM_BYTES>() );
144  * obs->pixelLabels->setSize(ROWS,COLS);
145  * obs->pixelLabels->setLabel(col,row, label_idx); // label_idxs =
146  *[0,2^NUM_BYTES-1]
147  * //...
148  * \endcode
149  *
150  * \note Starting at serialization version 2 (MRPT 0.9.1+), the confidence
151  *channel is stored as an image instead of a matrix to optimize memory and disk
152  *space.
153  * \note Starting at serialization version 3 (MRPT 0.9.1+), the 3D point cloud
154  *and the rangeImage can both be stored externally to save rawlog space.
155  * \note Starting at serialization version 5 (MRPT 0.9.5+), the new field \a
156  *range_is_depth
157  * \note Starting at serialization version 6 (MRPT 0.9.5+), the new field \a
158  *intensityImageChannel
159  * \note Starting at serialization version 7 (MRPT 1.3.1+), new fields for
160  *semantic labeling
161  * \note Since MRPT 1.5.0, external files format can be selected at runtime
162  *with `CObservation3DRangeScan::EXTERNALS_AS_TEXT`
163  *
164  * \sa mrpt::hwdrivers::CSwissRanger3DCamera, mrpt::hwdrivers::CKinect,
165  *CObservation
166  * \ingroup mrpt_obs_grp
167  */
169 {
171 
172  protected:
173  /** If set to true, m_points3D_external_file is valid. */
175  /** 3D points are in CImage::getImagesPathBase()+<this_file_name> */
177 
178  /** If set to true, m_rangeImage_external_file is valid. */
180  /** rangeImage is in CImage::getImagesPathBase()+<this_file_name> */
182 
183  public:
184  CObservation3DRangeScan() = default;
185 
186  ~CObservation3DRangeScan() override;
187 
188  /** @name Delayed-load manual control methods.
189  @{ */
190  /** Makes sure all images and other fields which may be externally stored
191  * are loaded in memory.
192  * Note that for all CImages, calling load() is not required since the
193  * images will be automatically loaded upon first access, so load()
194  * shouldn't be needed to be called in normal cases by the user.
195  * If all the data were alredy loaded or this object has no externally
196  * stored data fields, calling this method has no effects.
197  * \sa unload
198  */
199  void load() const override;
200  /** Unload all images, for the case they being delayed-load images stored in
201  * external files (othewise, has no effect).
202  * \sa load
203  */
204  void unload() override;
205  /** @} */
206 
207  /** Unprojects the RGB+D image pair into a 3D point cloud (with color if the
208  * target map supports it) and optionally at a given 3D pose. The 3D point
209  * coordinates are computed from the depth image (\a rangeImage) and the
210  * depth camera camera parameters (\a cameraParams). There exist two set of
211  * formulas for projecting the i'th point, depending on the value of
212  * "range_is_depth". In all formulas below, "rangeImage" is the matrix of
213  * ranges and the pixel coordinates are (r,c).
214  *
215  * 1) [range_is_depth=true] With "range equals depth" or "Kinect-like
216  * depth mode": the range values
217  * are in fact distances along the "+X" axis, not real 3D ranges (this
218  * is the way Kinect reports ranges):
219  *
220  * \code
221  * x(i) = rangeImage(r,c) * rangeUnits
222  * y(i) = (r_cx - c) * x(i) / r_fx
223  * z(i) = (r_cy - r) * x(i) / r_fy
224  * \endcode
225  *
226  *
227  * 2) [range_is_depth=false] With "normal ranges": range means distance in
228  * 3D. This must be set when
229  * processing data from the SwissRange 3D camera, among others.
230  *
231  * \code
232  * Ky = (r_cx - c)/r_fx
233  * Kz = (r_cy - r)/r_fy
234  *
235  * x(i) = rangeImage(r,c) * rangeUnits / sqrt( 1 + Ky^2 + Kz^2 )
236  * y(i) = Ky * x(i)
237  * z(i) = Kz * x(i)
238  * \endcode
239  *
240  * The color of each point is determined by projecting the 3D local point
241  * into the RGB image using \a cameraParamsIntensity.
242  *
243  * By default the local (sensor-centric) coordinates of points are
244  * directly stored into the local map, but if indicated so in \a
245  * takeIntoAccountSensorPoseOnRobot
246  * the points are transformed with \a sensorPose. Furthermore, if
247  * provided, those coordinates are transformed with \a robotPoseInTheWorld
248  *
249  * \note For multi-return sensors, only the layer specified in
250  * T3DPointsProjectionParams::layer will be unprojected.
251  *
252  * \tparam POINTMAP Supported maps are all those covered by
253  * mrpt::opengl::PointCloudAdapter (mrpt::maps::CPointsMap and derived,
254  * mrpt::opengl::CPointCloudColoured, PCL point clouds,...)
255  */
256  template <class POINTMAP>
257  inline void unprojectInto(
258  POINTMAP& dest_pointcloud,
259  const T3DPointsProjectionParams& projectParams =
261  const TRangeImageFilterParams& filterParams = TRangeImageFilterParams())
262  {
263  detail::unprojectInto<POINTMAP>(
264  *this, dest_pointcloud, projectParams, filterParams);
265  }
266 
267  /** Convert this 3D observation into an "equivalent 2D fake laser scan",
268  * with a configurable vertical FOV.
269  *
270  * The result is a 2D laser scan with more "rays" (N) than columns has the
271  * 3D observation (W), exactly: N = W * oversampling_ratio.
272  * This oversampling is required since laser scans sample the space at
273  * evenly-separated angles, while
274  * a range camera follows a tangent-like distribution. By oversampling we
275  * make sure we don't leave "gaps" unseen by the virtual "2D laser".
276  *
277  * All obstacles within a frustum are considered and the minimum distance
278  * is kept in each direction.
279  * The horizontal FOV of the frustum is automatically computed from the
280  * intrinsic parameters, but the
281  * vertical FOV must be provided by the user, and can be set to be
282  * assymetric which may be useful
283  * depending on the zone of interest where to look for obstacles.
284  *
285  * All spatial transformations are riguorosly taken into account in this
286  * class, using the depth camera
287  * intrinsic calibration parameters.
288  *
289  * The timestamp of the new object is copied from the 3D object.
290  * Obviously, a requisite for calling this method is the 3D observation
291  * having range data,
292  * i.e. hasRangeImage must be true. It's not needed to have RGB data nor
293  * the raw 3D point clouds
294  * for this method to work.
295  *
296  * If `scanParams.use_origin_sensor_pose` is `true`, the points will be
297  * projected to 3D and then reprojected
298  * as seen from a different sensorPose at the vehicle frame origin.
299  * Otherwise (the default), the output 2D observation will share the
300  * sensorPose of the input 3D scan
301  * (using a more efficient algorithm that avoids trigonometric functions).
302  *
303  * \param[out] out_scan2d The resulting 2D equivalent scan.
304  *
305  * \sa The example in
306  * https://www.mrpt.org/tutorials/mrpt-examples/example-kinect-to-2d-laser-demo/
307  */
308  void convertTo2DScan(
310  const T3DPointsTo2DScanParams& scanParams,
311  const TRangeImageFilterParams& filterParams =
313 
314  /** Whether external files (3D points, range and confidence) are to be
315  * saved as `.txt` text files (MATLAB compatible) or `*.bin` binary
316  *(faster).
317  * Loading always will determine the type by inspecting the file extension.
318  * \note Default=false
319  **/
320  static void EXTERNALS_AS_TEXT(bool value);
321  static bool EXTERNALS_AS_TEXT();
322 
323  /** \name Point cloud
324  * @{ */
325  /** true means the field points3D contains valid data. */
326  bool hasPoints3D{false};
327  /** If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud
328  * detected by the camera. \sa resizePoints3DVectors */
329  std::vector<float> points3D_x, points3D_y, points3D_z;
330  /** If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point
331  * in \a points3D_x, points3D_y, points3D_z */
332  std::vector<uint16_t> points3D_idxs_x, points3D_idxs_y; //!<
333 
334  /** Use this method instead of resizing all three \a points3D_x, \a
335  * points3D_y & \a points3D_z to allow the usage of the internal memory
336  * pool. */
337  void resizePoints3DVectors(const size_t nPoints);
338 
339  /** Get the size of the scan pointcloud. \note Method is added for
340  * compatibility with its CObservation2DRangeScan counterpart */
341  size_t getScanSize() const;
342  /** @} */
343 
344  /** \name Point cloud external storage functions
345  * @{ */
346  inline bool points3D_isExternallyStored() const
347  {
349  }
350  inline std::string points3D_getExternalStorageFile() const
351  {
353  }
355  std::string& out_path) const;
357  {
358  std::string tmp;
360  return tmp;
361  }
362  /** Users won't normally want to call this, it's only used from internal
363  * MRPT programs. \sa EXTERNALS_AS_TEXT */
365  const std::string& fileName, const std::string& use_this_base_dir);
366  /** Users normally won't need to use this */
367  inline void points3D_overrideExternalStoredFlag(bool isExternal)
368  {
369  m_points3D_external_stored = isExternal;
370  }
371  /** @} */
372 
373  /** \name Range (depth) image
374  * @{ */
375  /** true means the field rangeImage contains valid data */
376  bool hasRangeImage{false};
377 
378  /** If hasRangeImage=true, a matrix of floats with the range data as
379  * captured by the camera (in meters).
380  * For sensors with multiple returns per pixels, this matrix holds the
381  * CLOSEST of all the returns.
382  *
383  * \sa range_is_depth, rangeUnits, rangeImageOtherLayers */
385 
386  /** Additional layer range/depth images. Text labels are arbitrary and
387  * sensor-dependent, e.g. "LAST", "SECOND", "3rd", etc. */
388  std::map<std::string, mrpt::math::CMatrix_u16> rangeImageOtherLayers;
389 
390  /** The conversion factor from integer units in rangeImage and actual
391  * distances in meters. Default is 0.001 m, that is 1 millimeter. \sa
392  * rangeImage */
393  float rangeUnits = 0.001f;
394 
395  /** true: Kinect-like ranges: entries of \a rangeImage are distances
396  * along the +X axis; false: Ranges in \a rangeImage are actual
397  * distances in 3D.
398  */
399  bool range_is_depth{true};
400 
401  /** Similar to calling "rangeImage.setSize(H,W)" but this method provides
402  * memory pooling to speed-up the memory allocation. */
403  void rangeImage_setSize(const int HEIGHT, const int WIDTH);
404 
405  /** Builds a visualization from the rangeImage.
406  * The image is built with the given color map (default: grayscale) and such
407  * that the colormap range is mapped to ranges 0 meters to the field
408  * "maxRange" in this object, unless overriden with the optional parameters.
409  * Note that the usage of optional<> allows any parameter to be left to its
410  * default placing `std::nullopt`.
411  *
412  * \param additionalLayerName If empty string or not provided, the main
413  * rangeImage will be used; otherwise, the given range image layer.
414  * \sa rangeImageAsImage
415  */
417  const std::optional<mrpt::img::TColormap> color = std::nullopt,
418  const std::optional<float> normMinRange = std::nullopt,
419  const std::optional<float> normMaxRange = std::nullopt,
420  const std::optional<std::string> additionalLayerName =
421  std::nullopt) const;
422 
423  /** Static method to convert a range matrix into an image.
424  * If val_max is left to zero, the maximum range in the matrix will be
425  * automatically used. \sa rangeImage_getAsImage
426  */
428  const mrpt::math::CMatrix_u16& ranges, float val_min, float val_max,
429  float rangeUnits,
430  const std::optional<mrpt::img::TColormap> color = std::nullopt);
431 
432  /** @} */
433 
434  /** \name Range Matrix external storage functions
435  * @{ */
436  inline bool rangeImage_isExternallyStored() const
437  {
439  }
441  const std::string& rangeImageLayer) const;
442 
443  /** rangeImageLayer: Empty for the main rangeImage matrix, otherwise, a key
444  * of rangeImageOtherLayers */
446  std::string& out_path, const std::string& rangeImageLayer) const;
448  const std::string& rangeImageLayer) const
449  {
450  std::string tmp;
451  rangeImage_getExternalStorageFileAbsolutePath(tmp, rangeImageLayer);
452  return tmp;
453  }
454  /** Users won't normally want to call this, it's only used from internal
455  * MRPT programs. \sa EXTERNALS_AS_TEXT */
457  const std::string& fileName, const std::string& use_this_base_dir);
458  /** Forces marking this observation as non-externally stored - it doesn't
459  * anything else apart from reseting the corresponding flag (Users won't
460  * normally want to call this, it's only used from internal MRPT programs)
461  */
463  {
465  }
466  /** @} */
467 
468  /** \name Intensity (RGB) channels
469  * @{ */
470  /** Enum type for intensityImageChannel */
472  {
473  /** Grayscale or RGB visible channel of the camera sensor. */
475  /** Infrarred (IR) channel */
476  CH_IR = 1
477  };
478 
479  /** true means the field intensityImage contains valid data */
480  bool hasIntensityImage{false};
481 
482  /** If hasIntensityImage=true, a color or gray-level intensity image of the
483  * same size than "rangeImage" */
485 
486  /** The source of the intensityImage; typically the visible channel \sa
487  * TIntensityChannelID */
489  /** @} */
490 
491  /** \name Confidence "channel"
492  * @{ */
493  /** true means the field confidenceImage contains valid data */
494  bool hasConfidenceImage{false};
495  /** If hasConfidenceImage=true, an image with the "confidence" value [range
496  * 0-255] as estimated by the capture drivers. */
498  /** @} */
499 
500  /** \name Pixel-wise classification labels (for semantic labeling, etc.)
501  * @{ */
502  /** Returns true if the field CObservation3DRangeScan::pixelLabels contains
503  * a non-NULL smart pointer.
504  * To enhance a 3D point cloud with labeling info, just assign an
505  * appropiate object to \a pixelLabels
506  */
507  bool hasPixelLabels() const { return pixelLabels ? true : false; }
508 
509  /** All information about pixel labeling is stored in this (smart pointer
510  * to) structure; refer to TPixelLabelInfo for details on the contents
511  * User is responsible of creating a new object of the desired data type.
512  * It will be automatically (de)serialized no matter its specific type. */
514 
515  /** @} */
516 
517  /** \name Sensor parameters
518  * @{ */
519 
520  /** Projection parameters of the depth camera. */
522  /** Projection parameters of the intensity (graylevel or RGB) camera. */
524 
525  /** Relative pose of the intensity camera wrt the depth camera (which is the
526  * coordinates origin for this observation).
527  * In a SwissRanger camera, this will be (0,0,0,-90deg,0,-90deg) since
528  * both cameras coincide.
529  * In a Kinect, this will include a small lateral displacement and a
530  * rotation, according to the drawing on the top of this page.
531  * \sa doDepthAndIntensityCamerasCoincide
532  */
534  0, 0, 0, -90.0_deg, 0.0_deg, -90.0_deg};
535 
536  /** Return true if \a relativePoseIntensityWRTDepth equals the pure rotation
537  * (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
538  * \sa relativePoseIntensityWRTDepth
539  */
541 
542  /** The maximum range allowed by the device, in meters (e.g. 8.0m, 5.0m,...)
543  */
544  float maxRange{5.0f};
545  /** The 6D pose of the sensor on the robot. */
547  /** The "sigma" error of the device in meters, used while inserting the scan
548  * in an occupancy grid. */
549  float stdError{0.01f};
550 
551  // See base class docs
552  void getSensorPose(mrpt::poses::CPose3D& out_sensorPose) const override
553  {
554  out_sensorPose = sensorPose;
555  }
556  // See base class docs
557  void setSensorPose(const mrpt::poses::CPose3D& newSensorPose) override
558  {
559  sensorPose = newSensorPose;
560  }
561 
562  /** @} */ // end sensor params
563 
564  /** Removes the distortion in both, depth and intensity images. Intrinsics
565  * (fx,fy,cx,cy) remains the same for each image after undistortion.
566  */
567  void undistort();
568 
569  // See base class docs
570  void getDescriptionAsText(std::ostream& o) const override;
571 
572  /** Very efficient method to swap the contents of two observations. */
573  void swap(CObservation3DRangeScan& o);
574  /** Extract a ROI of the 3D observation as a new one. \note PixelLabels are
575  * *not* copied to the output subimage. */
576  void getZoneAsObs(
577  CObservation3DRangeScan& obs, const unsigned int& r1,
578  const unsigned int& r2, const unsigned int& c1, const unsigned int& c2);
579 
580  /** A Levenberg-Marquart-based optimizer to recover the calibration
581  * parameters of a 3D camera given a range (depth) image and the
582  * corresponding 3D point cloud.
583  * \param camera_offset The offset (in meters) in the +X direction of the
584  * point cloud. It's 1cm for SwissRanger SR4000.
585  * \return The final average reprojection error per pixel (typ <0.05 px)
586  */
588  const CObservation3DRangeScan& in_obs,
589  mrpt::img::TCamera& out_camParams, const double camera_offset = 0.01);
590 
591  /** Look-up-table struct for unprojectInto() */
593  {
594  /** x,y,z: +X pointing forward (depth), +z up
595  * These doesn't account for the sensor pose.
596  */
598 
599  /** x,y,z: in the rotated frame of coordinates of the sensorPose.
600  * So, sensed points are directly measured_range(or depth) times this
601  * vector, plus the translation (no rotation!) of the sensor pose.
602  */
604  };
605 
606  /** Gets (or generates upon first request) the 3D point cloud projection
607  * look-up-table for the current depth camera intrinsics & distortion
608  * parameters.
609  * Returns a const reference to a global variable. Multithread safe.
610  * \sa unprojectInto */
611  const unproject_LUT_t& get_unproj_lut() const;
612 
613 }; // End of class def.
614 } // namespace mrpt::obs
615 
616 namespace mrpt::opengl
617 {
618 /** Specialization mrpt::opengl::PointCloudAdapter<CObservation3DRangeScan>
619  * \ingroup mrpt_adapters_grp */
620 template <>
622 {
623  private:
625 
626  public:
627  /** The type of each point XYZ coordinates */
628  using coords_t = float;
629  /** Has any color RGB info? */
630  static constexpr bool HAS_RGB = false;
631  /** Has native RGB info (as floats)? */
632  static constexpr bool HAS_RGBf = false;
633  /** Has native RGB info (as uint8_t)? */
634  static constexpr bool HAS_RGBu8 = false;
635 
636  /** Constructor (accept a const ref for convenience) */
638  : m_obj(*const_cast<mrpt::obs::CObservation3DRangeScan*>(&obj))
639  {
640  }
641  /** Get number of points */
642  inline size_t size() const { return m_obj.points3D_x.size(); }
643  /** Set number of points (to uninitialized values) */
644  inline void resize(const size_t N)
645  {
646  if (N) m_obj.hasPoints3D = true;
647  m_obj.resizePoints3DVectors(N);
648  }
649  /** Does nothing as of now */
650  inline void setDimensions(size_t height, size_t width) {}
651  /** Get XYZ coordinates of i'th point */
652  template <typename T>
653  inline void getPointXYZ(const size_t idx, T& x, T& y, T& z) const
654  {
655  x = m_obj.points3D_x[idx];
656  y = m_obj.points3D_y[idx];
657  z = m_obj.points3D_z[idx];
658  }
659  /** Set XYZ coordinates of i'th point */
660  inline void setPointXYZ(
661  const size_t idx, const coords_t x, const coords_t y, const coords_t z)
662  {
663  m_obj.points3D_x[idx] = x;
664  m_obj.points3D_y[idx] = y;
665  m_obj.points3D_z[idx] = z;
666  }
667  /** Set XYZ coordinates of i'th point */
668  inline void setInvalidPoint(const size_t idx)
669  {
670  m_obj.points3D_x[idx] = 0;
671  m_obj.points3D_y[idx] = 0;
672  m_obj.points3D_z[idx] = 0;
673  }
674 
675 }; // end of PointCloudAdapter<CObservation3DRangeScan>
676 } // namespace mrpt::opengl
681 
682 #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>
std::map< std::string, mrpt::math::CMatrix_u16 > rangeImageOtherLayers
Additional layer range/depth images.
mrpt::aligned_std_vector< float > Kxs
x,y,z: +X pointing forward (depth), +z up These doesn&#39;t account for the sensor pose.
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
void unprojectInto(POINTMAP &dest_pointcloud, const T3DPointsProjectionParams &projectParams=T3DPointsProjectionParams(), const TRangeImageFilterParams &filterParams=TRangeImageFilterParams())
Unprojects the RGB+D image pair into a 3D point cloud (with color if the target map supports it) and ...
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 ...
MRPT_FILL_ENUM_MEMBER(mrpt::obs::CObservation3DRangeScan, CH_VISIBLE)
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::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.
bool m_rangeImage_external_stored
If set to true, m_rangeImage_external_file is valid.
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
void undistort()
Removes the distortion in both, depth and intensity images.
void setDimensions(size_t height, size_t width)
Does nothing as of now.
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
void resize(const size_t N)
Set number of points (to uninitialized values)
Used in CObservation3DRangeScan::unprojectInto()
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.
PointCloudAdapter(const mrpt::obs::CObservation3DRangeScan &obj)
Constructor (accept a const ref for convenience)
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.
mrpt::aligned_std_vector< float > Kxs_rot
x,y,z: in the rotated frame of coordinates of the sensorPose.
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 unprojectInto(mrpt::obs::CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const mrpt::obs::T3DPointsProjectionParams &projectParams, const mrpt::obs::TRangeImageFilterParams &filterParams)
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"...
Used in CObservation3DRangeScan::convertTo2DScan()
std::string m_points3D_external_file
3D points are in CImage::getImagesPathBase()+<this_file_name>
This namespace contains representation of robot actions and observations.
bool hasRangeImage
true means the field rangeImage contains valid data
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:26
mrpt::math::CMatrix_u16 rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters)...
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.
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid...
void points3D_overrideExternalStoredFlag(bool isExternal)
Users normally won&#39;t need to use this.
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.
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
std::string rangeImage_getExternalStorageFileAbsolutePath(const std::string &rangeImageLayer) const
void rangeImage_getExternalStorageFileAbsolutePath(std::string &out_path, const std::string &rangeImageLayer) const
rangeImageLayer: Empty for the main rangeImage matrix, otherwise, a key of rangeImageOtherLayers ...
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.
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
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...
std::string rangeImage_getExternalStorageFile(const std::string &rangeImageLayer) const
Used in CObservation3DRangeScan::unprojectInto()
bool hasIntensityImage
true means the field intensityImage contains valid data
void setInvalidPoint(const size_t idx)
Set XYZ coordinates of i&#39;th point.
std::vector< float > points3D_x
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera...
mrpt::img::CImage rangeImage_getAsImage(const std::optional< mrpt::img::TColormap > color=std::nullopt, const std::optional< float > normMinRange=std::nullopt, const std::optional< float > normMaxRange=std::nullopt, const std::optional< std::string > additionalLayerName=std::nullopt) const
Builds a visualization from the rangeImage.
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
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 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.
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
bool doDepthAndIntensityCamerasCoincide() const
Return true if relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
size_t getScanSize() const
Get the size of the scan pointcloud.
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
float rangeUnits
The conversion factor from integer units in rangeImage and actual distances in meters.
static mrpt::img::CImage rangeImageAsImage(const mrpt::math::CMatrix_u16 &ranges, float val_min, float val_max, float rangeUnits, const std::optional< mrpt::img::TColormap > color=std::nullopt)
Static method to convert a range matrix into an image.
Grayscale or RGB visible channel of the camera sensor.
const unproject_LUT_t & get_unproj_lut() const
Gets (or generates upon first request) the 3D point cloud projection look-up-table for the current de...
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:148
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 2.0.5 Git: e84511500 Thu Jul 2 23:53:34 2020 +0200 at vie jul 3 00:00:10 CEST 2020