Loading...
Searching...
No Matches
ensenso_grabber.h
/*
* Software License Agreement (BSD License)
*
* Point Cloud Library (PCL) - www.pointclouds.org
* Copyright (c) 2014-, Open Perception, Inc.
*
* All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* * Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* * Redistributions in binary form must reproduce the above
* copyright notice, this list of conditions and the following
* disclaimer in the documentation and/or other materials provided
* with the distribution.
* * Neither the name of the copyright holder(s) nor the names of its
* contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
* Author: Victor Lamoine (victor.lamoine@gmail.com)
*/
#pragma once
#include <pcl/pcl_config.h>
#include <pcl/common/time.h>
#include <pcl/common/io.h>
#include <Eigen/Geometry>
#include <Eigen/StdVector>
#include <pcl/io/grabber.h>
#include <pcl/common/synchronizer.h>
#include <nxLib.h> // Ensenso SDK
#include <thread>
namespace pcl
{
struct PointXYZ;
template <typename T> class PointCloud;
/** @brief Grabber for IDS-Imaging Ensenso's devices.\n
* The [Ensenso SDK](http://www.ensenso.de/manual/) allow to use multiple Ensenso devices to produce a single cloud.\n
* This feature is not implemented here, it is up to the user to configure multiple Ensenso cameras.\n
* @author Victor Lamoine (victor.lamoine@gmail.com)\n
* @ingroup io
*/
class PCL_EXPORTS EnsensoGrabber : public Grabber
{
using PairOfImages = std::pair<pcl::PCLImage, pcl::PCLImage>;
public:
/** @cond */
using Ptr = shared_ptr<EnsensoGrabber>;
using ConstPtr = shared_ptr<const EnsensoGrabber>;
// Define callback signature typedefs
using sig_cb_ensenso_point_cloud = void(const pcl::PointCloud<pcl::PointXYZ>::Ptr&);
using sig_cb_ensenso_images = void(const shared_ptr<PairOfImages>&);
using sig_cb_ensenso_point_cloud_images = void(const pcl::PointCloud<pcl::PointXYZ>::Ptr&,const shared_ptr<PairOfImages>&);
/** @endcond */
/** @brief Constructor */
EnsensoGrabber ();
/** @brief Destructor inherited from the Grabber interface. It never throws. */
~EnsensoGrabber () noexcept override;
/** @brief Searches for available devices
* @returns The number of Ensenso devices connected */
int
enumDevices () const;
/** @brief Opens an Ensenso device
* @param[in] device The device ID to open
* @return True if successful, false otherwise */
bool
openDevice (const int device = 0);
/** @brief Closes the Ensenso device
* @return True if successful, false otherwise */
bool
closeDevice ();
/** @brief Start the point cloud and or image acquisition
* @note Opens device "0" if no device is open */
void
start () override;
/** @brief Stop the data acquisition */
void
stop () override;
/** @brief Check if the data acquisition is still running
* @return True if running, false otherwise */
bool
isRunning () const override;
/** @brief Check if a TCP port is opened
* @return True if open, false otherwise */
bool
isTcpPortOpen () const;
/** @brief Get class name
* @returns A string containing the class name */
std::string
getName () const override;
/** @brief Configure Ensenso capture settings
* @param[in] auto_exposure If set to yes, the exposure parameter will be ignored
* @param[in] auto_gain If set to yes, the gain parameter will be ignored
* @param[in] bining Pixel bining: 1, 2 or 4
* @param[in] exposure In milliseconds, from 0.01 to 20 ms
* @param[in] front_light Infrared front light (useful for calibration)
* @param[in] gain Float between 1 and 4
* @param[in] gain_boost
* @param[in] hardware_gamma
* @param[in] hdr High Dynamic Range (check compatibility with other options in Ensenso manual)
* @param[in] pixel_clock In MegaHertz, from 5 to 85
* @param[in] projector Use the central infrared projector or not
* @param[in] target_brightness Between 40 and 210
* @param[in] trigger_mode
* @param[in] use_disparity_map_area_of_interest
* @return True if successful, false otherwise
* @note See [Capture tree item](http://www.ensenso.de/manual/index.html?capture.htm) for more
* details about the parameters. */
bool
configureCapture (const bool auto_exposure = true,
const bool auto_gain = true,
const int bining = 1,
const float exposure = 0.32,
const bool front_light = false,
const int gain = 1,
const bool gain_boost = false,
const bool hardware_gamma = false,
const bool hdr = false,
const int pixel_clock = 10,
const bool projector = true,
const int target_brightness = 80,
const std::string trigger_mode = "Software",
const bool use_disparity_map_area_of_interest = false) const;
/** @brief Capture a single point cloud and store it
* @param[out] cloud The cloud to be filled
* @return True if successful, false otherwise
* @warning A device must be opened and not running */
bool
grabSingleCloud (pcl::PointCloud<pcl::PointXYZ> &cloud) const;
/** @brief Set up the Ensenso sensor and API to do 3D extrinsic calibration using the Ensenso 2D patterns
* @param[in] grid_spacing
* @return True if successful, false otherwise
*
* Configures the capture parameters to default values (eg: @c projector = @c false and @c front_light = @c true)
* Discards all previous patterns, configures @c grid_spacing
* @warning A device must be opened and must not be running.
* @note See the [Ensenso manual](http://www.ensenso.de/manual/index.html?calibratehandeyeparameters.htm) for more
* information about the extrinsic calibration process.
* @note [GridSize](http://www.ensenso.de/manual/index.html?gridsize.htm) item is protected in the NxTree, you can't modify it.
*/
bool
initExtrinsicCalibration (const int grid_spacing) const;
/** @brief Clear calibration patterns buffer */
bool
clearCalibrationPatternBuffer () const;
/** @brief Captures a calibration pattern
* @return the number of calibration patterns stored in the nxTree, -1 on error
* @warning A device must be opened and must not be running.
* @note You should use @ref initExtrinsicCalibration before */
int
captureCalibrationPattern () const;
/** @brief Estimate the calibration pattern pose
* @param[out] pattern_pose the calibration pattern pose
* @return true if successful, false otherwise
* @warning A device must be opened and must not be running.
* @note At least one calibration pattern must have been captured before, use @ref captureCalibrationPattern before */
bool
estimateCalibrationPatternPose (Eigen::Affine3d &pattern_pose) const;
/** @brief Computes the calibration matrix using the collected patterns and the robot poses vector
* @param[in] robot_poses A list of robot poses, 1 for each pattern acquired (in the same order)
* @param[out] json The extrinsic calibration data in JSON format
* @param[in] setup Moving or Fixed, please refer to the Ensenso documentation
* @param[in] target Please refer to the Ensenso documentation
* @param[in] guess_tf Guess transformation for the calibration matrix (translation in meters)
* @param[in] pretty_format JSON formatting style
* @return True if successful, false otherwise
* @warning This can take up to 120 seconds
* @note Check the result with @ref getResultAsJson.
* If you want to permanently store the result, use @ref storeEEPROMExtrinsicCalibration. */
bool
computeCalibrationMatrix (const std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > &robot_poses,
std::string &json,
const std::string setup = "Moving", // Default values: Moving or Fixed
const std::string target = "Hand", // Default values: Hand or Workspace
const Eigen::Affine3d &guess_tf = Eigen::Affine3d::Identity (),
const bool pretty_format = true) const;
/** @brief Copy the link defined in the Link node of the nxTree to the EEPROM
* @return True if successful, false otherwise
* Refer to @ref setExtrinsicCalibration for more information about how the EEPROM works.\n
* After calling @ref computeCalibrationMatrix, this enables to permanently store the matrix.
* @note The target must be specified (@ref computeCalibrationMatrix specifies the target) */
bool
storeEEPROMExtrinsicCalibration () const;
/** @brief Clear the extrinsic calibration stored in the EEPROM by writing an identity matrix
* @return True if successful, false otherwise */
bool
clearEEPROMExtrinsicCalibration ();
/** @brief Update Link node in NxLib tree
* @param[in] target "Hand" or "Workspace" for example
* @param[in] euler_angle
* @param[in] rotation_axis
* @param[in] translation Translation in meters
* @return True if successful, false otherwise
* @warning Translation are in meters, rotation angles in radians! (stored in mm/radians in Ensenso tree)
* @note If a calibration has been stored in the EEPROM, it is copied in the Link node at nxLib tree start.
* This method overwrites the Link node but does not write to the EEPROM.
*
* More information on the parameters can be found in [Link node](http://www.ensenso.de/manual/index.html?cameralink.htm)
* section of the Ensenso manual.
*
* The point cloud you get from the Ensenso is already transformed using this calibration matrix.
* Make sure it is the identity transformation if you want the original point cloud! (use @ref clearEEPROMExtrinsicCalibration)
* Use @ref storeEEPROMExtrinsicCalibration to permanently store this transformation */
bool
setExtrinsicCalibration (const double euler_angle,
Eigen::Vector3d &rotation_axis,
const Eigen::Vector3d &translation,
const std::string target = "Hand") const;
/** @brief Update Link node in NxLib tree with an identity matrix
* @param[in] target "Hand" or "Workspace" for example
* @return True if successful, false otherwise */
bool
setExtrinsicCalibration (const std::string target = "Hand");
/** @brief Update Link node in NxLib tree
* @param[in] transformation Transformation matrix
* @param[in] target "Hand" or "Workspace" for example
* @return True if successful, false otherwise
* @warning Translation are in meters, rotation angles in radians! (stored in mm/radians in Ensenso tree)
* @note If a calibration has been stored in the EEPROM, it is copied in the Link node at nxLib tree start.
* This method overwrites the Link node but does not write to the EEPROM.
*
* More information on the parameters can be found in [Link node](http://www.ensenso.de/manual/index.html?cameralink.htm)
* section of the Ensenso manual.
*
* The point cloud you get from the Ensenso is already transformed using this calibration matrix.
* Make sure it is the identity transformation if you want the original point cloud! (use @ref clearEEPROMExtrinsicCalibration)
* Use @ref storeEEPROMExtrinsicCalibration to permanently store this transformation */
bool
setExtrinsicCalibration (const Eigen::Affine3d &transformation,
const std::string target = "Hand");
/** @brief Obtain the number of frames per second (FPS) */
float
getFramesPerSecond () const override;
/** @brief Open TCP port to enable access via the [nxTreeEdit](http://www.ensenso.de/manual/software_components.htm) program.
* @param[in] port The port number
* @return True if successful, false otherwise */
bool
openTcpPort (const int port = 24000);
/** @brief Close TCP port program
* @return True if successful, false otherwise
* @warning If you do not close the TCP port the program might exit with the port still open, if it is the case
* use @code ps -ef @endcode and @code kill PID @endcode to kill the application and effectively close the port. */
bool
closeTcpPort ();
/** @brief Returns the full NxLib tree as a JSON string
* @param[in] pretty_format JSON formatting style
* @return A string containing the NxLib tree in JSON format */
std::string
getTreeAsJson (const bool pretty_format = true) const;
/** @brief Returns the Result node (of the last command) as a JSON string
* @param[in] pretty_format JSON formatting style
* @return A string containing the Result node in JSON format
*/
std::string
getResultAsJson (const bool pretty_format = true) const;
/** @brief Get the Euler angles corresponding to a JSON string (an angle axis transformation)
* @param[in] json A string containing the angle axis transformation in JSON format
* @param[out] x The X translation
* @param[out] y The Y translation
* @param[out] z The Z translation
* @param[out] w The yaW angle
* @param[out] p The Pitch angle
* @param[out] r The Roll angle
* @return True if successful, false otherwise
* @warning The units are meters and radians!
* @note See: [transformation page](http://www.ensenso.de/manual/transformation.htm) in the EnsensoSDK documentation
*/
bool
jsonTransformationToEulerAngles (const std::string &json,
double &x,
double &y,
double &z,
double &w,
double &p,
double &r) const;
/** @brief Get the angle axis parameters corresponding to a JSON string
* @param[in] json A string containing the angle axis transformation in JSON format
* @param[out] alpha Euler angle
* @param[out] axis Axis vector
* @param[out] translation Translation vector
* @return True if successful, false otherwise
* @warning The units are meters and radians!
* @note See: [transformation page](http://www.ensenso.de/manual/transformation.htm) in the EnsensoSDK documentation
*/
bool
jsonTransformationToAngleAxis (const std::string json,
double &alpha,
Eigen::Vector3d &axis,
Eigen::Vector3d &translation) const;
/** @brief Get the JSON string corresponding to a 4x4 matrix
* @param[in] transformation The input transformation
* @param[out] matrix A matrix containing JSON transformation
* @return True if successful, false otherwise
* @warning The units are meters and radians!
* @note See: [ConvertTransformation page](http://www.ensenso.de/manual/index.html?cmdconverttransformation.htm) in the EnsensoSDK documentation
*/
bool
jsonTransformationToMatrix (const std::string transformation,
Eigen::Affine3d &matrix) const;
/** @brief Get the JSON string corresponding to the Euler angles transformation
* @param[in] x The X translation
* @param[in] y The Y translation
* @param[in] z The Z translation
* @param[in] w The yaW angle
* @param[in] p The Pitch angle
* @param[in] r The Roll angle
* @param[out] json A string containing the Euler angles transformation in JSON format
* @param[in] pretty_format JSON formatting style
* @return True if successful, false otherwise
* @warning The units are meters and radians!
* @note See: [transformation page](http://www.ensenso.de/manual/transformation.htm) in the EnsensoSDK documentation
*/
bool
eulerAnglesTransformationToJson (const double x,
const double y,
const double z,
const double w,
const double p,
const double r,
std::string &json,
const bool pretty_format = true) const;
/** @brief Get the JSON string corresponding to an angle axis transformation
* @param[in] x The X angle
* @param[in] y The Y angle
* @param[in] z The Z angle
* @param[in] rx The X component of the Euler axis
* @param[in] ry The Y component of the Euler axis
* @param[in] rz The Z component of the Euler axis
* @param[in] alpha The Euler rotation angle
* @param[out] json A string containing the angle axis transformation in JSON format
* @param[in] pretty_format JSON formatting style
* @return True if successful, false otherwise
* @warning The units are meters and radians! (the Euler axis doesn't need to be normalized)
* @note See: [transformation page](http://www.ensenso.de/manual/transformation.htm) in the EnsensoSDK documentation
*/
bool
angleAxisTransformationToJson (const double x,
const double y,
const double z,
const double rx,
const double ry,
const double rz,
const double alpha,
std::string &json,
const bool pretty_format = true) const;
/** @brief Get the JSON string corresponding to a 4x4 matrix
* @param[in] matrix The input matrix
* @param[out] json A string containing the matrix transformation in JSON format
* @param[in] pretty_format JSON formatting style
* @return True if successful, false otherwise
* @warning The units are meters and radians!
* @note See: [ConvertTransformation page](http://www.ensenso.de/manual/index.html?cmdconverttransformation.htm)
* in the EnsensoSDK documentation */
bool
matrixTransformationToJson (const Eigen::Affine3d &matrix,
std::string &json,
const bool pretty_format = true) const;
/** @brief Reference to the NxLib tree root
* @warning You must handle NxLib exceptions manually when playing with @ref root_ !
* See ensensoExceptionHandling in ensenso_grabber.cpp */
shared_ptr<const NxLibItem> root_;
/** @brief Reference to the camera tree
* @warning You must handle NxLib exceptions manually when playing with @ref camera_ ! */
NxLibItem camera_;
protected:
/** @brief Grabber thread */
std::thread grabber_thread_;
/** @brief Boost point cloud signal */
boost::signals2::signal<sig_cb_ensenso_point_cloud>* point_cloud_signal_;
/** @brief Boost images signal */
boost::signals2::signal<sig_cb_ensenso_images>* images_signal_;
/** @brief Boost images + point cloud signal */
boost::signals2::signal<sig_cb_ensenso_point_cloud_images>* point_cloud_images_signal_;
/** @brief Whether an Ensenso device is opened or not */
bool device_open_{false};
/** @brief Whether an TCP port is opened or not */
bool tcp_open_{false};
/** @brief Whether an Ensenso device is running or not */
bool running_{false};
/** @brief Point cloud capture/processing frequency */
pcl::EventFrequency frequency_;
/** @brief Mutual exclusion for FPS computation */
mutable std::mutex fps_mutex_;
/** @brief Convert an Ensenso time stamp into a PCL/ROS time stamp
* @param[in] ensenso_stamp
* @return PCL stamp
* The Ensenso API returns the time elapsed from January 1st, 1601 (UTC); on Linux OS the reference time is January 1st, 1970 (UTC).
* See [time-stamp page](http://www.ensenso.de/manual/index.html?json_types.htm) for more info about the time stamp conversion. */
std::uint64_t
static
getPCLStamp (const double ensenso_stamp);
/** @brief Get OpenCV image type corresponding to the parameters given
* @param channels number of channels in the image
* @param bpe bytes per element
* @param isFlt is float
* @return the OpenCV type as a string */
std::string
static
getOpenCVType (const int channels,
const int bpe,
const bool isFlt);
/** @brief Continuously asks for images and or point clouds data from the device and publishes them if available.
* PCL time stamps are filled for both the images and clouds grabbed (see @ref getPCLStamp)
* @note The cloud time stamp is the RAW image time stamp */
void
processGrabbing ();
};
} // namespace pcl
pcl::EnsensoGrabber
Grabber for IDS-Imaging Ensenso's devices.
Definition ensenso_grabber.h:67
pcl::EnsensoGrabber::getPCLStamp
static std::uint64_t getPCLStamp(const double ensenso_stamp)
Convert an Ensenso time stamp into a PCL/ROS time stamp.
pcl::EnsensoGrabber::getOpenCVType
static std::string getOpenCVType(const int channels, const int bpe, const bool isFlt)
Get OpenCV image type corresponding to the parameters given.
pcl::EnsensoGrabber::processGrabbing
void processGrabbing()
Continuously asks for images and or point clouds data from the device and publishes them if available...
pcl::EnsensoGrabber::~EnsensoGrabber
~EnsensoGrabber() noexcept override
Destructor inherited from the Grabber interface.
pcl::EnsensoGrabber::frequency_
pcl::EventFrequency frequency_
Point cloud capture/processing frequency.
Definition ensenso_grabber.h:455
pcl::EnsensoGrabber::EnsensoGrabber
EnsensoGrabber()
Constructor.
pcl::EnsensoGrabber::fps_mutex_
std::mutex fps_mutex_
Mutual exclusion for FPS computation.
Definition ensenso_grabber.h:458
pcl::EventFrequency
A helper class to measure frequency of a certain event.
Definition time.h:133
pcl::Grabber
Grabber interface for PCL 1.x device drivers.
Definition grabber.h:60
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition point_cloud.h:173
pcl::PointCloud::Ptr
shared_ptr< PointCloud< PointT > > Ptr
Definition point_cloud.h:413
time.h
Define methods for measuring time spent in code blocks.
Eigen
Definition bfgs.h:10
boost
Definition boost_graph.h:48
pcl
Definition convolution.h:46
pcl::PointXYZ
A point structure representing Euclidean xyz coordinates.
Definition point_types.hpp:351