ensenso_grabber.h

Go to the documentation of this file.

#ifndef __ENSENSO_ENSENSO_GRABBER__
#define __ENSENSO_ENSENSO_GRABBER__

// PCL
#include <pcl/pcl_config.h>
#include <pcl/common/io.h>
#include <pcl/common/time.h>
#include <pcl/io/eigen.h>
#include <pcl/io/boost.h>
#include <pcl/io/grabber.h>
#include <pcl/common/synchronizer.h>
// Others
#include <Eigen/Geometry>
#include <boost/thread.hpp>
#include <sensor_msgs/CameraInfo.h>
#include <geometry_msgs/TransformStamped.h>
// Ensenso SDK
#include <nxLib.h>
namespace pcl
{
template <typename T>
struct PCLGenImage : PCLImage
{
    std::vector<T> data;
    typedef boost::shared_ptr< ::pcl::PCLGenImage<T> > Ptr;
    typedef boost::shared_ptr< ::pcl::PCLGenImage<T>  const> ConstPtr;
};

struct Transform
{
    double tx, ty, tz, qx, qy, qz, qw;
};
class PCL_EXPORTS EnsensoGrabber : public Grabber
{
    typedef std::pair<pcl::PCLGenImage<pcl::uint8_t>, pcl::PCLGenImage<pcl::uint8_t> > PairOfImages;

public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW

    typedef boost::shared_ptr<EnsensoGrabber> Ptr;
    typedef boost::shared_ptr<const EnsensoGrabber> ConstPtr;

    // Define callback signature typedefs
    typedef void
    (sig_cb_ensenso_point_cloud)(const pcl::PointCloud<pcl::PointXYZ>::Ptr &);

    typedef void
    (sig_cb_ensenso_point_cloud_rgb)(const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &);

    typedef void
    (sig_cb_ensenso_images)(const boost::shared_ptr<PairOfImages>&, const boost::shared_ptr<PairOfImages>&);

    typedef void
    (sig_cb_ensenso_images_rgb)(const boost::shared_ptr<PairOfImages>&, const boost::shared_ptr<PairOfImages>&, const boost::shared_ptr<PairOfImages>&);

    typedef void
    (sig_cb_ensenso_image_depth)(const boost::shared_ptr<pcl::PCLGenImage<float> >&);

    EnsensoGrabber ();

    virtual ~EnsensoGrabber () throw ();

    bool calibrateHandEye ( const std::vector<Eigen::Affine3d, Eigen::aligned_allocator<Eigen::Affine3d> > &robot_poses,
                            const Eigen::Affine3d &camera_seed,
                            const Eigen::Affine3d &pattern_seed,
                            const std::string setup,
                            Eigen::Affine3d &estimated_camera_pose,
                            Eigen::Affine3d &estimated_pattern_pose,
                            int &iterations,
                            double &reprojection_error) const;

    bool closeDevices ();

    bool closeTcpPort (void);

    int collectPattern (const bool buffer=true) const;

    double decodePattern () const;

    bool discardPatterns () const;

    int enumDevices () const;

    bool estimatePatternPose (Eigen::Affine3d &pose, const bool average=false) const;

    std::string getName () const;

    bool getCameraInfo(std::string cam, sensor_msgs::CameraInfo &cam_info) const;

    bool getTFLeftToRGB(Transform& tf) const;

    bool getLastCalibrationPattern (std::vector<int> &grid_size, double &grid_spacing,
                                    std::vector<Eigen::Vector2d> &left_points,
                                    std::vector<Eigen::Vector2d> &right_points,
                                    Eigen::Affine3d &pose) const;

    float getFramesPerSecond () const;

    int getPatternCount () const;

    bool grabSingleCloud (pcl::PointCloud<pcl::PointXYZ> &cloud);

    bool isRunning () const;

    bool isTcpPortOpen () const;

    bool openDevice (std::string serial);


    bool openMonoDevice (std::string serial);

    bool openTcpPort (const int port = 24000);

    bool restoreDefaultConfiguration () const;

    bool setEnableCUDA (const bool enable=true) const;

    bool setFindPattern (const bool enable=true);

    bool setUseRGB (const bool enable=true);

    bool setAutoBlackLevel (const bool enable=true) const;

    bool setAutoExposure (const bool enable=true) const;

    bool setAutoGain (const bool enable=true) const;

    bool setBinning (const int binning=1) const;

    bool setBlackLevelOffset (const float offset=1.0) const;

    bool setExposure (const float exposure=1.0) const;

    bool setFlexView (const bool enable=false, const int imagepairs=2) const;

    bool setFrontLight (const bool enable=false) const;

    bool setGain (const float gain=1.0) const;

    bool setGainBoost (const bool enable=false) const;

    bool setGridSpacing (const double grid_spacing) const;

    bool setHardwareGamma (const bool enable=true) const;

    bool setHdr (const bool enable=false) const;

    bool setMinimumDisparity (const int disparity=-64) const;

    bool setNumberOfDisparities (const int number=128) const;

    bool setOptimizationProfile (const std::string profile="AlignedAndDiagonal") const;

    bool setPixelClock (const int pixel_clock=24) const;

    bool setProjector (const bool enable=true) const;

    bool setScaling (const float scaling=1.0) const;

    bool setTargetBrightness (const int target=80) const;

    bool setTriggerMode (const std::string mode="Software") const;

    bool setRGBTriggerDelay (const float delay=10) const;

    bool setUseOpenGL (const bool enable) const;

    bool setUseDisparityMapAreaOfInterest (const bool enable=false) const;

    bool setDepthChangeCost(const int changecost) const;

    bool setDepthStepCost(const int stepcost) const;

    bool setShadowingThreshold(const int shadowingthreshold) const;

    bool setUniquenessRatio(const int ratio) const;

    bool setMedianFilterRadius(const int radius) const;

    bool setSpeckleComponentThreshold(const int threshold) const;

    bool setSpeckleRegionSize(const int threshold) const;

    bool setFillBorderSpread(const int maximumspread) const;

    bool setFillRegionSize(const int regionsize) const;

    bool setSurfaceConnectivity(const int threshold) const;

    bool setNearPlane(const int near);

    bool setFarPlane(const int far);


    void start ();

    void stop ();

    void storeCalibrationPattern (const bool enable);

protected:
    boost::thread grabber_thread_;

    boost::signals2::signal<sig_cb_ensenso_point_cloud>* point_cloud_signal_;

    boost::signals2::signal<sig_cb_ensenso_point_cloud_rgb>* point_cloud_rgb_signal_;

    boost::signals2::signal<sig_cb_ensenso_images>* images_signal_;

    boost::signals2::signal<sig_cb_ensenso_images_rgb>* images_rgb_signal_;

    boost::signals2::signal<sig_cb_ensenso_image_depth>* image_depth_signal_;

    NxLibItem camera_;

    NxLibItem monocam_;

    boost::shared_ptr<const NxLibItem> root_;

    bool use_rgb_;

    bool find_pattern_;


    bool device_open_;

    bool mono_device_open_;

    bool tcp_open_;

    Eigen::Affine3d last_pattern_pose_;

    std::string last_stereo_pattern_;

    bool store_calibration_pattern_;

    mutable boost::mutex pattern_mutex_;

    bool running_;

    int near_plane_;

    int far_plane_;

    float fps_;

    double timestamp_;

    Transform tf_left_to_rgb_;

    mutable boost::mutex fps_mutex_;

    pcl::uint64_t static getPCLStamp (const double ensenso_stamp);

    std::string static getOpenCVType (const int channels, const int bpe, const bool isFlt);

    bool jsonToMatrix (const std::string json, Eigen::Affine3d &matrix) const;

    bool matrixToJson (const Eigen::Affine3d &matrix, std::string &json, const bool pretty_format=true) const;

    void processGrabbing ();

    void triggerCameras();

    void getImage(const NxLibItem& image_node, pcl::PCLGenImage<pcl::uint8_t>& image_out);

    void getDepthDataRGB(const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr& cloud, const pcl::PCLGenImage<float>::Ptr& depthimage);

    void getDepthData(const pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, const pcl::PCLGenImage<float>::Ptr& depthimage);

};
}  // namespace pcl

#endif // __PCL_IO_ENSENSO_GRABBER__