MRPT  2.0.3
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"
MRPT_FILL_ENUM_MEMBER
MRPT_FILL_ENUM_MEMBER(mrpt::obs::CObservation3DRangeScan, CH_VISIBLE)
mrpt::obs::CObservation3DRangeScan::TIntensityChannelID
TIntensityChannelID
Enum type for intensityImageChannel.
Definition: CObservation3DRangeScan.h:471
mrpt::opengl::PointCloudAdapter
An adapter to different kinds of point cloud object.
Definition: pointcloud_adapters.h:37
mrpt::obs::CObservation3DRangeScan::points3D_overrideExternalStoredFlag
void points3D_overrideExternalStoredFlag(bool isExternal)
Users normally won't need to use this.
Definition: CObservation3DRangeScan.h:367
mrpt::opengl::PointCloudAdapter< mrpt::obs::CObservation3DRangeScan >::resize
void resize(const size_t N)
Set number of points (to uninitialized values)
Definition: CObservation3DRangeScan.h:644
mrpt::obs::CObservation3DRangeScan::load
void load() const override
Makes sure all images and other fields which may be externally stored are loaded in memory.
Definition: CObservation3DRangeScan.cpp:623
mrpt::obs::CObservation3DRangeScan::unproject_LUT_t::Kxs
mrpt::aligned_std_vector< float > Kxs
x,y,z: +X pointing forward (depth), +z up These doesn't account for the sensor pose.
Definition: CObservation3DRangeScan.h:597
mrpt::opengl::PointCloudAdapter< mrpt::obs::CObservation3DRangeScan >::setInvalidPoint
void setInvalidPoint(const size_t idx)
Set XYZ coordinates of i'th point.
Definition: CObservation3DRangeScan.h:668
mrpt::obs::CObservation3DRangeScan::range_is_depth
bool range_is_depth
true: Kinect-like ranges: entries of rangeImage are distances along the +X axis; false: Ranges in ran...
Definition: CObservation3DRangeScan.h:399
mrpt::obs::CObservation3DRangeScan::hasPoints3D
bool hasPoints3D
true means the field points3D contains valid data.
Definition: CObservation3DRangeScan.h:326
MRPT_ENUM_TYPE_END
#define MRPT_ENUM_TYPE_END()
Definition: TEnumType.h:78
mrpt::obs::CObservation3DRangeScan::points3D_getExternalStorageFile
std::string points3D_getExternalStorageFile() const
Definition: CObservation3DRangeScan.h:350
mrpt::obs::T3DPointsProjectionParams
Used in CObservation3DRangeScan::unprojectInto()
Definition: T3DPointsProjectionParams.h:20
mrpt::opengl::PointCloudAdapter< mrpt::obs::CObservation3DRangeScan >::PointCloudAdapter
PointCloudAdapter(const mrpt::obs::CObservation3DRangeScan &obj)
Constructor (accept a const ref for convenience)
Definition: CObservation3DRangeScan.h:637
mrpt::obs::CObservation3DRangeScan::rangeImage_getExternalStorageFileAbsolutePath
void rangeImage_getExternalStorageFileAbsolutePath(std::string &out_path, const std::string &rangeImageLayer) const
rangeImageLayer: Empty for the main rangeImage matrix, otherwise, a key of rangeImageOtherLayers
Definition: CObservation3DRangeScan.cpp:723
mrpt::obs::TPixelLabelInfoBase::Ptr
std::shared_ptr< TPixelLabelInfoBase > Ptr
Used in CObservation3DRangeScan::pixelLabels.
Definition: TPixelLabelInfo.h:33
DEFINE_SERIALIZABLE
#define DEFINE_SERIALIZABLE(class_name, NS)
This declaration must be inserted in all CSerializable classes definition, within the class declarati...
Definition: CSerializable.h:152
mrpt::obs::CObservation3DRangeScan::cameraParamsIntensity
mrpt::img::TCamera cameraParamsIntensity
Projection parameters of the intensity (graylevel or RGB) camera.
Definition: CObservation3DRangeScan.h:523
mrpt::opengl::PointCloudAdapter< mrpt::obs::CObservation3DRangeScan >::coords_t
float coords_t
The type of each point XYZ coordinates.
Definition: CObservation3DRangeScan.h:628
mrpt::obs::CObservation2DRangeScan
A "CObservation"-derived class that represents a 2D range scan measurement (typically from a laser sc...
Definition: CObservation2DRangeScan.h:54
mrpt::obs::CObservation3DRangeScan::undistort
void undistort()
Removes the distortion in both, depth and intensity images.
Definition: CObservation3DRangeScan.cpp:1490
mrpt::obs::CObservation3DRangeScan::unproject_LUT_t::Kys_rot
mrpt::aligned_std_vector< float > Kys_rot
Definition: CObservation3DRangeScan.h:603
mrpt::obs::CObservation3DRangeScan::recoverCameraCalibrationParameters
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...
Definition: CObservation3DRangeScan.cpp:967
mrpt::opengl::PointCloudAdapter< mrpt::obs::CObservation3DRangeScan >::getPointXYZ
void getPointXYZ(const size_t idx, T &x, T &y, T &z) const
Get XYZ coordinates of i'th point.
Definition: CObservation3DRangeScan.h:653
mrpt::obs::CObservation3DRangeScan::convertTo2DScan
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...
Definition: CObservation3DRangeScan.cpp:1196
mrpt::obs::CObservation3DRangeScan::points3D_getExternalStorageFileAbsolutePath
std::string points3D_getExternalStorageFileAbsolutePath() const
Definition: CObservation3DRangeScan.h:356
mrpt::obs::CObservation3DRangeScan::m_points3D_external_stored
bool m_points3D_external_stored
If set to true, m_points3D_external_file is valid.
Definition: CObservation3DRangeScan.h:174
mrpt::obs::CObservation3DRangeScan::points3D_z
std::vector< float > points3D_z
Definition: CObservation3DRangeScan.h:329
color_maps.h
mrpt::obs::CObservation3DRangeScan::~CObservation3DRangeScan
~CObservation3DRangeScan() override
Definition: CObservation3DRangeScan.cpp:290
mrpt::obs::CObservation3DRangeScan::unproject_LUT_t::Kys
mrpt::aligned_std_vector< float > Kys
Definition: CObservation3DRangeScan.h:597
mrpt::obs::CObservation3DRangeScan::rangeImageOtherLayers
std::map< std::string, mrpt::math::CMatrix_u16 > rangeImageOtherLayers
Additional layer range/depth images.
Definition: CObservation3DRangeScan.h:388
T3DPointsTo2DScanParams.h
mrpt::obs::CObservation3DRangeScan::points3D_convertToExternalStorage
void points3D_convertToExternalStorage(const std::string &fileName, const std::string &use_this_base_dir)
Users won't normally want to call this, it's only used from internal MRPT programs.
Definition: CObservation3DRangeScan.cpp:764
mrpt::obs::CObservation3DRangeScan::rangeImage_getExternalStorageFile
std::string rangeImage_getExternalStorageFile(const std::string &rangeImageLayer) const
Definition: CObservation3DRangeScan.cpp:710
mrpt::obs::CObservation3DRangeScan::rangeImageAsImage
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.
Definition: CObservation3DRangeScan.cpp:1561
mrpt
This is the global namespace for all Mobile Robot Programming Toolkit (MRPT) libraries.
Definition: BaseAppDataSource.h:15
mrpt::obs::CObservation3DRangeScan::hasPixelLabels
bool hasPixelLabels() const
Returns true if the field CObservation3DRangeScan::pixelLabels contains a non-NULL smart pointer.
Definition: CObservation3DRangeScan.h:507
mrpt::obs::CObservation3DRangeScan::rangeImage_isExternallyStored
bool rangeImage_isExternallyStored() const
Definition: CObservation3DRangeScan.h:436
mrpt::obs::CObservation3DRangeScan::unprojectInto
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 ...
Definition: CObservation3DRangeScan.h:257
MRPT_ENUM_TYPE_BEGIN
#define MRPT_ENUM_TYPE_BEGIN(_ENUM_TYPE_WITH_NS)
Definition: TEnumType.h:62
mrpt::obs::CObservation3DRangeScan::points3D_y
std::vector< float > points3D_y
Definition: CObservation3DRangeScan.h:329
CPose2D.h
mrpt::obs::T3DPointsTo2DScanParams
Used in CObservation3DRangeScan::convertTo2DScan()
Definition: T3DPointsTo2DScanParams.h:18
mrpt::obs::CObservation3DRangeScan::rangeImage_setSize
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 ...
Definition: CObservation3DRangeScan.cpp:1148
mrpt::obs
This namespace contains representation of robot actions and observations.
Definition: CParticleFilter.h:17
mrpt::obs::CObservation3DRangeScan::unproject_LUT_t::Kzs
mrpt::aligned_std_vector< float > Kzs
Definition: CObservation3DRangeScan.h:597
mrpt::obs::CObservation3DRangeScan::resizePoints3DVectors
void resizePoints3DVectors(const size_t nPoints)
Use this method instead of resizing all three points3D_x, points3D_y & points3D_z to allow the usage ...
Definition: CObservation3DRangeScan.cpp:1085
mrpt::obs::CObservation3DRangeScan::setSensorPose
void setSensorPose(const mrpt::poses::CPose3D &newSensorPose) override
A general method to change the sensor pose on the robot.
Definition: CObservation3DRangeScan.h:557
mrpt::obs::CObservation3DRangeScan::cameraParams
mrpt::img::TCamera cameraParams
Projection parameters of the depth camera.
Definition: CObservation3DRangeScan.h:521
TPixelLabelInfo.h
mrpt::obs::CObservation3DRangeScan::getDescriptionAsText
void getDescriptionAsText(std::ostream &o) const override
Build a detailed, multi-line textual description of the observation contents and dump it to the outpu...
Definition: CObservation3DRangeScan.cpp:1362
mrpt::obs::CObservation3DRangeScan::pixelLabels
TPixelLabelInfoBase::Ptr pixelLabels
All information about pixel labeling is stored in this (smart pointer to) structure; refer to TPixelL...
Definition: CObservation3DRangeScan.h:513
mrpt::obs::CObservation3DRangeScan::relativePoseIntensityWRTDepth
mrpt::poses::CPose3D relativePoseIntensityWRTDepth
Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this ...
Definition: CObservation3DRangeScan.h:533
mrpt::aligned_std_vector
std::vector< T, mrpt::aligned_allocator_cpp11< T > > aligned_std_vector
Definition: aligned_std_vector.h:15
mrpt::obs::CObservation3DRangeScan::getScanSize
size_t getScanSize() const
Get the size of the scan pointcloud.
Definition: CObservation3DRangeScan.cpp:1140
T3DPointsProjectionParams.h
mrpt::obs::CObservation3DRangeScan::unproject_LUT_t::Kxs_rot
mrpt::aligned_std_vector< float > Kxs_rot
x,y,z: in the rotated frame of coordinates of the sensorPose.
Definition: CObservation3DRangeScan.h:603
mrpt::obs::CObservation3DRangeScan::intensityImage
mrpt::img::CImage intensityImage
If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage".
Definition: CObservation3DRangeScan.h:484
mrpt::obs::CObservation3DRangeScan::sensorPose
mrpt::poses::CPose3D sensorPose
The 6D pose of the sensor on the robot.
Definition: CObservation3DRangeScan.h:546
mrpt::obs::CObservation3DRangeScan::CObservation3DRangeScan
CObservation3DRangeScan()=default
TRangeImageFilter.h
mrpt::obs::CObservation3DRangeScan
A range or depth 3D scan measurement, as from a time-of-flight range camera or a structured-light dep...
Definition: CObservation3DRangeScan.h:168
mrpt::obs::CObservation3DRangeScan::intensityImageChannel
TIntensityChannelID intensityImageChannel
The source of the intensityImage; typically the visible channel.
Definition: CObservation3DRangeScan.h:488
mrpt::poses::CPose3D
A class used to store a 3D pose (a 3D translation + a rotation in 3D).
Definition: CPose3D.h:85
mrpt::obs::CObservation3DRangeScan::hasIntensityImage
bool hasIntensityImage
true means the field intensityImage contains valid data
Definition: CObservation3DRangeScan.h:480
mrpt::obs::CObservation3DRangeScan::m_rangeImage_external_file
std::string m_rangeImage_external_file
rangeImage is in CImage::getImagesPathBase()+<this_file_name>
Definition: CObservation3DRangeScan.h:181
mrpt::obs::CObservation3DRangeScan::points3D_x
std::vector< float > points3D_x
If hasPoints3D=true, the (X,Y,Z) coordinates of the 3D point cloud detected by the camera.
Definition: CObservation3DRangeScan.h:329
TEnumType.h
mrpt::obs::CObservation3DRangeScan::hasRangeImage
bool hasRangeImage
true means the field rangeImage contains valid data
Definition: CObservation3DRangeScan.h:376
mrpt::obs::CObservation3DRangeScan::stdError
float stdError
The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid.
Definition: CObservation3DRangeScan.h:549
aligned_std_vector.h
mrpt::obs::CObservation3DRangeScan::points3D_idxs_y
std::vector< uint16_t > points3D_idxs_y
Definition: CObservation3DRangeScan.h:332
mrpt::img::TCamera
Parameters for the Brown-Conrady camera lens distortion model.
Definition: TCamera.h:26
mrpt::obs::CObservation3DRangeScan::confidenceImage
mrpt::img::CImage confidenceImage
If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the ca...
Definition: CObservation3DRangeScan.h:497
mrpt::obs::CObservation3DRangeScan::points3D_idxs_x
std::vector< uint16_t > points3D_idxs_x
If hasPoints3D=true, the (x,y) pixel coordinates for each (X,Y,Z) point in points3D_x,...
Definition: CObservation3DRangeScan.h:332
mrpt::img::CImage
A class for storing images as grayscale or RGB bitmaps.
Definition: img/CImage.h:148
CPolygon.h
mrpt::obs::detail::unprojectInto
void unprojectInto(mrpt::obs::CObservation3DRangeScan &src_obs, POINTMAP &dest_pointcloud, const mrpt::obs::T3DPointsProjectionParams &projectParams, const mrpt::obs::TRangeImageFilterParams &filterParams)
Definition: CObservation3DRangeScan_project3D_impl.h:111
CPose3D.h
mrpt::obs::CObservation3DRangeScan::get_unproj_lut
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...
Definition: CObservation3DRangeScan.cpp:80
mrpt::obs::CObservation3DRangeScan::CH_IR
@ CH_IR
Infrarred (IR) channel.
Definition: CObservation3DRangeScan.h:476
mrpt::obs::CObservation3DRangeScan::rangeImage
mrpt::math::CMatrix_u16 rangeImage
If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters).
Definition: CObservation3DRangeScan.h:384
mrpt::obs::CObservation3DRangeScan::maxRange
float maxRange
The maximum range allowed by the device, in meters (e.g.
Definition: CObservation3DRangeScan.h:544
mrpt::obs::CObservation3DRangeScan::hasConfidenceImage
bool hasConfidenceImage
true means the field confidenceImage contains valid data
Definition: CObservation3DRangeScan.h:494
serialization_frwds.h
mrpt::obs::CObservation3DRangeScan::m_points3D_external_file
std::string m_points3D_external_file
3D points are in CImage::getImagesPathBase()+<this_file_name>
Definition: CObservation3DRangeScan.h:176
mrpt::obs::CObservation3DRangeScan::unproject_LUT_t::Kzs_rot
mrpt::aligned_std_vector< float > Kzs_rot
Definition: CObservation3DRangeScan.h:603
mrpt::obs::CObservation3DRangeScan::doDepthAndIntensityCamerasCoincide
bool doDepthAndIntensityCamerasCoincide() const
Return true if relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,...
Definition: CObservation3DRangeScan.cpp:1182
CObservation.h
pointcloud_adapters.h
mrpt::obs::CObservation3DRangeScan::getSensorPose
void getSensorPose(mrpt::poses::CPose3D &out_sensorPose) const override
A general method to retrieve the sensor pose on the robot.
Definition: CObservation3DRangeScan.h:552
mrpt::obs::CObservation3DRangeScan::CH_VISIBLE
@ CH_VISIBLE
Grayscale or RGB visible channel of the camera sensor.
Definition: CObservation3DRangeScan.h:474
mrpt::obs::CObservation3DRangeScan::getZoneAsObs
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.
Definition: CObservation3DRangeScan.cpp:1023
mrpt::obs::CObservation3DRangeScan::rangeImage_convertToExternalStorage
void rangeImage_convertToExternalStorage(const std::string &fileName, const std::string &use_this_base_dir)
Users won't normally want to call this, it's only used from internal MRPT programs.
Definition: CObservation3DRangeScan.cpp:814
mrpt::obs::CObservation
Declares a class that represents any robot's observation.
Definition: CObservation.h:43
mrpt::opengl::PointCloudAdapter< mrpt::obs::CObservation3DRangeScan >::setPointXYZ
void setPointXYZ(const size_t idx, const coords_t x, const coords_t y, const coords_t z)
Set XYZ coordinates of i'th point.
Definition: CObservation3DRangeScan.h:660
mrpt::obs::CObservation3DRangeScan::points3D_isExternallyStored
bool points3D_isExternallyStored() const
Definition: CObservation3DRangeScan.h:346
CImage.h
mrpt::opengl::PointCloudAdapter< mrpt::obs::CObservation3DRangeScan >::size
size_t size() const
Get number of points.
Definition: CObservation3DRangeScan.h:642
mrpt::obs::CObservation3DRangeScan::unload
void unload() override
Unload all images, for the case they being delayed-load images stored in external files (othewise,...
Definition: CObservation3DRangeScan.cpp:695
mrpt::obs::CObservation3DRangeScan::rangeImage_getExternalStorageFileAbsolutePath
std::string rangeImage_getExternalStorageFileAbsolutePath(const std::string &rangeImageLayer) const
Definition: CObservation3DRangeScan.h:447
mrpt::opengl::PointCloudAdapter< mrpt::obs::CObservation3DRangeScan >::setDimensions
void setDimensions(size_t height, size_t width)
Does nothing as of now.
Definition: CObservation3DRangeScan.h:650
CMatrixF.h
CSerializable.h
mrpt::obs::CObservation3DRangeScan::rangeUnits
float rangeUnits
The conversion factor from integer units in rangeImage and actual distances in meters.
Definition: CObservation3DRangeScan.h:393
mrpt::opengl::PointCloudAdapter< mrpt::obs::CObservation3DRangeScan >::m_obj
mrpt::obs::CObservation3DRangeScan & m_obj
Definition: CObservation3DRangeScan.h:624
mrpt::obs::CObservation3DRangeScan::swap
void swap(CObservation3DRangeScan &o)
Very efficient method to swap the contents of two observations.
Definition: CObservation3DRangeScan.cpp:584
mrpt::opengl
The namespace for 3D scene representation and rendering.
Definition: CGlCanvasBase.h:13
mrpt::obs::CObservation3DRangeScan::EXTERNALS_AS_TEXT
static bool EXTERNALS_AS_TEXT()
Definition: CObservation3DRangeScan.cpp:186
mrpt::obs::CObservation3DRangeScan::rangeImage_forceResetExternalStorage
void rangeImage_forceResetExternalStorage()
Forces marking this observation as non-externally stored - it doesn't anything else apart from reseti...
Definition: CObservation3DRangeScan.h:462
mrpt::math::CMatrixDynamic< uint16_t >
mrpt::obs::TRangeImageFilterParams
Used in CObservation3DRangeScan::unprojectInto()
Definition: TRangeImageFilter.h:17
mrpt::obs::CObservation3DRangeScan::unproject_LUT_t
Look-up-table struct for unprojectInto()
Definition: CObservation3DRangeScan.h:592
CObservation2DRangeScan.h
mrpt::obs::CObservation3DRangeScan::rangeImage_getAsImage
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.
Definition: CObservation3DRangeScan.cpp:1615
mrpt::obs::CObservation3DRangeScan::m_rangeImage_external_stored
bool m_rangeImage_external_stored
If set to true, m_rangeImage_external_file is valid.
Definition: CObservation3DRangeScan.h:179



Page generated by Doxygen 1.8.17 for MRPT 2.0.3 at Fri May 15 23:51:15 UTC 2020