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