1 #ifndef __ENSENSO_ENSENSO_GRABBER__ 2 #define __ENSENSO_ENSENSO_GRABBER__ 5 #include <pcl/pcl_config.h> 6 #include <pcl/common/io.h> 7 #include <pcl/common/time.h> 8 #include <pcl/common/transforms.h> 9 #include <pcl/io/eigen.h> 10 #include <pcl/io/boost.h> 11 #include <pcl/io/grabber.h> 12 #include <pcl/common/synchronizer.h> 14 #include <Eigen/Geometry> 15 #include <boost/thread.hpp> 16 #include <sensor_msgs/CameraInfo.h> 17 #include <geometry_msgs/TransformStamped.h> 32 double tx, ty,
tz, qx, qy, qz, qw;
43 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
51 (sig_cb_ensenso_point_cloud)(
const pcl::PointCloud<pcl::PointXYZ>::Ptr &);
54 (sig_cb_ensenso_point_cloud_rgb)(
const pcl::PointCloud<pcl::PointXYZRGBA>::Ptr &);
60 (sig_cb_ensenso_images_rgb)(
const boost::shared_ptr<PairOfImages>&,
const boost::shared_ptr<PairOfImages>&,
const boost::shared_ptr<PairOfImages>&);
92 bool calibrateHandEye ( const
std::vector<
Eigen::Affine3d,
Eigen::aligned_allocator<
Eigen::Affine3d> > &robot_poses,
93 const
Eigen::Affine3d &camera_seed,
94 const
Eigen::Affine3d &pattern_seed,
96 Eigen::Affine3d &estimated_camera_pose,
97 Eigen::Affine3d &estimated_pattern_pose,
99 double &reprojection_error) const;
103 bool closeDevices ();
109 bool closeTcpPort (
void);
115 int collectPattern (const
bool buffer=true) const;
119 double decodePattern () const;
124 bool discardPatterns () const;
128 int enumDevices () const;
139 bool estimatePatternPose (
Eigen::Affine3d &pose, const
bool average=false) const;
143 std::
string getName () const;
151 bool getCameraInfo(
std::
string cam,
sensor_msgs::CameraInfo &cam_info) const;
167 bool getLastCalibrationPattern (
std::vector<
int> &grid_size,
double &grid_spacing,
168 std::vector<
Eigen::Vector2d> &left_points,
169 std::vector<
Eigen::Vector2d> &right_points,
170 Eigen::Affine3d &pose) const;
173 float getFramesPerSecond () const;
177 int getPatternCount () const;
187 bool isRunning () const;
191 bool isTcpPortOpen () const;
196 bool openDevice (
std::
string serial);
202 bool openMonoDevice (
std::
string serial);
208 bool openTcpPort (const
int port = 24000);
212 bool restoreDefaultConfiguration () const;
218 bool setEnableCUDA (const
bool enable=true) const;
223 bool setFindPattern (const
bool enable=true);
228 bool setUseRGB (const
bool enable=true);
233 bool setAutoBlackLevel (const
bool enable=true) const;
238 bool setAutoExposure (const
bool enable=true) const;
243 bool setAutoGain (const
bool enable=true) const;
253 bool setBinning (const
int binning=1) const;
260 bool setBlackLevelOffset (const
float offset=1.0) const;
267 bool setExposure (const
float exposure=1.0) const;
282 bool setFlexView (const
bool enable=false, const
int imagepairs=2) const;
290 bool setFrontLight (const
bool enable=false) const;
297 bool setGain (const
float gain=1.0) const;
302 bool setGainBoost (const
bool enable=false) const;
307 bool setGridSpacing (const
double grid_spacing) const;
313 bool setHardwareGamma (const
bool enable=true) const;
319 bool setHdr (const
bool enable=false) const;
327 bool setMinimumDisparity (const
int disparity=-64) const;
335 bool setNumberOfDisparities (const
int number=128) const;
349 bool setOptimizationProfile (const
std::
string profile="AlignedAndDiagonal") const;
357 bool setPixelClock (const
int pixel_clock=24) const;
364 bool setProjector (const
bool enable=true) const;
377 bool setScaling (const
float scaling=1.0) const;
382 bool setTargetBrightness (const
int target=80) const;
394 bool setTriggerMode (const
std::
string mode="Software") const;
400 bool setRGBTriggerDelay (const
float delay=10) const;
406 bool setUseOpenGL (const
bool enable) const;
420 bool setUseDisparityMapAreaOfInterest (const
bool enable=false) const;
427 bool setDepthChangeCost(const
int changecost) const;
436 bool setDepthStepCost(const
int stepcost) const;
448 bool setShadowingThreshold(const
int shadowingthreshold) const;
456 bool setUniquenessRatio(const
int ratio) const;
465 bool setMedianFilterRadius(const
int radius) const;
477 bool setSpeckleComponentThreshold(const
int threshold) const;
485 bool setSpeckleRegionSize(const
int threshold) const;
492 bool setFillBorderSpread(const
int maximumspread) const;
499 bool setFillRegionSize(const
int regionsize) const;
504 bool setSurfaceConnectivity(const
int threshold) const;
510 bool setNearPlane(const
int near);
516 bool setFarPlane(const
int far);
528 void storeCalibrationPattern (const
bool enable);
535 boost::signals2::signal<sig_cb_ensenso_point_cloud>* point_cloud_signal_;
538 boost::signals2::signal<sig_cb_ensenso_point_cloud_rgb>* point_cloud_rgb_signal_;
541 boost::signals2::signal<sig_cb_ensenso_images>* images_signal_;
544 boost::signals2::signal<sig_cb_ensenso_images_rgb>* images_rgb_signal_;
547 boost::signals2::signal<sig_cb_ensenso_image_depth>* image_depth_signal_;
555 boost::shared_ptr<const NxLibItem> root_;
568 bool mono_device_open_;
577 std::
string last_stereo_pattern_;
580 bool store_calibration_pattern_;
583 mutable
boost::mutex pattern_mutex_;
613 pcl::uint64_t static getPCLStamp (const
double ensenso_stamp);
620 std::
string static getOpenCVType (const
int channels, const
int bpe, const
bool isFlt);
627 bool jsonToMatrix (const
std::
string json,
Eigen::Affine3d &matrix) const;
635 bool matrixToJson (const
Eigen::Affine3d &matrix,
std::
string &json, const
bool pretty_format=true) const;
640 void processGrabbing ();
645 void triggerCameras();
651 void getImage(const NxLibItem& image_node,
pcl::
PCLGenImage<
pcl::uint8_t>& image_out);
666 #endif // __PCL_IO_ENSENSO_GRABBER__
sensor_msgs::PointCloud2 PointCloud
pcl::PointXYZRGBA PointXYZRGBA
std::pair< pcl::PCLGenImage< pcl::uint8_t >, pcl::PCLGenImage< pcl::uint8_t > > PairOfImages
Grabber for IDS-Imaging Ensenso's devices.
boost::shared_ptr< ::pcl::PCLGenImage< T > const > ConstPtr
boost::shared_ptr< ::pcl::PCLGenImage< T > > Ptr