Lane detection

rr_lane_detection/CMakeLists.txt

@@ -19,5 +19,5 @@ catkin_package()
 include_directories(${OpenCV_INCLUDE_DIRS})
 include_directories(include ${catkin_INCLUDE_DIRS})
 
-add_executable(lane_detection src/lane_detection.cpp src/thresholding.cpp src/lane_detection_processor.cpp include/lane_detection_processor.hpp include/thresholding.hpp)
-target_link_libraries(lane_detection ${catkin_LIBRARIES} ${OpenCV_LIBS})
+add_executable(lane_detection src/lane_detection.cpp src/thresholding.cpp src/shadow_removal.cpp src/lane_detection_processor.cpp include/lane_detection_processor.hpp include/thresholding.hpp include/shadow_removal.hpp)
+target_link_libraries(lane_detection ${catkin_LIBRARIES} ${OpenCV_LIBS})

rr_lane_detection/include/lane_detection_processor.hpp

@@ -53,7 +53,7 @@ class lane_detection_processor
     int adapt_hsv_patch_size_;
     cv::Scalar bounds_;
     cv::Mat multibounds_;
-    cv::Mat Im1_HSV_, Im1_HSV_warped_, mask_warped_1_,mask_warped_2_, mask_;
+    cv::Mat Im1_Shadows_Removed, Im1_HSV_, Im1_HSV_warped_, mask_warped_1_,mask_warped_2_, mask_;
 
     std::string opencv_file_name_;
     cv::Mat image_coords_;

rr_lane_detection/include/shadow_removal.hpp

@@ -0,0 +1,14 @@
+/** @file shadow_removal.hpp
+ *  @author Brian Tran
+ *  @competition IARRC 2018
+ */
+#ifndef __SHADOW_REMOVAL_HPP
+#define __SHADOW_REMOVAL_HPP
+
+// includes
+#include <opencv2/opencv.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc/imgproc.hpp>
+
+void RemoveShadows(const cv::Mat &input_image, cv::Mat &output_image);
+#endif //__SHADOW_REMOVAL_HPP

rr_lane_detection/src/lane_detection.cpp

@@ -18,6 +18,7 @@
 
 //Helper includes
 #include "thresholding.hpp"
+#include "shadow_removal.hpp"
 #include "lane_detection_processor.hpp"
 
 /** @brief main file that starts the subscribers and calls spin

rr_lane_detection/src/lane_detection_processor.cpp

@@ -6,6 +6,9 @@
 
 #include "lane_detection_processor.hpp"
 #include "thresholding.hpp"
+#include "shadow_removal.hpp"
+#include <iostream>
+#include <sstream>
 
 /** @brief sets the rosparams and obtains the perspective transforms
  *  @param nh is the node handle of the node
@@ -100,6 +103,8 @@ void lane_detection_processor::FindLanes(const sensor_msgs::Image::ConstPtr &msg
     cv_input_bridge_ = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::BGR8);
     cv_input_bridge_->image.copyTo(im_input_);
 
+    RemoveShadows(im_input_, Im1_Shadows_Removed);
+
     cvtColor(im_input_, Im1_HSV_, CV_BGR2HSV, 3);
     cv::warpPerspective(Im1_HSV_, Im1_HSV_warped_, transform_matrix_, BEV_size_, cv::INTER_LINEAR, cv::BORDER_REPLICATE);
 
@@ -117,20 +122,20 @@ void lane_detection_processor::FindLanes(const sensor_msgs::Image::ConstPtr &msg
 
     cv::Mat out;               // dst must be a different Mat
 
-  if (simulation_) {
-    cv::Mat src = GetContours(mask_warped_1_ &mask_, blob_size_);
-    cv::flip(src, out, 1);
-
-    cv::Mat1b element(4, 4, uchar(1));
+    if (simulation_) {
+      cv::Mat src = GetContours(mask_warped_1_ &mask_, blob_size_);
+      cv::flip(src, out, 1);
+      
+      cv::Mat1b element(4, 4, uchar(1));
 
-		// use square as mask
-		cv::erode(out, out, element);
-		cv::dilate(out, out, element);
-  }
-  else
-  {
-    out = GetContours(mask_warped_1_ &mask_, blob_size_);
-  }
+      // use square as mask
+      cv::erode(out, out, element);
+      cv::dilate(out, out, element);
+    }
+    else
+    {
+      out = GetContours(mask_warped_1_ &mask_, blob_size_);
+    }
 
     //find mask_
     //Copy to output bridge

rr_lane_detection/src/shadow_removal.cpp

@@ -0,0 +1,135 @@
+/** @file shadow_removal.cpp
+ *  @author Brian Tran
+ *  @competition IARRC 2018
+ */
+
+#include "shadow_removal.hpp"
+
+/** @brief This function is intended to detect shadows and reduce the difference
+ *         in illumination in those regions
+ *
+ * 	"output_image","input_image", of type CV_8U (unsigned 8 bit int), where
+ * 	true == 255 else false == 0
+ * 
+ *  @param input_image is an image Matrix
+ *  @return output_image is the processed output image with the same
+ *          width and height as the input matrix
+ */
+void RemoveShadows(const cv::Mat &input_image, cv::Mat &output_image) {
+	// Configurable constants
+    const double THRESHOLD_TOLERANCE = 1.2;//0.8;
+	const double MORPH_KERNEL_SIZE = 3;
+    const double MIN_SHADOW_AREA = 30;
+    const double MASK_DILATION = 8;//6;
+    const double EXPANDED_SHADOW_MASK_DILATION = 10;
+    const double CORRECTION_MASK_EROSION = 6;//4;
+    const double MED_FILTER_KERNEL_SIZE = 15;
+
+	// Get the mean of L, a, b planes
+	cv::Mat shadow_lab_image;
+	cvtColor(input_image, shadow_lab_image, CV_BGR2Lab, 0);
+
+	std::vector<cv::Mat> channels;
+	split(shadow_lab_image, channels);
+	cv::Scalar mean_l, stdev_l;
+	meanStdDev(channels[0], mean_l, stdev_l);
+	cv::Scalar mean_a = mean(channels[1]);
+	cv::Scalar mean_b = mean(channels[2]);
+
+	// Extract shadows using best method based on mean values
+	cv::Mat shadow_pixels_mask;
+	double shadow_threshold = mean_l[0] - (stdev_l[0] / THRESHOLD_TOLERANCE);
+
+	if (mean_a[0] + mean_b[0] > 256) {
+		cv::inRange(shadow_lab_image, cv::Scalar(0, 0, 0), cv::Scalar(shadow_threshold, 255, 255), shadow_pixels_mask);
+	} else {
+		cv::Mat mask_l, mask_b;
+		cv::inRange(shadow_lab_image, cv::Scalar(0, 0, 0), cv::Scalar(shadow_threshold, 255, 255), mask_l);
+		cv::inRange(shadow_lab_image, cv::Scalar(0, 0, 0), cv::Scalar(255, 255, shadow_threshold), mask_b);
+		shadow_pixels_mask = mask_l | mask_b;
+	}
+
+	// Morphological operations to clean up misclassified pixels
+	cv::Mat shadow_mask_morph;
+	cv::Mat kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(MORPH_KERNEL_SIZE, MORPH_KERNEL_SIZE));
+
+	cv::morphologyEx(shadow_pixels_mask, shadow_mask_morph, cv::MORPH_OPEN, kernel);
+	cv::morphologyEx(shadow_mask_morph, shadow_mask_morph, cv::MORPH_CLOSE, kernel);
+
+	std::vector<std::vector<cv::Point> > contours;
+	cv::findContours(shadow_mask_morph, contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
+
+	for (int i = 0; i < contours.size(); i++) {
+		double area = cv::contourArea(contours[i]);
+
+		// Fill in contours with area smaller than min. shadow area
+		if (area < MIN_SHADOW_AREA) {
+			cv::drawContours(shadow_mask_morph, contours, i, CV_RGB(0, 0, 0), CV_FILLED);
+		}
+	}
+
+	kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(MASK_DILATION, MASK_DILATION));
+	cv::dilate(shadow_mask_morph, shadow_mask_morph, kernel);
+
+	// Shadow regions
+	cv::Mat connected_components;
+	int num_components = cv::connectedComponents(shadow_mask_morph, connected_components, 4);
+
+	// Shadow removal
+	cv::Mat shadow_removed_image = input_image.clone();
+
+	for (int i = 1; i <= num_components; i++) {
+		cv::Mat component_mask;
+		cv::inRange(connected_components, cv::Scalar(i), cv::Scalar(i), component_mask);
+
+		cv::Mat dilated_component_mask, dilated_component_edge;
+		kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(EXPANDED_SHADOW_MASK_DILATION, EXPANDED_SHADOW_MASK_DILATION));
+		cv::dilate(component_mask, dilated_component_mask, kernel);
+		cv::bitwise_xor(dilated_component_mask, component_mask, dilated_component_edge);
+
+		// Get mean BGR in masks
+		cv::Scalar outer_mean_bgr = mean(shadow_removed_image, dilated_component_edge);
+		cv::Scalar inner_mean_bgr = mean(shadow_removed_image, component_mask);
+
+		cv::Scalar bgr_ratios(outer_mean_bgr[0] / inner_mean_bgr[0],
+							outer_mean_bgr[1] / inner_mean_bgr[1],
+							outer_mean_bgr[2] / inner_mean_bgr[2]);
+
+		if (std::isinf(bgr_ratios[0])) {
+			bgr_ratios[0] = 1;
+		}
+		if (std::isinf(bgr_ratios[1])) {
+			bgr_ratios[1] = 1;
+		}
+		if (std::isinf(bgr_ratios[2])) {
+			bgr_ratios[2] = 1;
+		}
+
+		// Multiply shadow region by ratios to remove shadow
+		cv::Mat component_region;
+		shadow_removed_image.copyTo(component_region, component_mask);
+		component_region = component_region.mul(bgr_ratios);
+		component_region.copyTo(shadow_removed_image, component_mask);
+	}
+
+	// Fix over-illuminated edges with median filter
+	cv::Mat corrected_image;
+	cvtColor(shadow_removed_image, corrected_image, CV_BGR2HSV, 0);
+
+	for (int i = 1; i <= num_components; i++) {
+		cv::Mat component_mask;
+		cv::inRange(connected_components, cv::Scalar(i), cv::Scalar(i), component_mask);
+
+		cv::Mat eroded_component_mask, component_edge;
+		kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(CORRECTION_MASK_EROSION, CORRECTION_MASK_EROSION));
+		cv::erode(component_mask, eroded_component_mask, kernel);
+		cv::bitwise_xor(component_mask, eroded_component_mask, component_edge);
+
+		cv::Mat median_filtered_image;
+		cv::medianBlur(corrected_image, median_filtered_image, MED_FILTER_KERNEL_SIZE);
+
+		median_filtered_image.copyTo(corrected_image, component_edge);
+	}
+
+	cvtColor(corrected_image, output_image, CV_HSV2BGR, 0);
+}

simulations/lane_detection/lanedetection.m

@@ -0,0 +1,129 @@
+%% Load original images
+im1name = 'whitecurve.png';
+im2name = 'whitelane.png';
+
+image1 = imread(im1name);
+image2 = imread(im2name);
+
+figure,
+subplot(2,2,1), imagesc(image1), title('White Curve'), grid on;
+subplot(2,2,2), imagesc(image2), title('White Lane'), grid on;
+
+%% Convert from RGB to HSV
+% image1_HSV = rgb2hsv(image1);
+% image2_HSV = rgb2hsv(image2);
+image1_HSL = rgb2hsl(image1);
+image2_HSL = rgb2hsl(image2);
+
+% subplot(2,2,1), imagesc(image1), title('White Curve'), grid on;
+% subplot(2,2,2), imagesc(image1_HSL), title('White Curve HSL');
+% subplot(2,2,3), imagesc(image2), title('White Lane'), grid on;
+% subplot(2,2,4), imagesc(image2_HSL), title('White Lane HSL');
+%
+% subplot(2,2,1),imagesc(image2),title('Original Image')
+% subplot(2,2,2),imagesc(image2_HSL(:,:,1)),title('Hue'),colorbar
+% subplot(2,2,3),imagesc(image2_HSL(:,:,2)),title('Saturation'),colorbar
+% subplot(2,2,4),imagesc(image2_HSL(:,:,3)),title('Lightness'),colorbar
+% 
+% subplot(2,2,1), imagesc(image1_HSV), title('White Curve HSV');
+% subplot(2,2,2), imagesc(image2_HSV), title('White Lane HSV');
+% subplot(2,2,3), imagesc(image1_HSL), title('White Curve HSL');
+% subplot(2,2,4), imagesc(image2_HSL), title('White Lane HSL');
+
+%% Extract yellow and white
+image1_white = image1_HSL(:, :, 3) > 0.8;
+image2_white = image2_HSL(:, :, 3) > 0.8;
+
+image1_yellow = (image1_HSL(:,:,1) > 40 & image1_HSL(:,:,1) < 80) & image1_HSL(:, :, 3) > 0.25 & image1_HSL(:, :, 2) > 0.25;
+image2_yellow = (image2_HSL(:,:,1) > 40 & image2_HSL(:,:,1) < 80) & image2_HSL(:, :, 3) > 0.25 & image2_HSL(:, :, 2) > 0.25;
+
+subplot(2,2,1), imagesc(image1_white), title('White Curve White');
+subplot(2,2,2), imagesc(image2_white), title('White Lane White');
+subplot(2,2,3), imagesc(image1_yellow), title('White Curve Yellow');
+subplot(2,2,4), imagesc(image2_yellow), title('White Lane Yellow');
+
+%% Combine
+image1_extract = double(image1 & (image1_white | image1_yellow));
+image2_extract = double(image2 & (image2_white | image2_yellow));
+
+subplot(2,2,1), imagesc(image1), title('White Curve');
+subplot(2,2,2), imagesc(image2), title('White Lane');
+subplot(2,2,3), imagesc(image1_extract), title('White Curve Extract');
+subplot(2,2,4), imagesc(image2_extract), title('White Lane Extract');
+
+%% Gaussian blur
+image1_gauss = imgaussfilt(image1_extract, 1.2);
+image2_gauss = imgaussfilt(image2_extract, 1.2);
+
+subplot(2,2,1), imagesc(image1_extract), title('White Curve Extract');
+subplot(2,2,2), imagesc(image2_extract), title('White Lane Extract');
+subplot(2,2,3), imagesc(image1_gauss), title('White Curve Gauss');
+subplot(2,2,4), imagesc(image2_gauss), title('White Lane Gauss');
+
+%% Canny edge detection
+image1_edge = edge(rgb2gray(image1_gauss), 'canny');
+image2_edge = edge(rgb2gray(image2_gauss), 'canny');
+
+subplot(2,2,1), imagesc(image1_gauss), title('White Curve Gauss');
+subplot(2,2,2), imagesc(image2_gauss), title('White Lane Gauss');
+subplot(2,2,3), imagesc(image1_edge), title('White Curve Canny');
+subplot(2,2,4), imagesc(image2_edge), title('White Lane Canny');
+
+%% Region of interest
+handle = imshow(image1_edge);
+roi = impoly(gca, [198 176;334 176;511 287.5;45 287.5]); % depends on image size
+mask = createMask(roi, handle);
+
+image1_roi = mask & image1_edge;
+image2_roi = mask & image2_edge;
+
+subplot(2,2,1), imagesc(image1_edge), title('White Curve Canny');
+subplot(2,2,2), imagesc(image2_edge), title('White Lane Canny');
+subplot(2,2,3), imagesc(image1_roi), title('White Curve Region');
+subplot(2,2,4), imagesc(image2_roi), title('White Lane Region');
+
+%% Hough transform
+fillgap = 200;
+minlength = 40;
+
+% White curve
+[H, theta, rho] = hough(image1_roi);
+peaks = houghpeaks(H, 5, 'threshold', ceil(0.1*max(H(:))));
+lines1 = houghlines(image1_roi, theta, rho, peaks, 'Fillgap', fillgap, 'MinLength', minlength);
+
+subplot(2,1,1), imshow(image1_roi), title('White Curve'), hold on
+plothoughlines(lines1);
+
+% White lane
+[H, theta, rho] = hough(image2_roi);
+peaks = houghpeaks(H, 5, 'threshold', ceil(0.1*max(H(:))));
+lines2 = houghlines(image2_roi, theta, rho, peaks, 'Fillgap', fillgap, 'MinLength', minlength);
+
+subplot(2,1,2), imshow(image2_roi), title('White Lane'), hold on
+plothoughlines(lines2);
+
+%% Separate left and right lanes
+clf('reset');
+
+subplot(2,1,1), imshow(image1_roi), title('White Curve'), hold on
+lines1 = plotleftrightlines(lines1);
+
+subplot(2,1,2), imshow(image2_roi), title('White Lane'), hold on
+lines2 = plotleftrightlines(lines2);
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+