[53] | 1 | /*********************************************************************
|
---|
| 2 | // created: 2013/06/19 - 18:40
|
---|
| 3 | // filename: ObstacleDetectionComponent.cpp
|
---|
| 4 | //
|
---|
| 5 | // author: Danilo Alves de Lima and Students of SY27
|
---|
| 6 | // Copyright Heudiasyc UMR UTC/CNRS 6599
|
---|
| 7 | //
|
---|
| 8 | // version: $Id: $
|
---|
| 9 | //
|
---|
| 10 | // purpose: Obstacle detection from stereo image data
|
---|
| 11 | //
|
---|
| 12 | //
|
---|
| 13 | *********************************************************************/
|
---|
| 14 | //#include "GeneralDefinitions.h"
|
---|
| 15 |
|
---|
| 16 | #include "ObstacleDetectionComponent.h"
|
---|
| 17 |
|
---|
| 18 | #include <iostream>
|
---|
| 19 | #include <string>
|
---|
| 20 | #include "opencv2/calib3d/calib3d.hpp"
|
---|
| 21 | #include "opencv2/core/core.hpp"
|
---|
| 22 |
|
---|
| 23 | #include "Pacpus/kernel/ComponentFactory.h"
|
---|
| 24 | #include "Pacpus/kernel/DbiteException.h"
|
---|
| 25 | #include "Pacpus/kernel/DbiteFileTypes.h"
|
---|
| 26 | #include "Pacpus/kernel/Log.h"
|
---|
| 27 |
|
---|
| 28 | using namespace std;
|
---|
| 29 | using namespace pacpus;
|
---|
| 30 |
|
---|
| 31 | DECLARE_STATIC_LOGGER("pacpus.base.ObstacleDetectionComponent");
|
---|
| 32 |
|
---|
| 33 | // Construct the factory
|
---|
| 34 | static ComponentFactory<ObstacleDetectionComponent> sFactory("ObstacleDetectionComponent");
|
---|
| 35 |
|
---|
| 36 | const int kMaxFilepathLength = 512; // TODO: should be same for all images
|
---|
| 37 |
|
---|
| 38 | static const string ObstacleDetectionMemoryName_ref = "DisparityMap-ref";
|
---|
| 39 | static const string ObstacleDetectionMemoryName_dispin = "DisparityMap-disp";
|
---|
| 40 |
|
---|
| 41 | static const string ObstacleDetectionMemoryName_mask1 = "ObstacleDetection-mask1";
|
---|
| 42 | static const string ObstacleDetectionMemoryName_mask2 = "ObstacleDetection-mask2";
|
---|
| 43 | static const string ObstacleDetectionMemoryName_dispout = "ObstacleDetection-disp";
|
---|
| 44 |
|
---|
[56] | 45 | static const string ObstacleDetectionMemoryName_dispMapNorm = "ObstacleDetection-dispMapNorm";
|
---|
| 46 | static const string ObstacleDetectionMemoryName_result = "ObstacleDetection-result";
|
---|
| 47 |
|
---|
[53] | 48 | //////////////////////////////////////////////////////////////////////////
|
---|
| 49 | /* ComparePoints1
|
---|
| 50 | Description:
|
---|
| 51 | Compare if the point 1 if less than 2 by the criteria of the higher y e higher x
|
---|
| 52 | Parameters:
|
---|
| 53 | pt1 = point 1
|
---|
| 54 | pt2 = point 2
|
---|
| 55 | */
|
---|
| 56 | bool ComparePoints1( cv::Point pt1, cv::Point pt2)
|
---|
| 57 | {
|
---|
| 58 | /*
|
---|
| 59 | · Strict: pred(X, X) is always false.
|
---|
| 60 |
|
---|
| 61 | · Weak: If !pred(X, Y) && !pred(Y, X), X==Y.
|
---|
| 62 |
|
---|
| 63 | · Ordering: If pred(X, Y) && pred(Y, Z), then pred(X, Z).
|
---|
| 64 | */
|
---|
| 65 | if(pt1.y > pt2.y)
|
---|
| 66 | {
|
---|
| 67 | return true;
|
---|
| 68 | }
|
---|
| 69 | else if((pt1.y == pt2.y)&&(pt1.x >= pt2.x))
|
---|
| 70 | {
|
---|
| 71 | return true;
|
---|
| 72 | }
|
---|
| 73 | else
|
---|
| 74 | {
|
---|
| 75 | return false;
|
---|
| 76 | }
|
---|
| 77 | }
|
---|
| 78 |
|
---|
| 79 | /// Constructor.
|
---|
| 80 | ObstacleDetectionComponent::ObstacleDetectionComponent(QString name)
|
---|
| 81 | : ComponentBase(name)
|
---|
| 82 | {
|
---|
| 83 | LOG_TRACE(Q_FUNC_INFO);
|
---|
| 84 |
|
---|
| 85 | setRecording(0);
|
---|
| 86 |
|
---|
| 87 | ANG_VARIATION = 20.0;
|
---|
| 88 | ANG_VARIATION2 = 7.0;
|
---|
| 89 |
|
---|
| 90 | this->cam_width = 1280; // Image width
|
---|
| 91 | this->cam_height = 960; // Image height
|
---|
| 92 | this->cam_channels = 3;
|
---|
| 93 | this->showdebug = false; // Show frame acquired
|
---|
| 94 |
|
---|
| 95 | // Size of the image data sizeof(char)*width*height*channels
|
---|
| 96 | this->mMaxImageInputSize1 = sizeof(char) * this->cam_width * this->cam_height * this->cam_channels;
|
---|
| 97 |
|
---|
| 98 | // Input data
|
---|
| 99 | this->shmem_ref = 0; // Shared memory control access to the image data
|
---|
| 100 | this->shmem_dispin = 0; // Shared memory control access to the image data
|
---|
| 101 |
|
---|
| 102 | // Output data
|
---|
| 103 | this->shmem_mask1 = 0; // Shared memory control access to the image data (free space mask)
|
---|
| 104 | this->shmem_mask2 = 0; // Shared memory control access to the image data (obstacles mask)
|
---|
| 105 | this->shmem_dispout = 0; // Shared memory control access to the image data (disparity map)
|
---|
| 106 | }
|
---|
| 107 |
|
---|
| 108 | //////////////////////////////////////////////////////////////////////////
|
---|
| 109 | /// Destructor.
|
---|
| 110 | ObstacleDetectionComponent::~ObstacleDetectionComponent()
|
---|
| 111 | {
|
---|
| 112 | LOG_TRACE(Q_FUNC_INFO);
|
---|
| 113 |
|
---|
| 114 | if(this->shmem_ref)
|
---|
| 115 | delete shmem_ref;
|
---|
| 116 |
|
---|
| 117 | this->shmem_ref = NULL;
|
---|
| 118 |
|
---|
| 119 | if(this->shmem_dispin)
|
---|
| 120 | delete shmem_dispin;
|
---|
| 121 |
|
---|
| 122 | this->shmem_dispin = NULL;
|
---|
| 123 |
|
---|
| 124 | if(this->shmem_mask1)
|
---|
| 125 | delete shmem_mask1;
|
---|
| 126 |
|
---|
| 127 | this->shmem_mask1 = NULL;
|
---|
| 128 |
|
---|
| 129 | if(this->shmem_mask2)
|
---|
| 130 | delete shmem_mask2;
|
---|
| 131 |
|
---|
| 132 | this->shmem_mask2 = NULL;
|
---|
| 133 |
|
---|
| 134 | if(this->shmem_dispout)
|
---|
| 135 | delete shmem_dispout;
|
---|
| 136 |
|
---|
| 137 | this->shmem_dispout = NULL;
|
---|
| 138 |
|
---|
[56] | 139 | if(this->shmem_dispMapNormalized)
|
---|
| 140 | delete shmem_dispMapNormalized;
|
---|
| 141 |
|
---|
| 142 | this->shmem_dispMapNormalized = NULL;
|
---|
| 143 |
|
---|
| 144 | if(this->shmem_result)
|
---|
| 145 | delete shmem_result;
|
---|
| 146 |
|
---|
| 147 | this->shmem_result = NULL;
|
---|
[53] | 148 | }
|
---|
| 149 |
|
---|
| 150 | //////////////////////////////////////////////////////////////////////////
|
---|
| 151 | /// Called by the ComponentManager to start the component
|
---|
| 152 | void ObstacleDetectionComponent::startActivity()
|
---|
| 153 | {
|
---|
| 154 | LOG_TRACE(Q_FUNC_INFO);
|
---|
| 155 |
|
---|
| 156 | this->mMaxImageInputSize1 = sizeof(unsigned char) * this->cam_width * this->cam_height * this->cam_channels;
|
---|
| 157 | this->mMaxImageInputSize2 = sizeof(unsigned short) * this->cam_width * this->cam_height;
|
---|
| 158 | this->mMaxImageOutputSize = sizeof(unsigned char) * this->cam_width * this->cam_height;
|
---|
| 159 |
|
---|
| 160 | this->ref_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize1;
|
---|
| 161 | this->dispin_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize2;
|
---|
| 162 | this->mask1_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageOutputSize;
|
---|
| 163 | this->mask2_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageOutputSize;
|
---|
| 164 | this->dispout_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize2;
|
---|
| 165 |
|
---|
| 166 | // Allocate memory position for the maximum expected data
|
---|
| 167 | this->ref_mem = malloc(this->ref_mem_size);
|
---|
| 168 | this->dispin_mem = malloc(this->dispin_mem_size);
|
---|
| 169 | this->mask1_mem = malloc(this->mask1_mem_size);
|
---|
| 170 | this->mask2_mem = malloc(this->mask2_mem_size);
|
---|
| 171 | this->dispout_mem = malloc(this->dispout_mem_size);
|
---|
| 172 |
|
---|
| 173 | this->shmem_ref = new ShMem(ObstacleDetectionMemoryName_ref.c_str(), this->ref_mem_size);
|
---|
| 174 |
|
---|
| 175 | this->shmem_dispin = new ShMem(ObstacleDetectionMemoryName_dispin.c_str(), this->dispin_mem_size);
|
---|
| 176 |
|
---|
| 177 | this->shmem_mask1 = new ShMem(ObstacleDetectionMemoryName_mask1.c_str(), this->mask1_mem_size);
|
---|
| 178 |
|
---|
| 179 | this->shmem_mask2 = new ShMem(ObstacleDetectionMemoryName_mask2.c_str(), this->mask2_mem_size);
|
---|
| 180 |
|
---|
| 181 | this->shmem_dispout = new ShMem(ObstacleDetectionMemoryName_dispout.c_str(), this->dispout_mem_size);
|
---|
| 182 |
|
---|
[56] | 183 | this->shmem_dispMapNormalized = new ShMem(ObstacleDetectionMemoryName_dispMapNorm.c_str(), (this->cam_width * this->cam_height));
|
---|
| 184 |
|
---|
| 185 | this->shmem_result = new ShMem(ObstacleDetectionMemoryName_result.c_str(), (this->cam_width * this->cam_height * 3));
|
---|
| 186 |
|
---|
[53] | 187 | // Run thread
|
---|
| 188 | THREAD_ALIVE = true;
|
---|
| 189 | start();
|
---|
| 190 | }
|
---|
| 191 |
|
---|
| 192 | //////////////////////////////////////////////////////////////////////////
|
---|
| 193 | /// Called by the ComponentManager to stop the component
|
---|
| 194 | void ObstacleDetectionComponent::stopActivity()
|
---|
| 195 | {
|
---|
| 196 | LOG_TRACE(Q_FUNC_INFO);
|
---|
| 197 |
|
---|
| 198 | if(THREAD_ALIVE)
|
---|
| 199 | {
|
---|
| 200 | // Stop thread
|
---|
| 201 | THREAD_ALIVE = false;
|
---|
| 202 |
|
---|
| 203 | while(is_running)
|
---|
| 204 | {
|
---|
| 205 | msleep(/*MS_DELAY*/10);
|
---|
| 206 | }
|
---|
| 207 |
|
---|
| 208 | if(this->shmem_ref)
|
---|
| 209 | delete shmem_ref;
|
---|
| 210 |
|
---|
| 211 | this->shmem_ref = NULL;
|
---|
| 212 |
|
---|
| 213 | if(this->shmem_dispin)
|
---|
| 214 | delete shmem_dispin;
|
---|
| 215 |
|
---|
| 216 | this->shmem_dispin = NULL;
|
---|
| 217 |
|
---|
| 218 | if(this->shmem_mask1)
|
---|
| 219 | delete shmem_mask1;
|
---|
| 220 |
|
---|
| 221 | this->shmem_mask1 = NULL;
|
---|
| 222 |
|
---|
| 223 | if(this->shmem_mask2)
|
---|
| 224 | delete shmem_mask2;
|
---|
| 225 |
|
---|
| 226 | this->shmem_mask2 = NULL;
|
---|
| 227 |
|
---|
| 228 | if(this->shmem_dispout)
|
---|
| 229 | delete shmem_dispout;
|
---|
| 230 |
|
---|
| 231 | this->shmem_dispout = NULL;
|
---|
| 232 |
|
---|
[56] | 233 | if(this->shmem_dispMapNormalized)
|
---|
| 234 | delete shmem_dispMapNormalized;
|
---|
| 235 |
|
---|
| 236 | this->shmem_dispMapNormalized = NULL;
|
---|
| 237 |
|
---|
| 238 | if(this->shmem_result)
|
---|
| 239 | delete shmem_result;
|
---|
| 240 |
|
---|
| 241 | this->shmem_result = NULL;
|
---|
| 242 |
|
---|
[53] | 243 | // Free the malloc memories
|
---|
| 244 | free(this->ref_mem);
|
---|
| 245 | free(this->dispin_mem);
|
---|
| 246 | free(this->mask1_mem);
|
---|
| 247 | free(this->mask2_mem);
|
---|
| 248 | free(this->dispout_mem);
|
---|
| 249 | }
|
---|
| 250 |
|
---|
| 251 | LOG_INFO("stopped component '" << name() << "'");
|
---|
| 252 | }
|
---|
| 253 |
|
---|
| 254 | //////////////////////////////////////////////////////////////////////////
|
---|
| 255 | /// Called by the ComponentManager to pass the XML parameters to the
|
---|
| 256 | /// component
|
---|
| 257 | ComponentBase::COMPONENT_CONFIGURATION ObstacleDetectionComponent::configureComponent(XmlComponentConfig config)
|
---|
| 258 | {
|
---|
| 259 | LOG_TRACE(Q_FUNC_INFO);
|
---|
| 260 |
|
---|
| 261 | // Initialize with default values
|
---|
| 262 | InitDefault();
|
---|
| 263 |
|
---|
| 264 | if (config.getProperty("recording") != QString::null)
|
---|
| 265 | setRecording(config.getProperty("recording").toInt());
|
---|
| 266 |
|
---|
| 267 | if (config.getProperty("cam_width") != QString::null)
|
---|
| 268 | this->cam_width = config.getProperty("cam_width").toInt();
|
---|
| 269 |
|
---|
| 270 | if (config.getProperty("cam_height") != QString::null)
|
---|
| 271 | this->cam_height = config.getProperty("cam_height").toInt();
|
---|
| 272 |
|
---|
| 273 | if (config.getProperty("cam_channels") != QString::null)
|
---|
| 274 | this->cam_channels = config.getProperty("cam_channels").toInt();
|
---|
| 275 |
|
---|
| 276 | if (config.getProperty("min_disp") != QString::null)
|
---|
| 277 | this->min_disp = config.getProperty("min_disp").toInt();
|
---|
| 278 |
|
---|
| 279 | if (config.getProperty("max_disp") != QString::null)
|
---|
| 280 | this->max_disp = config.getProperty("max_disp").toInt();
|
---|
| 281 |
|
---|
| 282 | if (config.getProperty("min_disp_norm") != QString::null)
|
---|
| 283 | this->min_disp_norm = config.getProperty("min_disp_norm").toInt();
|
---|
| 284 |
|
---|
| 285 | if (config.getProperty("max_disp_norm") != QString::null)
|
---|
| 286 | this->max_disp_norm = config.getProperty("max_disp_norm").toInt();
|
---|
| 287 |
|
---|
| 288 | if (config.getProperty("showdebug") != QString::null)
|
---|
| 289 | this->showdebug = (bool)config.getProperty("showdebug").toInt();
|
---|
| 290 |
|
---|
| 291 | if (config.getProperty("destiny_roi_x") != QString::null)
|
---|
| 292 | this->destiny_roi_x = config.getProperty("destiny_roi_x").toInt();
|
---|
| 293 |
|
---|
| 294 | if (config.getProperty("destiny_roi_y") != QString::null)
|
---|
| 295 | this->destiny_roi_y = config.getProperty("destiny_roi_y").toInt();
|
---|
| 296 |
|
---|
| 297 | if (config.getProperty("destiny_roi_width") != QString::null)
|
---|
| 298 | this->destiny_roi_width = config.getProperty("destiny_roi_width").toInt();
|
---|
| 299 |
|
---|
| 300 | if (config.getProperty("destiny_roi_height") != QString::null)
|
---|
| 301 | this->destiny_roi_height = config.getProperty("destiny_roi_height").toInt();
|
---|
| 302 |
|
---|
| 303 | if( ((this->destiny_roi_height != this->cam_height)||(this->destiny_roi_width != this->cam_width))&&
|
---|
| 304 | ((this->destiny_roi_height <= this->cam_height)&&(this->destiny_roi_width <= this->cam_width)) )
|
---|
| 305 | this->use_roi = true;
|
---|
| 306 |
|
---|
| 307 | LOG_INFO("configured component '" << name() << "'");
|
---|
| 308 | return ComponentBase::CONFIGURED_OK;
|
---|
| 309 | }
|
---|
| 310 |
|
---|
| 311 | /**
|
---|
| 312 | * Initialize default values
|
---|
| 313 | */
|
---|
| 314 | void ObstacleDetectionComponent::InitDefault()
|
---|
| 315 | {
|
---|
| 316 | // Default
|
---|
| 317 | this->cam_width = this->destiny_roi_width = 1280; // Image width
|
---|
| 318 | this->cam_height = this->destiny_roi_height = 960; // Image height
|
---|
| 319 |
|
---|
| 320 | this->destiny_roi_x = this->destiny_roi_y = 0; // Destiny image roi x and y
|
---|
| 321 |
|
---|
| 322 | this->use_roi = false;
|
---|
| 323 | this->min_disp = 0;
|
---|
| 324 | this->max_disp = 255;
|
---|
| 325 |
|
---|
| 326 | this->min_disp_norm = 0; // Minimum disparity value to equalize the disp map 16
|
---|
| 327 | this->max_disp_norm = 255; // Maximum disparity value to equalize the disp map 16
|
---|
| 328 |
|
---|
| 329 | this->showdebug = false; // Show frame acquired
|
---|
| 330 | }
|
---|
| 331 |
|
---|
| 332 | // Thread loop
|
---|
| 333 | void ObstacleDetectionComponent::run()
|
---|
| 334 | {
|
---|
| 335 | LOG_TRACE(Q_FUNC_INFO);
|
---|
| 336 |
|
---|
| 337 | this->is_running = true;
|
---|
| 338 |
|
---|
| 339 |
|
---|
| 340 | if(this->CurrentReferenceFrame.cols != this->cam_width)
|
---|
| 341 | {
|
---|
| 342 | this->CurrentReferenceFrame = cv::Mat(cv::Size(this->cam_width , this->cam_height), CV_MAKETYPE(CV_8U, this->cam_channels));
|
---|
| 343 | }
|
---|
| 344 |
|
---|
| 345 | // Create the image in which will be read the disparities
|
---|
| 346 | if(this->CurrentDisparityMap16.cols != this->cam_width)
|
---|
| 347 | {
|
---|
| 348 | this->CurrentDisparityMap16 = cv::Mat( this->cam_height, this->cam_width, CV_16S, cv::Scalar(0) );
|
---|
| 349 | }
|
---|
| 350 |
|
---|
| 351 | if(this->CurrentSurfaceMask.cols != this->cam_width)
|
---|
| 352 | {
|
---|
| 353 | this->CurrentSurfaceMask = cv::Mat(this->cam_height, this->cam_width, CV_MAKETYPE(CV_8U, 1), cv::Scalar(0) );
|
---|
| 354 | }
|
---|
| 355 |
|
---|
| 356 | if(this->CurrentObstaclesMask.cols != this->cam_width)
|
---|
| 357 | {
|
---|
| 358 | this->CurrentObstaclesMask = cv::Mat(this->cam_height, this->cam_width, CV_MAKETYPE(CV_8U, 1), cv::Scalar(0) );
|
---|
| 359 | }
|
---|
| 360 |
|
---|
| 361 | // Images for type convertion
|
---|
| 362 | cv::Mat Disp_map = cv::Mat( this->CurrentDisparityMap16.rows, this->CurrentDisparityMap16.cols, CV_8UC1 );
|
---|
| 363 | cv::Mat Disp_map16;
|
---|
| 364 |
|
---|
| 365 | // Keeps the last image timestamp;
|
---|
| 366 | road_time_t last_reading = 0;
|
---|
| 367 |
|
---|
| 368 | // Initialize the output images header
|
---|
| 369 | this->Mask1ImageHeader.image.width = this->cam_width;
|
---|
| 370 | this->Mask1ImageHeader.image.height = this->cam_height;
|
---|
| 371 | this->Mask1ImageHeader.image.channels = 1;
|
---|
| 372 | this->Mask1ImageHeader.image.width_step = (size_t)(this->Mask1ImageHeader.image.height * this->Mask1ImageHeader.image.channels);
|
---|
| 373 | this->Mask1ImageHeader.image.data_size = this->mMaxImageOutputSize;
|
---|
| 374 |
|
---|
| 375 | this->Mask2ImageHeader.image.width = this->cam_width;
|
---|
| 376 | this->Mask2ImageHeader.image.height = this->cam_height;
|
---|
| 377 | this->Mask2ImageHeader.image.channels = 1;
|
---|
| 378 | this->Mask2ImageHeader.image.width_step = (size_t)(this->Mask2ImageHeader.image.height * this->Mask2ImageHeader.image.channels);
|
---|
| 379 | this->Mask2ImageHeader.image.data_size = this->mMaxImageOutputSize;
|
---|
| 380 |
|
---|
| 381 | this->DispOutImageHeader.image.width = this->cam_width;
|
---|
| 382 | this->DispOutImageHeader.image.height = this->cam_height;
|
---|
| 383 | this->DispOutImageHeader.image.channels = 1;
|
---|
| 384 | this->DispOutImageHeader.image.width_step = (size_t)(sizeof(unsigned short)*this->DispOutImageHeader.image.height * this->DispOutImageHeader.image.channels);
|
---|
| 385 | this->DispOutImageHeader.image.data_size = this->mMaxImageInputSize2;
|
---|
| 386 |
|
---|
| 387 | // Time measurement
|
---|
| 388 | road_time_t init_time = 0;
|
---|
| 389 |
|
---|
| 390 | while (THREAD_ALIVE)
|
---|
| 391 | {
|
---|
| 392 | init_time = road_time();
|
---|
| 393 |
|
---|
| 394 | //LOG_INFO("Grab new image");
|
---|
| 395 | // header + image
|
---|
| 396 | this->shmem_ref->read(this->ref_mem, this->ref_mem_size);
|
---|
| 397 | this->shmem_dispin->read(this->dispin_mem, this->dispin_mem_size);
|
---|
| 398 |
|
---|
| 399 | // Header
|
---|
| 400 | memcpy( &this->RefImageHeader, this->ref_mem, sizeof(TimestampedStructImage));
|
---|
| 401 | memcpy( &this->DispInImageHeader, this->dispin_mem, sizeof(TimestampedStructImage));
|
---|
| 402 |
|
---|
| 403 | // Check image header
|
---|
| 404 | bool is_ok = false;
|
---|
| 405 | if( (this->RefImageHeader.image.data_size == this->mMaxImageInputSize1) && (this->RefImageHeader.time != last_reading) &&
|
---|
| 406 | (this->DispInImageHeader.image.data_size == this->mMaxImageInputSize2) && (this->DispInImageHeader.time == this->RefImageHeader.time) )
|
---|
| 407 | {
|
---|
| 408 | is_ok = true;
|
---|
| 409 | last_reading = this->RefImageHeader.time;
|
---|
| 410 |
|
---|
| 411 | /*std::cout << "Expected image w: " << ImageHeader.image.width << std::endl;
|
---|
| 412 | std::cout << "Expected image h: " << ImageHeader.image.height << std::endl;
|
---|
| 413 | std::cout << "Expected image c: " << ImageHeader.image.channels << std::endl;
|
---|
| 414 | std::cout << "Expected image data: " << ImageHeader.image.data_size << std::endl;
|
---|
| 415 | std::cout << "Expected image size: " << image_mem << std::endl;*/
|
---|
| 416 | }
|
---|
| 417 | /*else
|
---|
| 418 | {
|
---|
| 419 | LOG_ERROR("Error in the image data size!");
|
---|
| 420 | }*/
|
---|
| 421 |
|
---|
| 422 | if(is_ok)
|
---|
| 423 | {
|
---|
| 424 | // Image data
|
---|
| 425 | memcpy( (unsigned char*)(this->CurrentReferenceFrame.data), (unsigned char*)((TimestampedStructImage*)this->ref_mem + 1), this->RefImageHeader.image.data_size);
|
---|
| 426 | memcpy( (unsigned short*)(this->CurrentDisparityMap16.data), (unsigned short*)((TimestampedStructImage*)this->dispin_mem + 1), this->DispInImageHeader.image.data_size);
|
---|
| 427 |
|
---|
| 428 | if(this->showdebug)
|
---|
| 429 | {
|
---|
| 430 | cv::namedWindow( "ObstacleDetectionComponent - Current Reference Frame", CV_WINDOW_NORMAL );
|
---|
| 431 | cv::imshow("ObstacleDetectionComponent - Current Reference Frame",this->CurrentReferenceFrame);
|
---|
| 432 | cv::waitKey(1);
|
---|
| 433 | }
|
---|
| 434 |
|
---|
| 435 | //============================================= Obstacle Detection ================================================
|
---|
| 436 |
|
---|
| 437 | cv::Mat v_disp_map, u_disp_map;
|
---|
| 438 |
|
---|
| 439 | cv::Mat roi_disp = (this->use_roi) ? this->CurrentDisparityMap16(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height)) :
|
---|
| 440 | this->CurrentDisparityMap16;
|
---|
| 441 |
|
---|
| 442 | // U/V disparity maps calculation
|
---|
| 443 | this->CalcUVDisparityMapNorm(roi_disp, v_disp_map, u_disp_map, Disp_map, this->min_disp_norm, this->max_disp_norm);
|
---|
| 444 |
|
---|
| 445 | /*
|
---|
| 446 | // Real disparity information
|
---|
| 447 | Disp_map16 = this->CurrentDisparityMap16/16;
|
---|
| 448 |
|
---|
| 449 | // Display it as a CV_8UC1 image
|
---|
| 450 | Disp_map16.convertTo( Disp_map, CV_8UC1);//, double(255)/double(this->max_disp - this->min_disp));
|
---|
| 451 |
|
---|
| 452 | if(this->showdebug)
|
---|
| 453 | {
|
---|
| 454 | cv::namedWindow( "ObstacleDetectionComponent - Current Disparity Map", CV_WINDOW_AUTOSIZE );
|
---|
| 455 | cv::imshow("ObstacleDetectionComponent - Current Disparity Map", Disp_map);
|
---|
| 456 | cv::waitKey(1);
|
---|
| 457 | }
|
---|
| 458 |
|
---|
| 459 | // U/V disparity maps calculation
|
---|
| 460 | std::pair<cv::Mat,cv::Mat> par_uv = this->CalcUVDisparityMap(Disp_map);
|
---|
| 461 | cv::Mat v_disp_map = par_uv.second;
|
---|
| 462 |
|
---|
| 463 | cv::Mat u_disp_map = par_uv.first;
|
---|
| 464 | */
|
---|
| 465 |
|
---|
| 466 | if(this->showdebug)
|
---|
| 467 | {
|
---|
| 468 | cv::namedWindow( "ObstacleDetectionComponent - Current V Disparity Map", CV_WINDOW_AUTOSIZE );
|
---|
| 469 | cv::imshow("ObstacleDetectionComponent - Current V Disparity Map", v_disp_map);
|
---|
| 470 |
|
---|
| 471 | cv::namedWindow( "ObstacleDetectionComponent - Current U Disparity Map", CV_WINDOW_AUTOSIZE );
|
---|
| 472 | cv::imshow("ObstacleDetectionComponent - Current U Disparity Map", u_disp_map);
|
---|
| 473 |
|
---|
| 474 | cv::namedWindow( "ObstacleDetectionComponent - Current Disparity Map Normalized", CV_WINDOW_AUTOSIZE );
|
---|
| 475 | cv::imshow("ObstacleDetectionComponent - Current Disparity Map Normalized", Disp_map);
|
---|
| 476 | cv::waitKey(1);
|
---|
| 477 | }
|
---|
| 478 |
|
---|
[56] | 479 | //memcpy(this->dispMapNorm_mem, &this->dispMapNormImageHeader, sizeof(TimestampedStructImage));
|
---|
| 480 | //memcpy((void*)((TimestampedStructImage*)this->dispMapNorm_mem + 1), (void*)Disp_map.data, this->dispMapNormImageHeader.image.data_size);
|
---|
| 481 | this->shmem_dispMapNormalized->write((void*)Disp_map.data, (this->cam_width * this->destiny_roi_height));
|
---|
| 482 |
|
---|
[53] | 483 | // Image to detect near obstacles
|
---|
| 484 | cv::Mat v_disp_map2 = v_disp_map.clone();
|
---|
| 485 |
|
---|
| 486 | // Find the driveable surface
|
---|
| 487 | //cv::Mat color_v_disp_map1 = this->FindSurface(v_disp_map, v_disp_map2);
|
---|
| 488 | cv::Mat color_v_disp_map1 = this->FindSurface2(v_disp_map, v_disp_map2);
|
---|
| 489 |
|
---|
| 490 | // Find near obstacles
|
---|
| 491 | //cv::Mat color_v_disp_map2 = this->FindNearObstacles(v_disp_map2, this->min_disp, this->max_disp);
|
---|
| 492 | std::pair<cv::Mat, cv::Mat> color_uv_disp_map = this->FindNearObstaclesUV(v_disp_map2, u_disp_map, this->min_disp, this->max_disp);
|
---|
| 493 |
|
---|
| 494 | if(this->showdebug)
|
---|
| 495 | {
|
---|
| 496 | cv::namedWindow("ObstacleDetectionComponent - Mapa de Disparidade V + Free Space",CV_WINDOW_AUTOSIZE);
|
---|
| 497 | cv::imshow("ObstacleDetectionComponent - Mapa de Disparidade V + Free Space", color_v_disp_map1);
|
---|
| 498 |
|
---|
| 499 | cv::namedWindow("ObstacleDetectionComponent - Mapa de Disparidade V + Obstacles",CV_WINDOW_AUTOSIZE);
|
---|
| 500 | cv::imshow("ObstacleDetectionComponent - Mapa de Disparidade V + Obstacles", color_uv_disp_map.first);
|
---|
| 501 |
|
---|
| 502 | cv::namedWindow("ObstacleDetectionComponent - Mapa de Disparidade U + Obstacles",CV_WINDOW_AUTOSIZE);
|
---|
| 503 | cv::imshow("ObstacleDetectionComponent - Mapa de Disparidade U + Obstacles", color_uv_disp_map.second);
|
---|
| 504 | cv::waitKey(1);
|
---|
| 505 | }
|
---|
[64] | 506 |
|
---|
[53] | 507 | // Remap the v-disparity point in the original image and create binary masks
|
---|
| 508 | //std::pair<cv::Mat, cv::Mat> masks = this->MaskSurface2(Disp_map, color_v_disp_map1, color_uv_disp_map.first, this->min_disp, this->max_disp, 1);
|
---|
| 509 | std::pair<cv::Mat, cv::Mat> masks = this->MaskSurface3(Disp_map, color_v_disp_map1, color_uv_disp_map.first, color_uv_disp_map.second, this->min_disp, this->max_disp, 1);
|
---|
| 510 |
|
---|
| 511 | /*if(this->showdebug)
|
---|
| 512 | {
|
---|
| 513 | cv::namedWindow("Mapa de Disparidade V + Mask1",CV_WINDOW_AUTOSIZE);
|
---|
| 514 | cv::imshow("Mapa de Disparidade V + Mask1", masks.first*255);
|
---|
| 515 |
|
---|
| 516 | cv::namedWindow("Mapa de Disparidade V + Mask2",CV_WINDOW_AUTOSIZE);
|
---|
| 517 | cv::imshow("Mapa de Disparidade V + Mask2", masks.second*255);
|
---|
| 518 | }*/
|
---|
| 519 |
|
---|
| 520 | //---------------------- Remove commun information -------------------------
|
---|
| 521 |
|
---|
| 522 | masks.second = masks.second - masks.second.mul( masks.first);
|
---|
| 523 |
|
---|
| 524 | //---------------------------------------------------------------------------
|
---|
| 525 |
|
---|
| 526 | //------------------ Write the result in the shared memory ------------------
|
---|
| 527 |
|
---|
| 528 | if(this->use_roi)
|
---|
| 529 | {
|
---|
| 530 | masks.first.copyTo(this->CurrentSurfaceMask(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height)));
|
---|
| 531 | masks.second.copyTo(this->CurrentObstaclesMask(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height)));
|
---|
| 532 | }
|
---|
| 533 | else
|
---|
| 534 | {
|
---|
| 535 | this->CurrentSurfaceMask = masks.first;
|
---|
| 536 | this->CurrentObstaclesMask = masks.second;
|
---|
| 537 | }
|
---|
| 538 |
|
---|
| 539 | //----------------------------- Mask 1 --------------------------------------
|
---|
| 540 | // Complete timestamp header of the mask image 1
|
---|
| 541 | this->Mask1ImageHeader.time = this->DispInImageHeader.time;
|
---|
| 542 | this->Mask1ImageHeader.timerange = this->DispInImageHeader.timerange;
|
---|
| 543 |
|
---|
| 544 | // Copy images header and data to memory
|
---|
| 545 | memcpy(this->mask1_mem, &this->Mask1ImageHeader, sizeof(TimestampedStructImage));
|
---|
| 546 | memcpy((void*)((TimestampedStructImage*)this->mask1_mem + 1), (void*)this->CurrentSurfaceMask.data, this->Mask1ImageHeader.image.data_size);
|
---|
| 547 | this->shmem_mask1->write(this->mask1_mem, this->mask1_mem_size);
|
---|
| 548 |
|
---|
| 549 | //----------------------------- Mask 2 --------------------------------------
|
---|
| 550 | // Complete timestamp header of the mask image 2
|
---|
| 551 | this->Mask2ImageHeader.time = this->DispInImageHeader.time;
|
---|
| 552 | this->Mask2ImageHeader.timerange = this->DispInImageHeader.timerange;
|
---|
| 553 |
|
---|
| 554 | // Copy images header and data to memory
|
---|
| 555 | memcpy(this->mask2_mem, &this->Mask2ImageHeader, sizeof(TimestampedStructImage));
|
---|
| 556 | memcpy((void*)((TimestampedStructImage*)this->mask2_mem + 1), (void*)this->CurrentObstaclesMask.data, this->Mask2ImageHeader.image.data_size);
|
---|
| 557 | this->shmem_mask2->write(this->mask2_mem, this->mask2_mem_size);
|
---|
| 558 |
|
---|
| 559 | //------------------------- Disparity map out -------------------------------
|
---|
| 560 | // Complete timestamp header of the disp image out
|
---|
| 561 | this->DispOutImageHeader.time = this->DispInImageHeader.time;
|
---|
| 562 | this->DispOutImageHeader.timerange = this->DispInImageHeader.timerange;
|
---|
| 563 |
|
---|
| 564 | // Copy images header and data to memory
|
---|
| 565 | memcpy(this->dispout_mem, &this->DispOutImageHeader, sizeof(TimestampedStructImage));
|
---|
| 566 | memcpy((void*)((TimestampedStructImage*)this->dispout_mem + 1), (void*)this->CurrentDisparityMap16.data, this->DispOutImageHeader.image.data_size);
|
---|
| 567 | this->shmem_dispout->write(this->dispout_mem, this->dispout_mem_size);
|
---|
| 568 | //---------------------------------------------------------------------------
|
---|
| 569 |
|
---|
| 570 | // ----------------- Apply the mask in the reference image ------------------
|
---|
[56] | 571 |
|
---|
| 572 | std::vector<cv::Mat> channels(3);
|
---|
| 573 | cv::Mat reference;
|
---|
| 574 | if(this->cam_channels == 1)
|
---|
[53] | 575 | {
|
---|
[56] | 576 | cv::cvtColor((this->CurrentReferenceFrame(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).clone(), reference, CV_GRAY2BGR);
|
---|
| 577 | }
|
---|
| 578 | else
|
---|
| 579 | {
|
---|
| 580 | reference = (this->CurrentReferenceFrame(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).clone();
|
---|
| 581 | }
|
---|
[53] | 582 |
|
---|
[56] | 583 | cv::split(reference, channels);
|
---|
[53] | 584 |
|
---|
[56] | 585 | masks.second = 1 - masks.second;
|
---|
| 586 | channels[1] = masks.second.mul(channels[0]); // Activate the red color as obstacles
|
---|
| 587 | channels[2] = masks.second.mul(channels[1]); // Activate the red color as obstacles
|
---|
[53] | 588 |
|
---|
[56] | 589 | //masks.second = masks.second - masks.first;
|
---|
| 590 | //channels[0] = (1 - masks.second).mul(channels[0]); // Activate the yellow color for unclassified area
|
---|
| 591 |
|
---|
| 592 | cv::merge(channels, reference);
|
---|
[53] | 593 |
|
---|
[56] | 594 | this->shmem_result->write((void*)reference.data, (this->destiny_roi_width * this->destiny_roi_height * 3));
|
---|
[53] | 595 |
|
---|
[56] | 596 | if(this->showdebug)
|
---|
| 597 | {
|
---|
| 598 | cv::namedWindow("ObstacleDetectionComponent - Final Classification", CV_WINDOW_AUTOSIZE);
|
---|
[53] | 599 | cv::imshow("ObstacleDetectionComponent - Final Classification", reference);
|
---|
| 600 | }
|
---|
[64] | 601 |
|
---|
[53] | 602 | //---------------------------------------------------------------------------
|
---|
| 603 |
|
---|
| 604 | //==================================================================================================================
|
---|
| 605 |
|
---|
| 606 | //std::cout << componentName.toStdString() << " cicle time: " << (road_time() - init_time)/1000000.0 << std::endl;
|
---|
| 607 | }
|
---|
| 608 | else
|
---|
| 609 | {
|
---|
| 610 | msleep(/*MS_DELAY*/10);
|
---|
| 611 | }
|
---|
| 612 |
|
---|
| 613 | if(this->showdebug)
|
---|
| 614 | cv::waitKey(1); // Give the system permission
|
---|
| 615 |
|
---|
| 616 | //std::cout << componentName.toStdString() << " cicle time: " << (road_time() - init_time)/1000000.0 << std::endl;
|
---|
| 617 | }
|
---|
| 618 |
|
---|
| 619 | this->is_running = false;
|
---|
| 620 |
|
---|
| 621 | // Destroy the window frame
|
---|
| 622 | if(this->showdebug)
|
---|
| 623 | cvDestroyAllWindows();
|
---|
| 624 | }
|
---|
| 625 |
|
---|
[64] | 626 | /* PointTriangulate
|
---|
| 627 | Description:
|
---|
| 628 | Calculate the point triangulation in the world
|
---|
| 629 | Parameters:
|
---|
| 630 | row,col = row and column in the image
|
---|
| 631 | x,y,z = world coordinates
|
---|
| 632 | disparity = disparity value
|
---|
| 633 |
|
---|
| 634 | bool ObstacleDetectionComponent::PointTriangulate(int row, int col, double &x, double &y, double &z, double disparity)
|
---|
| 635 | {
|
---|
| 636 | bool valid_point = false;
|
---|
| 637 |
|
---|
| 638 | if(disparity > 0.0 && disparity < 255.0)
|
---|
| 639 | {
|
---|
| 640 | z = this->cam_width * this->cam_fx * this->cam_baseline / disparity;
|
---|
| 641 | double u = col / (this->cam_width - 1.0) - this->cam_cx;
|
---|
| 642 | double v = row / (this->cam_height - 1.0) - this->cam_cy;
|
---|
| 643 |
|
---|
| 644 | x = u * z / this->cam_fx;
|
---|
| 645 |
|
---|
| 646 | y = v * z / this->cam_fy;
|
---|
| 647 |
|
---|
| 648 | valid_point = true;
|
---|
| 649 | }
|
---|
| 650 |
|
---|
| 651 | return valid_point;
|
---|
| 652 | }*/
|
---|
| 653 |
|
---|
| 654 |
|
---|
[53] | 655 | // Function to calculate the U/V disparity map
|
---|
| 656 | std::pair<cv::Mat, cv::Mat> ObstacleDetectionComponent::CalcUVDisparityMap(cv::Mat disp_map)
|
---|
| 657 | {
|
---|
| 658 | int l, c, pixel; // local variables for row, line and pixel
|
---|
| 659 | unsigned char intensity; // pixel intensity
|
---|
| 660 |
|
---|
| 661 | unsigned char* ptr1; // row pointer for 8 bits image
|
---|
| 662 | //unsigned short* ptr2; // row pointer for 16 bits image
|
---|
| 663 |
|
---|
| 664 | // U disparity map iamge
|
---|
| 665 | cv::Mat u_disp = cv::Mat::zeros(cv::Size(disp_map.cols, 256), CV_8UC1);
|
---|
| 666 |
|
---|
| 667 | // V disparity map image
|
---|
| 668 | cv::Mat v_disp = cv::Mat::zeros(cv::Size(256, disp_map.rows), CV_8UC1);
|
---|
| 669 |
|
---|
| 670 | // run accross the image and add 1 to the respective U/V disparity column
|
---|
| 671 | for (l = 0; l < disp_map.rows; ++l)
|
---|
| 672 | {
|
---|
| 673 | ptr1 = disp_map.ptr<unsigned char>(l);
|
---|
| 674 |
|
---|
| 675 | for (c = 0; c < disp_map.cols; ++c)
|
---|
| 676 | {
|
---|
| 677 | intensity = (unsigned char)ptr1[c];
|
---|
| 678 |
|
---|
| 679 | if( (intensity > this->min_disp)&&(intensity < this->max_disp))
|
---|
| 680 | {
|
---|
| 681 | pixel = intensity*u_disp.cols + c;
|
---|
| 682 | u_disp.data[pixel] = (unsigned char)(u_disp.data[pixel] + 1);
|
---|
| 683 |
|
---|
| 684 | pixel = l*v_disp.cols + intensity;
|
---|
| 685 | v_disp.data[pixel] = (unsigned char)(v_disp.data[pixel] + 1);
|
---|
| 686 | }
|
---|
| 687 | }
|
---|
| 688 | }
|
---|
| 689 |
|
---|
| 690 | return std::make_pair(u_disp, v_disp);
|
---|
| 691 | }
|
---|
| 692 |
|
---|
| 693 | /* CalcUVDisparityMapNorm
|
---|
| 694 | Description:
|
---|
| 695 | Function to calculate the U/V disparity map from a disp map normalized
|
---|
| 696 | Parameters:
|
---|
| 697 | disp_map16 = original disparity map 16
|
---|
| 698 | disp_map_norm = resulted disparity map normalized
|
---|
| 699 | min_d_norm = Minimum disparity value to equalize the disp map 16
|
---|
| 700 | max_d_norm = Maximum disparity value to equalize the disp map 16
|
---|
| 701 | */
|
---|
| 702 | void ObstacleDetectionComponent::CalcUVDisparityMapNorm(cv::Mat disp_map16, cv::Mat &v_disp_map, cv::Mat &u_disp_map, cv::Mat &disp_map_norm, int min_d_norm, int max_d_norm)
|
---|
| 703 | {
|
---|
| 704 | int l, c, pixel; // local variables for row, line and pixel
|
---|
| 705 | unsigned char intensity; // pixel intensity
|
---|
| 706 | int intensity_norm; // pixel intensity
|
---|
| 707 |
|
---|
| 708 | unsigned char* ptr1; // row pointer for 8 bits image
|
---|
| 709 | unsigned short* ptr2; // row pointer for 16 bits image
|
---|
| 710 |
|
---|
| 711 | // Disparity map image normalized
|
---|
| 712 | disp_map_norm = cv::Mat::zeros(cv::Size(disp_map16.cols, disp_map16.rows), CV_8UC1);
|
---|
| 713 |
|
---|
| 714 | // U disparity map image
|
---|
| 715 | u_disp_map = cv::Mat::zeros(cv::Size(disp_map16.cols, 256), CV_8UC1);
|
---|
| 716 |
|
---|
| 717 | // V disparity map image
|
---|
| 718 | v_disp_map = cv::Mat::zeros(cv::Size(256, disp_map16.rows), CV_8UC1);
|
---|
| 719 |
|
---|
| 720 | // percorre a imagem original e soma 1 na coluna do mapa de disparidade V com a mesma
|
---|
| 721 | // intensidade do pixel
|
---|
| 722 | for (l = 0; l < disp_map16.rows; ++l)
|
---|
| 723 | {
|
---|
| 724 | ptr1 = disp_map_norm.ptr<unsigned char>(l);
|
---|
| 725 | ptr2 = disp_map16.ptr<unsigned short>(l);
|
---|
| 726 |
|
---|
| 727 | for (c = 0; c < disp_map16.cols; ++c)
|
---|
| 728 | {
|
---|
| 729 | intensity = (unsigned char)(ptr2[c]/16);
|
---|
| 730 | intensity_norm = (int)((float)((ptr2[c]/16.0f - (float)min_d_norm)*(255.0f)/((float)max_d_norm - (float)min_d_norm)) + 0.5f);
|
---|
| 731 |
|
---|
| 732 | if( (intensity > this->min_disp)&&(intensity < this->max_disp)&&(intensity_norm > 0)&&(intensity_norm < 255))
|
---|
| 733 | {
|
---|
| 734 | pixel = intensity_norm*u_disp_map.cols + c;
|
---|
| 735 | u_disp_map.data[pixel] = (unsigned char)(u_disp_map.data[pixel] + 1);
|
---|
| 736 |
|
---|
| 737 | pixel = l*v_disp_map.cols + intensity_norm;
|
---|
| 738 | v_disp_map.data[pixel] = (unsigned char)(v_disp_map.data[pixel] + 1);
|
---|
| 739 |
|
---|
| 740 | ptr1[c] = (unsigned char)intensity_norm;
|
---|
| 741 | }
|
---|
| 742 | }
|
---|
| 743 | }
|
---|
| 744 |
|
---|
| 745 | return;
|
---|
| 746 | }
|
---|
| 747 |
|
---|
| 748 | // Function to find the free space surface from a V-disparity map
|
---|
| 749 | cv::Mat ObstacleDetectionComponent::FindSurface(cv::Mat &v_disp_map, cv::Mat &v_disp_map2)
|
---|
| 750 | {
|
---|
| 751 | // Parameters of canny and hough transform
|
---|
| 752 | int tshold1 = 154;
|
---|
| 753 | int tshold2 = 48;
|
---|
| 754 | int n_points = 48; //59
|
---|
| 755 | int minLineLenght = 35; //40
|
---|
| 756 | int maxLineGap = 12;
|
---|
| 757 |
|
---|
| 758 |
|
---|
| 759 | // Binary image
|
---|
| 760 | cv::Mat Img_bin = v_disp_map.clone();//cvCloneImage(v_disp_map);
|
---|
| 761 |
|
---|
| 762 | // Color V disparity map with red lines
|
---|
| 763 | cv::Mat color_img = cv::Mat( cv::Size(v_disp_map.cols, v_disp_map.rows), CV_8UC3 );
|
---|
| 764 |
|
---|
| 765 | // Convert to color image
|
---|
| 766 | cv::cvtColor(v_disp_map, color_img, CV_GRAY2BGR);
|
---|
| 767 |
|
---|
| 768 | cv::equalizeHist( Img_bin, Img_bin);
|
---|
| 769 |
|
---|
| 770 | if(this->showdebug)
|
---|
| 771 | {
|
---|
| 772 | // Janela de exibicao
|
---|
| 773 | cv::namedWindow("ObstacleDetectionComponent - Equalized Image",CV_WINDOW_AUTOSIZE);
|
---|
| 774 | cv::imshow("ObstacleDetectionComponent - Equalized Image", Img_bin);
|
---|
| 775 | }
|
---|
| 776 |
|
---|
| 777 | cv::Canny(Img_bin, Img_bin, tshold1, tshold2, 3);
|
---|
| 778 |
|
---|
| 779 | // Closing
|
---|
| 780 | //cv::dilate(Img_bin, Img_bin, cv::Mat(), cv::Point(-1,-1), 2 );
|
---|
| 781 | //cv::erode(Img_bin, Img_bin, cv::Mat(), cv::Point(-1,-1), 1 );
|
---|
| 782 |
|
---|
| 783 | if(this->showdebug)
|
---|
| 784 | {
|
---|
| 785 | // Janela de exibicao
|
---|
| 786 | cv::namedWindow("ObstacleDetectionComponent - Binary Image",CV_WINDOW_AUTOSIZE);
|
---|
| 787 | cv::imshow("ObstacleDetectionComponent - Binary Image", Img_bin);
|
---|
| 788 | }
|
---|
| 789 |
|
---|
| 790 | std::vector<cv::Vec4i> lines; //vector for storing the lines found by HoughLine
|
---|
| 791 |
|
---|
| 792 | // Probabilistic Hough Transform
|
---|
| 793 | cv::HoughLinesP( Img_bin, lines, 1, CV_PI/180, n_points, minLineLenght, maxLineGap );
|
---|
| 794 |
|
---|
| 795 | //=============================== Use the lines filter to remove invalid segments ============================
|
---|
| 796 |
|
---|
| 797 | std::vector<cv::Point> nova_lista = this->LinesFiltering(lines);
|
---|
| 798 |
|
---|
| 799 | if(!nova_lista.empty())
|
---|
| 800 | {
|
---|
| 801 | cv::Point pt_ant = *(nova_lista.begin());
|
---|
| 802 |
|
---|
| 803 | // Filter the mean angle
|
---|
| 804 | for(std::vector<cv::Point>::iterator it = nova_lista.begin(); it != nova_lista.end(); ++it)
|
---|
| 805 | {
|
---|
| 806 | cv::line( color_img, pt_ant, *it, CV_RGB(255,0,0), 8, 8 );
|
---|
| 807 |
|
---|
| 808 | pt_ant = *it;
|
---|
| 809 | }
|
---|
| 810 | }
|
---|
| 811 |
|
---|
| 812 | //============================================================================================================
|
---|
| 813 |
|
---|
| 814 | //======================== Remove invalid line segments by slope angle only ==================================
|
---|
| 815 | //if (lines.size() != 0)
|
---|
| 816 | //{
|
---|
| 817 | // cv::Point pt1, pt2;
|
---|
| 818 | // double theta;
|
---|
| 819 |
|
---|
| 820 | // for(int i = 0; i < (int)lines.size();++i)
|
---|
| 821 | // {
|
---|
| 822 | // pt1.x = lines[i][0];//(CvPoint*)cvGetSeqElem(lines,i);
|
---|
| 823 | // pt1.y = lines[i][1];
|
---|
| 824 | // pt2.x = lines[i][2];
|
---|
| 825 | // pt2.y = lines[i][3];
|
---|
| 826 |
|
---|
| 827 | // CheckPoints(pt1, pt2); //Verifica a ordem dos pontos
|
---|
| 828 | //
|
---|
| 829 | // theta = Inclination(pt1, pt2); //calcula a inclinacao da reta encontrada
|
---|
| 830 |
|
---|
| 831 | // // Valor atual do angulo em graus
|
---|
| 832 | // theta = ((theta*360.0)/(2.0*CV_PI));
|
---|
| 833 |
|
---|
| 834 | // // Verifica se a reta possui inclinacao para ser possivel plano
|
---|
| 835 | // if(theta> (90.0 + ANG_VARIATION))
|
---|
| 836 | // {
|
---|
| 837 |
|
---|
| 838 | // //Desenha as retas em vermelho
|
---|
| 839 | // cv::line( color_img, pt1, pt2, CV_RGB(255,0,0), 4, 8 );
|
---|
| 840 | // }
|
---|
| 841 | // }
|
---|
| 842 | //}
|
---|
| 843 | //==========================================================================================================
|
---|
| 844 |
|
---|
| 845 | std::vector<cv::Mat> channels(3);
|
---|
| 846 |
|
---|
| 847 | // Get the V-disparity without detected red lines
|
---|
| 848 | cv::split(color_img, channels);
|
---|
| 849 | v_disp_map2 = channels[0];
|
---|
| 850 |
|
---|
| 851 | // Janela de exibicao
|
---|
| 852 | //cv::namedWindow("Mapa de Disparidade V + Hough",CV_WINDOW_AUTOSIZE);
|
---|
| 853 | //cv::imshow("Mapa de Disparidade V + Hough", color_img);
|
---|
| 854 | //cv::imshow("Mapa de Disparidade V + Hough", v_disp_map2);
|
---|
| 855 |
|
---|
| 856 | return color_img;
|
---|
| 857 | }
|
---|
| 858 |
|
---|
| 859 | /* FindSurface2
|
---|
| 860 | Description:
|
---|
| 861 | Function to find the free space surface from a V-disparity map, based on the frontal plane.
|
---|
| 862 | Return the V-dysparity map with the red line representing the free surface.
|
---|
| 863 | Parameters:
|
---|
| 864 | v_disp_map = Original V-disparity map
|
---|
| 865 | v_disp_map2 = Orignal V-disparity map less the surface detected
|
---|
| 866 | */
|
---|
| 867 | cv::Mat ObstacleDetectionComponent::FindSurface2(cv::Mat &v_disp_map, cv::Mat &v_disp_map2)
|
---|
| 868 | {
|
---|
| 869 | // Parameters
|
---|
| 870 | int tshold = 40; // Min threshold for the first max value
|
---|
| 871 | int tshold2 = 35; // Min threshold for the step variation
|
---|
| 872 | int maxLineLenght = 25;
|
---|
| 873 | int min_disp_value = 20;
|
---|
| 874 |
|
---|
| 875 | // Binary image
|
---|
| 876 | cv::Mat Img_bin = v_disp_map.clone();
|
---|
| 877 | cv::Mat Img_mask = v_disp_map.clone();
|
---|
| 878 |
|
---|
| 879 | // Color V disparity map with red lines
|
---|
| 880 | cv::Mat color_img = cv::Mat( cv::Size(v_disp_map.cols, v_disp_map.rows), CV_8UC3 );
|
---|
| 881 |
|
---|
| 882 | // Convert to color image
|
---|
| 883 | cv::cvtColor(v_disp_map, color_img, CV_GRAY2BGR);
|
---|
| 884 |
|
---|
| 885 |
|
---|
| 886 | //================= Segment the most probable road surface region ===================================
|
---|
| 887 | /*cv::threshold(Img_mask, Img_mask, tshold, 1, CV_THRESH_BINARY);
|
---|
| 888 | cv::dilate(Img_mask, Img_mask, cv::Mat(), cv::Point(-1,-1), 2 );
|
---|
| 889 | cv::erode(Img_mask, Img_mask, cv::Mat(), cv::Point(-1,-1), 1 );
|
---|
| 890 |
|
---|
| 891 | if(this->showdebug)
|
---|
| 892 | {
|
---|
| 893 | cv::imshow( "ObstacleDetectionComponent - Img_mask", Img_mask*255 );
|
---|
| 894 | }
|
---|
| 895 |
|
---|
| 896 | Img_bin = Img_bin.mul(Img_mask);*/
|
---|
| 897 | //===================================================================================================
|
---|
| 898 |
|
---|
| 899 | // Generate grad_x and grad_y
|
---|
| 900 | cv::Mat grad, grad_H, grad_S, grad_x, grad_y;
|
---|
| 901 | cv::Mat abs_grad_x, abs_grad_y;
|
---|
| 902 |
|
---|
| 903 | // Gradient X
|
---|
| 904 | //Scharr( src_gray, grad_x, ddepth, 1, 0, scale, delta, BORDER_DEFAULT );
|
---|
| 905 | //cv::Sobel( Img_bin, grad_x, CV_16S, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT );
|
---|
| 906 | //cv::convertScaleAbs( grad_x, abs_grad_x );
|
---|
| 907 |
|
---|
| 908 | // Gradient Y
|
---|
| 909 | //Scharr( src_gray, grad_y, ddepth, 0, 1, scale, delta, BORDER_DEFAULT );
|
---|
| 910 | cv::Sobel( Img_bin, grad_y, CV_16S, 0, 1, 3, 1, 0, cv::BORDER_DEFAULT );
|
---|
| 911 | cv::convertScaleAbs( grad_y, abs_grad_y );
|
---|
| 912 |
|
---|
| 913 | // Total Gradient (approximate)
|
---|
| 914 | //cv::addWeighted( abs_grad_x, 0.2, abs_grad_y, 0.8, 0, grad );
|
---|
| 915 |
|
---|
| 916 | //abs_grad_y = abs_grad_y*255.0;
|
---|
| 917 | abs_grad_y.convertTo(Img_mask, CV_8U);
|
---|
| 918 |
|
---|
| 919 | cv::threshold(Img_mask, Img_mask, tshold2, 1, CV_THRESH_BINARY);
|
---|
| 920 | cv::dilate(Img_mask, Img_mask, cv::Mat(), cv::Point(-1,-1), 2 );
|
---|
| 921 | cv::erode(Img_mask, Img_mask, cv::Mat(), cv::Point(-1,-1), 1 );
|
---|
| 922 |
|
---|
| 923 | Img_bin = Img_bin.mul(Img_mask);
|
---|
| 924 |
|
---|
| 925 | //cv::equalizeHist( abs_grad_y, Img_bin);
|
---|
| 926 |
|
---|
| 927 | if(this->showdebug)
|
---|
| 928 | {
|
---|
| 929 | //cv::imshow( "ObstacleDetectionComponent - Sobel X", abs_grad_x );
|
---|
| 930 | cv::imshow( "ObstacleDetectionComponent - Sobel Y", abs_grad_y );
|
---|
| 931 | cv::imshow( "ObstacleDetectionComponent - Img_mask", Img_mask*255 );
|
---|
| 932 | //cv::imshow( "ObstacleDetectionComponent - Sobel", grad );
|
---|
| 933 | //cv::imshow( "ObstacleDetectionComponent - Equalized Image", Img_bin);
|
---|
| 934 | }
|
---|
| 935 |
|
---|
| 936 | //============================== Mark the most significative points as free space ==============================
|
---|
| 937 |
|
---|
| 938 | int row_step_count, last_row_step_count; // Keep the sum of the last valid pixels in the step
|
---|
| 939 | int previous_col = 0;
|
---|
| 940 |
|
---|
| 941 | int left_limit, right_limit; // Auxiliary variables
|
---|
| 942 |
|
---|
| 943 | for(int row = Img_bin.rows - 1; row > 0; --row)
|
---|
| 944 | {
|
---|
| 945 | row_step_count = 0;
|
---|
| 946 | last_row_step_count = 0;
|
---|
| 947 | left_limit = -1;
|
---|
| 948 | right_limit = -1;
|
---|
| 949 |
|
---|
| 950 | unsigned char* ptr1 = Img_bin.ptr<unsigned char>(row);
|
---|
| 951 |
|
---|
| 952 | if(previous_col == 0)
|
---|
| 953 | {
|
---|
| 954 | for(int col = 0; col < Img_bin.cols; ++col)
|
---|
| 955 | {
|
---|
| 956 | // Find max
|
---|
| 957 | if(ptr1[previous_col] < ptr1[col])
|
---|
| 958 | {
|
---|
| 959 | previous_col = col;
|
---|
| 960 |
|
---|
| 961 | tshold2 = (int)(0.95*ptr1[previous_col]); // adjust the tshold
|
---|
| 962 | }
|
---|
| 963 | }
|
---|
| 964 |
|
---|
| 965 | if(ptr1[previous_col] < tshold)
|
---|
| 966 | {
|
---|
| 967 | previous_col = 0; // position
|
---|
| 968 | }
|
---|
| 969 | }
|
---|
| 970 |
|
---|
| 971 | if(previous_col != 0)
|
---|
| 972 | {
|
---|
| 973 | int left = (previous_col - maxLineLenght >= 0) ? previous_col-maxLineLenght : 0;
|
---|
| 974 | int right = (previous_col + maxLineLenght < Img_bin.cols) ? previous_col + maxLineLenght : Img_bin.cols;
|
---|
| 975 |
|
---|
| 976 | int limits_found = 0;
|
---|
| 977 | int l_col = previous_col;
|
---|
| 978 | int r_col = previous_col;
|
---|
| 979 |
|
---|
| 980 | int left_count = 0;
|
---|
| 981 | int right_count = 0;
|
---|
| 982 |
|
---|
| 983 | while(!limits_found)
|
---|
| 984 | {
|
---|
| 985 | //------------------------------- To the left ----------------------------------
|
---|
| 986 | if(left_limit == -1)
|
---|
| 987 | {
|
---|
| 988 | if(l_col > left)
|
---|
| 989 | --l_col;
|
---|
| 990 | else
|
---|
| 991 | left_limit = l_col + left_count;
|
---|
| 992 | }
|
---|
| 993 |
|
---|
| 994 | // Find max
|
---|
| 995 | if(ptr1[previous_col] < ptr1[l_col])
|
---|
| 996 | {
|
---|
| 997 | previous_col = l_col;
|
---|
| 998 | tshold2 = (int)(0.95*ptr1[previous_col]); // adjust the tshold
|
---|
| 999 | }
|
---|
| 1000 |
|
---|
| 1001 | // Check if the pixel intensity is lower than tshold
|
---|
| 1002 | if(left_count < 5)
|
---|
| 1003 | {
|
---|
| 1004 | if(ptr1[l_col] < tshold2)
|
---|
| 1005 | {
|
---|
| 1006 | ++left_count;
|
---|
| 1007 | }
|
---|
| 1008 | else
|
---|
| 1009 | {
|
---|
| 1010 | left_count = 0;
|
---|
| 1011 | }
|
---|
| 1012 | }
|
---|
| 1013 | else
|
---|
| 1014 | {
|
---|
| 1015 | if(left_limit == -1)
|
---|
| 1016 | left_limit = l_col + left_count;
|
---|
| 1017 | }
|
---|
| 1018 | //------------------------------------------------------------------------------
|
---|
| 1019 |
|
---|
| 1020 | //------------------------------- To the right ----------------------------------
|
---|
| 1021 | if(right_limit == -1)
|
---|
| 1022 | {
|
---|
| 1023 | if(r_col < right)
|
---|
| 1024 | ++r_col;
|
---|
| 1025 | else
|
---|
| 1026 | right_limit = r_col - right_count;
|
---|
| 1027 | }
|
---|
| 1028 |
|
---|
| 1029 | // Find max
|
---|
| 1030 | if(ptr1[previous_col] < ptr1[r_col])
|
---|
| 1031 | {
|
---|
| 1032 | previous_col = r_col;
|
---|
| 1033 | tshold2 = (int)(0.95*ptr1[previous_col]); // adjust the tshold
|
---|
| 1034 | }
|
---|
| 1035 |
|
---|
| 1036 | // Check if the pixel intensity is lower than tshold
|
---|
| 1037 | if(right_count < 5)
|
---|
| 1038 | {
|
---|
| 1039 | if(ptr1[r_col] < tshold2)
|
---|
| 1040 | {
|
---|
| 1041 | ++right_count;
|
---|
| 1042 | }
|
---|
| 1043 | else
|
---|
| 1044 | {
|
---|
| 1045 | right_count = 0;
|
---|
| 1046 | }
|
---|
| 1047 | }
|
---|
| 1048 | else
|
---|
| 1049 | {
|
---|
| 1050 | if(right_limit == -1)
|
---|
| 1051 | right_limit = r_col - right_count;
|
---|
| 1052 | }
|
---|
| 1053 | //------------------------------------------------------------------------------
|
---|
| 1054 |
|
---|
| 1055 | if((left_limit != -1)&&(right_limit != -1))
|
---|
| 1056 | {
|
---|
| 1057 | limits_found = 1;
|
---|
| 1058 | }
|
---|
| 1059 | }
|
---|
| 1060 |
|
---|
| 1061 | if((left_limit != -1)&&(right_limit != -1)&&(ptr1[previous_col] > tshold2))
|
---|
| 1062 | {
|
---|
| 1063 | cv::line( color_img, cv::Point(left_limit, row), cv::Point(right_limit, row), CV_RGB(255,0,0), 1, 8 );
|
---|
| 1064 | }
|
---|
| 1065 | }
|
---|
| 1066 | }
|
---|
| 1067 |
|
---|
| 1068 | //==============================================================================================================
|
---|
| 1069 |
|
---|
| 1070 | std::vector<cv::Mat> channels(3);
|
---|
| 1071 |
|
---|
| 1072 | // Get the V-disparity without detected red lines
|
---|
| 1073 | cv::split(color_img, channels);
|
---|
| 1074 | v_disp_map2 = channels[0];
|
---|
| 1075 |
|
---|
| 1076 | // Janela de exibicao
|
---|
| 1077 | //cv::namedWindow("Mapa de Disparidade V + Hough",CV_WINDOW_AUTOSIZE);
|
---|
| 1078 | //cv::imshow("Mapa de Disparidade V + Hough", color_img);
|
---|
| 1079 | //cv::imshow("Mapa de Disparidade V + Hough", v_disp_map2);
|
---|
| 1080 |
|
---|
| 1081 | return color_img;
|
---|
| 1082 | }
|
---|
| 1083 |
|
---|
| 1084 | // Function to find the free space surface from a V-disparity map with mean average
|
---|
| 1085 | cv::Mat ObstacleDetectionComponent::FindAverageSurface(cv::Mat &v_disp_map, cv::Mat &v_disp_map2)
|
---|
| 1086 | {
|
---|
| 1087 | // Parameters of threshold and hough transform
|
---|
| 1088 | int tshold1 = 3;
|
---|
| 1089 | int n_points = 48; //59
|
---|
| 1090 | int minLineLenght = 35; //40
|
---|
| 1091 | int maxLineGap = 12;
|
---|
| 1092 |
|
---|
| 1093 | // Imagem atual em 32F
|
---|
| 1094 | cv::Mat v_disp_map_current32F = cv::Mat(cv::Size(v_disp_map.cols, v_disp_map.rows), CV_32F, 1);
|
---|
| 1095 |
|
---|
| 1096 | // Converte tipo de imagem
|
---|
| 1097 | v_disp_map.convertTo(v_disp_map_current32F, CV_32F);//cvConvertScale(v_disp_map, v_disp_map_current32F);
|
---|
| 1098 |
|
---|
| 1099 | //Mean disparity map for noise attenuation
|
---|
| 1100 | static cv::Mat v_disp_map_mean = v_disp_map_current32F.clone();
|
---|
| 1101 |
|
---|
| 1102 | // Imagem binária da diferenca
|
---|
| 1103 | cv::Mat ImgBinaria = v_disp_map.clone();//cvCloneImage(v_disp_map);
|
---|
| 1104 |
|
---|
| 1105 | // Color V disparity map with red lines
|
---|
| 1106 | cv::Mat color_img = cv::Mat( cv::Size(v_disp_map.cols, v_disp_map.rows), CV_8UC3 );
|
---|
| 1107 |
|
---|
| 1108 | // Convert to color image
|
---|
| 1109 | cv::cvtColor(v_disp_map, color_img, CV_GRAY2BGR);
|
---|
| 1110 |
|
---|
| 1111 | // Running Average
|
---|
| 1112 | cv::accumulateWeighted(v_disp_map_current32F, v_disp_map_mean, 0.20);//cvRunningAvg(v_disp_map_current32F, v_disp_map_mean, 0.20);
|
---|
| 1113 |
|
---|
| 1114 | // Convert scale
|
---|
| 1115 | v_disp_map_mean.convertTo( ImgBinaria, CV_8U); //cvConvertScale(v_disp_map_mean, ImgBinaria, 1.0, 0.0);
|
---|
| 1116 |
|
---|
| 1117 | // Janela de exibicao
|
---|
| 1118 | //cv::namedWindow("Imagem Media Movel",CV_WINDOW_AUTOSIZE);
|
---|
| 1119 | //cv::imshow("Imagem Media Movel", ImgBinaria);
|
---|
| 1120 |
|
---|
| 1121 | cv::threshold(ImgBinaria, ImgBinaria, tshold1, 255,CV_THRESH_BINARY);
|
---|
| 1122 |
|
---|
| 1123 | // Janela de exibicao
|
---|
| 1124 | //cv::namedWindow("Mapa de Disparidade V + binarizacao",CV_WINDOW_AUTOSIZE);
|
---|
| 1125 | //cv::imshow("Mapa de Disparidade V + binarizacao", ImgBinaria);
|
---|
| 1126 |
|
---|
| 1127 | // create 3x3 matrix
|
---|
| 1128 | //static cv::Mat kernel_3x3 = (cv::Mat_<unsigned char>(3,3) << 1, 0, 0, 1, 1, 0, 1, 1, 1);
|
---|
| 1129 |
|
---|
| 1130 | // Fechamento
|
---|
| 1131 | //cv::erode(ImgBinaria,ImgBinaria,NULL, cv::Point(-1,-1), 1);
|
---|
| 1132 | //cv::erode(ImgBinaria,ImgBinaria,kernel_3x3, cv::Point(1,1), 1);
|
---|
| 1133 |
|
---|
| 1134 | // Janela de exibicao
|
---|
| 1135 | //cv::namedWindow("Mapa de Disparidade V + binarizacao + erode",CV_WINDOW_AUTOSIZE);
|
---|
| 1136 | //cv::imshow("Mapa de Disparidade V + binarizacao + erode", ImgBinaria);
|
---|
| 1137 |
|
---|
| 1138 | // Fechamento
|
---|
| 1139 | //cv::dilate(ImgBinaria,ImgBinaria,NULL, cv::Point(-1,-1), 1);
|
---|
| 1140 | //cv::erode(ImgBinaria,ImgBinaria,NULL, cv::Point(-1,-1), 2);
|
---|
| 1141 |
|
---|
| 1142 | std::vector<cv::Vec4i> lines; //vector for storing the lines found by HoughLine
|
---|
| 1143 |
|
---|
| 1144 | // Probabilistic Hough Transform
|
---|
| 1145 | cv::HoughLinesP( ImgBinaria, lines, 1, CV_PI/180, n_points, minLineLenght, maxLineGap );
|
---|
| 1146 |
|
---|
| 1147 | //=============================== Use the lines filter to remove invalid segments ============================
|
---|
| 1148 |
|
---|
| 1149 | //std::vector<cv::Point> nova_lista = this->LinesFiltering(lines);
|
---|
| 1150 | //
|
---|
| 1151 | //if(!nova_lista.empty())
|
---|
| 1152 | //{
|
---|
| 1153 | // cv::Point pt_ant = *(nova_lista.begin());
|
---|
| 1154 |
|
---|
| 1155 | // // Filter the mean angle
|
---|
| 1156 | // for(std::vector<cv::Point>::iterator it = nova_lista.begin(); it != nova_lista.end(); ++it)
|
---|
| 1157 | // {
|
---|
| 1158 | // cv::line( color_img, pt_ant, *it, CV_RGB(255,0,0), 3, 8 );
|
---|
| 1159 |
|
---|
| 1160 | // pt_ant = *it;
|
---|
| 1161 | // }
|
---|
| 1162 | //}
|
---|
| 1163 |
|
---|
| 1164 | //============================================================================================================
|
---|
| 1165 |
|
---|
| 1166 | //======================== Remove invalid line segments by slope angle only ==================================
|
---|
| 1167 | if (lines.size() != 0)
|
---|
| 1168 | {
|
---|
| 1169 | cv::Point pt1, pt2;
|
---|
| 1170 | double theta;
|
---|
| 1171 |
|
---|
| 1172 | for(int i = 0; i < (int)lines.size();++i)
|
---|
| 1173 | {
|
---|
| 1174 | pt1.x = lines[i][0];//(CvPoint*)cvGetSeqElem(lines,i);
|
---|
| 1175 | pt1.y = lines[i][1];
|
---|
| 1176 | pt2.x = lines[i][2];
|
---|
| 1177 | pt2.y = lines[i][3];
|
---|
| 1178 |
|
---|
| 1179 | CheckPoints(pt1, pt2); //Verifica a ordem dos pontos
|
---|
| 1180 |
|
---|
| 1181 | theta = Inclination(pt1, pt2); //calcula a inclinacao da reta encontrada
|
---|
| 1182 |
|
---|
| 1183 | // Valor atual do angulo em graus
|
---|
| 1184 | theta = ((theta*360.0)/(2.0*CV_PI));
|
---|
| 1185 |
|
---|
| 1186 | // Verifica se a reta possui inclinacao para ser possivel plano
|
---|
| 1187 | if(theta> (90.0 + ANG_VARIATION))
|
---|
| 1188 | {
|
---|
| 1189 |
|
---|
| 1190 | //Desenha as retas em vermelho
|
---|
| 1191 | cv::line( color_img, pt1, pt2, CV_RGB(255,0,0), 6, 8 );
|
---|
| 1192 | }
|
---|
| 1193 | }
|
---|
| 1194 | }
|
---|
| 1195 | //==========================================================================================================
|
---|
| 1196 |
|
---|
| 1197 | std::vector<cv::Mat> channels(3);
|
---|
| 1198 |
|
---|
| 1199 | // Get the V-disparity without detected red lines
|
---|
| 1200 | cv::split(color_img, channels);
|
---|
| 1201 | v_disp_map2 = channels[0];
|
---|
| 1202 |
|
---|
| 1203 | // Janela de exibicao
|
---|
| 1204 | //cv::namedWindow("Mapa de Disparidade V + Hough",CV_WINDOW_AUTOSIZE);
|
---|
| 1205 | //cv::imshow("Mapa de Disparidade V + Hough", color_img);
|
---|
| 1206 | //cv::imshow("Mapa de Disparidade V + Hough", v_disp_map2);
|
---|
| 1207 |
|
---|
| 1208 | return color_img;
|
---|
| 1209 | }
|
---|
| 1210 |
|
---|
| 1211 | // Function to find the near obstacles from a v-Disparity map
|
---|
| 1212 | cv::Mat ObstacleDetectionComponent::FindNearObstacles(cv::Mat v_disp_map, int min_d, int max_d)
|
---|
| 1213 | {
|
---|
| 1214 | // Parameters of threshold and hough transform
|
---|
| 1215 | int tshold1 = 3;
|
---|
| 1216 | int n_points = 15;
|
---|
| 1217 | int minLineLenght = 10;
|
---|
| 1218 | int maxLineGap = 12;
|
---|
| 1219 |
|
---|
| 1220 | // Image to be processed
|
---|
| 1221 | cv::Mat v_disp_map_aux = v_disp_map.clone();
|
---|
| 1222 |
|
---|
| 1223 | // Image with the obstacles highlighted in red
|
---|
| 1224 | cv::Mat color_img = cv::Mat( cv::Size(v_disp_map.cols, v_disp_map.rows), CV_8UC3);
|
---|
| 1225 |
|
---|
| 1226 | // Convert color space
|
---|
| 1227 | cv::cvtColor(v_disp_map_aux, color_img, CV_GRAY2BGR);
|
---|
| 1228 |
|
---|
| 1229 | // Image binarization
|
---|
| 1230 | cv::threshold(v_disp_map_aux, v_disp_map_aux, tshold1, 255, CV_THRESH_BINARY);
|
---|
| 1231 |
|
---|
| 1232 | // Janela de exibicao
|
---|
| 1233 | //cv::namedWindow("Mapa de Disparidade V + Threshold 2",CV_WINDOW_AUTOSIZE);
|
---|
| 1234 | //cv::imshow("Mapa de Disparidade V + Threshold 2", v_disp_map_aux);
|
---|
| 1235 |
|
---|
| 1236 | // Define valid ROI
|
---|
| 1237 | cv::Mat v_disp_map_aux_ROI = v_disp_map_aux(cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
|
---|
| 1238 | cv::Mat color_img_ROI = color_img( cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
|
---|
| 1239 |
|
---|
| 1240 | std::vector<cv::Vec4i> lines; //vector for storing the lines found by HoughLine
|
---|
| 1241 |
|
---|
| 1242 | // Probabilistic hough transform
|
---|
| 1243 | cv::HoughLinesP( v_disp_map_aux_ROI, lines, 1, CV_PI/180, n_points, minLineLenght, maxLineGap );
|
---|
| 1244 |
|
---|
| 1245 | if (lines.size() != 0)
|
---|
| 1246 | {
|
---|
| 1247 | cv::Point pt1, pt2;
|
---|
| 1248 | double theta;
|
---|
| 1249 |
|
---|
| 1250 | for(int i = 0; i < (int)lines.size(); ++i)
|
---|
| 1251 | {
|
---|
| 1252 | pt1.x = lines[i][0];//(CvPoint*)cvGetSeqElem(lines,i);
|
---|
| 1253 | pt1.y = lines[i][1];
|
---|
| 1254 | pt2.x = lines[i][2];
|
---|
| 1255 | pt2.y = lines[i][3];
|
---|
| 1256 |
|
---|
| 1257 | this->CheckPoints(pt1, pt2); //Verify the points order
|
---|
| 1258 |
|
---|
| 1259 | theta = this->Inclination(pt1, pt2); // line slope
|
---|
| 1260 |
|
---|
| 1261 | // In degrees
|
---|
| 1262 | theta = ((theta*360.0)/(2.0*CV_PI));
|
---|
| 1263 |
|
---|
| 1264 | // Verifica se a reta possui inclinacao para ser possivel plano
|
---|
| 1265 | if((theta < (90.0 + ANG_VARIATION2))&& (theta > (90.0 - ANG_VARIATION2)))
|
---|
| 1266 | {
|
---|
| 1267 | //Desenha as retas em vermelho
|
---|
| 1268 | cv::line( color_img_ROI, pt1, pt2, CV_RGB(255,0,0), 5, 8 );
|
---|
| 1269 | }
|
---|
| 1270 | }
|
---|
| 1271 | }
|
---|
| 1272 |
|
---|
| 1273 | // Janela de exibicao
|
---|
| 1274 | //cv::namedWindow("Mapa de Disparidade V + Hough1",CV_WINDOW_AUTOSIZE);
|
---|
| 1275 | //cv::imshow("Mapa de Disparidade V + Hough1", v_disp_map_aux);
|
---|
| 1276 |
|
---|
| 1277 | // Janela de exibicao
|
---|
| 1278 | //cv::namedWindow("Mapa de Disparidade V + Hough2",CV_WINDOW_AUTOSIZE);
|
---|
| 1279 | //cv::imshow("Mapa de Disparidade V + Hough2", color_img);
|
---|
| 1280 |
|
---|
| 1281 | return color_img;
|
---|
| 1282 | }
|
---|
| 1283 |
|
---|
| 1284 | // Function to find the near obstacles from the v/u-Disparity maps
|
---|
| 1285 | std::pair<cv::Mat, cv::Mat> ObstacleDetectionComponent::FindNearObstaclesUV(cv::Mat v_disp_map, cv::Mat u_disp_map, int min_d, int max_d)
|
---|
| 1286 | {
|
---|
| 1287 | // Parameters of threshold and hough transform
|
---|
| 1288 | int tshold1 = 3;
|
---|
| 1289 | int tshold2 = 15;
|
---|
| 1290 | int n_points = 15;
|
---|
| 1291 | int minLineLenght = 10;
|
---|
| 1292 | int maxLineGap = 12;
|
---|
| 1293 |
|
---|
| 1294 | std::vector<cv::Mat> channels(3);
|
---|
| 1295 |
|
---|
| 1296 | //=================================== V-disparity map process =============================================================
|
---|
| 1297 | //// Image to be processed
|
---|
| 1298 | //cv::Mat v_disp_map_aux = v_disp_map.clone();
|
---|
| 1299 | //
|
---|
| 1300 | //// Image with the obstacles highlighted in red
|
---|
| 1301 | //cv::Mat v_color_img = cv::Mat( cv::Size(v_disp_map.cols, v_disp_map.rows), CV_8UC3);
|
---|
| 1302 |
|
---|
| 1303 | //// Convert color space
|
---|
| 1304 | //cv::cvtColor(v_disp_map_aux, v_color_img, CV_GRAY2BGR);
|
---|
| 1305 |
|
---|
| 1306 | //cv::equalizeHist( v_disp_map_aux, v_disp_map_aux);
|
---|
| 1307 |
|
---|
| 1308 | //// Image binarization
|
---|
| 1309 | //cv::threshold(v_disp_map_aux, v_disp_map_aux, tshold1, 255, CV_THRESH_BINARY);
|
---|
| 1310 |
|
---|
| 1311 | //// Janela de exibicao
|
---|
| 1312 | //if(this->showdebug)
|
---|
| 1313 | //{
|
---|
| 1314 | // cv::namedWindow("Mapa de Disparidade V + Threshold 2",CV_WINDOW_AUTOSIZE);
|
---|
| 1315 | // cv::imshow("Mapa de Disparidade V + Threshold 2", v_disp_map_aux);
|
---|
| 1316 | //}
|
---|
| 1317 |
|
---|
| 1318 | //// Define valid ROI
|
---|
| 1319 | //cv::Mat v_disp_map_aux_ROI = v_disp_map_aux(cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
|
---|
| 1320 | //cv::Mat v_color_img_ROI = v_color_img( cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
|
---|
| 1321 |
|
---|
| 1322 | //std::vector<cv::Vec4i> lines; //vector for storing the lines found by HoughLine
|
---|
| 1323 |
|
---|
| 1324 | //// Probabilistic hough transform
|
---|
| 1325 | //cv::HoughLinesP( v_disp_map_aux_ROI, lines, 1, CV_PI/180, n_points, minLineLenght, maxLineGap );
|
---|
| 1326 | //
|
---|
| 1327 | //if (lines.size() != 0)
|
---|
| 1328 | //{
|
---|
| 1329 | // cv::Point pt1, pt2;
|
---|
| 1330 | // double theta;
|
---|
| 1331 |
|
---|
| 1332 | // for(int i = 0; i < (int)lines.size(); ++i)
|
---|
| 1333 | // {
|
---|
| 1334 | // pt1.x = lines[i][0];//(CvPoint*)cvGetSeqElem(lines,i);
|
---|
| 1335 | // pt1.y = lines[i][1];
|
---|
| 1336 | // pt2.x = lines[i][2];
|
---|
| 1337 | // pt2.y = lines[i][3];
|
---|
| 1338 |
|
---|
| 1339 | // this->CheckPoints(pt1, pt2); //Verify the points order
|
---|
| 1340 | //
|
---|
| 1341 | // theta = this->Inclination(pt1, pt2); // line slope
|
---|
| 1342 |
|
---|
| 1343 | // // In degrees
|
---|
| 1344 | // theta = ((theta*360.0)/(2.0*CV_PI));
|
---|
| 1345 |
|
---|
| 1346 | // // Verifica se a reta possui inclinacao para ser possivel plano
|
---|
| 1347 | // //if((theta < (90.0 + ANG_VARIATION2))&& (theta > (90.0 - ANG_VARIATION2)))
|
---|
| 1348 | // //{
|
---|
| 1349 | // //Desenha as retas em vermelho
|
---|
| 1350 | // cv::line( v_color_img_ROI, pt1, pt2, CV_RGB(255,0,0), 5, 8 );
|
---|
| 1351 | // //}
|
---|
| 1352 | // }
|
---|
| 1353 | //}
|
---|
| 1354 | //
|
---|
| 1355 | //// Janela de exibicao
|
---|
| 1356 | ////cv::namedWindow("Mapa de Disparidade V + Hough1",CV_WINDOW_AUTOSIZE);
|
---|
| 1357 | ////cv::imshow("Mapa de Disparidade V + Hough1", v_disp_map_aux);
|
---|
| 1358 |
|
---|
| 1359 | //// Janela de exibicao
|
---|
| 1360 | ////cv::namedWindow("Mapa de Disparidade V + Hough2",CV_WINDOW_AUTOSIZE);
|
---|
| 1361 | ////cv::imshow("Mapa de Disparidade V + Hough2", color_img);
|
---|
| 1362 |
|
---|
| 1363 | //================================================================================================================================
|
---|
| 1364 |
|
---|
| 1365 | //============================================== V-disparity map process =========================================================
|
---|
| 1366 | // Image to be processed
|
---|
| 1367 | cv::Mat v_disp_map_aux = v_disp_map.clone();
|
---|
| 1368 |
|
---|
| 1369 | // Image with the obstacles highlighted in red
|
---|
| 1370 | cv::Mat v_color_img = cv::Mat( v_disp_map.rows, v_disp_map.cols, CV_8UC3, cv::Scalar(0,0,0));
|
---|
| 1371 |
|
---|
| 1372 | // Convert color space
|
---|
| 1373 | //cv::cvtColor(v_disp_map_aux, v_color_img, CV_GRAY2BGR);
|
---|
| 1374 |
|
---|
| 1375 | //cv::equalizeHist( v_disp_map_aux, v_disp_map_aux);
|
---|
| 1376 |
|
---|
| 1377 | // Image binarization
|
---|
| 1378 | cv::threshold(v_disp_map_aux, v_disp_map_aux, tshold1, 255, CV_THRESH_BINARY);
|
---|
| 1379 |
|
---|
| 1380 | // Janela de exibicao
|
---|
| 1381 | if(this->showdebug)
|
---|
| 1382 | {
|
---|
| 1383 | cv::namedWindow("Mapa de Disparidade V + Threshold 2",CV_WINDOW_AUTOSIZE);
|
---|
| 1384 | cv::imshow("Mapa de Disparidade V + Threshold 2", v_disp_map_aux);
|
---|
| 1385 | }
|
---|
| 1386 |
|
---|
| 1387 | // Define valid ROI
|
---|
| 1388 | cv::Mat v_disp_map_aux_ROI = v_disp_map_aux(cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
|
---|
| 1389 | cv::Mat v_color_img_ROI = v_color_img( cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
|
---|
| 1390 |
|
---|
| 1391 | cv::split( v_color_img, channels);
|
---|
| 1392 | channels[2] = v_disp_map_aux;
|
---|
| 1393 | cv::merge( channels, v_color_img);
|
---|
| 1394 |
|
---|
| 1395 | //================================================================================================================================
|
---|
| 1396 |
|
---|
| 1397 | //=============================================== U-disparity map process ========================================================
|
---|
| 1398 | // Image to be processed
|
---|
| 1399 | cv::Mat u_disp_map_aux = u_disp_map.clone();
|
---|
| 1400 |
|
---|
| 1401 | // Image with the obstacles highlighted in red
|
---|
| 1402 | cv::Mat u_color_img = cv::Mat( cv::Size(u_disp_map.cols, u_disp_map.rows), CV_8UC3, cv::Scalar(0, 0, 0));
|
---|
| 1403 |
|
---|
| 1404 | // Convert color space
|
---|
| 1405 | //cv::cvtColor(u_disp_map_aux, u_color_img, CV_GRAY2BGR);
|
---|
| 1406 |
|
---|
| 1407 | // Image binarization
|
---|
| 1408 | cv::threshold(u_disp_map_aux, u_disp_map_aux, tshold2, 255, CV_THRESH_BINARY);
|
---|
| 1409 |
|
---|
| 1410 | // Closing
|
---|
| 1411 | cv::dilate(u_disp_map_aux, u_disp_map_aux, cv::Mat(), cv::Point(-1,-1), 1 );
|
---|
| 1412 | cv::erode(u_disp_map_aux, u_disp_map_aux, cv::Mat(), cv::Point(-1,-1), 1 );
|
---|
| 1413 |
|
---|
| 1414 |
|
---|
| 1415 | if(this->showdebug)
|
---|
| 1416 | {
|
---|
| 1417 | // Janela de exibicao
|
---|
| 1418 | cv::namedWindow("ObstacleDetectionComponent - Image bin u-disp",CV_WINDOW_AUTOSIZE);
|
---|
| 1419 | cv::imshow("ObstacleDetectionComponent - Image bin u-disp", u_disp_map_aux);
|
---|
| 1420 | }
|
---|
| 1421 |
|
---|
| 1422 | cv::split( u_color_img, channels);
|
---|
| 1423 | channels[2] = u_disp_map_aux;
|
---|
| 1424 | cv::merge( channels, u_color_img);
|
---|
| 1425 |
|
---|
| 1426 | // Define valid ROI
|
---|
| 1427 | //cv::Mat u_disp_map_aux_ROI = v_disp_map_aux(cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
|
---|
| 1428 | //cv::Mat u_color_img_ROI = v_color_img( cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
|
---|
| 1429 | //================================================================================================================================
|
---|
| 1430 |
|
---|
| 1431 | return std::make_pair(v_color_img, u_color_img);
|
---|
| 1432 | }
|
---|
| 1433 |
|
---|
| 1434 | /* LinesFiltering
|
---|
| 1435 | Description:
|
---|
| 1436 | Filter the detected lines related to the distance and angle between them.
|
---|
| 1437 | Parameters:
|
---|
| 1438 | lines = line list (point 1 and 2)
|
---|
| 1439 | */
|
---|
| 1440 | std::vector<cv::Point> ObstacleDetectionComponent::LinesFiltering(std::vector<cv::Vec4i> lines)
|
---|
| 1441 | {
|
---|
| 1442 | std::vector<cv::Point> lista_pontos;
|
---|
| 1443 |
|
---|
| 1444 | std::vector<cv::Point> lista_retorno;
|
---|
| 1445 |
|
---|
| 1446 | std::vector<double> angles;
|
---|
| 1447 |
|
---|
| 1448 | cv::Point pt1, pt2;
|
---|
| 1449 | double theta;
|
---|
| 1450 | double angulo_medio = 0.0;
|
---|
| 1451 |
|
---|
| 1452 | if (lines.size() != 0)
|
---|
| 1453 | {
|
---|
| 1454 | //std::cout << "Found "<< lines.size() << " lines!\n";
|
---|
| 1455 |
|
---|
| 1456 | // Filtro de angulo maximo
|
---|
| 1457 | for(int i = 0; i < (int)lines.size(); ++i)
|
---|
| 1458 | {
|
---|
| 1459 | pt1.x = lines[i][0];//(CvPoint*)cvGetSeqElem(lines,i);
|
---|
| 1460 | pt1.y = lines[i][1];
|
---|
| 1461 | pt2.x = lines[i][2];
|
---|
| 1462 | pt2.y = lines[i][3];
|
---|
| 1463 |
|
---|
| 1464 | CheckPoints(pt1, pt2); //Verifica a ordem dos pontos
|
---|
| 1465 |
|
---|
| 1466 | theta = Inclination(pt1, pt2); //calcula a inclinacao da reta encontrada
|
---|
| 1467 |
|
---|
| 1468 | // Valor atual do angulo em graus
|
---|
| 1469 | theta = ((theta*360.0)/(2.0*CV_PI));
|
---|
| 1470 |
|
---|
| 1471 | //std::cout << "Angle "<< theta << "!\n";
|
---|
| 1472 |
|
---|
| 1473 | // Verifica se a reta possui inclinacao para ser possivel plano
|
---|
| 1474 | if(theta> (90.0 + ANG_VARIATION))
|
---|
| 1475 | {
|
---|
| 1476 | lista_pontos.push_back(pt1);
|
---|
| 1477 | lista_pontos.push_back(pt2);
|
---|
| 1478 |
|
---|
| 1479 | angles.push_back(theta);
|
---|
| 1480 | }
|
---|
| 1481 | }
|
---|
| 1482 |
|
---|
| 1483 | if(!lista_pontos.empty())
|
---|
| 1484 | {
|
---|
| 1485 | std::sort(lista_pontos.begin(), lista_pontos.end(), ComparePoints1);
|
---|
| 1486 | angulo_medio = this->CalcMedian(angles);
|
---|
| 1487 | }
|
---|
| 1488 |
|
---|
| 1489 | CvPoint last_item;
|
---|
| 1490 |
|
---|
| 1491 | // Percorre os pontos ordenados e procura a casca convexa mais a esquerda
|
---|
| 1492 | for(std::vector<cv::Point>::iterator it = lista_pontos.begin(); it != lista_pontos.end(); ++it)
|
---|
| 1493 | {
|
---|
| 1494 | if(lista_retorno.empty())
|
---|
| 1495 | {
|
---|
| 1496 | lista_retorno.push_back(*it);
|
---|
| 1497 |
|
---|
| 1498 | last_item = *it;
|
---|
| 1499 | }
|
---|
| 1500 | else
|
---|
| 1501 | {
|
---|
| 1502 | if( (it + 1) != lista_pontos.end())
|
---|
| 1503 | {
|
---|
| 1504 | // Se tiverem o mesmo y, substitui o ponto
|
---|
| 1505 | if(last_item.y == (*it).y)
|
---|
| 1506 | {
|
---|
| 1507 | // Troca o ultimo elemento
|
---|
| 1508 | lista_retorno.pop_back();
|
---|
| 1509 |
|
---|
| 1510 | lista_retorno.push_back(*it);
|
---|
| 1511 |
|
---|
| 1512 | last_item = *it;
|
---|
| 1513 | }
|
---|
| 1514 | else
|
---|
| 1515 | {
|
---|
| 1516 | theta = Inclination(*it, last_item); //calcula a inclinacao da reta encontrada
|
---|
| 1517 |
|
---|
| 1518 | // Valor atual do angulo em graus
|
---|
| 1519 | theta = ((theta*360.0)/(2.0*CV_PI));
|
---|
| 1520 |
|
---|
| 1521 | lista_retorno.push_back(*it);
|
---|
| 1522 |
|
---|
| 1523 | // Percorre os pontos ordenados e procura o maior angulo entre a casca convexa mais a esquerda
|
---|
| 1524 | for(std::vector<cv::Point>::iterator it2 = it + 1; it2 != lista_pontos.end(); ++it2)
|
---|
| 1525 | {
|
---|
| 1526 | // Verifica se o angulo atual e menor que o anterior
|
---|
| 1527 | if(theta < ((Inclination(*it2, last_item)*360.0)/(2.0*CV_PI)))
|
---|
| 1528 | {
|
---|
| 1529 | // Troca o ultimo elemento
|
---|
| 1530 | lista_retorno.pop_back();
|
---|
| 1531 |
|
---|
| 1532 | lista_retorno.push_back(*it2);
|
---|
| 1533 |
|
---|
| 1534 | theta = Inclination(*it2, last_item); //calcula a inclinacao da reta encontrada
|
---|
| 1535 |
|
---|
| 1536 | // Valor atual do angulo em graus
|
---|
| 1537 | theta = ((theta*360.0)/(2.0*CV_PI));
|
---|
| 1538 |
|
---|
| 1539 | it = it2;
|
---|
| 1540 | }
|
---|
| 1541 | } // for
|
---|
| 1542 |
|
---|
| 1543 | last_item = *it;
|
---|
| 1544 | } // else
|
---|
| 1545 | } // if( (it + 1) != lista_pontos.end())
|
---|
| 1546 | } // else
|
---|
| 1547 | } // for
|
---|
| 1548 |
|
---|
| 1549 | if(!lista_retorno.empty())
|
---|
| 1550 | {
|
---|
| 1551 | last_item = *(lista_retorno.begin());
|
---|
| 1552 |
|
---|
| 1553 | // Filtro de angulo medio
|
---|
| 1554 | for(std::vector<cv::Point>::iterator it = lista_retorno.begin() + 1; it != lista_retorno.end(); ++it)
|
---|
| 1555 | {
|
---|
| 1556 | theta = Inclination(*it, last_item); //calcula a inclinacao da reta encontrada
|
---|
| 1557 |
|
---|
| 1558 | // Valor atual do angulo em graus
|
---|
| 1559 | theta = ((theta*360.0)/(2.0*CV_PI));
|
---|
| 1560 |
|
---|
| 1561 | // Verifica se a reta possui inclinacao fora da media
|
---|
| 1562 | if(theta < (0.95*angulo_medio))//((theta > (1.05*angulo_medio))&&(theta < (0.95*angulo_medio)))
|
---|
| 1563 | {
|
---|
| 1564 | lista_retorno.resize(it - lista_retorno.begin());
|
---|
| 1565 |
|
---|
| 1566 | break;
|
---|
| 1567 | }
|
---|
| 1568 |
|
---|
| 1569 | last_item = *it;
|
---|
| 1570 | }
|
---|
| 1571 | }
|
---|
| 1572 |
|
---|
| 1573 | }
|
---|
| 1574 |
|
---|
| 1575 | return lista_retorno;
|
---|
| 1576 | }
|
---|
| 1577 |
|
---|
| 1578 | //Function to check the points order, making the second one with the highest y ever
|
---|
| 1579 | void ObstacleDetectionComponent::CheckPoints(cv::Point &pt1, cv::Point &pt2)
|
---|
| 1580 | {
|
---|
| 1581 | int aux_x, aux_y;
|
---|
| 1582 |
|
---|
| 1583 | if(pt1.y > pt2.y)
|
---|
| 1584 | {
|
---|
| 1585 | aux_x = pt1.x;
|
---|
| 1586 | aux_y = pt1.y;
|
---|
| 1587 |
|
---|
| 1588 | pt1.x = pt2.x;
|
---|
| 1589 | pt1.y = pt2.y;
|
---|
| 1590 |
|
---|
| 1591 | pt2.x = aux_x;
|
---|
| 1592 | pt2.y = aux_y;
|
---|
| 1593 | }
|
---|
| 1594 |
|
---|
| 1595 | return;
|
---|
| 1596 | }
|
---|
| 1597 |
|
---|
| 1598 | // Function to calculate the line slope of pt1 to pt2
|
---|
| 1599 | double ObstacleDetectionComponent::Inclination(cv::Point pt1, cv::Point pt2)
|
---|
| 1600 | {
|
---|
| 1601 | double theta; //angle
|
---|
| 1602 |
|
---|
| 1603 | theta = fabs((atan2((pt1.y-pt2.y+0.0),(pt1.x-pt2.x+0.0)))); // slope
|
---|
| 1604 |
|
---|
| 1605 | return theta;
|
---|
| 1606 | }
|
---|
| 1607 |
|
---|
| 1608 | /* CalcMedian
|
---|
| 1609 | Description:
|
---|
| 1610 | Calcule the median value of a vector.
|
---|
| 1611 | Parametros:
|
---|
| 1612 | vector = vector with data to calculate the median
|
---|
| 1613 | */
|
---|
| 1614 | template<class A>
|
---|
| 1615 | A ObstacleDetectionComponent::CalcMedian(std::vector<A> vetor) const
|
---|
| 1616 | {
|
---|
| 1617 | A mediana;
|
---|
| 1618 |
|
---|
| 1619 | std::sort(vetor.begin(), vetor.end());
|
---|
| 1620 |
|
---|
| 1621 | mediana = vetor[(int)((double)(vetor.size())/2.0 + 0.5) - 1];
|
---|
| 1622 |
|
---|
| 1623 | return mediana;
|
---|
| 1624 | }
|
---|
| 1625 |
|
---|
| 1626 | // Function to calculate the free space (v_disp_1) and obstacles (v_disp_2) masks from
|
---|
| 1627 | // a red highlighted V-Disparity map
|
---|
| 1628 | std::pair<cv::Mat, cv::Mat> ObstacleDetectionComponent::MaskSurface2(cv::Mat disp_map, cv::Mat v_disp_1, cv::Mat v_disp_2, int min_d, int max_d, int value)
|
---|
| 1629 | {
|
---|
| 1630 | // Free space mask
|
---|
| 1631 | cv::Mat mask1;
|
---|
| 1632 |
|
---|
| 1633 | // Obstacle Mask
|
---|
| 1634 | cv::Mat mask2;
|
---|
| 1635 |
|
---|
| 1636 | // Fill the destiny images with background value
|
---|
| 1637 | if(value == 1)
|
---|
| 1638 | {
|
---|
| 1639 | mask1 = cv::Mat::zeros( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
|
---|
| 1640 | mask2 = cv::Mat::zeros( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
|
---|
| 1641 | }
|
---|
| 1642 | else
|
---|
| 1643 | {
|
---|
| 1644 | mask1 = cv::Mat::ones( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
|
---|
| 1645 | mask2 = cv::Mat::ones( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
|
---|
| 1646 | }
|
---|
| 1647 |
|
---|
| 1648 | int l, c, pixel, v_disp_pixel; // local variables for row, column and pixel
|
---|
| 1649 | unsigned char intensity; // pixel intensity
|
---|
| 1650 | unsigned char intensity_B, intensity_G, intensity_R; //pixel intensity
|
---|
| 1651 |
|
---|
| 1652 | /*uint8_t* pixelPtr_mask1 = (uint8_t*)mask1.data;
|
---|
| 1653 | uint8_t* pixelPtr_mask2 = (uint8_t*)mask2.data;
|
---|
| 1654 | uint8_t* pixelPtr_disp_map = (uint8_t*)disp_map.data;
|
---|
| 1655 | uint8_t* pixelPtr_v_disp1 = (uint8_t*)v_disp_1.data;
|
---|
| 1656 | uint8_t* pixelPtr_v_disp2 = (uint8_t*)v_disp_2.data;*/
|
---|
| 1657 |
|
---|
| 1658 | // run the images associating the red pixels in the v-disparities in the mask1 e mask2
|
---|
| 1659 | for (l = 0; l < disp_map.rows; ++l)
|
---|
| 1660 | {
|
---|
| 1661 | for (c = 0; c < disp_map.cols; ++c)
|
---|
| 1662 | {
|
---|
| 1663 | pixel = l*disp_map.cols + c;
|
---|
| 1664 |
|
---|
| 1665 | intensity = (unsigned char)disp_map.data[pixel];
|
---|
| 1666 |
|
---|
| 1667 | //--------- Marca os planos trafegaveis --------------------
|
---|
| 1668 | // Pixel Azul
|
---|
| 1669 | v_disp_pixel = l*v_disp_1.cols*3 + intensity*3;
|
---|
| 1670 | intensity_B = (unsigned char)v_disp_1.data[v_disp_pixel];
|
---|
| 1671 |
|
---|
| 1672 | // Pixel Verde
|
---|
| 1673 | v_disp_pixel = l*v_disp_1.cols*3 + intensity*3 + 1;
|
---|
| 1674 | intensity_G = (unsigned char)v_disp_1.data[v_disp_pixel];
|
---|
| 1675 |
|
---|
| 1676 | // Pixel Vermelho
|
---|
| 1677 | v_disp_pixel = l*v_disp_1.cols*3 + intensity*3 + 2;
|
---|
| 1678 | intensity_R = (unsigned char)v_disp_1.data[v_disp_pixel];
|
---|
| 1679 |
|
---|
| 1680 | if((intensity_B == 0)&&(intensity_G == 0)&&(intensity_R == 255))
|
---|
| 1681 | {
|
---|
| 1682 | mask1.data[pixel] = value;
|
---|
| 1683 | }
|
---|
| 1684 | //----------------------------------------------------------
|
---|
| 1685 |
|
---|
| 1686 | //--------- Marca os obstaculos ----------------------------
|
---|
| 1687 | if( (intensity >= min_d)&&(intensity <= max_d))
|
---|
| 1688 | {
|
---|
| 1689 | // Pixel Azul
|
---|
| 1690 | v_disp_pixel = l*v_disp_2.cols*3 + intensity*3;
|
---|
| 1691 | intensity_B = (unsigned char)v_disp_2.data[v_disp_pixel];
|
---|
| 1692 |
|
---|
| 1693 | // Pixel Verde
|
---|
| 1694 | v_disp_pixel = l*v_disp_2.cols*3 + intensity*3 + 1;
|
---|
| 1695 | intensity_G = (unsigned char)v_disp_2.data[v_disp_pixel];
|
---|
| 1696 |
|
---|
| 1697 | // Pixel Vermelho
|
---|
| 1698 | v_disp_pixel = l*v_disp_2.cols*3 + intensity*3 + 2;
|
---|
| 1699 | intensity_R = (unsigned char)v_disp_2.data[v_disp_pixel];
|
---|
| 1700 |
|
---|
| 1701 | if((intensity_B == 0)&&(intensity_G == 0)&&(intensity_R == 255))
|
---|
| 1702 | {
|
---|
| 1703 | mask2.data[pixel] = value;
|
---|
| 1704 | }
|
---|
| 1705 | }
|
---|
| 1706 | //----------------------------------------------------------
|
---|
| 1707 | }
|
---|
| 1708 | }
|
---|
| 1709 |
|
---|
| 1710 | /*cv::Mat element = cv::getStructuringElement( cv::MORPH_RECT,
|
---|
| 1711 | cv::Size( 3, 3 ),
|
---|
| 1712 | cv::Point( 1, 1 ) );*/
|
---|
| 1713 |
|
---|
| 1714 | // Espande as regioes brancas da imagem
|
---|
| 1715 | if(value == 1)
|
---|
| 1716 | {
|
---|
| 1717 | //cv::dilate(mask1, mask1, element, cv::Point(1,1), 2);
|
---|
| 1718 | //cv::dilate(mask2, mask2, element, cv::Point(1,1), 2);
|
---|
| 1719 |
|
---|
| 1720 | cv::dilate(mask1, mask1, cv::Mat(), cv::Point(-1,-1), 2 );
|
---|
| 1721 | cv::dilate(mask2, mask2, cv::Mat(), cv::Point(-1,-1), 2 );
|
---|
| 1722 | }
|
---|
| 1723 | else
|
---|
| 1724 | {
|
---|
| 1725 | //cv::erode(mask1, mask1, element, cv::Point(1,1), 2);
|
---|
| 1726 | //cv::erode(mask2, mask2, element, cv::Point(1,1), 2);
|
---|
| 1727 |
|
---|
| 1728 | cv::erode(mask1,mask1,NULL, cv::Point(-1,-1), 2);
|
---|
| 1729 | cv::erode(mask2,mask2,NULL, cv::Point(-1,-1), 2);
|
---|
| 1730 | }
|
---|
| 1731 |
|
---|
| 1732 | //cvNamedWindow("Imagem da mascara1",CV_WINDOW_AUTOSIZE);
|
---|
| 1733 | //cvShowImage("Imagem da mascara1", mask1);
|
---|
| 1734 |
|
---|
| 1735 | //cvNamedWindow("Imagem da mascara2",CV_WINDOW_AUTOSIZE);
|
---|
| 1736 | //cvShowImage("Imagem da mascara2", mask2);
|
---|
| 1737 |
|
---|
| 1738 | return std::make_pair(mask1, mask2);
|
---|
| 1739 | }
|
---|
| 1740 |
|
---|
| 1741 | // Function to calculate the free space (v_disp_1) and obstacles (v_disp_2/u_disp) masks from
|
---|
| 1742 | // a red highlighted V-Disparity map
|
---|
| 1743 | std::pair<cv::Mat, cv::Mat> ObstacleDetectionComponent::MaskSurface3(cv::Mat disp_map, cv::Mat v_disp_1, cv::Mat v_disp_2, cv::Mat u_disp, int min_d, int max_d, int value)
|
---|
| 1744 | {
|
---|
| 1745 | // Free space mask
|
---|
| 1746 | cv::Mat mask1;
|
---|
| 1747 |
|
---|
| 1748 | // Obstacle Mask
|
---|
| 1749 | cv::Mat mask2;
|
---|
| 1750 |
|
---|
| 1751 | // Fill the destiny images with background value
|
---|
| 1752 | if(value == 1)
|
---|
| 1753 | {
|
---|
| 1754 | mask1 = cv::Mat::zeros( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
|
---|
| 1755 | mask2 = cv::Mat::zeros( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
|
---|
| 1756 | }
|
---|
| 1757 | else
|
---|
| 1758 | {
|
---|
| 1759 | mask1 = cv::Mat::ones( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
|
---|
| 1760 | mask2 = cv::Mat::ones( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
|
---|
| 1761 | }
|
---|
| 1762 |
|
---|
| 1763 | int l, c, pixel, v_disp_pixel, u_disp_pixel; // local variables for row, column and pixel
|
---|
| 1764 | unsigned char intensity; // pixel intensity
|
---|
| 1765 | unsigned char intensity_B_v, intensity_G_v, intensity_R_v; //pixel intensity
|
---|
| 1766 | unsigned char intensity_B_u, intensity_G_u, intensity_R_u; //pixel intensity
|
---|
| 1767 |
|
---|
| 1768 | /*uint8_t* pixelPtr_mask1 = (uint8_t*)mask1.data;
|
---|
| 1769 | uint8_t* pixelPtr_mask2 = (uint8_t*)mask2.data;
|
---|
| 1770 | uint8_t* pixelPtr_disp_map = (uint8_t*)disp_map.data;
|
---|
| 1771 | uint8_t* pixelPtr_v_disp1 = (uint8_t*)v_disp_1.data;
|
---|
| 1772 | uint8_t* pixelPtr_v_disp2 = (uint8_t*)v_disp_2.data;*/
|
---|
| 1773 |
|
---|
| 1774 | // run the images associating the red pixels in the v-disparities in the mask1 e mask2
|
---|
| 1775 | for (l = 0; l < disp_map.rows; ++l)
|
---|
| 1776 | {
|
---|
| 1777 | for (c = 0; c < disp_map.cols; ++c)
|
---|
| 1778 | {
|
---|
| 1779 | pixel = l*disp_map.cols + c;
|
---|
| 1780 |
|
---|
| 1781 | intensity = (unsigned char)disp_map.data[pixel];
|
---|
| 1782 |
|
---|
| 1783 | //--------- Marca os planos trafegaveis --------------------
|
---|
| 1784 | // Pixel Azul
|
---|
| 1785 | v_disp_pixel = l*v_disp_1.cols*3 + intensity*3;
|
---|
| 1786 | intensity_B_v = (unsigned char)v_disp_1.data[v_disp_pixel];
|
---|
| 1787 |
|
---|
| 1788 | // Pixel Verde
|
---|
| 1789 | v_disp_pixel = l*v_disp_1.cols*3 + intensity*3 + 1;
|
---|
| 1790 | intensity_G_v = (unsigned char)v_disp_1.data[v_disp_pixel];
|
---|
| 1791 |
|
---|
| 1792 | // Pixel Vermelho
|
---|
| 1793 | v_disp_pixel = l*v_disp_1.cols*3 + intensity*3 + 2;
|
---|
| 1794 | intensity_R_v = (unsigned char)v_disp_1.data[v_disp_pixel];
|
---|
| 1795 |
|
---|
| 1796 | if((intensity_B_v == 0)&&(intensity_G_v == 0)&&(intensity_R_v == 255))
|
---|
| 1797 | {
|
---|
| 1798 | mask1.data[pixel] = value;
|
---|
| 1799 | }
|
---|
| 1800 | //----------------------------------------------------------
|
---|
| 1801 |
|
---|
| 1802 | //--------- Marca os obstaculos ----------------------------
|
---|
| 1803 | if( (intensity >= min_d)&&(intensity <= max_d))
|
---|
| 1804 | {
|
---|
| 1805 | // Pixel Azul
|
---|
| 1806 | v_disp_pixel = l*v_disp_2.cols*3 + intensity*3;
|
---|
| 1807 | u_disp_pixel = intensity*u_disp.cols*3 + c*3;
|
---|
| 1808 | intensity_B_v = (unsigned char)v_disp_2.data[v_disp_pixel];
|
---|
| 1809 | intensity_B_u = (unsigned char)u_disp.data[u_disp_pixel];
|
---|
| 1810 |
|
---|
| 1811 | // Pixel Verde
|
---|
| 1812 | v_disp_pixel = l*v_disp_2.cols*3 + intensity*3 + 1;
|
---|
| 1813 | u_disp_pixel = intensity*u_disp.cols*3 + c*3 + 1;
|
---|
| 1814 | intensity_G_v = (unsigned char)v_disp_2.data[v_disp_pixel];
|
---|
| 1815 | intensity_G_u = (unsigned char)u_disp.data[u_disp_pixel];
|
---|
| 1816 |
|
---|
| 1817 | // Pixel Vermelho
|
---|
| 1818 | v_disp_pixel = l*v_disp_2.cols*3 + intensity*3 + 2;
|
---|
| 1819 | u_disp_pixel = intensity*u_disp.cols*3 + c*3 + 2;
|
---|
| 1820 | intensity_R_v = (unsigned char)v_disp_2.data[v_disp_pixel];
|
---|
| 1821 | intensity_R_u = (unsigned char)u_disp.data[u_disp_pixel];
|
---|
| 1822 |
|
---|
| 1823 | if((intensity_B_v == 0)&&(intensity_G_v == 0)&&(intensity_R_v == 255)&&(intensity_B_u == 0)&&(intensity_G_u == 0)&&(intensity_R_u == 255))
|
---|
| 1824 | {
|
---|
| 1825 | mask2.data[pixel] = value;
|
---|
| 1826 | }
|
---|
| 1827 | }
|
---|
| 1828 | //----------------------------------------------------------
|
---|
| 1829 | }
|
---|
| 1830 | }
|
---|
| 1831 |
|
---|
| 1832 | /*cv::Mat element = cv::getStructuringElement( cv::MORPH_RECT,
|
---|
| 1833 | cv::Size( 3, 3 ),
|
---|
| 1834 | cv::Point( 1, 1 ) );*/
|
---|
| 1835 |
|
---|
| 1836 | // Espande as regioes brancas da imagem
|
---|
| 1837 | if(value == 1)
|
---|
| 1838 | {
|
---|
| 1839 | //cv::dilate(mask1, mask1, element, cv::Point(1,1), 2);
|
---|
| 1840 | //cv::dilate(mask2, mask2, element, cv::Point(1,1), 2);
|
---|
| 1841 |
|
---|
| 1842 | cv::dilate(mask1, mask1, cv::Mat(), cv::Point(-1,-1), 2 );
|
---|
| 1843 | cv::dilate(mask2, mask2, cv::Mat(), cv::Point(-1,-1), 2 );
|
---|
| 1844 | }
|
---|
| 1845 | else
|
---|
| 1846 | {
|
---|
| 1847 | //cv::erode(mask1, mask1, element, cv::Point(1,1), 2);
|
---|
| 1848 | //cv::erode(mask2, mask2, element, cv::Point(1,1), 2);
|
---|
| 1849 |
|
---|
| 1850 | cv::erode(mask1,mask1,NULL, cv::Point(-1,-1), 2);
|
---|
| 1851 | cv::erode(mask2,mask2,NULL, cv::Point(-1,-1), 2);
|
---|
| 1852 | }
|
---|
| 1853 |
|
---|
| 1854 | //cvNamedWindow("Imagem da mascara1",CV_WINDOW_AUTOSIZE);
|
---|
| 1855 | //cvShowImage("Imagem da mascara1", mask1);
|
---|
| 1856 |
|
---|
| 1857 | //cvNamedWindow("Imagem da mascara2",CV_WINDOW_AUTOSIZE);
|
---|
| 1858 | //cvShowImage("Imagem da mascara2", mask2);
|
---|
| 1859 |
|
---|
| 1860 | return std::make_pair(mask1, mask2);
|
---|
| 1861 | } |
---|