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

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

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