Fawkes API  Fawkes Development Version
bumblebee2_thread.h
1 
2 /***************************************************************************
3  * bumblebee2_thread.h - Acquire data from Bumblebee2 stereo camera
4  *
5  * Created: Wed Jul 17 13:15:42 2013
6  * Copyright 2013 Tim Niemueller [www.niemueller.de]
7  ****************************************************************************/
8 
9 /* This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU Library General Public License for more details.
18  *
19  * Read the full text in the LICENSE.GPL file in the doc directory.
20  */
21 
22 #ifndef _PLUGINS_PERCEPTION_BUMBLEBEE2_BUMBLEBEE2_THREAD_H_
23 #define _PLUGINS_PERCEPTION_BUMBLEBEE2_BUMBLEBEE2_THREAD_H_
24 
25 // must be first for reliable ROS detection
26 #include <aspect/blackboard.h>
27 #include <aspect/blocked_timing.h>
28 #include <aspect/clock.h>
29 #include <aspect/configurable.h>
30 #include <aspect/logging.h>
31 #include <aspect/pointcloud.h>
32 #include <aspect/tf.h>
33 #include <core/threading/thread.h>
34 #include <pcl/point_cloud.h>
35 #include <pcl/point_types.h>
36 
37 namespace fawkes {
38 class SwitchInterface;
39 class OpenCVStereoParamsInterface;
40 class Time;
41 #ifdef USE_TIMETRACKER
42 class TimeTracker;
43 #endif
44 } // namespace fawkes
45 
46 namespace firevision {
47 class Bumblebee2Camera;
48 class SharedMemoryImageBuffer;
49 } // namespace firevision
50 
51 namespace cv {
52 class Mat;
53 }
54 
55 class TriclopsData;
56 class TriclopsColorImage;
57 
60  public fawkes::ClockAspect,
61  public fawkes::LoggingAspect,
66 {
67 public:
69  virtual ~Bumblebee2Thread();
70 
71  virtual void init();
72  virtual void loop();
73  virtual void finalize();
74 
75 private:
76  void get_triclops_context_from_camera();
77  void deinterlace_green(unsigned char *src,
78  unsigned char *dest,
79  unsigned int width,
80  unsigned int height);
81  void fill_xyz_xyzrgb(const short int * dispdata,
82  const TriclopsColorImage * img_right_rect_color,
85  void fill_xyzrgb(const short int * dispdata,
86  const TriclopsColorImage * img_rect_color,
88  void fill_xyz(const short int *dispdata, pcl::PointCloud<pcl::PointXYZ> &pcl_xyz);
89 
90  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
91 protected:
92  virtual void
93  run()
94  {
95  Thread::run();
96  }
97 
98 private:
99  typedef enum { STEREO_MATCHER_TRICLOPS, STEREO_MATCHER_OPENCV } StereoMatcher;
100 
101  typedef enum { OPENCV_STEREO_BM, OPENCV_STEREO_SGBM } OpenCVStereoAlgorithm;
102 
105 
106  fawkes::SwitchInterface * switch_if_;
108 
110  TriclopsData * triclops_;
111 
112  unsigned int width_;
113  unsigned int height_;
114  float baseline_;
115  float focal_length_;
116  float center_row_;
117  float center_col_;
118 
119  unsigned char *buffer_green_;
120  unsigned char *buffer_rgb_;
121  unsigned char *buffer_rgb_left_;
122  unsigned char *buffer_rgb_right_;
123  unsigned char *buffer_yuv_left_;
124  unsigned char *buffer_yuv_right_;
125  unsigned char *buffer_rgb_planar_left_;
126  unsigned char *buffer_rgb_planar_right_;
127 
128  firevision::SharedMemoryImageBuffer *shm_img_rgb_right_;
129  firevision::SharedMemoryImageBuffer *shm_img_rgb_left_;
130  firevision::SharedMemoryImageBuffer *shm_img_yuv_right_;
131  firevision::SharedMemoryImageBuffer *shm_img_yuv_left_;
132  firevision::SharedMemoryImageBuffer *shm_img_rectified_right_;
133  firevision::SharedMemoryImageBuffer *shm_img_rectified_left_;
134  firevision::SharedMemoryImageBuffer *shm_img_prefiltered_right_;
135  firevision::SharedMemoryImageBuffer *shm_img_prefiltered_left_;
136  firevision::SharedMemoryImageBuffer *shm_img_rgb_rect_left_;
137  firevision::SharedMemoryImageBuffer *shm_img_rgb_rect_right_;
138  firevision::SharedMemoryImageBuffer *shm_img_disparity_;
139 
140  std::string cfg_base_frame_;
141  std::string cfg_frames_prefix_;
142  float cfg_frames_interval_;
143  StereoMatcher cfg_stereo_matcher_;
144 
145  // OpenCV-specific settings
146  OpenCVStereoAlgorithm cfg_opencv_stereo_algorithm_;
147  int cfg_bm_pre_filter_type_;
148  unsigned int cfg_bm_pre_filter_size_;
149  unsigned int cfg_bm_pre_filter_cap_;
150  unsigned int cfg_bm_sad_window_size_;
151  int cfg_bm_min_disparity_;
152  unsigned int cfg_bm_num_disparities_;
153  unsigned int cfg_bm_texture_threshold_;
154  unsigned int cfg_bm_uniqueness_ratio_;
155  unsigned int cfg_bm_speckle_window_size_;
156  unsigned int cfg_bm_speckle_range_;
157  bool cfg_bm_try_smaller_windows_;
158 
159  bool cfg_sgbm_p1_auto_;
160  bool cfg_sgbm_p2_auto_;
161  int cfg_sgbm_p1_;
162  int cfg_sgbm_p2_;
163  int cfg_sgbm_disp_12_max_diff_;
164 
165  float disparity_scale_factor_;
166 
167  cv::Mat *cv_disparity_;
168 
169  fawkes::Time * tf_last_publish_;
171  fawkes::tf::StampedTransform *tf_right_;
172 
173 #ifdef USE_TIMETRACKER
174  fawkes::TimeTracker *tt_;
175  unsigned int tt_loopcount_;
176  unsigned int ttc_full_loop_;
177  unsigned int ttc_transforms_;
178  unsigned int ttc_msgproc_;
179  unsigned int ttc_capture_;
180  unsigned int ttc_preprocess_;
181  unsigned int ttc_rectify_;
182  unsigned int ttc_stereo_match_;
183  unsigned int ttc_pcl_xyzrgb_;
184  unsigned int ttc_pcl_xyz_;
185 #endif
186 };
187 
188 #endif
fawkes::tf::StampedTransform
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
Bumblebee2Thread::Bumblebee2Thread
Bumblebee2Thread()
Constructor.
Definition: bumblebee2_thread.cpp:74
fawkes::SwitchInterface
SwitchInterface Fawkes BlackBoard Interface.
Definition: SwitchInterface.h:34
fawkes::RefPtr
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
Bumblebee2Thread::loop
virtual void loop()
Code to execute in the thread.
Definition: bumblebee2_thread.cpp:510
Bumblebee2Thread
Thread to acquire data from Bumblebee2 stereo camera.
Definition: bumblebee2_thread.h:66
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
firevision::SharedMemoryImageBuffer
Shared memory image buffer.
Definition: shm_image.h:184
Bumblebee2Thread::init
virtual void init()
Initialize the thread.
Definition: bumblebee2_thread.cpp:87
fawkes::OpenCVStereoParamsInterface
OpenCVStereoParamsInterface Fawkes BlackBoard Interface.
Definition: OpenCVStereoParamsInterface.h:34
fawkes::BlackBoardAspect
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
fawkes::PointCloudAspect
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:38
fawkes
Fawkes library namespace.
fawkes::LoggingAspect
Thread aspect to log output.
Definition: logging.h:33
fawkes::TransformAspect
Thread aspect to access the transform system.
Definition: tf.h:39
pcl::PointCloud< pcl::PointXYZ >
firevision::Bumblebee2Camera
Bumblebee2 camera.
Definition: bumblebee2.h:35
fawkes::Time
A class for handling time.
Definition: time.h:93
fawkes::TimeTracker
Time tracking utility.
Definition: tracker.h:37
Bumblebee2Thread::~Bumblebee2Thread
virtual ~Bumblebee2Thread()
Destructor.
Definition: bumblebee2_thread.cpp:82
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::ConfigurableAspect
Thread aspect to access configuration data.
Definition: configurable.h:33
Bumblebee2Thread::finalize
virtual void finalize()
Finalize the thread.
Definition: bumblebee2_thread.cpp:455
Bumblebee2Thread::run
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: bumblebee2_thread.h:93
fawkes::ClockAspect
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34