Point Cloud Library (PCL)  1.8.1
ensenso_grabber.h
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Point Cloud Library (PCL) - www.pointclouds.org
5  * Copyright (c) 2014-, Open Perception, Inc.
6  *
7  * All rights reserved.
8  *
9  * Redistribution and use in source and binary forms, with or without
10  * modification, are permitted provided that the following conditions
11  * are met:
12  *
13  * * Redistributions of source code must retain the above copyright
14  * notice, this list of conditions and the following disclaimer.
15  * * Redistributions in binary form must reproduce the above
16  * copyright notice, this list of conditions and the following
17  * disclaimer in the documentation and/or other materials provided
18  * with the distribution.
19  * * Neither the name of the copyright holder(s) nor the names of its
20  * contributors may be used to endorse or promote products derived
21  * from this software without specific prior written permission.
22  *
23  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  * POSSIBILITY OF SUCH DAMAGE.
35  *
36  * Author: Victor Lamoine (victor.lamoine@gmail.com)
37  */
38 
39 #include <pcl/pcl_config.h>
40 
41 #ifndef __PCL_IO_ENSENSO_GRABBER__
42 #define __PCL_IO_ENSENSO_GRABBER__
43 
44 #include <pcl/common/time.h>
45 #include <pcl/common/io.h>
46 #include <pcl/io/eigen.h>
47 #include <Eigen/Geometry>
48 #include <Eigen/StdVector>
49 #include <pcl/io/boost.h>
50 #include <boost/thread.hpp>
51 #include <boost/lexical_cast.hpp> // TODO: Remove when setExtrinsicCalibration is fixed
52 
53 #include <pcl/io/grabber.h>
54 #include <pcl/common/synchronizer.h>
55 
56 #include <nxLib.h> // Ensenso SDK
57 
58 namespace pcl
59 {
60  struct PointXYZ;
61  template <typename T> class PointCloud;
62 
63  /** @brief Grabber for IDS-Imaging Ensenso's devices.\n
64  * The [Ensenso SDK](http://www.ensenso.de/manual/) allow to use multiple Ensenso devices to produce a single cloud.\n
65  * This feature is not implemented here, it is up to the user to configure multiple Ensenso cameras.\n
66  * @author Victor Lamoine (victor.lamoine@gmail.com)\n
67  * @ingroup io
68  */
69  class PCL_EXPORTS EnsensoGrabber : public Grabber
70  {
71  typedef std::pair<pcl::PCLImage, pcl::PCLImage> PairOfImages;
72 
73  public:
74  /** @cond */
75  typedef boost::shared_ptr<EnsensoGrabber> Ptr;
76  typedef boost::shared_ptr<const EnsensoGrabber> ConstPtr;
77 
78  // Define callback signature typedefs
79  typedef void
80  (sig_cb_ensenso_point_cloud) (const pcl::PointCloud<pcl::PointXYZ>::Ptr &);
81 
82  typedef void
83  (sig_cb_ensenso_images) (const boost::shared_ptr<PairOfImages> &);
84 
85  typedef void
86  (sig_cb_ensenso_point_cloud_images) (const pcl::PointCloud<pcl::PointXYZ>::Ptr &,
87  const boost::shared_ptr<PairOfImages> &);
88  /** @endcond */
89 
90  /** @brief Constructor */
91  EnsensoGrabber ();
92 
93  /** @brief Destructor inherited from the Grabber interface. It never throws. */
94  virtual
95  ~EnsensoGrabber () throw ();
96 
97  /** @brief Searches for available devices
98  * @returns The number of Ensenso devices connected */
99  int
100  enumDevices () const;
101 
102  /** @brief Opens an Ensenso device
103  * @param[in] device The device ID to open
104  * @return True if successful, false otherwise */
105  bool
106  openDevice (const int device = 0);
107 
108  /** @brief Closes the Ensenso device
109  * @return True if successful, false otherwise */
110  bool
111  closeDevice ();
112 
113  /** @brief Start the point cloud and or image acquisition
114  * @note Opens device "0" if no device is open */
115  void
116  start ();
117 
118  /** @brief Stop the data acquisition */
119  void
120  stop ();
121 
122  /** @brief Check if the data acquisition is still running
123  * @return True if running, false otherwise */
124  bool
125  isRunning () const;
126 
127  /** @brief Check if a TCP port is opened
128  * @return True if open, false otherwise */
129  bool
130  isTcpPortOpen () const;
131 
132  /** @brief Get class name
133  * @returns A string containing the class name */
134  std::string
135  getName () const;
136 
137  /** @brief Configure Ensenso capture settings
138  * @param[in] auto_exposure If set to yes, the exposure parameter will be ignored
139  * @param[in] auto_gain If set to yes, the gain parameter will be ignored
140  * @param[in] bining Pixel bining: 1, 2 or 4
141  * @param[in] exposure In milliseconds, from 0.01 to 20 ms
142  * @param[in] front_light Infrared front light (useful for calibration)
143  * @param[in] gain Float between 1 and 4
144  * @param[in] gain_boost
145  * @param[in] hardware_gamma
146  * @param[in] hdr High Dynamic Range (check compatibility with other options in Ensenso manual)
147  * @param[in] pixel_clock In MegaHertz, from 5 to 85
148  * @param[in] projector Use the central infrared projector or not
149  * @param[in] target_brightness Between 40 and 210
150  * @param[in] trigger_mode
151  * @param[in] use_disparity_map_area_of_interest
152  * @return True if successful, false otherwise
153  * @note See [Capture tree item](http://www.ensenso.de/manual/index.html?capture.htm) for more
154  * details about the parameters. */
155  bool
156  configureCapture (const bool auto_exposure = true,
157  const bool auto_gain = true,
158  const int bining = 1,
159  const float exposure = 0.32,
160  const bool front_light = false,
161  const int gain = 1,
162  const bool gain_boost = false,
163  const bool hardware_gamma = false,
164  const bool hdr = false,
165  const int pixel_clock = 10,
166  const bool projector = true,
167  const int target_brightness = 80,
168  const std::string trigger_mode = "Software",
169  const bool use_disparity_map_area_of_interest = false) const;
170 
171  /** @brief Capture a single point cloud and store it
172  * @param[out] cloud The cloud to be filled
173  * @return True if successful, false otherwise
174  * @warning A device must be opened and not running */
175  bool
176  grabSingleCloud (pcl::PointCloud<pcl::PointXYZ> &cloud);
177 
178  /** @brief Set up the Ensenso sensor and API to do 3D extrinsic calibration using the Ensenso 2D patterns
179  * @param[in] grid_spacing
180  * @return True if successful, false otherwise
181  *
182  * Configures the capture parameters to default values (eg: @c projector = @c false and @c front_light = @c true)
183  * Discards all previous patterns, configures @c grid_spacing
184  * @warning A device must be opened and must not be running.
185  * @note See the [Ensenso manual](http://www.ensenso.de/manual/index.html?calibratehandeyeparameters.htm) for more
186  * information about the extrinsic calibration process.
187  * @note [GridSize](http://www.ensenso.de/manual/index.html?gridsize.htm) item is protected in the NxTree, you can't modify it.
188  */
189  bool
190  initExtrinsicCalibration (const int grid_spacing) const;
191 
192  /** @brief Clear calibration patterns buffer */
193  bool
194  clearCalibrationPatternBuffer () const;
195 
196  /** @brief Captures a calibration pattern
197  * @return the number of calibration patterns stored in the nxTree, -1 on error
198  * @warning A device must be opened and must not be running.
199  * @note You should use @ref initExtrinsicCalibration before */
200  int
201  captureCalibrationPattern () const;
202 
203  /** @brief Estimate the calibration pattern pose
204  * @param[out] pattern_pose the calibration pattern pose
205  * @return true if successful, false otherwise
206  * @warning A device must be opened and must not be running.
207  * @note At least one calibration pattern must have been captured before, use @ref captureCalibrationPattern before */
208  bool
209  estimateCalibrationPatternPose (Eigen::Affine3d &pattern_pose) const;
210 
211  /** @brief Computes the calibration matrix using the collected patterns and the robot poses vector
212  * @param[in] robot_poses A list of robot poses, 1 for each pattern acquired (in the same order)
213  * @param[out] json The extrinsic calibration data in JSON format
214  * @param[in] setup Moving or Fixed, please refer to the Ensenso documentation
215  * @param[in] target Please refer to the Ensenso documentation
216  * @param[in] guess_tf Guess transformation for the calibration matrix (translation in meters)
217  * @param[in] pretty_format JSON formatting style
218  * @return True if successful, false otherwise
219  * @warning This can take up to 120 seconds
220  * @note Check the result with @ref getResultAsJson.
221  * If you want to permanently store the result, use @ref storeEEPROMExtrinsicCalibration. */
222  bool
223  computeCalibrationMatrix (const std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > &robot_poses,
224  std::string &json,
225  const std::string setup = "Moving", // Default values: Moving or Fixed
226  const std::string target = "Hand", // Default values: Hand or Workspace
227  const Eigen::Affine3d &guess_tf = Eigen::Affine3d::Identity (),
228  const bool pretty_format = true) const;
229 
230  /** @brief Copy the link defined in the Link node of the nxTree to the EEPROM
231  * @return True if successful, false otherwise
232  * Refer to @ref setExtrinsicCalibration for more information about how the EEPROM works.\n
233  * After calling @ref computeCalibrationMatrix, this enables to permanently store the matrix.
234  * @note The target must be specified (@ref computeCalibrationMatrix specifies the target) */
235  bool
236  storeEEPROMExtrinsicCalibration () const;
237 
238  /** @brief Clear the extrinsic calibration stored in the EEPROM by writing an identity matrix
239  * @return True if successful, false otherwise */
240  bool
241  clearEEPROMExtrinsicCalibration ();
242 
243  /** @brief Update Link node in NxLib tree
244  * @param[in] target "Hand" or "Workspace" for example
245  * @param[in] euler_angle
246  * @param[in] rotation_axis
247  * @param[in] translation Translation in meters
248  * @return True if successful, false otherwise
249  * @warning Translation are in meters, rotation angles in radians! (stored in mm/radians in Ensenso tree)
250  * @note If a calibration has been stored in the EEPROM, it is copied in the Link node at nxLib tree start.
251  * This method overwrites the Link node but does not write to the EEPROM.
252  *
253  * More information on the parameters can be found in [Link node](http://www.ensenso.de/manual/index.html?cameralink.htm)
254  * section of the Ensenso manual.
255  *
256  * The point cloud you get from the Ensenso is already transformed using this calibration matrix.
257  * Make sure it is the identity transformation if you want the original point cloud! (use @ref clearEEPROMExtrinsicCalibration)
258  * Use @ref storeEEPROMExtrinsicCalibration to permanently store this transformation */
259  bool
260  setExtrinsicCalibration (const double euler_angle,
261  Eigen::Vector3d &rotation_axis,
262  const Eigen::Vector3d &translation,
263  const std::string target = "Hand");
264 
265  /** @brief Update Link node in NxLib tree with an identity matrix
266  * @param[in] target "Hand" or "Workspace" for example
267  * @return True if successful, false otherwise */
268  bool
269  setExtrinsicCalibration (const std::string target = "Hand");
270 
271  /** @brief Update Link node in NxLib tree
272  * @param[in] transformation Transformation matrix
273  * @param[in] target "Hand" or "Workspace" for example
274  * @return True if successful, false otherwise
275  * @warning Translation are in meters, rotation angles in radians! (stored in mm/radians in Ensenso tree)
276  * @note If a calibration has been stored in the EEPROM, it is copied in the Link node at nxLib tree start.
277  * This method overwrites the Link node but does not write to the EEPROM.
278  *
279  * More information on the parameters can be found in [Link node](http://www.ensenso.de/manual/index.html?cameralink.htm)
280  * section of the Ensenso manual.
281  *
282  * The point cloud you get from the Ensenso is already transformed using this calibration matrix.
283  * Make sure it is the identity transformation if you want the original point cloud! (use @ref clearEEPROMExtrinsicCalibration)
284  * Use @ref storeEEPROMExtrinsicCalibration to permanently store this transformation */
285  bool
286  setExtrinsicCalibration (const Eigen::Affine3d &transformation,
287  const std::string target = "Hand");
288 
289  /** @brief Obtain the number of frames per second (FPS) */
290  float
291  getFramesPerSecond () const;
292 
293  /** @brief Open TCP port to enable access via the [nxTreeEdit](http://www.ensenso.de/manual/software_components.htm) program.
294  * @param[in] port The port number
295  * @return True if successful, false otherwise */
296  bool
297  openTcpPort (const int port = 24000);
298 
299  /** @brief Close TCP port program
300  * @return True if successful, false otherwise
301  * @warning If you do not close the TCP port the program might exit with the port still open, if it is the case
302  * use @code ps -ef @endcode and @code kill PID @endcode to kill the application and effectively close the port. */
303  bool
304  closeTcpPort (void);
305 
306  /** @brief Returns the full NxLib tree as a JSON string
307  * @param[in] pretty_format JSON formatting style
308  * @return A string containing the NxLib tree in JSON format */
309  std::string
310  getTreeAsJson (const bool pretty_format = true) const;
311 
312  /** @brief Returns the Result node (of the last command) as a JSON string
313  * @param[in] pretty_format JSON formatting style
314  * @return A string containing the Result node in JSON format
315  */
316  std::string
317  getResultAsJson (const bool pretty_format = true) const;
318 
319  /** @brief Get the Euler angles corresponding to a JSON string (an angle axis transformation)
320  * @param[in] json A string containing the angle axis transformation in JSON format
321  * @param[out] x The X translation
322  * @param[out] y The Y translation
323  * @param[out] z The Z translation
324  * @param[out] w The yaW angle
325  * @param[out] p The Pitch angle
326  * @param[out] r The Roll angle
327  * @return True if successful, false otherwise
328  * @warning The units are meters and radians!
329  * @note See: [transformation page](http://www.ensenso.de/manual/transformation.htm) in the EnsensoSDK documentation
330  */
331  bool
332  jsonTransformationToEulerAngles (const std::string &json,
333  double &x,
334  double &y,
335  double &z,
336  double &w,
337  double &p,
338  double &r) const;
339 
340  /** @brief Get the angle axis parameters corresponding to a JSON string
341  * @param[in] json A string containing the angle axis transformation in JSON format
342  * @param[out] alpha Euler angle
343  * @param[out] axis Axis vector
344  * @param[out] translation Translation vector
345  * @return True if successful, false otherwise
346  * @warning The units are meters and radians!
347  * @note See: [transformation page](http://www.ensenso.de/manual/transformation.htm) in the EnsensoSDK documentation
348  */
349  bool
350  jsonTransformationToAngleAxis (const std::string json,
351  double &alpha,
352  Eigen::Vector3d &axis,
353  Eigen::Vector3d &translation) const;
354 
355 
356  /** @brief Get the JSON string corresponding to a 4x4 matrix
357  * @param[in] transformation The input transformation
358  * @param[out] matrix A matrix containing JSON transformation
359  * @return True if successful, false otherwise
360  * @warning The units are meters and radians!
361  * @note See: [ConvertTransformation page](http://www.ensenso.de/manual/index.html?cmdconverttransformation.htm) in the EnsensoSDK documentation
362  */
363  bool
364  jsonTransformationToMatrix (const std::string transformation,
365  Eigen::Affine3d &matrix) const;
366 
367 
368  /** @brief Get the JSON string corresponding to the Euler angles transformation
369  * @param[in] x The X translation
370  * @param[in] y The Y translation
371  * @param[in] z The Z translation
372  * @param[in] w The yaW angle
373  * @param[in] p The Pitch angle
374  * @param[in] r The Roll angle
375  * @param[out] json A string containing the Euler angles transformation in JSON format
376  * @param[in] pretty_format JSON formatting style
377  * @return True if successful, false otherwise
378  * @warning The units are meters and radians!
379  * @note See: [transformation page](http://www.ensenso.de/manual/transformation.htm) in the EnsensoSDK documentation
380  */
381  bool
382  eulerAnglesTransformationToJson (const double x,
383  const double y,
384  const double z,
385  const double w,
386  const double p,
387  const double r,
388  std::string &json,
389  const bool pretty_format = true) const;
390 
391  /** @brief Get the JSON string corresponding to an angle axis transformation
392  * @param[in] x The X angle
393  * @param[in] y The Y angle
394  * @param[in] z The Z angle
395  * @param[in] rx The X component of the Euler axis
396  * @param[in] ry The Y component of the Euler axis
397  * @param[in] rz The Z component of the Euler axis
398  * @param[in] alpha The Euler rotation angle
399  * @param[out] json A string containing the angle axis transformation in JSON format
400  * @param[in] pretty_format JSON formatting style
401  * @return True if successful, false otherwise
402  * @warning The units are meters and radians! (the Euler axis doesn't need to be normalized)
403  * @note See: [transformation page](http://www.ensenso.de/manual/transformation.htm) in the EnsensoSDK documentation
404  */
405  bool
406  angleAxisTransformationToJson (const double x,
407  const double y,
408  const double z,
409  const double rx,
410  const double ry,
411  const double rz,
412  const double alpha,
413  std::string &json,
414  const bool pretty_format = true) const;
415 
416  /** @brief Get the JSON string corresponding to a 4x4 matrix
417  * @param[in] matrix The input matrix
418  * @param[out] json A string containing the matrix transformation in JSON format
419  * @param[in] pretty_format JSON formatting style
420  * @return True if successful, false otherwise
421  * @warning The units are meters and radians!
422  * @note See: [ConvertTransformation page](http://www.ensenso.de/manual/index.html?cmdconverttransformation.htm)
423  * in the EnsensoSDK documentation */
424  bool
425  matrixTransformationToJson (const Eigen::Affine3d &matrix,
426  std::string &json,
427  const bool pretty_format = true) const;
428 
429  /** @brief Reference to the NxLib tree root
430  * @warning You must handle NxLib exceptions manually when playing with @ref root_ !
431  * See ensensoExceptionHandling in ensenso_grabber.cpp */
432  boost::shared_ptr<const NxLibItem> root_;
433 
434  /** @brief Reference to the camera tree
435  * @warning You must handle NxLib exceptions manually when playing with @ref camera_ ! */
436  NxLibItem camera_;
437 
438  protected:
439  /** @brief Grabber thread */
440  boost::thread grabber_thread_;
441 
442  /** @brief Boost point cloud signal */
443  boost::signals2::signal<sig_cb_ensenso_point_cloud>* point_cloud_signal_;
444 
445  /** @brief Boost images signal */
446  boost::signals2::signal<sig_cb_ensenso_images>* images_signal_;
447 
448  /** @brief Boost images + point cloud signal */
449  boost::signals2::signal<sig_cb_ensenso_point_cloud_images>* point_cloud_images_signal_;
450 
451  /** @brief Whether an Ensenso device is opened or not */
453 
454  /** @brief Whether an TCP port is opened or not */
455  bool tcp_open_;
456 
457  /** @brief Whether an Ensenso device is running or not */
458  bool running_;
459 
460  /** @brief Point cloud capture/processing frequency */
462 
463  /** @brief Mutual exclusion for FPS computation */
464  mutable boost::mutex fps_mutex_;
465 
466  /** @brief Convert an Ensenso time stamp into a PCL/ROS time stamp
467  * @param[in] ensenso_stamp
468  * @return PCL stamp
469  * The Ensenso API returns the time elapsed from January 1st, 1601 (UTC); on Linux OS the reference time is January 1st, 1970 (UTC).
470  * See [time-stamp page](http://www.ensenso.de/manual/index.html?json_types.htm) for more info about the time stamp conversion. */
471  pcl::uint64_t
472  static
473  getPCLStamp (const double ensenso_stamp);
474 
475  /** @brief Get OpenCV image type corresponding to the parameters given
476  * @param channels number of channels in the image
477  * @param bpe bytes per element
478  * @param isFlt is float
479  * @return the OpenCV type as a string */
480  std::string
481  static
482  getOpenCVType (const int channels,
483  const int bpe,
484  const bool isFlt);
485 
486  /** @brief Continuously asks for images and or point clouds data from the device and publishes them if available.
487  * PCL time stamps are filled for both the images and clouds grabbed (see @ref getPCLStamp)
488  * @note The cloud time stamp is the RAW image time stamp */
489  void
490  processGrabbing ();
491  };
492 } // namespace pcl
493 
494 #endif // __PCL_IO_ENSENSO_GRABBER__
495 
boost::signals2::signal< sig_cb_ensenso_images > * images_signal_
Boost images signal.
pcl::EventFrequency frequency_
Point cloud capture/processing frequency.
boost::signals2::signal< sig_cb_ensenso_point_cloud_images > * point_cloud_images_signal_
Boost images + point cloud signal.
bool device_open_
Whether an Ensenso device is opened or not.
NxLibItem camera_
Reference to the camera tree.
A helper class to measure frequency of a certain event.
Definition: time.h:151
Grabber interface for PCL 1.x device drivers.
Definition: grabber.h:58
bool tcp_open_
Whether an TCP port is opened or not.
boost::shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:428
boost::thread grabber_thread_
Grabber thread.
boost::signals2::signal< sig_cb_ensenso_point_cloud > * point_cloud_signal_
Boost point cloud signal.
bool running_
Whether an Ensenso device is running or not.
PointCloud represents the base class in PCL for storing collections of 3D points. ...
boost::shared_ptr< const NxLibItem > root_
Reference to the NxLib tree root.
Grabber for IDS-Imaging Ensenso&#39;s devices.
boost::mutex fps_mutex_
Mutual exclusion for FPS computation.
Define methods for measuring time spent in code blocks.