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
Thread to acquire data from Bumblebee2 stereo camera.
virtual void run()
Stub to see name in backtrace for easier debugging.
virtual void loop()
Code to execute in the thread.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
Bumblebee2Thread()
Constructor.
virtual ~Bumblebee2Thread()
Destructor.
Thread aspect to access to BlackBoard.
Definition: blackboard.h:34
Thread aspect to use blocked timing.
Thread aspect that allows to obtain the current time from the clock.
Definition: clock.h:34
Thread aspect to access configuration data.
Definition: configurable.h:33
Thread aspect to log output.
Definition: logging.h:33
OpenCVStereoParamsInterface Fawkes BlackBoard Interface.
Thread aspect to provide and access point clouds.
Definition: pointcloud.h:38
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
SwitchInterface Fawkes BlackBoard Interface.
Thread class encapsulation of pthreads.
Definition: thread.h:46
Time tracking utility.
Definition: tracker.h:37
A class for handling time.
Definition: time.h:93
Thread aspect to access the transform system.
Definition: tf.h:39
Transform that contains a timestamp and frame IDs.
Definition: types.h:92
Bumblebee2 camera.
Definition: bumblebee2.h:35
Shared memory image buffer.
Definition: shm_image.h:184
Fawkes library namespace.