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