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 <pcl/point_cloud.h>
27 #include <pcl/point_types.h>
28 
29 #include <core/threading/thread.h>
30 #include <aspect/blocked_timing.h>
31 #include <aspect/clock.h>
32 #include <aspect/configurable.h>
33 #include <aspect/logging.h>
34 #include <aspect/blackboard.h>
35 #include <aspect/tf.h>
36 #include <aspect/pointcloud.h>
37 
38 namespace fawkes {
39  class SwitchInterface;
40  class OpenCVStereoParamsInterface;
41  class Time;
42 #ifdef USE_TIMETRACKER
43  class TimeTracker;
44 #endif
45 }
46 
47 namespace firevision {
48  class Bumblebee2Camera;
49  class SharedMemoryImageBuffer;
50 }
51 
52 namespace cv {
53  class Mat;
54 }
55 
56 class TriclopsData;
57 class TriclopsColorImage;
58 
60 : public fawkes::Thread,
62  public fawkes::ClockAspect,
63  public fawkes::LoggingAspect,
68 {
69  public:
71  virtual ~Bumblebee2Thread();
72 
73  virtual void init();
74  virtual void loop();
75  virtual void finalize();
76 
77  private:
78  void get_triclops_context_from_camera();
79  void deinterlace_green(unsigned char* src, unsigned char* dest,
80  unsigned int width, 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,
90 
91 
92  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
93  protected: virtual void run() { Thread::run(); }
94 
95  private:
96  typedef enum {
97  STEREO_MATCHER_TRICLOPS,
98  STEREO_MATCHER_OPENCV
99  } StereoMatcher;
100 
101  typedef enum {
102  OPENCV_STEREO_BM,
103  OPENCV_STEREO_SGBM
104  } OpenCVStereoAlgorithm;
105 
108 
109  fawkes::SwitchInterface *switch_if_;
111 
113  TriclopsData *triclops_;
114 
115  unsigned int width_;
116  unsigned int height_;
117  float baseline_;
118  float focal_length_;
119  float center_row_;
120  float center_col_;
121 
122  unsigned char *buffer_green_;
123  unsigned char *buffer_rgb_;
124  unsigned char *buffer_rgb_left_;
125  unsigned char *buffer_rgb_right_;
126  unsigned char *buffer_yuv_left_;
127  unsigned char *buffer_yuv_right_;
128  unsigned char *buffer_rgb_planar_left_;
129  unsigned char *buffer_rgb_planar_right_;
130 
131  firevision::SharedMemoryImageBuffer *shm_img_rgb_right_;
132  firevision::SharedMemoryImageBuffer *shm_img_rgb_left_;
133  firevision::SharedMemoryImageBuffer *shm_img_yuv_right_;
134  firevision::SharedMemoryImageBuffer *shm_img_yuv_left_;
135  firevision::SharedMemoryImageBuffer *shm_img_rectified_right_;
136  firevision::SharedMemoryImageBuffer *shm_img_rectified_left_;
137  firevision::SharedMemoryImageBuffer *shm_img_prefiltered_right_;
138  firevision::SharedMemoryImageBuffer *shm_img_prefiltered_left_;
139  firevision::SharedMemoryImageBuffer *shm_img_rgb_rect_left_;
140  firevision::SharedMemoryImageBuffer *shm_img_rgb_rect_right_;
141  firevision::SharedMemoryImageBuffer *shm_img_disparity_;
142 
143  std::string cfg_base_frame_;
144  std::string cfg_frames_prefix_;
145  float cfg_frames_interval_;
146  StereoMatcher cfg_stereo_matcher_;
147 
148  // OpenCV-specific settings
149  OpenCVStereoAlgorithm cfg_opencv_stereo_algorithm_;
150  int cfg_bm_pre_filter_type_;
151  unsigned int cfg_bm_pre_filter_size_;
152  unsigned int cfg_bm_pre_filter_cap_;
153  unsigned int cfg_bm_sad_window_size_;
154  int cfg_bm_min_disparity_;
155  unsigned int cfg_bm_num_disparities_;
156  unsigned int cfg_bm_texture_threshold_;
157  unsigned int cfg_bm_uniqueness_ratio_;
158  unsigned int cfg_bm_speckle_window_size_;
159  unsigned int cfg_bm_speckle_range_;
160  bool cfg_bm_try_smaller_windows_;
161 
162  bool cfg_sgbm_p1_auto_;
163  bool cfg_sgbm_p2_auto_;
164  int cfg_sgbm_p1_;
165  int cfg_sgbm_p2_;
166  int cfg_sgbm_disp_12_max_diff_;
167 
168  float disparity_scale_factor_;
169 
170  cv::Mat *cv_disparity_;
171 
172  fawkes::Time *tf_last_publish_;
174  fawkes::tf::StampedTransform *tf_right_;
175 
176 #ifdef USE_TIMETRACKER
177  fawkes::TimeTracker *tt_;
178  unsigned int tt_loopcount_;
179  unsigned int ttc_full_loop_;
180  unsigned int ttc_transforms_;
181  unsigned int ttc_msgproc_;
182  unsigned int ttc_capture_;
183  unsigned int ttc_preprocess_;
184  unsigned int ttc_rectify_;
185  unsigned int ttc_stereo_match_;
186  unsigned int ttc_pcl_xyzrgb_;
187  unsigned int ttc_pcl_xyz_;
188 #endif
189 };
190 
191 #endif
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:36
Thread to acquire data from Bumblebee2 stereo camera.
Fawkes library namespace.
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:40
virtual void run()
Code to execute in the thread.
Definition: thread.cpp:939
virtual void run()
Stub to see name in backtrace for easier debugging.
A class for handling time.
Definition: time.h:91
Thread class encapsulation of pthreads.
Definition: thread.h:42
Thread aspect to access the transform system.
Definition: tf.h:42
Thread aspect to use blocked timing.
SwitchInterface Fawkes BlackBoard Interface.
Bumblebee2 camera.
Definition: bumblebee2.h:37
OpenCVStereoParamsInterface Fawkes BlackBoard Interface.
Transform that contains a timestamp and frame IDs.
Definition: types.h:96
Shared memory image buffer.
Definition: shm_image.h:181
Time tracking utility.
Definition: tracker.h:38
Thread aspect to log output.
Definition: logging.h:35
Thread aspect to access configuration data.
Definition: configurable.h:35
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49