add lane detect with control of vehicle cutting and smoothed display

img_processing/CMakeLists.txt

@@ -17,6 +17,12 @@ if(SHOW_FRAMES)
     add_definitions(-DSHOW_FRAMES)
 endif()
 
+# Check if LANE_DETECT flag is set
+option(LANE_DETECT "Enable lane detection" ON)
+if(LANE_DETECT)
+    add_definitions(-DLANE_DETECT)
+endif()
+
 # make the library with the src files
 file(GLOB SOURCES "src/*.cpp" "../logger/*.cpp" "tests/utils.cpp" "../communication/src/*.cpp" "../communication/sockets/*.cpp")
 add_library(ImageProcessingLib ${SOURCES})
@@ -26,7 +32,7 @@ add_library(CommunicationLib STATIC ../communication/src/communication.cpp ../co
 
 configure_file( ${CMAKE_BINARY_DIR}/config.json COPYONLY)
 # create test executable
-add_executable(runTests tests/main.cpp tests/test_serialize.cpp tests/test_detector.cpp tests/test_dynamic_tracker.cpp tests/test_distance.cpp tests/test_manager.cpp tests/test_velocity.cpp)
+add_executable(runTests tests/main.cpp tests/test_serialize.cpp tests/test_detector.cpp tests/test_dynamic_tracker.cpp tests/test_distance.cpp tests/test_manager.cpp tests/test_velocity.cpp tests/test_lane_detector.cpp)
 target_link_libraries(runTests ImageProcessingLib ${OpenCV_LIBS} ${GTEST_LIBRARIES} pthread CommunicationLib)
 
 #adding tests

img_processing/data.json

@@ -0,0 +1,12 @@
+[
+    {
+        "path": "C:/Users/user1/Desktop/day/sun/sun_in_street.mov",
+        "focal_length": 1500,
+        "is_travel": true
+    },
+    {
+        "path": "C:/Users/user1/Desktop/day/close_cars/road.mov",
+        "focal_length": 2000,
+        "is_travel": true
+    }
+]

img_processing/include/lane_detector.h

@@ -0,0 +1,105 @@
+
+#ifndef __LANE_DETECTION__
+#define __LANE_DETECTION__
+
+#define _USE_MATH_DEFINES
+#include <cmath>
+#include "regression.h"
+#include "log_manager.h"
+#include <opencv2/highgui.hpp>
+#include <stdexcept>
+
+#define CAR_IN_IMAGE 80
+
+struct LaneDrawingInfo {
+    int rightX1;  //x bottom right
+    int rightX2;  //x top right
+    int leftX1;   //x bottom left
+    int leftX2;   //x top left
+    int y1;       //y bottom right & left
+    int y2;       //y top right & left
+};
+
+class LaneDetector {
+   public:
+    void init();
+    void manageLaneDetector(std::shared_ptr<cv::Mat> frame);
+    void drawLanesOnImage(std::shared_ptr<cv::Mat> img);
+
+   private:
+    std::shared_ptr<cv::Mat> image;
+    bool first;
+    bool withoutCar;
+    int imgCols;
+    int imgRows;
+    LaneDrawingInfo drawingInfo;
+
+    /**
+    * Returns true when the image is classified as daytime. 
+    * Note: this is based on the mean pixel value of an image and might not
+    * always lead to accurate results.
+    */
+    bool isDayTime();
+
+    /**
+    * Filter source image so that only the white and yellow pixels remain.
+    * A gray filter will also be added if the source image is classified as taken during the night.
+    * One assumption for lane detection here is that lanes are either white or yellow.
+    * @param isDayTime true if image is taken during the day, false if at night
+    * @return Mat filtered image
+    */
+    cv::Mat filterColors(bool isDayTime);
+
+    /**
+    * Apply grayscale transform on image.
+    * @return grayscale image
+    */
+    cv::Mat applyGrayscale(cv::Mat source);
+
+    /**
+    * Apply Gaussian blur to image.
+    * @return blurred image
+    */
+    cv::Mat applyGaussianBlur(cv::Mat source);
+
+    /**
+    * Detect edges of image by applying canny edge detection.
+    * @return image with detected edges
+    */
+    cv::Mat applyCanny(cv::Mat source);
+
+    /**
+    * Apply an image mask. 
+    * Only keep the part of the image defined by the
+    * polygon formed from four points. The rest of the image is set to black.
+    * @return Mat image with mask
+    */
+    cv::Mat RegionOfInterest(cv::Mat source);
+
+    /**
+    * Returns a vector with the detected hough lines.
+    * @param canny image resulted from a canny transform
+    * @param source image on which hough lines are drawn
+    * @param drawHough draw detected lines on source image if true. 
+    * It will also show the image with de lines drawn on it, which is why
+    * it is not recommended to pass in true when working with a video. 
+    * @return vector<Vec4i> contains hough lines.
+    */
+    std::vector<cv::Vec4i> houghLines(cv::Mat canny, cv::Mat source,
+                                      bool drawHough);
+
+    /**
+    * Creates mask and blends it with source image so that the lanes
+    * are drawn on the source image.
+    * @param lines vector < vec4i > holding the lines
+    * @return Mat image with lines drawn on it
+    */
+    bool drawLanes(std::vector<cv::Vec4i> lines);
+
+    /**
+    * Drawing the lane on the road only
+    */
+    void cutCar();
+};
+
+#endif /*__LANE_DETECTION__*/

img_processing/include/log_manager.h

@@ -12,7 +12,8 @@ enum class ErrorType {
     FILE_ERROR,
     DETECTION_ERROR,
     TRACKING_ERROR,
-    MODEL_ERROR
+    MODEL_ERROR,
+    SIZE_ERROR
 };
 
 enum class InfoType {
@@ -29,7 +30,10 @@ enum class InfoType {
     MODE
 };
 
-enum class DebugType { PRINT };
+enum class DebugType { 
+    PRINT,
+    DRAW_PREV 
+};
 
 class LogManager {
    public:

img_processing/include/manager.h

@@ -5,10 +5,12 @@
 #include "alerter.h"
 #include "detector.h"
 #include "distance.h"
+#include "lane_detector.h"
 #include "dynamic_tracker.h"
 #include "log_manager.h"
 #include "velocity.h"
 #include "communication.h"
+
 class Manager {
    public:
     static logger imgLogger;
@@ -30,6 +32,8 @@ class Manager {
     Velocity velocity;
     DynamicTracker dynamicTracker;
     Alerter alerter;
+    LaneDetector laneDetector;
+    int longTime;
     int iterationCnt;
     uint32_t destID;
     uint32_t processID;
@@ -42,7 +46,7 @@ class Manager {
     bool isResetTracker(bool isTravel);
     bool isTrack(bool isTravel);
     void sendAlerts(std::vector<std::vector<uint8_t>> &alerts);
-    void runOnVideo(std::string videoPath);
+    void runOnVideo(std::string videoPath, bool isTravel);
     int readIdFromJson(const char *target);
 };
 #endif  //__MANAGER_H__

img_processing/include/regression.h

@@ -0,0 +1,63 @@
+#ifndef __REGRESSION__
+#define __REGRESSION__
+
+#include <vector>
+#include <numeric>
+#include <opencv2/opencv.hpp>
+
+/**
+ * @brief Multiplies two vectors and then calculates the sum of the multiplied values.
+ * vector A and B must be the same size and their values must be of the same type.
+ * 
+ * @param A vector<T>.
+ * @param B vector<T>.
+ * @return X sum of the multiplied values.
+ */
+template <typename T, typename X>
+X multiplyAndSum(std::vector<T> A, std::vector<T> B)
+{
+    X sum;
+    std::vector<T> temp;
+    for (int i = 0; i < A.size(); i++) {
+        temp.push_back(A[i] * B[i]);
+    }
+    sum = std::accumulate(temp.begin(), temp.end(), 0.0);
+
+    return sum;
+}
+
+/**
+ * @brief Calculates the coefficients (slope and intercept) of the best fitting line
+ * given independent and dependent values. Vector A and B must be the same size 
+ * and their values must be of the same type.
+ * 
+ * @param A vector<T> (independent values).
+ * @param B vector<T> (dependent values).
+ * @return vector<X> where first element is the slope (b1), second element is intercept (b0).
+ */
+template <typename T, typename X>
+std::vector<X> estimateCoefficients(std::vector<T> A, std::vector<T> B)
+{
+    // Sample size
+    int N = A.size();
+
+    // Calculate mean of X and Y
+    X meanA = std::accumulate(A.begin(), A.end(), 0.0) / A.size();
+    X meanB = std::accumulate(B.begin(), B.end(), 0.0) / B.size();
+
+    // Calculating cross-deviation and deviation about x
+    X SSxy = multiplyAndSum<T, T>(A, B) - (N * meanA * meanB);
+    X SSxx = multiplyAndSum<T, T>(A, A) - (N * meanA * meanA);
+
+    // Calculating regression coefficients
+    X slopeB1 = SSxy / SSxx;
+    X interceptB0 = meanB - (slopeB1 * meanA);
+
+    // Return vector, insert slope first and then intercept
+    std::vector<X> coef;
+    coef.push_back(slopeB1);
+    coef.push_back(interceptB0);
+    return coef;
+}
+
+#endif /*__REGRESSION__*/