Index: trunk/StereoVisionDisparity/CMakeLists.txt
===================================================================
--- trunk/StereoVisionDisparity/CMakeLists.txt	(revision 51)
+++ trunk/StereoVisionDisparity/CMakeLists.txt	(revision 53)
@@ -59,4 +59,6 @@
 	DisparityMap.h
 	StereoVisionDisparityExp.h
+	ObstacleDetectionComponent.cpp
+	ObstacleDetectionComponent.h
 	${PLUGIN_CPP}
 )
@@ -70,4 +72,5 @@
 	UDisparity.h	
 	DisparityMap.h
+	ObstacleDetectionComponent.h
 	${PLUGIN_H}
 )
Index: trunk/StereoVisionDisparity/DisparityMap.cpp
===================================================================
--- trunk/StereoVisionDisparity/DisparityMap.cpp	(revision 51)
+++ trunk/StereoVisionDisparity/DisparityMap.cpp	(revision 53)
@@ -443,6 +443,5 @@
 		bool is_ok = false;
 		if( (this->LeftImage.image.data_size == this->mMaxImageInputSize) && (this->LeftImage.time != last_reading) &&
-			(this->RightImage.image.data_size == this->mMaxImageInputSize) /*&& (this->LeftImage.time == this->RightImage.time)*/ )
-			/* /////!!!!!\\\\ A DECOMMENTER PLUS TARD (avec le delta de temps entre l'image droite et gauche)*/
+			(this->RightImage.image.data_size == this->mMaxImageInputSize) && (this->LeftImage.time == this->RightImage.time) )
 		{
 			is_ok = true;
Index: trunk/StereoVisionDisparity/ObstacleDetectionComponent.cpp
===================================================================
--- trunk/StereoVisionDisparity/ObstacleDetectionComponent.cpp	(revision 53)
+++ trunk/StereoVisionDisparity/ObstacleDetectionComponent.cpp	(revision 53)
@@ -0,0 +1,1800 @@
+/*********************************************************************
+//  created:    2013/06/19 - 18:40
+//  filename:   ObstacleDetectionComponent.cpp
+//
+//  author:     Danilo Alves de Lima and Students of SY27
+//              Copyright Heudiasyc UMR UTC/CNRS 6599
+// 
+//  version:    $Id: $
+//
+//  purpose:   Obstacle detection from stereo image data
+//			   
+//
+*********************************************************************/
+//#include "GeneralDefinitions.h"
+
+#include "ObstacleDetectionComponent.h"
+
+#include <iostream>
+#include <string>
+#include "opencv2/calib3d/calib3d.hpp"
+#include "opencv2/core/core.hpp"
+
+#include "Pacpus/kernel/ComponentFactory.h"
+#include "Pacpus/kernel/DbiteException.h"
+#include "Pacpus/kernel/DbiteFileTypes.h"
+#include "Pacpus/kernel/Log.h"
+
+using namespace std;
+using namespace pacpus;
+
+DECLARE_STATIC_LOGGER("pacpus.base.ObstacleDetectionComponent");
+
+// Construct the factory
+static ComponentFactory<ObstacleDetectionComponent> sFactory("ObstacleDetectionComponent");
+
+const int kMaxFilepathLength = 512; // TODO: should be same for all images
+
+static const string ObstacleDetectionMemoryName_ref = "DisparityMap-ref";
+static const string ObstacleDetectionMemoryName_dispin = "DisparityMap-disp";
+
+static const string ObstacleDetectionMemoryName_mask1 = "ObstacleDetection-mask1";
+static const string ObstacleDetectionMemoryName_mask2 = "ObstacleDetection-mask2";
+static const string ObstacleDetectionMemoryName_dispout = "ObstacleDetection-disp";
+
+//////////////////////////////////////////////////////////////////////////
+/*	ComparePoints1
+	Description:
+		Compare if the point 1 if less than 2 by the criteria of the higher y e higher x
+	Parameters:
+		pt1 = point 1
+		pt2 = point 2
+*/
+bool ComparePoints1( cv::Point pt1, cv::Point pt2)
+{
+	/*
+		· Strict: pred(X, X) is always false.
+
+		· Weak: If !pred(X, Y) && !pred(Y, X), X==Y.
+
+		· Ordering: If pred(X, Y) && pred(Y, Z), then pred(X, Z).
+	*/
+	if(pt1.y > pt2.y)
+	{
+		return true;
+	}
+	else if((pt1.y == pt2.y)&&(pt1.x >= pt2.x))
+	{
+		return true;
+	}
+	else
+	{
+		return false;
+	}
+}
+
+/// Constructor.
+ObstacleDetectionComponent::ObstacleDetectionComponent(QString name)
+	: ComponentBase(name)
+{   
+	LOG_TRACE(Q_FUNC_INFO);
+
+	setRecording(0);
+
+	ANG_VARIATION = 20.0;
+	ANG_VARIATION2 = 7.0;
+
+	this->cam_width = 1280;		// Image width
+	this->cam_height = 960;		// Image height
+	this->cam_channels = 3;
+	this->showdebug = false;	// Show frame acquired
+
+	// Size of the image data sizeof(char)*width*height*channels
+	this->mMaxImageInputSize1 = sizeof(char) * this->cam_width * this->cam_height * this->cam_channels;
+
+	// Input data
+	this->shmem_ref = 0;			// Shared memory control access to the image data
+	this->shmem_dispin = 0;			// Shared memory control access to the image data
+
+	// Output data
+	this->shmem_mask1 = 0;		// Shared memory control access to the image data (free space mask)
+	this->shmem_mask2 = 0;		// Shared memory control access to the image data (obstacles mask)
+	this->shmem_dispout = 0;	// Shared memory control access to the image data (disparity map)
+}
+
+//////////////////////////////////////////////////////////////////////////
+/// Destructor.
+ObstacleDetectionComponent::~ObstacleDetectionComponent()
+{
+	LOG_TRACE(Q_FUNC_INFO);
+
+	if(this->shmem_ref)
+		delete shmem_ref;
+
+	this->shmem_ref = NULL;
+
+	if(this->shmem_dispin)
+		delete shmem_dispin;
+
+	this->shmem_dispin = NULL;
+
+	if(this->shmem_mask1)
+		delete shmem_mask1;
+
+	this->shmem_mask1 = NULL;
+
+	if(this->shmem_mask2)
+		delete shmem_mask2;
+
+	this->shmem_mask2 = NULL;
+
+	if(this->shmem_dispout)
+		delete shmem_dispout;
+
+	this->shmem_dispout = NULL;
+
+}
+
+//////////////////////////////////////////////////////////////////////////
+/// Called by the ComponentManager to start the component
+void ObstacleDetectionComponent::startActivity()
+{
+	LOG_TRACE(Q_FUNC_INFO);
+
+	this->mMaxImageInputSize1 = sizeof(unsigned char) * this->cam_width * this->cam_height * this->cam_channels;
+	this->mMaxImageInputSize2 = sizeof(unsigned short) * this->cam_width * this->cam_height;
+	this->mMaxImageOutputSize = sizeof(unsigned char) * this->cam_width * this->cam_height;
+
+	this->ref_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize1;
+	this->dispin_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize2;
+	this->mask1_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageOutputSize;
+	this->mask2_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageOutputSize;
+	this->dispout_mem_size = sizeof(TimestampedStructImage) + this->mMaxImageInputSize2;
+
+	// Allocate memory position for the maximum expected data
+	this->ref_mem = malloc(this->ref_mem_size);
+	this->dispin_mem = malloc(this->dispin_mem_size);
+	this->mask1_mem = malloc(this->mask1_mem_size); 
+	this->mask2_mem = malloc(this->mask2_mem_size);
+	this->dispout_mem = malloc(this->dispout_mem_size);
+
+	this->shmem_ref = new ShMem(ObstacleDetectionMemoryName_ref.c_str(), this->ref_mem_size);
+
+	this->shmem_dispin = new ShMem(ObstacleDetectionMemoryName_dispin.c_str(), this->dispin_mem_size);
+
+	this->shmem_mask1 = new ShMem(ObstacleDetectionMemoryName_mask1.c_str(), this->mask1_mem_size);
+
+	this->shmem_mask2 = new ShMem(ObstacleDetectionMemoryName_mask2.c_str(), this->mask2_mem_size);
+
+	this->shmem_dispout = new ShMem(ObstacleDetectionMemoryName_dispout.c_str(), this->dispout_mem_size);
+
+	// Run thread
+	THREAD_ALIVE = true;
+	start();
+}
+
+//////////////////////////////////////////////////////////////////////////
+/// Called by the ComponentManager to stop the component
+void ObstacleDetectionComponent::stopActivity()
+{
+	LOG_TRACE(Q_FUNC_INFO);
+
+	if(THREAD_ALIVE)
+	{
+		// Stop thread
+		THREAD_ALIVE = false;
+
+		while(is_running)
+		{
+			msleep(/*MS_DELAY*/10);
+		}
+
+		if(this->shmem_ref)
+			delete shmem_ref;
+
+		this->shmem_ref = NULL;
+
+		if(this->shmem_dispin)
+			delete shmem_dispin;
+
+		this->shmem_dispin = NULL;
+
+		if(this->shmem_mask1)
+			delete shmem_mask1;
+
+		this->shmem_mask1 = NULL;
+
+		if(this->shmem_mask2)
+			delete shmem_mask2;
+
+		this->shmem_mask2 = NULL;
+
+		if(this->shmem_dispout)
+			delete shmem_dispout;
+
+		this->shmem_dispout = NULL;
+
+		// Free the malloc memories
+		free(this->ref_mem);
+		free(this->dispin_mem);
+		free(this->mask1_mem); 
+		free(this->mask2_mem);
+		free(this->dispout_mem);
+
+	}
+
+	LOG_INFO("stopped component '" << name() << "'");
+}
+
+//////////////////////////////////////////////////////////////////////////
+/// Called by the ComponentManager to pass the XML parameters to the
+/// component
+ComponentBase::COMPONENT_CONFIGURATION ObstacleDetectionComponent::configureComponent(XmlComponentConfig config)
+{
+	LOG_TRACE(Q_FUNC_INFO);
+
+	// Initialize with default values
+	InitDefault();
+
+	if (config.getProperty("recording") != QString::null)
+		setRecording(config.getProperty("recording").toInt());
+
+	if (config.getProperty("cam_width") != QString::null)
+		this->cam_width = config.getProperty("cam_width").toInt();
+
+	if (config.getProperty("cam_height") != QString::null)
+		this->cam_height = config.getProperty("cam_height").toInt();
+
+	if (config.getProperty("cam_channels") != QString::null)
+		this->cam_channels = config.getProperty("cam_channels").toInt();
+
+	if (config.getProperty("min_disp") != QString::null)
+		this->min_disp = config.getProperty("min_disp").toInt();
+
+	if (config.getProperty("max_disp") != QString::null)
+		this->max_disp = config.getProperty("max_disp").toInt();
+
+	if (config.getProperty("min_disp_norm") != QString::null)
+		this->min_disp_norm = config.getProperty("min_disp_norm").toInt();
+
+	if (config.getProperty("max_disp_norm") != QString::null)
+		this->max_disp_norm = config.getProperty("max_disp_norm").toInt();
+
+	if (config.getProperty("showdebug") != QString::null)
+		this->showdebug = (bool)config.getProperty("showdebug").toInt();
+
+	if (config.getProperty("destiny_roi_x") != QString::null)
+		this->destiny_roi_x = config.getProperty("destiny_roi_x").toInt();
+
+	if (config.getProperty("destiny_roi_y") != QString::null)
+		this->destiny_roi_y = config.getProperty("destiny_roi_y").toInt();
+
+	if (config.getProperty("destiny_roi_width") != QString::null)
+		this->destiny_roi_width = config.getProperty("destiny_roi_width").toInt();
+
+	if (config.getProperty("destiny_roi_height") != QString::null)
+		this->destiny_roi_height = config.getProperty("destiny_roi_height").toInt();
+
+	if( ((this->destiny_roi_height != this->cam_height)||(this->destiny_roi_width != this->cam_width))&&
+		((this->destiny_roi_height <= this->cam_height)&&(this->destiny_roi_width <= this->cam_width)) )
+		this->use_roi = true;
+
+	LOG_INFO("configured component '" << name() << "'");
+    return ComponentBase::CONFIGURED_OK;
+}
+
+/** 
+* Initialize default values
+*/
+void ObstacleDetectionComponent::InitDefault()
+{
+	// Default
+	this->cam_width = this->destiny_roi_width =  1280;		// Image width
+	this->cam_height = this->destiny_roi_height = 960;		// Image height
+	
+	this->destiny_roi_x = this->destiny_roi_y = 0;		// Destiny image roi x and y
+	
+	this->use_roi = false;
+	this->min_disp = 0;
+	this->max_disp = 255;
+
+	this->min_disp_norm = 0;		// Minimum disparity value to equalize the disp map 16
+	this->max_disp_norm = 255;		// Maximum disparity value to equalize the disp map 16
+
+	this->showdebug = false;							// Show frame acquired
+}
+
+// Thread loop
+void ObstacleDetectionComponent::run()
+{
+	LOG_TRACE(Q_FUNC_INFO);
+
+	this->is_running = true;
+	
+	
+	if(this->CurrentReferenceFrame.cols != this->cam_width)
+	{
+		this->CurrentReferenceFrame = cv::Mat(cv::Size(this->cam_width , this->cam_height), CV_MAKETYPE(CV_8U, this->cam_channels));
+	}
+
+	// Create the image in which will be read the disparities
+	if(this->CurrentDisparityMap16.cols != this->cam_width)
+	{
+		this->CurrentDisparityMap16 = cv::Mat( this->cam_height, this->cam_width, CV_16S, cv::Scalar(0) );
+	}
+
+	if(this->CurrentSurfaceMask.cols != this->cam_width)
+	{
+		this->CurrentSurfaceMask = cv::Mat(this->cam_height, this->cam_width, CV_MAKETYPE(CV_8U, 1), cv::Scalar(0) );
+	}
+
+	if(this->CurrentObstaclesMask.cols != this->cam_width)
+	{
+		this->CurrentObstaclesMask = cv::Mat(this->cam_height, this->cam_width, CV_MAKETYPE(CV_8U, 1), cv::Scalar(0) );
+	}
+
+	// Images for type convertion
+	cv::Mat Disp_map = cv::Mat( this->CurrentDisparityMap16.rows, this->CurrentDisparityMap16.cols, CV_8UC1 );
+	cv::Mat Disp_map16;
+	
+	// Keeps the last image timestamp;
+	road_time_t last_reading = 0;
+
+	// Initialize the output images header
+	this->Mask1ImageHeader.image.width = this->cam_width;
+	this->Mask1ImageHeader.image.height = this->cam_height;
+	this->Mask1ImageHeader.image.channels = 1;
+	this->Mask1ImageHeader.image.width_step = (size_t)(this->Mask1ImageHeader.image.height * this->Mask1ImageHeader.image.channels);
+	this->Mask1ImageHeader.image.data_size = this->mMaxImageOutputSize;
+
+	this->Mask2ImageHeader.image.width = this->cam_width;
+	this->Mask2ImageHeader.image.height = this->cam_height;
+	this->Mask2ImageHeader.image.channels = 1;
+	this->Mask2ImageHeader.image.width_step = (size_t)(this->Mask2ImageHeader.image.height * this->Mask2ImageHeader.image.channels);
+	this->Mask2ImageHeader.image.data_size = this->mMaxImageOutputSize;
+
+	this->DispOutImageHeader.image.width = this->cam_width;
+	this->DispOutImageHeader.image.height = this->cam_height;
+	this->DispOutImageHeader.image.channels = 1;
+	this->DispOutImageHeader.image.width_step = (size_t)(sizeof(unsigned short)*this->DispOutImageHeader.image.height * this->DispOutImageHeader.image.channels);
+	this->DispOutImageHeader.image.data_size = this->mMaxImageInputSize2;
+
+	// Time measurement
+	road_time_t init_time = 0;
+
+	while (THREAD_ALIVE) 
+	{
+		init_time = road_time();
+
+		//LOG_INFO("Grab new image");
+		// header + image
+		this->shmem_ref->read(this->ref_mem, this->ref_mem_size);
+		this->shmem_dispin->read(this->dispin_mem, this->dispin_mem_size);
+
+		// Header
+		memcpy( &this->RefImageHeader, this->ref_mem, sizeof(TimestampedStructImage)); 
+		memcpy( &this->DispInImageHeader, this->dispin_mem, sizeof(TimestampedStructImage)); 
+	
+		// Check image header
+		bool is_ok = false;
+		if( (this->RefImageHeader.image.data_size == this->mMaxImageInputSize1) && (this->RefImageHeader.time != last_reading) &&
+			(this->DispInImageHeader.image.data_size == this->mMaxImageInputSize2) && (this->DispInImageHeader.time == this->RefImageHeader.time) )
+		{
+			is_ok = true;
+			last_reading = this->RefImageHeader.time;
+			
+			/*std::cout << "Expected image w:  " << ImageHeader.image.width << std::endl;
+			std::cout << "Expected image h:  " << ImageHeader.image.height << std::endl;
+			std::cout << "Expected image c:  " << ImageHeader.image.channels << std::endl;
+			std::cout << "Expected image data:  " << ImageHeader.image.data_size << std::endl;
+			std::cout << "Expected image size:  " << image_mem << std::endl;*/
+		}
+		/*else
+		{
+			LOG_ERROR("Error in the image data size!");
+		}*/
+
+		if(is_ok)
+		{
+			// Image data
+			memcpy( (unsigned char*)(this->CurrentReferenceFrame.data), (unsigned char*)((TimestampedStructImage*)this->ref_mem + 1), this->RefImageHeader.image.data_size); 
+			memcpy( (unsigned short*)(this->CurrentDisparityMap16.data), (unsigned short*)((TimestampedStructImage*)this->dispin_mem + 1), this->DispInImageHeader.image.data_size); 
+		
+			if(this->showdebug)
+			{			
+				cv::namedWindow( "ObstacleDetectionComponent - Current Reference Frame", CV_WINDOW_NORMAL );
+				cv::imshow("ObstacleDetectionComponent - Current Reference Frame",this->CurrentReferenceFrame);
+				cv::waitKey(1);
+			}
+
+			//============================================= Obstacle Detection ================================================
+
+			cv::Mat v_disp_map, u_disp_map;
+
+			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)) :
+				this->CurrentDisparityMap16;
+
+			// U/V disparity maps calculation
+			this->CalcUVDisparityMapNorm(roi_disp, v_disp_map, u_disp_map, Disp_map, this->min_disp_norm, this->max_disp_norm);
+
+			/*
+			// Real disparity information
+			Disp_map16 = this->CurrentDisparityMap16/16;
+					
+			// Display it as a CV_8UC1 image
+			Disp_map16.convertTo( Disp_map, CV_8UC1);//, double(255)/double(this->max_disp - this->min_disp));
+
+			if(this->showdebug)
+			{		
+				cv::namedWindow( "ObstacleDetectionComponent - Current Disparity Map", CV_WINDOW_AUTOSIZE );
+				cv::imshow("ObstacleDetectionComponent - Current Disparity Map", Disp_map);
+				cv::waitKey(1);
+			}
+
+			// U/V disparity maps calculation
+			std::pair<cv::Mat,cv::Mat> par_uv = this->CalcUVDisparityMap(Disp_map);
+			cv::Mat v_disp_map = par_uv.second;
+
+			cv::Mat u_disp_map = par_uv.first;
+			*/
+
+			if(this->showdebug)
+			{			
+				cv::namedWindow( "ObstacleDetectionComponent - Current V Disparity Map", CV_WINDOW_AUTOSIZE );
+				cv::imshow("ObstacleDetectionComponent - Current V Disparity Map", v_disp_map);
+				
+				cv::namedWindow( "ObstacleDetectionComponent - Current U Disparity Map", CV_WINDOW_AUTOSIZE );
+				cv::imshow("ObstacleDetectionComponent - Current U Disparity Map", u_disp_map);
+
+				cv::namedWindow( "ObstacleDetectionComponent - Current Disparity Map Normalized", CV_WINDOW_AUTOSIZE );
+				cv::imshow("ObstacleDetectionComponent - Current Disparity Map Normalized", Disp_map);
+				cv::waitKey(1);
+			}
+
+			// Image to detect near obstacles
+			cv::Mat v_disp_map2 = v_disp_map.clone();
+
+			// Find the driveable surface
+			//cv::Mat color_v_disp_map1 = this->FindSurface(v_disp_map, v_disp_map2);
+			cv::Mat color_v_disp_map1 = this->FindSurface2(v_disp_map, v_disp_map2);
+
+			// Find near obstacles
+			//cv::Mat color_v_disp_map2 = this->FindNearObstacles(v_disp_map2, this->min_disp, this->max_disp);
+			std::pair<cv::Mat, cv::Mat> color_uv_disp_map = this->FindNearObstaclesUV(v_disp_map2, u_disp_map, this->min_disp, this->max_disp);
+			
+			if(this->showdebug)
+			{
+				cv::namedWindow("ObstacleDetectionComponent - Mapa de Disparidade V + Free Space",CV_WINDOW_AUTOSIZE);
+				cv::imshow("ObstacleDetectionComponent - Mapa de Disparidade V + Free Space", color_v_disp_map1);
+
+				cv::namedWindow("ObstacleDetectionComponent - Mapa de Disparidade V + Obstacles",CV_WINDOW_AUTOSIZE);
+				cv::imshow("ObstacleDetectionComponent - Mapa de Disparidade V + Obstacles", color_uv_disp_map.first);
+
+				cv::namedWindow("ObstacleDetectionComponent - Mapa de Disparidade U + Obstacles",CV_WINDOW_AUTOSIZE);
+				cv::imshow("ObstacleDetectionComponent - Mapa de Disparidade U + Obstacles", color_uv_disp_map.second);
+				cv::waitKey(1);
+			}
+
+			// Remap the v-disparity point in the original image and create binary masks
+			//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);
+			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);
+
+			/*if(this->showdebug)
+			{
+				cv::namedWindow("Mapa de Disparidade V + Mask1",CV_WINDOW_AUTOSIZE);
+				cv::imshow("Mapa de Disparidade V + Mask1", masks.first*255);
+
+				cv::namedWindow("Mapa de Disparidade V + Mask2",CV_WINDOW_AUTOSIZE);
+				cv::imshow("Mapa de Disparidade V + Mask2", masks.second*255);
+			}*/
+
+			//---------------------- Remove commun information -------------------------
+				
+			masks.second = masks.second - masks.second.mul( masks.first);
+
+			//---------------------------------------------------------------------------
+
+			//------------------ Write the result in the shared memory ------------------
+			
+			if(this->use_roi)
+			{
+				masks.first.copyTo(this->CurrentSurfaceMask(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height)));
+				masks.second.copyTo(this->CurrentObstaclesMask(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height)));
+			}
+			else
+			{
+				this->CurrentSurfaceMask = masks.first;
+				this->CurrentObstaclesMask = masks.second;
+			}
+
+			//----------------------------- Mask 1 --------------------------------------
+			// Complete timestamp header of the mask image 1
+			this->Mask1ImageHeader.time = this->DispInImageHeader.time;
+			this->Mask1ImageHeader.timerange = this->DispInImageHeader.timerange;
+
+			// Copy images header and data to memory
+			memcpy(this->mask1_mem, &this->Mask1ImageHeader, sizeof(TimestampedStructImage)); 
+			memcpy((void*)((TimestampedStructImage*)this->mask1_mem + 1), (void*)this->CurrentSurfaceMask.data, this->Mask1ImageHeader.image.data_size); 
+			this->shmem_mask1->write(this->mask1_mem, this->mask1_mem_size);
+
+			//----------------------------- Mask 2 --------------------------------------
+			// Complete timestamp header of the mask image 2
+			this->Mask2ImageHeader.time = this->DispInImageHeader.time;
+			this->Mask2ImageHeader.timerange = this->DispInImageHeader.timerange;
+
+			// Copy images header and data to memory
+			memcpy(this->mask2_mem, &this->Mask2ImageHeader, sizeof(TimestampedStructImage)); 
+			memcpy((void*)((TimestampedStructImage*)this->mask2_mem + 1), (void*)this->CurrentObstaclesMask.data, this->Mask2ImageHeader.image.data_size); 
+			this->shmem_mask2->write(this->mask2_mem, this->mask2_mem_size);
+
+			//------------------------- Disparity map out -------------------------------
+			// Complete timestamp header of the disp image out
+			this->DispOutImageHeader.time = this->DispInImageHeader.time;
+			this->DispOutImageHeader.timerange = this->DispInImageHeader.timerange;
+
+			// Copy images header and data to memory
+			memcpy(this->dispout_mem, &this->DispOutImageHeader, sizeof(TimestampedStructImage)); 
+			memcpy((void*)((TimestampedStructImage*)this->dispout_mem + 1), (void*)this->CurrentDisparityMap16.data, this->DispOutImageHeader.image.data_size); 
+			this->shmem_dispout->write(this->dispout_mem, this->dispout_mem_size);
+			//---------------------------------------------------------------------------
+
+			// ----------------- Apply the mask in the reference image ------------------
+			if(this->showdebug)
+			{
+				std::vector<cv::Mat> channels(3);
+				cv::Mat reference;
+				if(this->cam_channels == 1)
+				{
+					cv::Mat img_gray = (this->CurrentReferenceFrame(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).clone();
+					cv::cvtColor(img_gray, reference, CV_GRAY2BGR);
+				}
+				else
+				{
+					reference = (this->CurrentReferenceFrame(cv::Rect(this->destiny_roi_x, this->destiny_roi_y, this->destiny_roi_width, this->destiny_roi_height))).clone();
+				}
+
+				cv::split(reference, channels);
+			
+				masks.second = 1 - masks.second;
+				channels[0] = masks.second.mul(channels[0]); // Activate the red color as obstacles
+				channels[1] = masks.second.mul(channels[1]); // Activate the red color as obstacles
+
+				masks.second = masks.second - masks.first;
+				channels[0] = (1 - masks.second).mul(channels[0]); // Activate the yellow color for unclassified area
+
+				cv::merge(channels, reference);
+
+				cv::namedWindow("ObstacleDetectionComponent - Final Classification",CV_WINDOW_AUTOSIZE);
+				cv::imshow("ObstacleDetectionComponent - Final Classification", reference);
+			}
+			//---------------------------------------------------------------------------
+
+			//==================================================================================================================
+
+			//std::cout << componentName.toStdString() << " cicle time: " << (road_time() - init_time)/1000000.0 << std::endl;
+		}
+		else
+		{
+			msleep(/*MS_DELAY*/10);
+		}
+
+		if(this->showdebug)
+			cv::waitKey(1); // Give the system permission
+
+		//std::cout << componentName.toStdString() << " cicle time: " << (road_time() - init_time)/1000000.0 << std::endl;
+	}
+
+	this->is_running = false;
+
+	// Destroy the window frame
+	if(this->showdebug)
+		cvDestroyAllWindows();
+}
+
+// Function to calculate the U/V disparity map
+std::pair<cv::Mat, cv::Mat> ObstacleDetectionComponent::CalcUVDisparityMap(cv::Mat disp_map)
+{
+	int l, c, pixel;  // local variables for row, line and pixel
+	unsigned char intensity;	  // pixel intensity 
+
+	unsigned char* ptr1; // row pointer for 8 bits image 
+	//unsigned short* ptr2; // row pointer for 16 bits image
+
+    // U disparity map iamge
+	cv::Mat u_disp = cv::Mat::zeros(cv::Size(disp_map.cols, 256), CV_8UC1);
+
+	// V disparity map image
+	cv::Mat v_disp = cv::Mat::zeros(cv::Size(256, disp_map.rows), CV_8UC1);
+    
+    // run accross the image and add 1 to the respective U/V disparity column
+	for (l = 0; l < disp_map.rows; ++l)
+    {
+		ptr1 = disp_map.ptr<unsigned char>(l);
+	
+		for (c = 0; c < disp_map.cols; ++c)
+		{
+			intensity = (unsigned char)ptr1[c];
+			
+			if( (intensity > this->min_disp)&&(intensity < this->max_disp))
+			{				
+				pixel =  intensity*u_disp.cols + c;
+				u_disp.data[pixel] = (unsigned char)(u_disp.data[pixel] + 1);
+
+				pixel =  l*v_disp.cols + intensity;
+				v_disp.data[pixel] = (unsigned char)(v_disp.data[pixel] + 1);				
+			}
+        }   
+	}
+
+	return std::make_pair(u_disp, v_disp);
+}
+
+/*	CalcUVDisparityMapNorm
+	Description:
+		Function to calculate the U/V disparity map from a disp map normalized
+	Parameters:
+		disp_map16 = original disparity map 16
+		disp_map_norm = resulted disparity map normalized
+		min_d_norm = Minimum disparity value to equalize the disp map 16
+		max_d_norm = Maximum disparity value to equalize the disp map 16
+*/
+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)
+{
+	int l, c, pixel;  // local variables for row, line and pixel
+	unsigned char intensity;	// pixel intensity 
+	int intensity_norm;			// pixel intensity 
+
+	unsigned char* ptr1; // row pointer for 8 bits image 
+	unsigned short* ptr2; // row pointer for 16 bits image
+
+	// Disparity map image normalized
+	disp_map_norm = cv::Mat::zeros(cv::Size(disp_map16.cols, disp_map16.rows), CV_8UC1);
+
+    // U disparity map image
+	u_disp_map = cv::Mat::zeros(cv::Size(disp_map16.cols, 256), CV_8UC1);
+
+	// V disparity map image
+	v_disp_map = cv::Mat::zeros(cv::Size(256, disp_map16.rows), CV_8UC1);
+    
+    // percorre a imagem original e soma 1 na coluna do mapa de disparidade V com a mesma
+	// intensidade do pixel
+	for (l = 0; l < disp_map16.rows; ++l)
+    {
+		ptr1 = disp_map_norm.ptr<unsigned char>(l);
+		ptr2 = disp_map16.ptr<unsigned short>(l);
+
+		for (c = 0; c < disp_map16.cols; ++c)
+		{
+			intensity = (unsigned char)(ptr2[c]/16);
+			intensity_norm = (int)((float)((ptr2[c]/16.0f - (float)min_d_norm)*(255.0f)/((float)max_d_norm - (float)min_d_norm)) + 0.5f);
+						
+			if( (intensity > this->min_disp)&&(intensity < this->max_disp)&&(intensity_norm > 0)&&(intensity_norm < 255))
+			{
+				pixel =  intensity_norm*u_disp_map.cols + c;
+				u_disp_map.data[pixel] = (unsigned char)(u_disp_map.data[pixel] + 1);
+
+				pixel =  l*v_disp_map.cols + intensity_norm;
+				v_disp_map.data[pixel] = (unsigned char)(v_disp_map.data[pixel] + 1);
+
+				ptr1[c] = (unsigned char)intensity_norm;
+			}
+        }   
+	}
+
+	return;
+}
+
+// Function to find the free space surface from a V-disparity map
+cv::Mat ObstacleDetectionComponent::FindSurface(cv::Mat &v_disp_map, cv::Mat &v_disp_map2)
+{
+	// Parameters of canny and hough transform
+	int tshold1 = 154;
+	int tshold2 = 48;
+	int n_points = 48; //59
+	int minLineLenght = 35; //40
+	int maxLineGap = 12;
+		
+		
+	// Binary image
+	cv::Mat Img_bin = v_disp_map.clone();//cvCloneImage(v_disp_map);
+
+	// Color V disparity map with red lines
+	cv::Mat color_img = cv::Mat( cv::Size(v_disp_map.cols, v_disp_map.rows), CV_8UC3 );
+
+	// Convert to color image
+	cv::cvtColor(v_disp_map, color_img, CV_GRAY2BGR);
+
+	cv::equalizeHist( Img_bin, Img_bin);
+
+	if(this->showdebug)
+	{
+		// Janela de exibicao
+		cv::namedWindow("ObstacleDetectionComponent - Equalized Image",CV_WINDOW_AUTOSIZE);
+		cv::imshow("ObstacleDetectionComponent - Equalized Image", Img_bin);
+	}
+
+	cv::Canny(Img_bin, Img_bin, tshold1, tshold2, 3);
+	
+	// Closing
+	//cv::dilate(Img_bin, Img_bin, cv::Mat(), cv::Point(-1,-1), 2 );
+	//cv::erode(Img_bin, Img_bin, cv::Mat(), cv::Point(-1,-1), 1 );
+
+	if(this->showdebug)
+	{
+		// Janela de exibicao
+		cv::namedWindow("ObstacleDetectionComponent - Binary Image",CV_WINDOW_AUTOSIZE);
+		cv::imshow("ObstacleDetectionComponent - Binary Image", Img_bin);
+	}
+		
+    std::vector<cv::Vec4i> lines;			//vector for storing the lines found by HoughLine
+
+	// Probabilistic Hough Transform
+	cv::HoughLinesP( Img_bin, lines, 1, CV_PI/180, n_points, minLineLenght, maxLineGap ); 
+
+	//=============================== Use the lines filter to remove invalid segments ============================ 
+
+	std::vector<cv::Point> nova_lista = this->LinesFiltering(lines);
+	
+	if(!nova_lista.empty())
+	{
+		cv::Point pt_ant = *(nova_lista.begin());
+
+		// Filter the mean angle
+		for(std::vector<cv::Point>::iterator it = nova_lista.begin(); it != nova_lista.end(); ++it)
+		{		
+			cv::line( color_img, pt_ant, *it, CV_RGB(255,0,0), 8, 8 );
+
+			pt_ant = *it;
+		}
+	}
+
+	//============================================================================================================
+
+	//======================== Remove invalid line segments by slope angle only ==================================
+	//if (lines.size() != 0)
+	//{
+	//	cv::Point pt1, pt2;
+	//	double theta;
+
+	//	for(int i = 0; i < (int)lines.size();++i)
+	//	{
+	//		pt1.x = lines[i][0];//(CvPoint*)cvGetSeqElem(lines,i);
+	//		pt1.y = lines[i][1];
+	//		pt2.x = lines[i][2];
+	//		pt2.y = lines[i][3];
+
+	//		CheckPoints(pt1, pt2); //Verifica a ordem dos pontos
+	//	
+	//		theta = Inclination(pt1, pt2); //calcula a inclinacao da reta encontrada
+
+	//		// Valor atual do angulo em graus
+	//		theta = ((theta*360.0)/(2.0*CV_PI));
+
+	//		// Verifica se a reta possui inclinacao para ser possivel plano
+	//		if(theta> (90.0 + ANG_VARIATION))
+	//		{
+
+	//			//Desenha as retas em vermelho
+	//			cv::line( color_img, pt1, pt2, CV_RGB(255,0,0), 4, 8 );
+	//		}
+	//	}		
+	//}
+	//==========================================================================================================
+
+	std::vector<cv::Mat> channels(3);
+
+	// Get the V-disparity without detected red lines
+	cv::split(color_img, channels);
+	v_disp_map2 = channels[0];
+	
+	// Janela de exibicao
+	//cv::namedWindow("Mapa de Disparidade V + Hough",CV_WINDOW_AUTOSIZE);
+	//cv::imshow("Mapa de Disparidade V + Hough", color_img);
+	//cv::imshow("Mapa de Disparidade V + Hough", v_disp_map2);
+
+	return color_img;
+}
+
+/*	FindSurface2
+	Description:
+		Function to find the free space surface from a V-disparity map, based on the frontal plane. 
+		Return the V-dysparity map with the red line representing the free surface.
+	Parameters:
+		v_disp_map = Original V-disparity map
+		v_disp_map2 = Orignal V-disparity map less the surface detected
+*/
+cv::Mat ObstacleDetectionComponent::FindSurface2(cv::Mat &v_disp_map, cv::Mat &v_disp_map2)
+{	
+	// Parameters
+	int tshold = 40;	// Min threshold for the first max value
+	int tshold2 = 35;	// Min threshold for the step variation
+	int maxLineLenght = 25;
+	int min_disp_value = 20;
+
+	// Binary image
+	cv::Mat Img_bin = v_disp_map.clone();
+	cv::Mat Img_mask = v_disp_map.clone();
+	
+	// Color V disparity map with red lines
+	cv::Mat color_img = cv::Mat( cv::Size(v_disp_map.cols, v_disp_map.rows), CV_8UC3 );
+
+	// Convert to color image
+	cv::cvtColor(v_disp_map, color_img, CV_GRAY2BGR);
+
+
+	//================= Segment the most probable road surface region ===================================
+	/*cv::threshold(Img_mask, Img_mask, tshold, 1, CV_THRESH_BINARY);
+	cv::dilate(Img_mask, Img_mask, cv::Mat(), cv::Point(-1,-1), 2 );
+	cv::erode(Img_mask, Img_mask, cv::Mat(), cv::Point(-1,-1), 1 );
+
+	if(this->showdebug)
+	{
+		cv::imshow( "ObstacleDetectionComponent - Img_mask", Img_mask*255 );
+	}
+	
+	Img_bin = Img_bin.mul(Img_mask);*/
+	//===================================================================================================
+
+	// Generate grad_x and grad_y
+	cv::Mat grad, grad_H, grad_S, grad_x, grad_y;
+	cv::Mat abs_grad_x, abs_grad_y;
+		
+	// Gradient X
+	//Scharr( src_gray, grad_x, ddepth, 1, 0, scale, delta, BORDER_DEFAULT );
+	//cv::Sobel( Img_bin, grad_x, CV_16S, 1, 0, 3, 1, 0, cv::BORDER_DEFAULT );
+	//cv::convertScaleAbs( grad_x, abs_grad_x );
+	
+	// Gradient Y
+	//Scharr( src_gray, grad_y, ddepth, 0, 1, scale, delta, BORDER_DEFAULT );
+	cv::Sobel( Img_bin, grad_y, CV_16S, 0, 1, 3, 1, 0, cv::BORDER_DEFAULT );
+	cv::convertScaleAbs( grad_y, abs_grad_y );
+	
+	// Total Gradient (approximate)
+	//cv::addWeighted( abs_grad_x, 0.2, abs_grad_y, 0.8, 0, grad );
+
+	//abs_grad_y = abs_grad_y*255.0;
+	abs_grad_y.convertTo(Img_mask, CV_8U);
+	
+	cv::threshold(Img_mask, Img_mask, tshold2, 1, CV_THRESH_BINARY);
+	cv::dilate(Img_mask, Img_mask, cv::Mat(), cv::Point(-1,-1), 2 );
+	cv::erode(Img_mask, Img_mask, cv::Mat(), cv::Point(-1,-1), 1 );
+
+	Img_bin = Img_bin.mul(Img_mask);
+
+	//cv::equalizeHist( abs_grad_y, Img_bin);
+
+	if(this->showdebug)
+	{
+		//cv::imshow( "ObstacleDetectionComponent - Sobel X", abs_grad_x );
+		cv::imshow( "ObstacleDetectionComponent - Sobel Y", abs_grad_y );
+		cv::imshow( "ObstacleDetectionComponent - Img_mask", Img_mask*255 );
+		//cv::imshow( "ObstacleDetectionComponent - Sobel", grad );
+		//cv::imshow( "ObstacleDetectionComponent - Equalized Image", Img_bin);
+	}
+
+	//============================== Mark the most significative points as free space ==============================
+	
+	int row_step_count, last_row_step_count;		// Keep the sum of the last valid pixels in the step
+	int previous_col = 0;
+
+	int left_limit, right_limit; // Auxiliary variables
+
+	for(int row = Img_bin.rows - 1; row > 0; --row)
+	{
+		row_step_count = 0;
+		last_row_step_count = 0;
+		left_limit = -1;
+		right_limit = -1;		
+
+		unsigned char* ptr1 = Img_bin.ptr<unsigned char>(row);
+
+		if(previous_col == 0)
+		{			
+			for(int col = 0; col < Img_bin.cols; ++col)
+			{
+				// Find max
+				if(ptr1[previous_col] < ptr1[col])
+				{
+					previous_col = col;
+
+					tshold2 = (int)(0.95*ptr1[previous_col]); // adjust the tshold
+				}
+			}
+			
+			if(ptr1[previous_col] < tshold)
+			{
+				previous_col = 0; // position
+			}
+		}
+				
+		if(previous_col != 0)
+		{
+			int left = (previous_col - maxLineLenght >= 0) ? previous_col-maxLineLenght : 0;
+			int right = (previous_col + maxLineLenght < Img_bin.cols) ? previous_col + maxLineLenght : Img_bin.cols;
+
+			int limits_found = 0;
+			int l_col = previous_col;
+			int r_col = previous_col;
+
+			int left_count = 0;
+			int right_count = 0;
+
+			while(!limits_found)
+			{
+				//------------------------------- To the left ----------------------------------
+				if(left_limit == -1)
+				{ 
+					if(l_col > left)
+						--l_col;
+					else
+						left_limit = l_col + left_count;
+				}				
+
+				// Find max
+				if(ptr1[previous_col] < ptr1[l_col])
+				{
+					previous_col = l_col;
+					tshold2 = (int)(0.95*ptr1[previous_col]); // adjust the tshold
+				}
+
+				// Check if the pixel intensity is lower than tshold
+				if(left_count < 5)
+				{
+					if(ptr1[l_col] < tshold2)
+					{
+						++left_count;
+					}
+					else
+					{
+						left_count = 0;
+					}
+				}
+				else
+				{
+					if(left_limit == -1)
+						left_limit = l_col + left_count;
+				}
+				//------------------------------------------------------------------------------
+
+				//------------------------------- To the right ----------------------------------
+				if(right_limit == -1)
+				{ 
+					if(r_col < right)
+						++r_col;
+					else
+						right_limit = r_col - right_count;
+				}				
+
+				// Find max
+				if(ptr1[previous_col] < ptr1[r_col])
+				{
+					previous_col = r_col;
+					tshold2 = (int)(0.95*ptr1[previous_col]); // adjust the tshold
+				}
+
+				// Check if the pixel intensity is lower than tshold
+				if(right_count < 5)
+				{
+					if(ptr1[r_col] < tshold2)
+					{
+						++right_count;
+					}
+					else
+					{
+						right_count = 0;
+					}
+				}
+				else
+				{
+					if(right_limit == -1)
+						right_limit = r_col - right_count;
+				}
+				//------------------------------------------------------------------------------
+
+				if((left_limit != -1)&&(right_limit != -1))
+				{
+					limits_found = 1;
+				}
+			}
+
+			if((left_limit != -1)&&(right_limit != -1)&&(ptr1[previous_col] > tshold2))
+			{
+				cv::line( color_img, cv::Point(left_limit, row), cv::Point(right_limit, row), CV_RGB(255,0,0), 1, 8 );
+			}
+		}
+	}
+
+	//==============================================================================================================
+			
+	std::vector<cv::Mat> channels(3);
+
+	// Get the V-disparity without detected red lines
+	cv::split(color_img, channels);
+	v_disp_map2 = channels[0];
+	
+	// Janela de exibicao
+	//cv::namedWindow("Mapa de Disparidade V + Hough",CV_WINDOW_AUTOSIZE);
+	//cv::imshow("Mapa de Disparidade V + Hough", color_img);
+	//cv::imshow("Mapa de Disparidade V + Hough", v_disp_map2);
+
+	return color_img;
+}
+
+// Function to find the free space surface from a V-disparity map with mean average
+cv::Mat ObstacleDetectionComponent::FindAverageSurface(cv::Mat &v_disp_map, cv::Mat &v_disp_map2)
+{
+	// Parameters of threshold and hough transform
+	int tshold1 = 3;
+	int n_points = 48; //59
+	int minLineLenght = 35; //40
+	int maxLineGap = 12;
+
+	// Imagem atual em 32F
+	cv::Mat v_disp_map_current32F = cv::Mat(cv::Size(v_disp_map.cols, v_disp_map.rows), CV_32F, 1);
+
+	// Converte tipo de imagem
+	v_disp_map.convertTo(v_disp_map_current32F, CV_32F);//cvConvertScale(v_disp_map, v_disp_map_current32F);
+
+	//Mean disparity map for noise attenuation
+	static cv::Mat v_disp_map_mean = v_disp_map_current32F.clone();
+
+	// Imagem binária da diferenca
+	cv::Mat ImgBinaria = v_disp_map.clone();//cvCloneImage(v_disp_map);
+
+	// Color V disparity map with red lines
+	cv::Mat color_img = cv::Mat( cv::Size(v_disp_map.cols, v_disp_map.rows), CV_8UC3 );
+
+	// Convert to color image
+	cv::cvtColor(v_disp_map, color_img, CV_GRAY2BGR);
+
+	// Running Average
+	cv::accumulateWeighted(v_disp_map_current32F, v_disp_map_mean, 0.20);//cvRunningAvg(v_disp_map_current32F, v_disp_map_mean, 0.20);
+
+	// Convert scale 
+	v_disp_map_mean.convertTo( ImgBinaria, CV_8U); //cvConvertScale(v_disp_map_mean, ImgBinaria, 1.0, 0.0);
+
+	// Janela de exibicao
+	//cv::namedWindow("Imagem Media Movel",CV_WINDOW_AUTOSIZE);
+	//cv::imshow("Imagem Media Movel", ImgBinaria);
+	
+	cv::threshold(ImgBinaria, ImgBinaria, tshold1, 255,CV_THRESH_BINARY);
+
+	// Janela de exibicao
+	//cv::namedWindow("Mapa de Disparidade V + binarizacao",CV_WINDOW_AUTOSIZE);
+	//cv::imshow("Mapa de Disparidade V + binarizacao", ImgBinaria);
+
+	// create 3x3 matrix
+	//static cv::Mat kernel_3x3 = (cv::Mat_<unsigned char>(3,3) << 1, 0, 0, 1, 1, 0, 1, 1, 1);
+	
+	// Fechamento
+	//cv::erode(ImgBinaria,ImgBinaria,NULL, cv::Point(-1,-1), 1);
+	//cv::erode(ImgBinaria,ImgBinaria,kernel_3x3, cv::Point(1,1), 1);
+
+	// Janela de exibicao
+	//cv::namedWindow("Mapa de Disparidade V + binarizacao + erode",CV_WINDOW_AUTOSIZE);
+	//cv::imshow("Mapa de Disparidade V + binarizacao + erode", ImgBinaria);
+
+	// Fechamento
+	//cv::dilate(ImgBinaria,ImgBinaria,NULL, cv::Point(-1,-1), 1);
+	//cv::erode(ImgBinaria,ImgBinaria,NULL, cv::Point(-1,-1), 2);
+
+    std::vector<cv::Vec4i> lines;			//vector for storing the lines found by HoughLine
+
+	// Probabilistic Hough Transform
+	cv::HoughLinesP( ImgBinaria, lines, 1, CV_PI/180, n_points, minLineLenght, maxLineGap ); 
+
+	//=============================== Use the lines filter to remove invalid segments ============================ 
+
+	//std::vector<cv::Point> nova_lista = this->LinesFiltering(lines);
+	//
+	//if(!nova_lista.empty())
+	//{
+	//	cv::Point pt_ant = *(nova_lista.begin());
+
+	//	// Filter the mean angle
+	//	for(std::vector<cv::Point>::iterator it = nova_lista.begin(); it != nova_lista.end(); ++it)
+	//	{		
+	//		cv::line( color_img, pt_ant, *it, CV_RGB(255,0,0), 3, 8 );
+
+	//		pt_ant = *it;
+	//	}
+	//}
+
+	//============================================================================================================
+
+	//======================== Remove invalid line segments by slope angle only ==================================
+	if (lines.size() != 0)
+	{
+		cv::Point pt1, pt2;
+		double theta;
+
+		for(int i = 0; i < (int)lines.size();++i)
+		{
+			pt1.x = lines[i][0];//(CvPoint*)cvGetSeqElem(lines,i);
+			pt1.y = lines[i][1];
+			pt2.x = lines[i][2];
+			pt2.y = lines[i][3];
+
+			CheckPoints(pt1, pt2); //Verifica a ordem dos pontos
+		
+			theta = Inclination(pt1, pt2); //calcula a inclinacao da reta encontrada
+
+			// Valor atual do angulo em graus
+			theta = ((theta*360.0)/(2.0*CV_PI));
+
+			// Verifica se a reta possui inclinacao para ser possivel plano
+			if(theta> (90.0 + ANG_VARIATION))
+			{
+
+				//Desenha as retas em vermelho
+				cv::line( color_img, pt1, pt2, CV_RGB(255,0,0), 6, 8 );
+			}
+		}		
+	}
+	//==========================================================================================================
+
+	std::vector<cv::Mat> channels(3);
+
+	// Get the V-disparity without detected red lines
+	cv::split(color_img, channels);
+	v_disp_map2 = channels[0];
+	
+	// Janela de exibicao
+	//cv::namedWindow("Mapa de Disparidade V + Hough",CV_WINDOW_AUTOSIZE);
+	//cv::imshow("Mapa de Disparidade V + Hough", color_img);
+	//cv::imshow("Mapa de Disparidade V + Hough", v_disp_map2);
+
+	return color_img;
+}
+
+// Function to find the near obstacles from a v-Disparity map
+cv::Mat ObstacleDetectionComponent::FindNearObstacles(cv::Mat v_disp_map, int min_d, int max_d)
+{
+	// Parameters of threshold and hough transform
+	int tshold1 = 3;
+	int n_points = 15;
+	int minLineLenght = 10;
+	int maxLineGap = 12;
+
+	// Image to be processed
+	cv::Mat v_disp_map_aux = v_disp_map.clone();
+
+	// Image with the obstacles highlighted in red
+	cv::Mat color_img = cv::Mat( cv::Size(v_disp_map.cols, v_disp_map.rows), CV_8UC3);
+
+	// Convert color space
+	cv::cvtColor(v_disp_map_aux, color_img, CV_GRAY2BGR);
+
+	// Image binarization
+	cv::threshold(v_disp_map_aux, v_disp_map_aux, tshold1, 255, CV_THRESH_BINARY);
+
+	// Janela de exibicao
+	//cv::namedWindow("Mapa de Disparidade V + Threshold 2",CV_WINDOW_AUTOSIZE);
+	//cv::imshow("Mapa de Disparidade V + Threshold 2", v_disp_map_aux);
+
+	// Define valid ROI
+	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));
+	cv::Mat color_img_ROI = color_img( cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
+
+	std::vector<cv::Vec4i> lines;			//vector for storing the lines found by HoughLine
+
+	// Probabilistic hough transform
+	cv::HoughLinesP( v_disp_map_aux_ROI, lines, 1, CV_PI/180, n_points, minLineLenght, maxLineGap ); 
+			
+	if (lines.size() != 0)
+	{
+		cv::Point pt1, pt2;
+		double theta;
+
+		for(int i = 0; i < (int)lines.size(); ++i)
+		{
+			pt1.x = lines[i][0];//(CvPoint*)cvGetSeqElem(lines,i);
+			pt1.y = lines[i][1];
+			pt2.x = lines[i][2];
+			pt2.y = lines[i][3];
+
+			this->CheckPoints(pt1, pt2); //Verify the points order
+		
+			theta = this->Inclination(pt1, pt2); // line slope
+
+			// In degrees
+			theta = ((theta*360.0)/(2.0*CV_PI));
+
+			// Verifica se a reta possui inclinacao para ser possivel plano
+			if((theta < (90.0 + ANG_VARIATION2))&& (theta > (90.0 - ANG_VARIATION2)))
+			{								
+				//Desenha as retas em vermelho
+				cv::line( color_img_ROI, pt1, pt2, CV_RGB(255,0,0), 5, 8 );
+			}
+		}		
+	}
+		
+	// Janela de exibicao
+	//cv::namedWindow("Mapa de Disparidade V + Hough1",CV_WINDOW_AUTOSIZE);
+	//cv::imshow("Mapa de Disparidade V + Hough1", v_disp_map_aux);
+
+	// Janela de exibicao
+	//cv::namedWindow("Mapa de Disparidade V + Hough2",CV_WINDOW_AUTOSIZE);
+	//cv::imshow("Mapa de Disparidade V + Hough2", color_img);
+	
+	return color_img;
+}
+
+// Function to find the near obstacles from the v/u-Disparity maps
+std::pair<cv::Mat, cv::Mat> ObstacleDetectionComponent::FindNearObstaclesUV(cv::Mat v_disp_map, cv::Mat u_disp_map, int min_d, int max_d)
+{
+	// Parameters of threshold and hough transform
+	int tshold1 = 3;
+	int tshold2 = 15;
+	int n_points = 15;
+	int minLineLenght = 10;
+	int maxLineGap = 12;
+
+	std::vector<cv::Mat> channels(3);
+
+	//=================================== V-disparity map process =============================================================
+	//// Image to be processed
+	//cv::Mat v_disp_map_aux = v_disp_map.clone();
+	//	
+	//// Image with the obstacles highlighted in red
+	//cv::Mat v_color_img = cv::Mat( cv::Size(v_disp_map.cols, v_disp_map.rows), CV_8UC3);
+
+	//// Convert color space
+	//cv::cvtColor(v_disp_map_aux, v_color_img, CV_GRAY2BGR);
+
+	//cv::equalizeHist( v_disp_map_aux, v_disp_map_aux);
+
+	//// Image binarization
+	//cv::threshold(v_disp_map_aux, v_disp_map_aux, tshold1, 255, CV_THRESH_BINARY);
+
+	//// Janela de exibicao
+	//if(this->showdebug)
+	//{
+	//	cv::namedWindow("Mapa de Disparidade V + Threshold 2",CV_WINDOW_AUTOSIZE);
+	//	cv::imshow("Mapa de Disparidade V + Threshold 2", v_disp_map_aux);
+	//}
+
+	//// Define valid ROI
+	//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));
+	//cv::Mat v_color_img_ROI = v_color_img( cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
+
+	//std::vector<cv::Vec4i> lines;			//vector for storing the lines found by HoughLine
+
+	//// Probabilistic hough transform
+	//cv::HoughLinesP( v_disp_map_aux_ROI, lines, 1, CV_PI/180, n_points, minLineLenght, maxLineGap ); 
+	//		
+	//if (lines.size() != 0)
+	//{
+	//	cv::Point pt1, pt2;
+	//	double theta;
+
+	//	for(int i = 0; i < (int)lines.size(); ++i)
+	//	{
+	//		pt1.x = lines[i][0];//(CvPoint*)cvGetSeqElem(lines,i);
+	//		pt1.y = lines[i][1];
+	//		pt2.x = lines[i][2];
+	//		pt2.y = lines[i][3];
+
+	//		this->CheckPoints(pt1, pt2); //Verify the points order
+	//	
+	//		theta = this->Inclination(pt1, pt2); // line slope
+
+	//		// In degrees
+	//		theta = ((theta*360.0)/(2.0*CV_PI));
+
+	//		// Verifica se a reta possui inclinacao para ser possivel plano
+	//		//if((theta < (90.0 + ANG_VARIATION2))&& (theta > (90.0 - ANG_VARIATION2)))
+	//		//{								
+	//			//Desenha as retas em vermelho
+	//			cv::line( v_color_img_ROI, pt1, pt2, CV_RGB(255,0,0), 5, 8 );
+	//		//}
+	//	}		
+	//}
+	//	
+	//// Janela de exibicao
+	////cv::namedWindow("Mapa de Disparidade V + Hough1",CV_WINDOW_AUTOSIZE);
+	////cv::imshow("Mapa de Disparidade V + Hough1", v_disp_map_aux);
+
+	//// Janela de exibicao
+	////cv::namedWindow("Mapa de Disparidade V + Hough2",CV_WINDOW_AUTOSIZE);
+	////cv::imshow("Mapa de Disparidade V + Hough2", color_img);
+
+	//================================================================================================================================
+
+	//============================================== V-disparity map process =========================================================
+	// Image to be processed
+	cv::Mat v_disp_map_aux = v_disp_map.clone();
+		
+	// Image with the obstacles highlighted in red
+	cv::Mat v_color_img = cv::Mat( v_disp_map.rows, v_disp_map.cols, CV_8UC3, cv::Scalar(0,0,0));
+	
+	// Convert color space
+	//cv::cvtColor(v_disp_map_aux, v_color_img, CV_GRAY2BGR);
+
+	//cv::equalizeHist( v_disp_map_aux, v_disp_map_aux);
+
+	// Image binarization
+	cv::threshold(v_disp_map_aux, v_disp_map_aux, tshold1, 255, CV_THRESH_BINARY);
+
+	// Janela de exibicao
+	if(this->showdebug)
+	{
+		cv::namedWindow("Mapa de Disparidade V + Threshold 2",CV_WINDOW_AUTOSIZE);
+		cv::imshow("Mapa de Disparidade V + Threshold 2", v_disp_map_aux);
+	}
+
+	// Define valid ROI
+	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));
+	cv::Mat v_color_img_ROI = v_color_img( cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
+
+	cv::split( v_color_img, channels);
+	channels[2] = v_disp_map_aux; 
+	cv::merge( channels, v_color_img);
+
+	//================================================================================================================================
+
+	//=============================================== U-disparity map process ========================================================
+	// Image to be processed
+	cv::Mat u_disp_map_aux = u_disp_map.clone();
+	
+	// Image with the obstacles highlighted in red
+	cv::Mat u_color_img = cv::Mat( cv::Size(u_disp_map.cols, u_disp_map.rows), CV_8UC3, cv::Scalar(0, 0, 0));
+
+	// Convert color space
+	//cv::cvtColor(u_disp_map_aux, u_color_img, CV_GRAY2BGR);
+
+	// Image binarization
+	cv::threshold(u_disp_map_aux, u_disp_map_aux, tshold2, 255, CV_THRESH_BINARY);
+		
+	// Closing
+	cv::dilate(u_disp_map_aux, u_disp_map_aux, cv::Mat(), cv::Point(-1,-1), 1 );
+	cv::erode(u_disp_map_aux, u_disp_map_aux, cv::Mat(), cv::Point(-1,-1), 1 );
+	
+	
+	if(this->showdebug)
+	{
+		// Janela de exibicao
+		cv::namedWindow("ObstacleDetectionComponent - Image bin u-disp",CV_WINDOW_AUTOSIZE);
+		cv::imshow("ObstacleDetectionComponent - Image bin u-disp", u_disp_map_aux);
+	}
+	
+	cv::split( u_color_img, channels);
+	channels[2] = u_disp_map_aux; 
+	cv::merge( channels, u_color_img);
+
+	// Define valid ROI
+	//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));
+	//cv::Mat u_color_img_ROI = v_color_img( cv::Rect(min_d, 0, max_d-min_d+1, v_disp_map_aux.rows));
+	//================================================================================================================================
+
+	return std::make_pair(v_color_img, u_color_img);
+}
+
+/*	LinesFiltering
+	Description:
+		Filter the detected lines related to the distance and angle between them.
+	Parameters:
+		lines = line list (point 1 and 2)
+*/
+std::vector<cv::Point> ObstacleDetectionComponent::LinesFiltering(std::vector<cv::Vec4i> lines)
+{
+	std::vector<cv::Point> lista_pontos;
+
+	std::vector<cv::Point> lista_retorno;
+
+	std::vector<double> angles;
+
+	cv::Point pt1, pt2;
+	double theta;
+	double angulo_medio = 0.0;
+
+	if (lines.size() != 0)
+	{
+		//std::cout << "Found "<< lines.size() << " lines!\n";
+
+		// Filtro de angulo maximo
+		for(int i = 0; i < (int)lines.size(); ++i)
+		{
+			pt1.x = lines[i][0];//(CvPoint*)cvGetSeqElem(lines,i);
+			pt1.y = lines[i][1];
+			pt2.x = lines[i][2];
+			pt2.y = lines[i][3];
+
+			CheckPoints(pt1, pt2); //Verifica a ordem dos pontos
+		
+			theta = Inclination(pt1, pt2); //calcula a inclinacao da reta encontrada
+
+			// Valor atual do angulo em graus
+			theta = ((theta*360.0)/(2.0*CV_PI));
+
+			//std::cout << "Angle "<< theta << "!\n";
+
+			// Verifica se a reta possui inclinacao para ser possivel plano
+			if(theta> (90.0 + ANG_VARIATION))
+			{
+				lista_pontos.push_back(pt1);
+				lista_pontos.push_back(pt2);
+				
+				angles.push_back(theta);
+			}
+		}
+
+		if(!lista_pontos.empty())
+		{
+			std::sort(lista_pontos.begin(), lista_pontos.end(), ComparePoints1);
+			angulo_medio = this->CalcMedian(angles);
+		}
+
+		CvPoint last_item;
+
+		// Percorre os pontos ordenados e procura a casca convexa mais a esquerda
+		for(std::vector<cv::Point>::iterator it = lista_pontos.begin(); it != lista_pontos.end(); ++it)
+		{
+			if(lista_retorno.empty())
+			{
+				lista_retorno.push_back(*it);
+
+				last_item = *it;
+			}
+			else
+			{
+				if( (it + 1) != lista_pontos.end())
+				{
+					// Se tiverem o mesmo y, substitui o ponto
+					if(last_item.y == (*it).y)
+					{
+						// Troca o ultimo elemento
+						lista_retorno.pop_back();
+												
+						lista_retorno.push_back(*it);
+
+						last_item = *it;
+					}
+					else
+					{
+						theta = Inclination(*it, last_item); //calcula a inclinacao da reta encontrada
+
+						// Valor atual do angulo em graus
+						theta = ((theta*360.0)/(2.0*CV_PI));
+
+						lista_retorno.push_back(*it);
+
+						// Percorre os pontos ordenados e procura o maior angulo entre a casca convexa mais a esquerda
+						for(std::vector<cv::Point>::iterator it2 = it + 1; it2 != lista_pontos.end(); ++it2)
+						{
+							// Verifica se o angulo atual e menor que o anterior
+							if(theta < ((Inclination(*it2, last_item)*360.0)/(2.0*CV_PI)))
+							{
+								// Troca o ultimo elemento
+								lista_retorno.pop_back();
+												
+								lista_retorno.push_back(*it2);
+
+								theta = Inclination(*it2, last_item); //calcula a inclinacao da reta encontrada
+
+								// Valor atual do angulo em graus
+								theta = ((theta*360.0)/(2.0*CV_PI));
+
+								it = it2;
+							}
+						} // for
+
+						last_item = *it;
+					} // else
+				} // if( (it + 1) != lista_pontos.end())
+			} // else
+		} // for
+
+		if(!lista_retorno.empty())
+		{
+			last_item = *(lista_retorno.begin());
+
+			// Filtro de angulo medio
+			for(std::vector<cv::Point>::iterator it = lista_retorno.begin() + 1; it != lista_retorno.end(); ++it)
+			{		
+				theta = Inclination(*it, last_item); //calcula a inclinacao da reta encontrada
+
+				// Valor atual do angulo em graus
+				theta = ((theta*360.0)/(2.0*CV_PI));
+
+				// Verifica se a reta possui inclinacao fora da media
+				if(theta < (0.95*angulo_medio))//((theta > (1.05*angulo_medio))&&(theta < (0.95*angulo_medio)))
+				{
+					lista_retorno.resize(it - lista_retorno.begin());
+
+					break;
+				}
+
+				last_item = *it;
+			}
+		}
+
+	}
+
+	return lista_retorno;
+}
+
+//Function to check the points order, making the second one with the highest y ever
+void ObstacleDetectionComponent::CheckPoints(cv::Point &pt1, cv::Point &pt2)
+{
+	int aux_x, aux_y;
+	
+	if(pt1.y > pt2.y)
+	{
+		aux_x = pt1.x;
+		aux_y = pt1.y;
+
+		pt1.x = pt2.x;
+		pt1.y = pt2.y;
+
+		pt2.x = aux_x; 
+		pt2.y = aux_y;
+	}
+
+	return;
+}
+
+// Function to calculate the line slope of pt1 to pt2
+double ObstacleDetectionComponent::Inclination(cv::Point pt1, cv::Point pt2)
+{
+	double theta; //angle
+		
+	theta = fabs((atan2((pt1.y-pt2.y+0.0),(pt1.x-pt2.x+0.0)))); // slope
+
+	return theta;
+}
+
+/*	CalcMedian
+	Description:
+		Calcule the median value of a vector.
+	Parametros:
+		vector = vector with data to calculate the median
+*/
+template<class A>
+A ObstacleDetectionComponent::CalcMedian(std::vector<A> vetor) const
+{
+	A mediana;
+
+	std::sort(vetor.begin(), vetor.end());
+
+	mediana = vetor[(int)((double)(vetor.size())/2.0 + 0.5) - 1];
+
+	return mediana;
+}
+
+// Function to calculate the free space (v_disp_1) and obstacles (v_disp_2) masks from 
+// a red highlighted V-Disparity map
+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)
+{
+	// Free space mask
+	cv::Mat mask1;
+
+	// Obstacle Mask
+	cv::Mat mask2;
+	
+	// Fill the destiny images with background value
+	if(value == 1)
+	{
+		mask1 = cv::Mat::zeros( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
+		mask2 = cv::Mat::zeros( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
+	}
+	else
+	{
+		mask1 = cv::Mat::ones( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
+		mask2 = cv::Mat::ones( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
+	}
+
+	int l, c, pixel, v_disp_pixel;	// local variables for row, column and pixel
+	unsigned char intensity;		// pixel intensity
+	unsigned char intensity_B, intensity_G, intensity_R;	//pixel intensity
+
+	/*uint8_t* pixelPtr_mask1 = (uint8_t*)mask1.data;
+	uint8_t* pixelPtr_mask2 = (uint8_t*)mask2.data;
+	uint8_t* pixelPtr_disp_map = (uint8_t*)disp_map.data;
+	uint8_t* pixelPtr_v_disp1 = (uint8_t*)v_disp_1.data;
+	uint8_t* pixelPtr_v_disp2 = (uint8_t*)v_disp_2.data;*/
+	
+    // run the images associating the red pixels in the v-disparities in the mask1 e mask2
+	for (l = 0; l < disp_map.rows; ++l)
+    {
+		for (c = 0; c < disp_map.cols; ++c)
+        {
+			pixel = l*disp_map.cols + c;
+
+			intensity = (unsigned char)disp_map.data[pixel];
+
+			//--------- Marca os planos trafegaveis --------------------
+			// Pixel Azul
+			v_disp_pixel = l*v_disp_1.cols*3 + intensity*3;
+			intensity_B = (unsigned char)v_disp_1.data[v_disp_pixel];
+
+			// Pixel Verde
+			v_disp_pixel = l*v_disp_1.cols*3 + intensity*3 + 1;
+			intensity_G = (unsigned char)v_disp_1.data[v_disp_pixel];
+
+			// Pixel Vermelho
+			v_disp_pixel = l*v_disp_1.cols*3 + intensity*3 + 2;
+			intensity_R = (unsigned char)v_disp_1.data[v_disp_pixel];
+			
+			if((intensity_B == 0)&&(intensity_G == 0)&&(intensity_R == 255))
+			{
+				mask1.data[pixel] = value;	
+			}
+			//----------------------------------------------------------
+
+			//--------- Marca os obstaculos ----------------------------					
+			if( (intensity >= min_d)&&(intensity <= max_d))
+			{				
+				// Pixel Azul
+				v_disp_pixel = l*v_disp_2.cols*3 + intensity*3;
+				intensity_B = (unsigned char)v_disp_2.data[v_disp_pixel];
+
+				// Pixel Verde
+				v_disp_pixel = l*v_disp_2.cols*3 + intensity*3 + 1;
+				intensity_G = (unsigned char)v_disp_2.data[v_disp_pixel];
+
+				// Pixel Vermelho
+				v_disp_pixel = l*v_disp_2.cols*3 + intensity*3 + 2;
+				intensity_R = (unsigned char)v_disp_2.data[v_disp_pixel];
+				
+				if((intensity_B == 0)&&(intensity_G == 0)&&(intensity_R == 255))
+				{
+					mask2.data[pixel] = value;
+				}
+			}
+			//----------------------------------------------------------
+        }   
+	}
+
+	/*cv::Mat element = cv::getStructuringElement( cv::MORPH_RECT,
+                                       cv::Size( 3, 3 ),
+                                       cv::Point( 1, 1 ) );*/
+
+	// Espande as regioes brancas da imagem
+	if(value == 1)
+	{
+		//cv::dilate(mask1, mask1, element, cv::Point(1,1), 2);
+		//cv::dilate(mask2, mask2, element, cv::Point(1,1), 2);
+		
+		cv::dilate(mask1, mask1, cv::Mat(), cv::Point(-1,-1), 2 );
+		cv::dilate(mask2, mask2, cv::Mat(), cv::Point(-1,-1), 2 );
+	}
+	else
+	{
+		//cv::erode(mask1, mask1, element, cv::Point(1,1), 2);
+		//cv::erode(mask2, mask2, element, cv::Point(1,1), 2);
+
+		cv::erode(mask1,mask1,NULL, cv::Point(-1,-1), 2);
+		cv::erode(mask2,mask2,NULL, cv::Point(-1,-1), 2);
+	}
+
+	//cvNamedWindow("Imagem da mascara1",CV_WINDOW_AUTOSIZE);
+	//cvShowImage("Imagem da mascara1", mask1);
+
+	//cvNamedWindow("Imagem da mascara2",CV_WINDOW_AUTOSIZE);
+	//cvShowImage("Imagem da mascara2", mask2);
+
+	return std::make_pair(mask1, mask2);
+}
+
+// Function to calculate the free space (v_disp_1) and obstacles (v_disp_2/u_disp) masks from 
+// a red highlighted V-Disparity map
+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)
+{
+	// Free space mask
+	cv::Mat mask1;
+
+	// Obstacle Mask
+	cv::Mat mask2;
+	
+	// Fill the destiny images with background value
+	if(value == 1)
+	{
+		mask1 = cv::Mat::zeros( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
+		mask2 = cv::Mat::zeros( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
+	}
+	else
+	{
+		mask1 = cv::Mat::ones( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
+		mask2 = cv::Mat::ones( cv::Size(disp_map.cols, disp_map.rows), CV_8UC1 );
+	}
+
+	int l, c, pixel, v_disp_pixel, u_disp_pixel;	// local variables for row, column and pixel
+	unsigned char intensity;		// pixel intensity
+	unsigned char intensity_B_v, intensity_G_v, intensity_R_v;	//pixel intensity
+	unsigned char intensity_B_u, intensity_G_u, intensity_R_u;	//pixel intensity
+
+	/*uint8_t* pixelPtr_mask1 = (uint8_t*)mask1.data;
+	uint8_t* pixelPtr_mask2 = (uint8_t*)mask2.data;
+	uint8_t* pixelPtr_disp_map = (uint8_t*)disp_map.data;
+	uint8_t* pixelPtr_v_disp1 = (uint8_t*)v_disp_1.data;
+	uint8_t* pixelPtr_v_disp2 = (uint8_t*)v_disp_2.data;*/
+	
+    // run the images associating the red pixels in the v-disparities in the mask1 e mask2
+	for (l = 0; l < disp_map.rows; ++l)
+    {
+		for (c = 0; c < disp_map.cols; ++c)
+        {
+			pixel = l*disp_map.cols + c;
+
+			intensity = (unsigned char)disp_map.data[pixel];
+
+			//--------- Marca os planos trafegaveis --------------------
+			// Pixel Azul
+			v_disp_pixel = l*v_disp_1.cols*3 + intensity*3;
+			intensity_B_v = (unsigned char)v_disp_1.data[v_disp_pixel];
+
+			// Pixel Verde
+			v_disp_pixel = l*v_disp_1.cols*3 + intensity*3 + 1;
+			intensity_G_v = (unsigned char)v_disp_1.data[v_disp_pixel];
+
+			// Pixel Vermelho
+			v_disp_pixel = l*v_disp_1.cols*3 + intensity*3 + 2;
+			intensity_R_v = (unsigned char)v_disp_1.data[v_disp_pixel];
+			
+			if((intensity_B_v == 0)&&(intensity_G_v == 0)&&(intensity_R_v == 255))
+			{
+				mask1.data[pixel] = value;	
+			}
+			//----------------------------------------------------------
+
+			//--------- Marca os obstaculos ----------------------------					
+			if( (intensity >= min_d)&&(intensity <= max_d))
+			{				
+				// Pixel Azul
+				v_disp_pixel = l*v_disp_2.cols*3 + intensity*3;
+				u_disp_pixel = intensity*u_disp.cols*3 + c*3;
+				intensity_B_v = (unsigned char)v_disp_2.data[v_disp_pixel];
+				intensity_B_u = (unsigned char)u_disp.data[u_disp_pixel];
+
+				// Pixel Verde
+				v_disp_pixel = l*v_disp_2.cols*3 + intensity*3 + 1;
+				u_disp_pixel = intensity*u_disp.cols*3 + c*3 + 1;
+				intensity_G_v = (unsigned char)v_disp_2.data[v_disp_pixel];
+				intensity_G_u = (unsigned char)u_disp.data[u_disp_pixel];
+
+				// Pixel Vermelho
+				v_disp_pixel = l*v_disp_2.cols*3 + intensity*3 + 2;
+				u_disp_pixel = intensity*u_disp.cols*3 + c*3 + 2;
+				intensity_R_v = (unsigned char)v_disp_2.data[v_disp_pixel];
+				intensity_R_u = (unsigned char)u_disp.data[u_disp_pixel];
+				
+				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))
+				{
+					mask2.data[pixel] = value;
+				}
+			}
+			//----------------------------------------------------------
+        }   
+	}
+
+	/*cv::Mat element = cv::getStructuringElement( cv::MORPH_RECT,
+                                       cv::Size( 3, 3 ),
+                                       cv::Point( 1, 1 ) );*/
+
+	// Espande as regioes brancas da imagem
+	if(value == 1)
+	{
+		//cv::dilate(mask1, mask1, element, cv::Point(1,1), 2);
+		//cv::dilate(mask2, mask2, element, cv::Point(1,1), 2);
+		
+		cv::dilate(mask1, mask1, cv::Mat(), cv::Point(-1,-1), 2 );
+		cv::dilate(mask2, mask2, cv::Mat(), cv::Point(-1,-1), 2 );
+	}
+	else
+	{
+		//cv::erode(mask1, mask1, element, cv::Point(1,1), 2);
+		//cv::erode(mask2, mask2, element, cv::Point(1,1), 2);
+
+		cv::erode(mask1,mask1,NULL, cv::Point(-1,-1), 2);
+		cv::erode(mask2,mask2,NULL, cv::Point(-1,-1), 2);
+	}
+
+	//cvNamedWindow("Imagem da mascara1",CV_WINDOW_AUTOSIZE);
+	//cvShowImage("Imagem da mascara1", mask1);
+
+	//cvNamedWindow("Imagem da mascara2",CV_WINDOW_AUTOSIZE);
+	//cvShowImage("Imagem da mascara2", mask2);
+
+	return std::make_pair(mask1, mask2);
+}
Index: trunk/StereoVisionDisparity/ObstacleDetectionComponent.h
===================================================================
--- trunk/StereoVisionDisparity/ObstacleDetectionComponent.h	(revision 53)
+++ trunk/StereoVisionDisparity/ObstacleDetectionComponent.h	(revision 53)
@@ -0,0 +1,193 @@
+/*********************************************************************
+//  created:    2013/06/25 - 18:40
+//  filename:   ObstacleDetectionComponent.h
+//
+//  author:     Danilo Alves de Lima
+//              Copyright Heudiasyc UMR UTC/CNRS 6599
+// 
+//  version:    $Id: $
+//
+//  purpose:    
+*********************************************************************/
+
+#ifndef OBSTACLEDETECTIONCOMPONENT_H
+#define OBSTACLEDETECTIONCOMPONENT_H
+
+#include <fstream>
+#include <qcoreevent.h>
+#include <qthread.h>
+#include <string>
+
+#include "opencv2/objdetect/objdetect.hpp"
+#include "opencv2/highgui/highgui.hpp"
+#include "opencv2/imgproc/imgproc.hpp"
+
+#include "Pacpus/kernel/ComponentBase.h"
+#include "Pacpus/kernel/DbiteFile.h"
+#include "Pacpus/PacpusTools/ShMem.h"
+#include "StereoVisionDisparityExp.h"
+
+#include "../../StdDbtPlayerComponents/ImageBaseStructure.h"
+
+#include <QThread>
+#include <QMutex>
+
+namespace pacpus {
+
+/** Class to provide the obstacle detection from a disparity map data */
+class STEREOVISIONDISPARITY_API ObstacleDetectionComponent:	public QThread, 
+															public ComponentBase
+{
+public:
+	//============================= DEFAULT ELEMENTS ===============================================
+    ObstacleDetectionComponent(QString name);
+    ~ObstacleDetectionComponent();
+
+    void run();
+
+    virtual void stopActivity(); /*!< to stop the processing thread */
+    virtual void startActivity(); /*!< to start the processing thread */
+    virtual ComponentBase::COMPONENT_CONFIGURATION configureComponent(XmlComponentConfig config);
+	//==============================================================================================
+		
+protected:
+
+	// Indicates that thread is running
+	bool is_running, THREAD_ALIVE;
+
+
+public:
+
+	/** 
+	* Initialize default values
+	*/
+	void InitDefault();
+
+private:
+
+	double ANG_VARIATION, ANG_VARIATION2;
+
+	int cam_width;			// Image width
+	int cam_height;			// image height
+	int cam_channels;		// number of channels
+	bool showdebug;			// Show frame acquired
+	int min_disp;			// Minimum disparity value to detect obstacles in the u/v-disp map
+	int max_disp;			// Maximum disparity value to detect obstacles in the u/v-disp map
+	int min_disp_norm;		// Minimum disparity value to equalize the disp map 16
+	int max_disp_norm;		// Maximum disparity value to equalize the disp map 16
+
+	int destiny_roi_x;		// Destiny image roi x
+	int destiny_roi_y;		// Destiny image roi y
+	int destiny_roi_width;	// Destiny image roi width
+	int destiny_roi_height;	// Destiny image roi height
+
+	bool use_roi;			// If is to use roi
+
+	int mMaxImageInputSize1;		// Size of the input image data in the memory
+	int mMaxImageInputSize2;		// Size of the input image data in the memory
+	int mMaxImageOutputSize;		// Size of the output image data in the memory
+
+	TimestampedStructImage RefImageHeader;			// Header for the reference image
+	TimestampedStructImage DispInImageHeader;		// Header for the input disp image
+	TimestampedStructImage Mask1ImageHeader;		// Header for the mask image 1
+	TimestampedStructImage Mask2ImageHeader;		// Header for the mask image 2
+	TimestampedStructImage DispOutImageHeader;		// Header for the output disp image
+
+	void* ref_mem;
+	void* dispin_mem;
+	void* mask1_mem;
+	void* mask2_mem;
+	void* dispout_mem;
+
+	size_t ref_mem_size;		// Image shared memory position size
+	size_t dispin_mem_size;		// Image shared memory position size
+	size_t mask1_mem_size;		// Image shared memory position size
+	size_t mask2_mem_size;		// Image shared memory position size
+	size_t dispout_mem_size;	// Image shared memory position size
+
+	// Imput data
+	ShMem * shmem_ref;			// Shared memory control access to the image data
+	ShMem * shmem_dispin;			// Shared memory control access to the image data
+
+	// Output data
+	ShMem * shmem_mask1;		// Shared memory control access to the image data (free space mask)
+	ShMem * shmem_mask2;		// Shared memory control access to the image data (obstacles mask)
+	ShMem * shmem_dispout;		// Shared memory control access to the image data (disparity map 16)
+
+	cv::Mat CurrentReferenceFrame;	// Reference image
+	cv::Mat CurrentDisparityMap16;	// Disparity Map
+
+	cv::Mat CurrentSurfaceMask;		// Surface mask
+	cv::Mat CurrentObstaclesMask;	// Obstacles mask
+
+	// Function to calculate the U/V disparity map
+	std::pair<cv::Mat, cv::Mat> CalcUVDisparityMap(cv::Mat disp_map);
+
+	/*	CalcUVDisparityMapNorm
+		Description:
+			Function to calculate the U/V disparity map from a disp map normalized
+		Parameters:
+			disp_map16 = original disparity map 16
+			disp_map_norm = resulted disparity map normalized
+			min_d_norm = Minimum disparity value to equalize the disp map 16
+			max_d_norm = Maximum disparity value to equalize the disp map 16
+	*/
+	void 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);
+
+	//Function to check the points order, making the second one with the highest y ever
+	void CheckPoints(cv::Point &pt1, cv::Point &pt2);
+
+	// Function to calculate the line slope of pt1 to pt2
+	double Inclination(cv::Point pt1, cv::Point pt2);
+
+	// Function to calculate the free space (v_disp_1) and obstacles (v_disp_2) masks from 
+	// a red highlighted V-Disparity map
+	std::pair<cv::Mat, cv::Mat> MaskSurface2(cv::Mat disp_map, cv::Mat v_disp_1, cv::Mat v_disp_2, int min_d = 0, int max_d = 255, int value = 0);
+
+	// Function to calculate the free space (v_disp_1) and obstacles (v_disp_2/u_disp) masks from 
+	// a red highlighted V-Disparity map
+	std::pair<cv::Mat, cv::Mat> MaskSurface3(cv::Mat disp_map, cv::Mat v_disp_1, cv::Mat v_disp_2, cv::Mat u_disp, int min_d = 0, int max_d = 255, int value = 0);
+	
+	// Function to find the free space surface from a V-disparity map
+	cv::Mat FindSurface(cv::Mat &v_disp_map, cv::Mat &v_disp_map2);
+
+	/*	FindSurface2
+		Description:
+			Function to find the free space surface from a V-disparity map, based on the frontal plane. 
+			Return the V-dysparity map with the red line representing the free surface.
+		Parameters:
+			v_disp_map = Original V-disparity map
+			v_disp_map2 = Orignal V-disparity map less the surface detected
+	*/
+	cv::Mat FindSurface2(cv::Mat &v_disp_map, cv::Mat &v_disp_map2);
+
+	// Function to find the free space surface from a V-disparity map with mean average
+	cv::Mat FindAverageSurface(cv::Mat &v_disp_map, cv::Mat &v_disp_map2);
+
+	// Function to find the near obstacles from a v-Disparity map
+	cv::Mat FindNearObstacles(cv::Mat v_disp_map, int min_d = 0, int max_d = 255);
+
+	// Function to find the near obstacles from the v/u-Disparity maps
+	std::pair<cv::Mat, cv::Mat> FindNearObstaclesUV(cv::Mat v_disp_map, cv::Mat u_disp_map, int min_d = 0, int max_d = 255);
+
+	/*	LinesFiltering
+		Description:
+			Filter the detected lines related to the distance and angle between them.
+		Parameters:
+			lines = line list (point 1 and 2)
+	*/
+	std::vector<cv::Point> LinesFiltering(std::vector<cv::Vec4i> lines);
+
+	/*	CalcMedian
+		Description:
+			Calcule the median value of a vector.
+		Parametros:
+			vector = vector with data to calculate the median
+	*/
+	template<class A>
+	A CalcMedian(std::vector<A> vetor) const;
+
+};
+
+}
+#endif // OBSTACLEDETECTIONCOMPONENT
