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