source: pacpussensors/trunk/StereoVisionDisparity/CameraObstacleGridComponent.h@ 70

Last change on this file since 70 was 70, checked in by phudelai, 10 years ago

Version finale de l'application provel
File size: 7.0 KB
Line 
1/*********************************************************************
2// created: 2013/06/27 - 18:40
3// filename: CameraObstacleGridComponent.h
4//
5// author: Danilo Alves de Lima
6// Copyright Heudiasyc UMR UTC/CNRS 6599
7//
8// version: $Id: $
9//
10// purpose:
11*********************************************************************/
12
13#ifndef CAMERAOBSTACLEGRIDCOMPONENT_H
14#define CAMERAOBSTACLEGRIDCOMPONENT_H
15
16#include <fstream>
17#include <qcoreevent.h>
18#include <qthread.h>
19#include <string>
20
21#include "opencv2/objdetect/objdetect.hpp"
22#include "opencv2/highgui/highgui.hpp"
23#include "opencv2/imgproc/imgproc.hpp"
24
25#include "Pacpus/kernel/ComponentBase.h"
26#include "Pacpus/kernel/DbiteFile.h"
27#include "Pacpus/PacpusTools/ShMem.h"
28//#include "Pacpus/PacpusTools/UDPConnection.h"
29#include "StereoVisionDisparityExp.h"
30#include "SensorsApplicationStructures.h"
31
32#include <QThread>
33#include <QMutex>
34
35namespace pacpus {
36
37/** Class to convert the image classification masks of free space and obstacles in a 2D grid */
38class STEREOVISIONDISPARITY_API CameraObstacleGridComponent: public QThread,
39 public ComponentBase
40{
41
42 Q_OBJECT
43
44public:
45 //============================= DEFAULT ELEMENTS ===============================================
46 CameraObstacleGridComponent(QString name);
47 ~CameraObstacleGridComponent();
48
49 // Run for stereo camera data
50 void run();
51
52 // Run for mono camera data
53 void run2();
54
55 virtual void stopActivity(); /*!< to stop the processing thread */
56 virtual void startActivity(); /*!< to start the processing thread */
57 virtual ComponentBase::COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config);
58 //==============================================================================================
59
60protected:
61
62 // Indicates that thread is running
63 bool is_running;
64
65public:
66
67 /**
68 * Initialize default values
69 */
70 void InitDefault();
71
72private:
73
74 int run_stereo; // Select the work mode (stereo camera data or mono camera data)
75
76 bool use_udpconnection; // Uses the udp connection to send the obstacles data
77
78 //QHostAddress destiny_IP; // Destiny IP
79 //quint16 communication_Port; // Communication port
80
81 int cam_width; // Image width
82 int cam_height; // image height
83 double cam_fov; // Field of view of the camera
84 int cam_valid_col0; // Initial valid column
85 int cam_valid_row0; // Initial valid row
86 int cam_valid_cols; // Valid columns
87 int cam_valid_rows; // Valid rows
88 double cam_fx; // Focus in x (pixels)
89 double cam_fy; // Focus in y (pixels)
90 double cam_cx; // Center in x (pixels)
91 double cam_cy; // Center in y (pixels)
92 double cam_baseline; // Baseline (meters)
93 double cam_tilt_angle; // Inclination angle of the camera
94 double cam_h; // Camera height
95 double CorrAccuracy; // Estimated camera correlation accuracy
96 bool showdebug; // Show frame acquired
97
98 int mMaxImageInputSize1; // Size of the input image data in the memory
99 int mMaxImageInputSize2; // Size of the input image data in the memory
100
101 int mMaxOutputSize1; // Size of the out data in the memory
102
103 TimestampedStructImage Mask1ImageHeader; // Header for the mask image 1
104 TimestampedStructImage Mask2ImageHeader; // Header for the mask image 2
105 TimestampedStructImage DispImageHeader; // Header for the disp image
106
107 //------------------------- Camera occupancy grid projection -----------------------------
108 int data_type; // 0 = camera data to laser format | 1 = camera data to occupancy grid format
109 TimestampedCameraOccData Camdata2Laser; // Camera obstacle data ( for projection like a laser )
110
111 TimestampedSensorOccData CamOccGrid; // Occupancy grid mapping from the stereo image ( for complete projection of the image data )
112
113 double D_MAX_CAM_GRID; // Sensor max valid distance in the grid
114 double sub_div; // Grid subdivision
115
116 double free_area_guess; // Frontal projected area expected to be a road surface (frontal road surface uncouvered by camera FOV)
117 //-----------------------------------------------------------------------------------------
118
119 void* mask1_mem;
120 void* mask2_mem;
121 void* disp_mem;
122
123 size_t mask1_mem_size; // Image shared memory position size
124 size_t mask2_mem_size; // Image shared memory position size
125 size_t disp_mem_size; // Image shared memory position size
126
127 // Imput data
128 ShMem * shmem_mask1; // Shared memory control access to the image data
129 ShMem * shmem_mask2; // Shared memory control access to the image data
130 ShMem * shmem_disp16; // Shared memory control access to the image data
131
132 // Output data
133 ShMem * shmem_obst; // Shared memory control access to the image data
134
135 // Output data via UDP
136// UDPConnection* udp_con;
137
138 cv::Mat CurrentMask1Frame; // Mask1 = free space image
139 cv::Mat CurrentMask2Frame; // Mask2 = obstacles image
140 cv::Mat CurrentDisparityMap16; // Disparity Map 16 bits
141
142 cv::Mat CurrentXMatrix; // Matrix with the x mapping (robot frame)
143 cv::Mat CurrentYMatrix; // Matrix with the y mapping (robot frame)
144
145 // Function to create the obstacle's grid by the obstacles mask image. Similar to a laser.
146 void CreateCamera2LaserGrid(cv::Mat mask_obstacles, cv::Mat disp_map16, double* radius_data, double* angle_data);
147
148 // Function to create the obstacle's grid by the road and obstacles mask images.
149 void CreateCameraGrid(cv::Mat mask_free, cv::Mat mask_obstacles, cv::Mat disp_map16, sensor_occ_data &occ_data);
150
151 /* CreateMonoCameraGrid
152 Description:
153 Function to create the road surface grid by the mono camera segmented image.
154 Parameters:
155 mask_free = road surface mask
156 occ_data = occupancy grid
157 max_dist = max distance to be considerated
158 img_step = step between the image rows and cols
159 */
160 void CreateMonoCameraGrid(cv::Mat mask_free, sensor_occ_data &occ_data, double max_dist, int img_step);
161
162 /* CreateProjectionMatrix
163 Description:
164 Function to create the matrixes with the image projection in the
165 robot plane.
166 Parameters:
167 max_dist = max distance to be considerated
168 */
169 void CreateProjectionMatrix(double max_dist);
170
171 /* PointTriangulate
172 Description:
173 Calculate the point triangulation in the world
174 Parameters:
175 row,col = row and column in the image
176 x,y,z = world coordinates
177 disparity = disparity value
178 */
179 bool PointTriangulate(int row, int col, double &x, double &y, double &z, double disparity);
180
181 /* Calc3DPointProjection
182 Description:
183 Calculate the 3D point projection in a world plane
184 Parameters:
185 row, col = row and column in the image
186 x_r, y_r= world coordinates (in the robot frame)
187 */
188 void Calc3DPointProjection(int row, int col, double &x_r, double &y_r);
189
190 // Draw the camera obstacle grid
191 cv::Mat DrawGrid(double* radius_data, double* angle_data);
192
193 // Draw the camera occupancy grid
194 cv::Mat DrawGrid(sensor_occ_data &occ_data);
195
196 bool recording, THREAD_ALIVE;
197
198 double maxDistToBrake;
199
200Q_SIGNALS:
201 void smallestDistance();
202};
203
204}
205#endif // CAMERAOBSTACLEGRIDCOMPONENT
Note: See TracBrowser for help on using the repository browser.