Main MRPT website > C++ reference
MRPT logo
CObservation3DRangeScan.h
Go to the documentation of this file.
1 /* +---------------------------------------------------------------------------+
2  | The Mobile Robot Programming Toolkit (MRPT) C++ library |
3  | |
4  | http://www.mrpt.org/ |
5  | |
6  | Copyright (C) 2005-2012 University of Malaga |
7  | |
8  | This software was written by the Machine Perception and Intelligent |
9  | Robotics Lab, University of Malaga (Spain). |
10  | Contact: Jose-Luis Blanco <jlblanco@ctima.uma.es> |
11  | |
12  | This file is part of the MRPT project. |
13  | |
14  | MRPT is free software: you can redistribute it and/or modify |
15  | it under the terms of the GNU General Public License as published by |
16  | the Free Software Foundation, either version 3 of the License, or |
17  | (at your option) any later version. |
18  | |
19  | MRPT is distributed in the hope that it will be useful, |
20  | but WITHOUT ANY WARRANTY; without even the implied warranty of |
21  | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
22  | GNU General Public License for more details. |
23  | |
24  | You should have received a copy of the GNU General Public License |
25  | along with MRPT. If not, see <http://www.gnu.org/licenses/>. |
26  | |
27  +---------------------------------------------------------------------------+ */
28 #ifndef CObservation3DRangeScan_H
29 #define CObservation3DRangeScan_H
30 
31 #include <mrpt/utils/CSerializable.h>
32 #include <mrpt/utils/CImage.h>
33 #include <mrpt/slam/CObservation.h>
34 #include <mrpt/slam/CObservation2DRangeScan.h>
35 #include <mrpt/poses/CPose3D.h>
36 #include <mrpt/poses/CPose2D.h>
37 #include <mrpt/math/CPolygon.h>
38 #include <mrpt/utils/adapters.h>
39 
40 
41 namespace mrpt
42 {
43 namespace slam
44 {
45  DEFINE_SERIALIZABLE_PRE_CUSTOM_BASE_LINKAGE( CObservation3DRangeScan, CObservation,OBS_IMPEXP )
46 
47  namespace detail {
48  // Implemented in CObservation3DRangeScan_project3D_impl.h
49  template <class POINTMAP>
50  void project3DPointsFromDepthImageInto(CObservation3DRangeScan & src_obs,POINTMAP & dest_pointcloud,const bool takeIntoAccountSensorPoseOnRobot,const mrpt::poses::CPose3D * robotPoseInTheWorld,const bool PROJ3D_USE_LUT);
51  }
52 
53  /** Declares a class derived from "CObservation" that
54  * encapsules a 3D range scan measurement (e.g. from a time of flight range camera).
55  * This kind of observations can carry one or more of these data fields:
56  * - 3D point cloud (as float's).
57  * - 2D range image (as a matrix): Each entry in the matrix "rangeImage(ROW,COLUMN)" contains a distance or a depth (in meters), depending on \a range_is_depth.
58  * - 2D intensity (grayscale or RGB) image (as a mrpt::utils::CImage): For SwissRanger cameras, a logarithmic A-law compression is used to convert the original 16bit intensity to a more standard 8bit graylevel.
59  * - 2D confidence image (as a mrpt::utils::CImage): For each pixel, a 0x00 and a 0xFF mean the lowest and highest confidence levels, respectively.
60  *
61  * The coordinates of the 3D point cloud are in meters with respect to the depth camera origin of coordinates
62  * (in SwissRanger, the front face of the camera: a small offset ~1cm in front of the physical focal point),
63  * with the +X axis pointing forward, +Y pointing left-hand and +Z pointing up. By convention, a 3D point with its coordinates set to (0,0,0), will be considered as invalid.
64  * The field CObservation3DRangeScan::relativePoseIntensityWRTDepth describes the change of coordinates from
65  * the depth camera to the intensity (RGB or grayscale) camera. In a SwissRanger camera both cameras coincide,
66  * so this pose is just a rotation (0,0,0,-90deg,0,-90deg). But in
67  * Microsoft Kinect there is also an offset, as shown in this figure:
68  *
69  * <div align=center>
70  * <img src="CObservation3DRangeScan_figRefSystem.png">
71  * </div>
72  *
73  * In any case, check the field \a relativePoseIntensityWRTDepth, or the method \a doDepthAndIntensityCamerasCoincide()
74  * to determine if both frames of reference coincide, since even for Kinect cameras both can coincide if the images
75  * have been rectified.
76  *
77  * The 2D images and matrices are stored as common images, with an up->down rows order and left->right, as usual.
78  * Optionally, the intensity and confidence channels can be set to delayed-load images for off-rawlog storage so it saves
79  * memory by having loaded in memory just the needed images. See the methods load() and unload().
80  * Due to the intensive storage requirements of this kind of observations, this observation is the only one in MRPT
81  * for which it's recommended to always call "load()" and "unload()" before and after using the observation, *ONLY* when
82  * the observation was read from a rawlog dataset, in order to make sure that all the externally stored data fields are
83  * loaded and ready in memory.
84  *
85  * Classes that grab observations of this type are:
86  * - mrpt::hwdrivers::CSwissRanger3DCamera
87  * - mrpt::hwdrivers::CKinect
88  *
89  * There are two sets of calibration parameters (see mrpt::vision::checkerBoardStereoCalibration() or the ready-to-use GUI program <a href="http://www.mrpt.org/Application:kinect-calibrate" >kinect-calibrate</a>):
90  * - cameraParams: Projection parameters of the depth camera.
91  * - cameraParamsIntensity: Projection parameters of the intensity (gray-level or RGB) camera.
92  *
93  * In some cameras, like SwissRanger, both are the same. It is possible in Kinect to rectify the range images such both cameras
94  * seem to coincide and then both sets of camera parameters will be identical.
95  *
96  * Range data can be interpreted in two different ways depending on the 3D camera (this field is already set to the
97  * correct setting when grabbing observations from an mrpt::hwdrivers sensor):
98  * - range_is_depth=true -> Kinect-like ranges: entries of \a rangeImage are distances along the +X axis
99  * - range_is_depth=false -> Ranges in \a rangeImage are actual distances in 3D.
100  *
101  * The "intensity" channel may come from different channels in sesnsors as Kinect. Look at field \a intensityImageChannel to
102  * find out if the image was grabbed from the visible (RGB) or IR channels.
103  *
104  * 3D point clouds can be generated at any moment after grabbing with CObservation3DRangeScan::project3DPointsFromDepthImage(), provided the correct
105  * calibration parameters.
106  *
107  * \note Starting at serialization version 2 (MRPT 0.9.1+), the confidence channel is stored as an image instead of a matrix to optimize memory and disk space.
108  * \note Starting at serialization version 3 (MRPT 0.9.1+), the 3D point cloud and the rangeImage can both be stored externally to save rawlog space.
109  * \note Starting at serialization version 5 (MRPT 0.9.5+), the new field \a range_is_depth
110  * \note Starting at serialization version 6 (MRPT 0.9.5+), the new field \a intensityImageChannel
111  *
112  * \sa mrpt::hwdrivers::CSwissRanger3DCamera, mrpt::hwdrivers::CKinect, CObservation
113  * \ingroup mrpt_obs_grp
114  */
115  class OBS_IMPEXP CObservation3DRangeScan : public CObservation
116  {
117  // This must be added to any CSerializable derived class:
118  DEFINE_SERIALIZABLE( CObservation3DRangeScan )
119 
120  protected:
121  bool m_points3D_external_stored; //!< If set to true, m_points3D_external_file is valid.
122  std::string m_points3D_external_file; //!< 3D points are in CImage::IMAGES_PATH_BASE+<this_file_name>
123 
124  bool m_rangeImage_external_stored; //!< If set to true, m_rangeImage_external_file is valid.
125  std::string m_rangeImage_external_file; //!< rangeImage is in CImage::IMAGES_PATH_BASE+<this_file_name>
126 
127  public:
128  CObservation3DRangeScan( ); //!< Default constructor
129  virtual ~CObservation3DRangeScan( ); //!< Destructor
130 
131  /** @name Delayed-load manual control methods.
132  @{ */
133  /** Makes sure all images and other fields which may be externally stored are loaded in memory.
134  * Note that for all CImages, calling load() is not required since the images will be automatically loaded upon first access, so load() shouldn't be needed to be called in normal cases by the user.
135  * If all the data were alredy loaded or this object has no externally stored data fields, calling this method has no effects.
136  * \sa unload
137  */
138  virtual void load() const;
139  /** Unload all images, for the case they being delayed-load images stored in external files (othewise, has no effect).
140  * \sa load
141  */
142  virtual void unload();
143  /** @} */
144 
145  /** Project the RGB+D images into a 3D point cloud (with color if the target map supports it) and optionally at a given 3D pose.
146  * The 3D point coordinates are computed from the depth image (\a rangeImage) and the depth camera camera parameters (\a cameraParams).
147  * There exist two set of formulas for projecting the i'th point, depending on the value of "range_is_depth".
148  * In all formulas below, "rangeImage" is the matrix of ranges and the pixel coordinates are (r,c).
149  *
150  * 1) [range_is_depth=true] With "range equals depth" or "Kinect-like depth mode": the range values
151  * are in fact distances along the "+X" axis, not real 3D ranges (this is the way Kinect reports ranges):
152  *
153  * \code
154  * x(i) = rangeImage(r,c)
155  * y(i) = (r_cx - c) * x(i) / r_fx
156  * z(i) = (r_cy - r) * x(i) / r_fy
157  * \endcode
158  *
159  *
160  * 2) [range_is_depth=false] With "normal ranges": range means distance in 3D. This must be set when
161  * processing data from the SwissRange 3D camera, among others.
162  *
163  * \code
164  * Ky = (r_cx - c)/r_fx
165  * Kz = (r_cy - r)/r_fy
166  *
167  * x(i) = rangeImage(r,c) / sqrt( 1 + Ky^2 + Kz^2 )
168  * y(i) = Ky * x(i)
169  * z(i) = Kz * x(i)
170  * \endcode
171  *
172  * The color of each point is determined by projecting the 3D local point into the RGB image using \a cameraParamsIntensity.
173  *
174  * By default the local coordinates of points are directly stored into the local map, but if indicated so in \a takeIntoAccountSensorPoseOnRobot
175  * the points are transformed with \a sensorPose. Furthermore, if provided, those coordinates are transformed with \a robotPoseInTheWorld
176  *
177  * \param[in] PROJ3D_USE_LUT (Only when range_is_depth=true) Whether to use a Look-up-table (LUT) to speed up the conversion. It's thread safe in all situations <b>except</b> when you call this method from different threads <b>and</b> with different camera parameter matrices. In all other cases, it's a good idea to left it enabled.
178  * \tparam POINTMAP Supported maps are all those covered by mrpt::utils::PointCloudAdapter (mrpt::slam::CPointsMap and derived, mrpt::opengl::CPointCloudColoured, PCL point clouds,...)
179  *
180  * \note In MRPT < 0.9.5, this method always assumes that ranges were in Kinect-like format.
181  */
182  template <class POINTMAP>
183  inline void project3DPointsFromDepthImageInto(
184  POINTMAP & dest_pointcloud,
185  const bool takeIntoAccountSensorPoseOnRobot,
186  const mrpt::poses::CPose3D *robotPoseInTheWorld=NULL,
187  const bool PROJ3D_USE_LUT=true)
188  {
189  detail::project3DPointsFromDepthImageInto<POINTMAP>(*this,dest_pointcloud,takeIntoAccountSensorPoseOnRobot,robotPoseInTheWorld,PROJ3D_USE_LUT);
190  }
191 
192  /** This method is equivalent to \c project3DPointsFromDepthImageInto() storing the projected 3D points (without color, in local coordinates) in this same class.
193  * For new code it's recommended to use instead \c project3DPointsFromDepthImageInto() which is much more versatile.
194  */
195  inline void project3DPointsFromDepthImage(const bool PROJ3D_USE_LUT=true) {
196  this->project3DPointsFromDepthImageInto(*this,false,NULL,PROJ3D_USE_LUT);
197  }
198 
199 
200  /** Convert this 3D observation into an "equivalent 2D fake laser scan", with a configurable vertical FOV.
201  *
202  * The result is a 2D laser scan with more "rays" (N) than columns has the 3D observation (W), exactly: N = W * oversampling_ratio.
203  * This oversampling is required since laser scans sample the space at evenly-separated angles, while
204  * a range camera follows a tangent-like distribution. By oversampling we make sure we don't leave "gaps" unseen by the virtual "2D laser".
205  *
206  * All obstacles within a frustum are considered and the minimum distance is kept in each direction.
207  * The horizontal FOV of the frustum is automatically computed from the intrinsic parameters, but the
208  * vertical FOV must be provided by the user, and can be set to be assymetric which may be useful
209  * depending on the zone of interest where to look for obstacles.
210  *
211  * All spatial transformations are riguorosly taken into account in this class, using the depth camera
212  * intrinsic calibration parameters.
213  *
214  * The timestamp of the new object is copied from the 3D object.
215  * Obviously, a requisite for calling this method is the 3D observation having range data,
216  * i.e. hasRangeImage must be true. It's not needed to have RGB data nor the raw 3D point clouds
217  * for this method to work.
218  *
219  * \param[out] out_scan2d The resulting 2D equivalent scan.
220  * \param[in] sensorLabel The sensor label that will have the newly created observation.
221  * \param[in] angle_sup (Default=5deg) The upper half-FOV angle (in radians)
222  * \param[in] angle_sup (Default=5deg) The lower half-FOV angle (in radians)
223  * \param[in] oversampling_ratio (Default=1.2=120%) How many more laser scans rays to create (read above).
224  *
225  * \sa The example in http://www.mrpt.org/Example_Kinect_To_2D_laser_scan
226  */
227  void convertTo2DScan(
228  mrpt::slam::CObservation2DRangeScan & out_scan2d,
229  const std::string & sensorLabel,
230  const double angle_sup = DEG2RAD(5),
231  const double angle_inf = DEG2RAD(5),
232  const double oversampling_ratio = 1.2 );
233 
234 
235  bool hasPoints3D; //!< true means the field points3D contains valid data.
236  std::vector<float> points3D_x; //!< If hasPoints3D=true, the X coordinates of the 3D point cloud detected by the camera. \sa resizePoints3DVectors
237  std::vector<float> points3D_y; //!< If hasPoints3D=true, the Y coordinates of the 3D point cloud detected by the camera. \sa resizePoints3DVectors
238  std::vector<float> points3D_z; //!< If hasPoints3D=true, the Z coordinates of the 3D point cloud detected by the camera. \sa resizePoints3DVectors
239 
240  /** Use this method instead of resizing all three \a points3D_x, \a points3D_y & \a points3D_z to allow the usage of the internal memory pool. */
241  void resizePoints3DVectors(const size_t nPoints);
242 
243  // 3D points external storage functions ---------
244  inline bool points3D_isExternallyStored() const { return m_points3D_external_stored; }
245  inline std::string points3D_getExternalStorageFile() const { return m_points3D_external_file; }
246  void points3D_getExternalStorageFileAbsolutePath(std::string &out_path) const;
247  inline std::string points3D_getExternalStorageFileAbsolutePath() const {
248  std::string tmp;
249  points3D_getExternalStorageFileAbsolutePath(tmp);
250  return tmp;
251  }
252  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.
253  // ---------
254 
255  bool hasRangeImage; //!< true means the field rangeImage contains valid data
256  mrpt::math::CMatrix rangeImage; //!< If hasRangeImage=true, a matrix of floats with the range data as captured by the camera (in meters) \sa range_is_depth
257  bool range_is_depth; //!< true: Kinect-like ranges: entries of \a rangeImage are distances along the +X axis; false: Ranges in \a rangeImage are actual distances in 3D.
258 
259  void rangeImage_setSize(const int HEIGHT, const int WIDTH); //!< Similar to calling "rangeImage.setSize(H,W)" but this method provides memory pooling to speed-up the memory allocation.
260 
261  // Range Matrix external storage functions ---------
262  inline bool rangeImage_isExternallyStored() const { return m_rangeImage_external_stored; }
263  inline std::string rangeImage_getExternalStorageFile() const { return m_rangeImage_external_file; }
264  void rangeImage_getExternalStorageFileAbsolutePath(std::string &out_path) const;
265  inline std::string rangeImage_getExternalStorageFileAbsolutePath() const {
266  std::string tmp;
267  rangeImage_getExternalStorageFileAbsolutePath(tmp);
268  return tmp;
269  }
270  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.
271  /** Forces marking this observation as non-externally stored - it doesn't anything else apart from reseting the corresponding flag (Users won't normally want to call this, it's only used from internal MRPT programs) */
272  void rangeImage_forceResetExternalStorage() { m_rangeImage_external_stored=false; }
273  // ---------
274 
275  /** Enum type for intensityImageChannel */
276  enum TIntensityChannelID
277  {
278  CH_VISIBLE = 0, //!< Grayscale or RGB visible channel of the camera sensor.
279  CH_IR = 1 //!< Infrarred (IR) channel
280  };
281 
282  bool hasIntensityImage; //!< true means the field intensityImage contains valid data
283  mrpt::utils::CImage intensityImage; //!< If hasIntensityImage=true, a color or gray-level intensity image of the same size than "rangeImage"
284  TIntensityChannelID intensityImageChannel; //!< The source of the intensityImage; typically the visible channel \sa TIntensityChannelID
285 
286  bool hasConfidenceImage; //!< true means the field confidenceImage contains valid data
287  mrpt::utils::CImage confidenceImage; //!< If hasConfidenceImage=true, an image with the "confidence" value [range 0-255] as estimated by the capture drivers.
288 
289  mrpt::utils::TCamera cameraParams; //!< Projection parameters of the depth camera.
290  mrpt::utils::TCamera cameraParamsIntensity; //!< Projection parameters of the intensity (graylevel or RGB) camera.
291 
292  /** Relative pose of the intensity camera wrt the depth camera (which is the coordinates origin for this observation).
293  * In a SwissRanger camera, this will be (0,0,0,-90deg,0,-90deg) since both cameras coincide.
294  * In a Kinect, this will include a small lateral displacement and a rotation, according to the drawing on the top of this page.
295  * \sa doDepthAndIntensityCamerasCoincide
296  */
297  mrpt::poses::CPose3D relativePoseIntensityWRTDepth;
298 
299  /** Return true if \a relativePoseIntensityWRTDepth equals the pure rotation (0,0,0,-90deg,0,-90deg) (with a small comparison epsilon)
300  * \sa relativePoseIntensityWRTDepth
301  */
302  bool doDepthAndIntensityCamerasCoincide() const;
303 
304 
305  float maxRange; //!< The maximum range allowed by the device, in meters (e.g. 8.0m, 5.0m,...)
306  CPose3D sensorPose; //!< The 6D pose of the sensor on the robot.
307  float stdError; //!< The "sigma" error of the device in meters, used while inserting the scan in an occupancy grid.
308 
309 
310  /** A general method to retrieve the sensor pose on the robot.
311  * Note that most sensors will return a full (6D) CPose3D, but see the derived classes for more details or special cases.
312  * \sa setSensorPose
313  */
314  void getSensorPose( CPose3D &out_sensorPose ) const { out_sensorPose = sensorPose; }
315 
316  /** A general method to change the sensor pose on the robot.
317  * Note that most sensors will use the full (6D) CPose3D, but see the derived classes for more details or special cases.
318  * \sa getSensorPose
319  */
320  void setSensorPose( const CPose3D &newSensorPose ) { sensorPose = newSensorPose; }
321 
322  void swap(CObservation3DRangeScan &o); //!< Very efficient method to swap the contents of two observations.
323 
324  void getZoneAsObs( CObservation3DRangeScan &obs, const unsigned int &r1, const unsigned int &r2, const unsigned int &c1, const unsigned int &c2 );
325 
326  /** A Levenberg-Marquart-based optimizer to recover the calibration parameters of a 3D camera given a range (depth) image and the corresponding 3D point cloud.
327  * \param camera_offset The offset (in meters) in the +X direction of the point cloud. It's 1cm for SwissRanger SR4000.
328  * \return The final average reprojection error per pixel (typ <0.05 px)
329  */
330  static double recoverCameraCalibrationParameters(
331  const CObservation3DRangeScan &in_obs,
332  mrpt::utils::TCamera &out_camParams,
333  const double camera_offset = 0.01 );
334 
335  /** Look-up-table struct for project3DPointsFromDepthImageInto() */
336  struct TCached3DProjTables
337  {
338  mrpt::vector_float Kzs,Kys;
339  TCamera prev_camParams;
340  };
341  static TCached3DProjTables m_3dproj_lut; //!< 3D point cloud projection look-up-table \sa project3DPointsFromDepthImage
342 
343  }; // End of class def.
344 
345 
346  } // End of namespace
347 
348  namespace utils
349  {
350  using namespace ::mrpt::slam;
351  // Specialization must occur in the same namespace
352  MRPT_DECLARE_TTYPENAME_PTR(CObservation3DRangeScan)
353 
354  // Enum <-> string converter:
355  template <>
356  struct TEnumTypeFiller<slam::CObservation3DRangeScan::TIntensityChannelID>
357  {
358  typedef slam::CObservation3DRangeScan::TIntensityChannelID enum_t;
359  static void fill(bimap<enum_t,std::string> &m_map)
360  {
361  m_map.insert(slam::CObservation3DRangeScan::CH_VISIBLE, "CH_VISIBLE");
362  m_map.insert(slam::CObservation3DRangeScan::CH_IR, "CH_IR");
363  }
364  };
365  }
366 
367  namespace utils
368  {
369  using mrpt::slam::CObservation3DRangeScan;
370 
371  /** Specialization mrpt::utils::PointCloudAdapter<CObservation3DRangeScan> \ingroup mrpt_adapters_grp */
372  template <>
373  class PointCloudAdapter<CObservation3DRangeScan> : public detail::PointCloudAdapterHelperNoRGB<CObservation3DRangeScan,float>
374  {
375  private:
376  CObservation3DRangeScan &m_obj;
377  public:
378  typedef float coords_t; //!< The type of each point XYZ coordinates
379  static const int HAS_RGB = 0; //!< Has any color RGB info?
380  static const int HAS_RGBf = 0; //!< Has native RGB info (as floats)?
381  static const int HAS_RGBu8 = 0; //!< Has native RGB info (as uint8_t)?
382 
383  /** Constructor (accept a const ref for convenience) */
384  inline PointCloudAdapter(const CObservation3DRangeScan &obj) : m_obj(*const_cast<CObservation3DRangeScan*>(&obj)) { }
385  /** Get number of points */
386  inline size_t size() const { return m_obj.points3D_x.size(); }
387  /** Set number of points (to uninitialized values) */
388  inline void resize(const size_t N) {
389  if (N) m_obj.hasPoints3D = true;
390  m_obj.resizePoints3DVectors(N);
391  }
392 
393  /** Get XYZ coordinates of i'th point */
394  template <typename T>
395  inline void getPointXYZ(const size_t idx, T &x,T &y, T &z) const {
396  x=m_obj.points3D_x[idx];
397  y=m_obj.points3D_y[idx];
398  z=m_obj.points3D_z[idx];
399  }
400  /** Set XYZ coordinates of i'th point */
401  inline void setPointXYZ(const size_t idx, const coords_t x,const coords_t y, const coords_t z) {
402  m_obj.points3D_x[idx]=x;
403  m_obj.points3D_y[idx]=y;
404  m_obj.points3D_z[idx]=z;
405  }
406  }; // end of PointCloudAdapter<CObservation3DRangeScan>
407  }
408 } // End of namespace
409 
410 #include "CObservation3DRangeScan_project3D_impl.h"
411 
412 #endif


Page generated by Doxygen 1.8.3 for MRPT 0.9.6 SVN: at Fri Feb 15 22:05:02 EST 2013