diff --git a/img_processing/CMakeLists.txt b/img_processing/CMakeLists.txt index 74cb072f..48992e54 100644 --- a/img_processing/CMakeLists.txt +++ b/img_processing/CMakeLists.txt @@ -17,6 +17,18 @@ 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() + +# Check if DETECT_SUN flag is set +option(DETECT_SUN "detect sun" ON) +if(DETECT_SUN) + add_definitions(-DDETECT_SUN) +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 +38,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_sun_detector.cpp tests/test_lane_detector.cpp) target_link_libraries(runTests ImageProcessingLib ${OpenCV_LIBS} ${GTEST_LIBRARIES} pthread CommunicationLib) #adding tests diff --git a/img_processing/data.json b/img_processing/data.json new file mode 100644 index 00000000..58b059a1 --- /dev/null +++ b/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 + } +] diff --git a/img_processing/include/distance.h b/img_processing/include/distance.h index e8913d51..d633166b 100644 --- a/img_processing/include/distance.h +++ b/img_processing/include/distance.h @@ -3,22 +3,19 @@ #define PERSON_HEIGHT 1700 #define CAR_WIDTH 2000 -#include "object_information_struct.h" #include "log_manager.h" +#include "object_information_struct.h" class Distance { public: void findDistance(std::vector &objectInformation); - static Distance &getInstance(const cv::Mat &image = cv::Mat()); + void setFocalLength(const cv::Mat &image); void setFocalLength(double focalLength); + void drawDistance(const std::shared_ptr image, + const std::vector &objects) const; private: - static Distance *instance; double focalLength; - - Distance(const cv::Mat &image); - Distance(const Distance &) = delete; - Distance &operator=(const Distance &) = delete; void findFocalLength(const cv::Mat &image); void addDistance(float distance, ObjectInformation &obj); }; diff --git a/img_processing/include/dynamic_tracker.h b/img_processing/include/dynamic_tracker.h index 99de28ab..f2846073 100644 --- a/img_processing/include/dynamic_tracker.h +++ b/img_processing/include/dynamic_tracker.h @@ -12,6 +12,8 @@ class DynamicTracker { const std::vector &detectionOutput); void tracking(const std::shared_ptr &frame, std::vector &objectInformation); + void drawTracking(const std::shared_ptr image, + const std::vector &objects) const; private: std::shared_ptr frame; diff --git a/img_processing/include/lane_detector.h b/img_processing/include/lane_detector.h new file mode 100644 index 00000000..c8789fec --- /dev/null +++ b/img_processing/include/lane_detector.h @@ -0,0 +1,105 @@ + +#ifndef __LANE_DETECTION__ +#define __LANE_DETECTION__ + +#include +#include +#include +#include "regression.h" +#include "log_manager.h" + +#define _USE_MATH_DEFINES +#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 frame); + void drawLane(std::shared_ptr img); + + private: + std::shared_ptr 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 contains hough lines. + */ + std::vector 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 lines); + + /** + * Drawing the lane on the road only + */ + void cutCar(); +}; + +#endif /*__LANE_DETECTION__*/ diff --git a/img_processing/include/log_manager.h b/img_processing/include/log_manager.h index 9c464bb4..4c5a4cdc 100644 --- a/img_processing/include/log_manager.h +++ b/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: diff --git a/img_processing/include/manager.h b/img_processing/include/manager.h index 44a93e66..6a97fe5e 100644 --- a/img_processing/include/manager.h +++ b/img_processing/include/manager.h @@ -3,12 +3,15 @@ #include #include "alerter.h" +#include "communication.h" #include "detector.h" #include "distance.h" #include "dynamic_tracker.h" +#include "lane_detector.h" #include "log_manager.h" +#include "sun_detector.h" #include "velocity.h" -#include "communication.h" + class Manager { public: static logger imgLogger; @@ -16,9 +19,10 @@ class Manager { // Gets the currentFrame and sends it for detection and then tracking, // finally if necessary sends a alert int processing(const cv::Mat &newFrame, bool mode); - void mainDemo(); // init all variabels and creat the instance of distance void init(); + void setIterationCnt(int cnt); + Distance *getDistance(); private: Communication communication; @@ -29,20 +33,25 @@ class Manager { Detector detector; Velocity velocity; DynamicTracker dynamicTracker; + Distance distance; Alerter alerter; + SunDetector sunDetector; + LaneDetector laneDetector; + int longTime; int iterationCnt; uint32_t destID; uint32_t processID; + bool isTravel; // Moves the current image to the prevFrame // and clears the memory of the currentFrame; void prepareForTheNext(); - void drawOutput(); - bool isDetect(bool isTravel); - bool isResetTracker(bool isTravel); - bool isTrack(bool isTravel); + int drawOutput(); + bool isDetect(); + bool isResetTracker(); + bool isTrack(); + bool isCalcVelocity(); void sendAlerts(std::vector> &alerts); - void runOnVideo(std::string videoPath); int readIdFromJson(const char *target); }; #endif //__MANAGER_H__ diff --git a/img_processing/include/object_information_struct.h b/img_processing/include/object_information_struct.h index 2558de24..3ef35593 100644 --- a/img_processing/include/object_information_struct.h +++ b/img_processing/include/object_information_struct.h @@ -2,6 +2,7 @@ #define __OBJECT_INFORMATION_STRUCT_H__ #include +#include #include "object_type_enum.h" #define MAX_PREV_DISTANCES_SIZE 10 @@ -15,7 +16,7 @@ struct ObjectInformation { std::deque prevDistances; float distance; std::deque prevVelocities; - float velocity; + std::optional velocity = std::nullopt; }; #endif // __OBJECT_INFORMATION_STRUCT_H__ \ No newline at end of file diff --git a/img_processing/include/regression.h b/img_processing/include/regression.h new file mode 100644 index 00000000..7421d26e --- /dev/null +++ b/img_processing/include/regression.h @@ -0,0 +1,63 @@ +#ifndef __REGRESSION__ +#define __REGRESSION__ + +#include +#include +#include + +/** + * @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. + * @param B vector. + * @return X sum of the multiplied values. + */ +template +X multiplyAndSum(std::vector A, std::vector B) +{ + X sum; + std::vector 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 (independent values). + * @param B vector (dependent values). + * @return vector where first element is the slope (b1), second element is intercept (b0). + */ +template +std::vector estimateCoefficients(std::vector A, std::vector 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(A, B) - (N * meanA * meanB); + X SSxx = multiplyAndSum(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 coef; + coef.push_back(slopeB1); + coef.push_back(interceptB0); + return coef; +} + +#endif /*__REGRESSION__*/ \ No newline at end of file diff --git a/img_processing/include/sun_detector.h b/img_processing/include/sun_detector.h new file mode 100644 index 00000000..1a1abd28 --- /dev/null +++ b/img_processing/include/sun_detector.h @@ -0,0 +1,17 @@ +#ifndef __SUN_DETECTOR_H__ +#define __SUN_DETECTOR_H__ + +#include + +class SunDetector { + public: + void detectSun(const std::shared_ptr &frame); + void drowSun(std::shared_ptr &frame); + + private: + cv::Point2f center; + float radius; + bool isSun; +}; + +#endif // __SUN_DETECTOR_H__ \ No newline at end of file diff --git a/img_processing/include/velocity.h b/img_processing/include/velocity.h index 36a4f8ee..9eb63800 100644 --- a/img_processing/include/velocity.h +++ b/img_processing/include/velocity.h @@ -9,6 +9,8 @@ class Velocity { Velocity() {} void returnVelocities(std::vector &objects); void init(double frameTimeDiff); + void drawVelocity(const std::shared_ptr image, + const std::vector &objects) const; private: double frameTimeDiff; diff --git a/img_processing/src/alerter.cpp b/img_processing/src/alerter.cpp index 00d926f6..e174c41a 100644 --- a/img_processing/src/alerter.cpp +++ b/img_processing/src/alerter.cpp @@ -1,6 +1,6 @@ -#include #include #include +#include #include "alerter.h" #include "alert.h" #include "manager.h" @@ -31,7 +31,7 @@ vector> Alerter::sendAlerts( if (isSendAlert(objectInformation)) { vector alertBuffer = makeAlertBuffer( objectInformation.type, objectInformation.distance, - objectInformation.velocity); + objectInformation.velocity.value()); alerts.push_back(alertBuffer); } } diff --git a/img_processing/src/distance.cpp b/img_processing/src/distance.cpp index 4fec4e92..987eb9cc 100644 --- a/img_processing/src/distance.cpp +++ b/img_processing/src/distance.cpp @@ -1,37 +1,19 @@ -#define MIN_LEGAL_HEIGHT 900 - -#include #include +#include #include #include "distance.h" #include "detector.h" #include "manager.h" +#define MIN_LEGAL_HEIGHT 900 + using namespace std; using namespace cv; -Distance *Distance::instance = nullptr; - -Distance::Distance(const cv::Mat &image) +void Distance::setFocalLength(const Mat &image) { findFocalLength(image); } - -Distance &Distance::getInstance(const cv::Mat &image) -{ - if (!instance) { - if (image.empty()) { - LogManager::logErrorMessage(ErrorType::IMAGE_ERROR, - "Could not load image"); - throw std::runtime_error( - "Could not load image. Distance instance creation failed."); - } - else - instance = new Distance(image); - } - return *instance; -} - void Distance::setFocalLength(double focalLength) { this->focalLength = focalLength; @@ -74,7 +56,7 @@ void Distance::findFocalLength(const cv::Mat &image) if (image.empty()) { LogManager::logErrorMessage(ErrorType::IMAGE_ERROR, "Could not load image"); - //throw std::runtime_error("Could not open or find the image"); + // throw std::runtime_error("Could not open or find the image"); return; } @@ -135,4 +117,30 @@ void Distance::addDistance(float distance, ObjectInformation &obj) obj.prevDistances.pop_front(); obj.prevDistances.push_back(distance); obj.distance = distance; +} + +void Distance::drawDistance(const shared_ptr image, + const vector &objects) const +{ + int fontFace = FONT_HERSHEY_SIMPLEX; + double fontScale = 0.6; + int thickness = 2; + int baseline = 0; + // Calculate text sizes + Size distanceTextSize = + getTextSize("distance", fontFace, fontScale, thickness, &baseline); + for (auto &obj : objects) { + std::stringstream ssDistance; + ssDistance << std::fixed << std::setprecision(2) << obj.distance; + + Point distanceTextOrg(obj.position.x + 5, + obj.position.y - distanceTextSize.height - 10); + + // Draw outline for distance text + putText(*image, ssDistance.str(), distanceTextOrg, fontFace, fontScale, + Scalar(0, 0, 0), thickness + 3); + // Write the distance text + putText(*image, ssDistance.str(), distanceTextOrg, fontFace, fontScale, + Scalar(255, 255, 255), thickness); + } } \ No newline at end of file diff --git a/img_processing/src/dynamic_tracker.cpp b/img_processing/src/dynamic_tracker.cpp index c1ed7881..1f6ea047 100644 --- a/img_processing/src/dynamic_tracker.cpp +++ b/img_processing/src/dynamic_tracker.cpp @@ -56,3 +56,15 @@ void DynamicTracker::tracking(const shared_ptr &frame, failedCount.erase(failedCount.begin() + idx); } } + +void DynamicTracker::drawTracking(const shared_ptr image, + const vector &objects) const +{ + for (const auto &objectInformation : objects) { + Scalar boxColor = + (objectInformation.distance < (Alerter::MIN_LEGAL_DISTANCE)) + ? Scalar(0, 0, 255) + : Scalar(0, 255, 0); + rectangle(*image, objectInformation.position, boxColor, 2); + } +} diff --git a/img_processing/src/lane_detector.cpp b/img_processing/src/lane_detector.cpp new file mode 100644 index 00000000..e669a861 --- /dev/null +++ b/img_processing/src/lane_detector.cpp @@ -0,0 +1,500 @@ +#include "lane_detector.h" + +using namespace std; +using namespace cv; + +void LaneDetector::init() +{ + this->first = true; + this->image = std::make_shared(); + this->withoutCar = true; +} + +void LaneDetector::manageLaneDetector(std::shared_ptr frame) +{ + if (frame->empty()) { + LogManager::logErrorMessage(ErrorType::IMAGE_ERROR, "No image"); + throw runtime_error("No image!"); + } + + this->image = frame; + this->imgCols = image->cols; + this->imgRows = image->rows; + + // Determine if video is taken during daytime or not + bool isDay = isDayTime(); + + // Filter image + Mat filteredIMG = filterColors(isDay); + + // Apply grayscale + Mat gray = applyGrayscale(filteredIMG); + + // Apply Gaussian blur + Mat gBlur = applyGaussianBlur(gray); + + // Find edges + Mat edges = applyCanny(gBlur); + + // Create mask (Region of Interest) + Mat maskedIMG = RegionOfInterest(edges); + + // Detect straight lines and draw the lanes if possible + std::vector linesP = houghLines(maskedIMG, *image, false); + + // lanes = drawLanes(linesP, check); + bool isdraw = drawLanes(linesP); + + // No path was found and there is no previous path, default drawing + if (!isdraw && first) { + this->drawingInfo.rightX1 = 1207; + this->drawingInfo.rightX2 = 754; + this->drawingInfo.leftX1 = 81; + this->drawingInfo.leftX2 = 505; + this->drawingInfo.y1 = imgRows; + this->drawingInfo.y2 = imgRows * (1 - 0.4); + if (this->withoutCar) { + cutCar(); + } + } + + this->first = false; +} +bool LaneDetector::isDayTime() +{ + /* In general, daytime images/videos require different + color filters than nighttime images/videos. For example, in darker light it is + better to add a gray color filter in addition to the white and yellow one */ + + Scalar s = mean(*image); // Mean pixel values + + /* Cut off values by looking at the mean pixel values of + multiple daytime and nighttime images */ + if (s[0] < 30 || s[1] < 33 && s[2] < 30) { + return false; + } + + return true; +} + +Mat LaneDetector::filterColors(bool isDayTime) +{ + Mat hsv, whiteMask, whiteImage, yellowMask, yellowImage, whiteYellow; + + // White mask + std::vector lowerWhite = {130, 130, 130}; + std::vector upperWhite = {255, 255, 255}; + inRange(*image, lowerWhite, upperWhite, whiteMask); + bitwise_and(*image, *image, whiteImage, whiteMask); + + // Yellow mask + cvtColor(*image, hsv, COLOR_BGR2HSV); + std::vector lowerYellow = {20, 100, 110}; + std::vector upperYellow = {30, 180, 240}; + inRange(hsv, lowerYellow, upperYellow, yellowMask); + bitwise_and(*image, *image, yellowImage, yellowMask); + + // Blend yellow and white together + addWeighted(whiteImage, 1., yellowImage, 1., 0., whiteYellow); + + // Add gray filter if image is not taken during the day + if (!isDayTime) { + // Gray mask + Mat grayMask, grayImage, grayAndWhite, dst; + std::vector lowerGray = {80, 80, 80}; + std::vector upperGray = {130, 130, 130}; + inRange(*image, lowerGray, upperGray, grayMask); + bitwise_and(*image, *image, grayImage, grayMask); + + // Blend gray, yellow and white together and return the result + addWeighted(grayImage, 1., whiteYellow, 1., 0., dst); + return dst; + } + + // Return white and yellow mask if image is taken during the day + return whiteYellow; +} + +Mat LaneDetector::applyGrayscale(Mat source) +{ + Mat dst; + cvtColor(source, dst, COLOR_BGR2GRAY); + + return dst; +} + +Mat LaneDetector::applyGaussianBlur(Mat source) +{ + Mat dst; + GaussianBlur(source, dst, Size(3, 3), 0); + + return dst; +} + +Mat LaneDetector::applyCanny(Mat source) +{ + Mat dst; + + if (CAR_IN_IMAGE >= source.rows) { + LogManager::logErrorMessage(ErrorType::SIZE_ERROR, + "car's size is bigger then image"); + throw runtime_error("car's size is bigger then image"); + } + + Mat roi = source(Rect(0, 0, source.cols, source.rows - CAR_IN_IMAGE)); + + Mat extendedImage(CAR_IN_IMAGE, roi.cols, roi.type(), Scalar(0, 0, 0)); + + vconcat(roi, extendedImage, roi); + + Canny(roi, dst, 50, 150); + + return dst; +} + +Mat LaneDetector::RegionOfInterest(Mat source) +{ + /* In an ideal situation, the ROI should only contain the road lanes. + We want to filter out all the other stuff, including things like arrow road + markings. We try to achieve that by creating two trapezoid masks: one big + trapezoid and a smaller one. The smaller one goes inside the bigger one. The + pixels in the space between them will be kept and all the other pixels will be + masked. If it goes well, the space between the two trapezoids contains only + the lanes. */ + + // Parameters big trapezoid + + // Width of bottom edge of trapezoid, expressed as percentage of image width + float trapezoidBottomWidth = 1.0; + // Above comment also applies here, but then for the top edge of trapezoid + float trapezoidTopWidth = 0.07; + // Height of the trapezoid expressed as percentage of image height + float trapezoidHeight = 0.5; + + // Parameters small trapezoid + + // will be added to trapezoidBottomWidth to create a less wide bottom edge + float smallBottomWidth = 0.45; + // Multiply the percentage trapoezoidTopWidth to create a less wide top edge + float smallTopWidth = 0.3; + // Height of the small trapezoid expressed as percentage of height of big trapezoid + float smallHeight = 1.0; + // Make the trapezoids float just above the bottom edge of the image + float bar = 0.97; + + // Vector which holds all the points of the two trapezoids + std::vector pts; + + // Large trapezoid + pts.push_back(cv::Point((source.cols * (1 - trapezoidBottomWidth)) / 2, + source.rows * bar)); // Bottom left + pts.push_back( + cv::Point((source.cols * (1 - trapezoidTopWidth)) / 2, + source.rows - source.rows * trapezoidHeight)); // Top left + pts.push_back( + cv::Point(source.cols - (source.cols * (1 - trapezoidTopWidth)) / 2, + source.rows - source.rows * trapezoidHeight)); // Top right + pts.push_back( + cv::Point(source.cols - (source.cols * (1 - trapezoidBottomWidth)) / 2, + source.rows * bar)); // Bottom right + + // Small trapezoid + pts.push_back(cv::Point( + (source.cols * (1 - trapezoidBottomWidth + smallBottomWidth)) / 2, + source.rows * bar)); // Bottom left + pts.push_back( + cv::Point((source.cols * (1 - trapezoidTopWidth * smallTopWidth)) / 2, + source.rows - source.rows * trapezoidHeight * + smallHeight)); // Top left + pts.push_back(cv::Point( + source.cols - + (source.cols * (1 - trapezoidTopWidth * smallTopWidth)) / 2, + source.rows - + source.rows * trapezoidHeight * smallHeight)); // Top right + pts.push_back(cv::Point( + source.cols - + (source.cols * (1 - trapezoidBottomWidth + smallBottomWidth)) / 2, + source.rows * bar)); // Bottom right + + // Create the mask + Mat mask = Mat::zeros(source.size(), source.type()); + fillPoly(mask, pts, Scalar(255, 255, 255)); + + /* Put the mask over the source image, + return an all black image, except for the part where the mask image + has nonzero pixels: all the pixels in the space between the two trapezoids */ + Mat maskedImage; + bitwise_and(source, mask, maskedImage); + + return maskedImage; +} + +std::vector LaneDetector::houghLines(Mat canny, Mat source, + bool drawHough) +{ + // Distance resolution in pixels of the Hough grid + double rho = 2; + // Angular resolution in radians of the Hough grid + double theta = 1 * M_PI / 180; + // Minimum number of votes (intersections in Hough grid cell) + int thresh = 15; + // Minimum number of pixels making up a line + double minLineLength = 10; + // Maximum gap in pixels between connectable line segments + double maxGapLength = 20; + + std::vector linesP; // Will hold the results of the detection + HoughLinesP(canny, linesP, rho, theta, thresh, minLineLength, maxGapLength); + if (drawHough) { + for (size_t i = 0; i < linesP.size(); i++) { + Vec4i l = linesP[i]; + line(source, Point(l[0], l[1]), Point(l[2], l[3]), + Scalar(0, 0, 255), 3, LINE_AA); + } + imshow("Hough Lines", source); + waitKey(5); + } + + return linesP; +} + +bool LaneDetector::drawLanes(std::vector lines) +{ + if (lines.size() == 0) { + // There are no lines, use the previous lines + return false; + } + + // variables for current paths- Set drawing lanes to true + bool drawRightLane = true; + bool drawLeftLane = true; + + // Find lines with a slope higher than the slope threshold + float slopeThreshold = 0.5; + std::vector slopes; + std::vector goodLines; + + for (int i = 0; i < lines.size(); i++) { + /* Each line is represented by a 4-element vector (x_1, y_1, x_2, y_2), + where (x_1,y_1) is the line's starting point and (x_2, y_2) is the ending + point */ + Vec4i l = lines[i]; + + double slope; + // Calculate the slope + if (l[2] - l[0] == 0) { // Avoid division by zero + slope = 999; // Infinite slope + } + else { + slope = (l[3] - l[1]) / (l[2] / l[0]); + } + + if (abs(slope) > slopeThreshold) { + slopes.push_back(slope); + goodLines.push_back(l); + } + } + + /* Split the good lines into two categories: right and left + The right lines have a positive slope and the left lines have a negative slope */ + + // Outlines' lanes. + std::vector rightLines; + std::vector leftLines; + int imgCenter = imgCols / 2; + + for (int i = 0; i < slopes.size(); i++) { + if (slopes[i] > 0 && goodLines[i][0] > imgCenter && + goodLines[i][2] > imgCenter) { + rightLines.push_back(goodLines[i]); + } + if (slopes[i] < 0 && goodLines[i][0] < imgCenter && + goodLines[i][2] < imgCenter) { + leftLines.push_back(goodLines[i]); + } + } + + /* form two lane lines out of all the lines we've detected. + A line is defined as 2 points: a starting point and an ending point. + Our goal at this step is to use linear regression to find the two + best fitting lines */ + + // Define the vervs here. need them in the condition. + int y1; + int y2; + int rightX1; + int rightX2; + int leftX1; + int leftX2; + + // right and left lanes + std::vector rightLinesX, rightLinesY, leftLinesX, leftLinesY; + double rightB1, rightB0, leftB1, leftB0; // Slope and intercept + + if (rightLines.size() != 0 && leftLines.size() != 0) { + // We start with the right side points + for (int i = 0; i < rightLines.size(); i++) { + // X of starting point of line + rightLinesX.push_back(rightLines[i][0]); + // X of ending point of line + rightLinesX.push_back(rightLines[i][2]); + // Y of starting point of line + rightLinesY.push_back(rightLines[i][1]); + // Y of ending point of line + rightLinesY.push_back(rightLines[i][3]); + } + + if (rightLinesX.size() > 0) { + std::vector coefRight = estimateCoefficients( + rightLinesX, rightLinesY); // y = b1x + b0 + rightB1 = coefRight[0]; + rightB0 = coefRight[1]; + } + else { + LogManager::logDebugMessage(DebugType::DRAW_PREV, + "drawRightLane false"); + return false; + } + + for (int i = 0; i < leftLines.size(); i++) { + // X of starting point of line + leftLinesX.push_back(leftLines[i][0]); + // X of ending point of line + leftLinesX.push_back(leftLines[i][2]); + // Y of starting point of line + leftLinesY.push_back(leftLines[i][1]); + // Y of ending point of line + leftLinesY.push_back(leftLines[i][3]); + } + if (leftLinesX.size() > 0) { + std::vector coefLeft = + estimateCoefficients(leftLinesX, leftLinesY); + leftB1 = coefLeft[0]; + leftB0 = coefLeft[1]; + } + else { + LogManager::logDebugMessage(DebugType::DRAW_PREV, + "drawLeftLane false"); + return false; + } + + /* Now we need to find the two points for the right and left lane: + starting points and ending points */ + + // Y coordinate of starting point of both the left and right lane + y1 = imgRows; + + /* 0.5 = trapezoidHeight (see RegionOfInterest), we set the y coordinate of + the ending point below the trapezoid height (0.4) to draw shorter lanes. I + think that looks nicer. */ + + // Y coordinate of ending point of both the left and right lane + y2 = imgRows * (1 - 0.4); + + // X coordinate of starting point of right lane + rightX1 = (y1 - rightB0) / rightB1; + // X coordinate of ending point of right lane + rightX2 = (y2 - rightB0) / rightB1; + // X coordinate of starting point of left lane + leftX1 = (y1 - leftB0) / leftB1; + // X coordinate of ending point of left lane + leftX2 = (y2 - leftB0) / leftB1; + + /* If the end point of the paths exceed the boundaries of + the image excessively, return prev drawings. */ + if (!first && (rightX2 < leftX2 || leftX2 > rightX2 || rightX1 < 0 || + rightX2 > imgCols || leftX1 > imgCols || leftX2 < 0 || + rightX1 > imgRows + 350 || leftX1 < -350 || + rightX1 - leftX1 < 300)) { + LogManager::logDebugMessage( + DebugType::DRAW_PREV, + "points outside the boundaries of the image"); + return false; + } + + else { + LogManager::logDebugMessage(DebugType::PRINT, "current path"); + } + + double angleThreshold = 45.0; // 45 degree angle + if (drawRightLane && drawLeftLane) { + double angle = + atan(abs((leftB1 - rightB1) / (1 + leftB1 * rightB1))) * + (180.0 / CV_PI); + + if (angle > angleThreshold && !first) { + LogManager::logDebugMessage(DebugType::DRAW_PREV, + "angle > angleThreshold"); + return false; + } + } + } + + else { + LogManager::logDebugMessage(DebugType::DRAW_PREV, "missLine"); + return false; + } + + // Draw paths + Mat mask = Mat::zeros(image->size(), image->type()); + + drawingInfo.rightX1 = rightX1; + drawingInfo.rightX2 = rightX2; + drawingInfo.leftX1 = leftX1; + drawingInfo.leftX2 = leftX2; + drawingInfo.y1 = y1; + drawingInfo.y2 = y2; + + if (this->withoutCar) { + cutCar(); + } + cout << "cuurent" << endl; + LogManager::logDebugMessage(DebugType::PRINT, "Using current path"); + + return true; +} + +void LaneDetector::drawLane(shared_ptr img) +{ + // Draw a transverse line 120 pixels above the bottom of the image + cv::line(*img, cv::Point(0, img->rows - CAR_IN_IMAGE), + cv::Point(img->cols, img->rows - CAR_IN_IMAGE), + cv::Scalar(255, 255, 255), 4); + + // Draw the lanes on the image using the updated values + cv::line(*img, cv::Point(drawingInfo.rightX1, drawingInfo.y1), + cv::Point(drawingInfo.rightX2, drawingInfo.y2), + cv::Scalar(0, 255, 0), 7); + cv::line(*img, cv::Point(drawingInfo.leftX1, drawingInfo.y1), + cv::Point(drawingInfo.leftX2, drawingInfo.y2), + cv::Scalar(255, 0, 0), 7); + + // Fill the area between the lanes + Point prevPts[4] = {Point(drawingInfo.leftX1, drawingInfo.y1), + Point(drawingInfo.leftX2, drawingInfo.y2), + Point(drawingInfo.rightX2, drawingInfo.y2), + Point(drawingInfo.rightX1, drawingInfo.y1)}; + Mat mask = Mat::zeros(img->size(), img->type()); + fillConvexPoly(mask, prevPts, 4, Scalar(235, 229, 52)); + + // Blend the mask and image together + addWeighted(*img, 0.9, mask, 0.3, 0.0, *img); +} + +void LaneDetector::cutCar() +{ + float rightSlope = static_cast(drawingInfo.y2 - drawingInfo.y1) / + (drawingInfo.rightX2 - drawingInfo.rightX1); + float leftSlope = static_cast(drawingInfo.y2 - drawingInfo.y1) / + (drawingInfo.leftX2 - drawingInfo.leftX1); + + // Acoording to straight formula: y = slope*x + d. + float dRight = drawingInfo.y1 - rightSlope * drawingInfo.rightX1; + float dLeft = drawingInfo.y1 - leftSlope * drawingInfo.leftX1; + drawingInfo.y1 -= CAR_IN_IMAGE; + + // Finding X by placing Y in the equation of the straight line + drawingInfo.rightX1 = + static_cast((drawingInfo.y1 - dRight) / rightSlope); + drawingInfo.leftX1 = static_cast((drawingInfo.y1 - dLeft) / leftSlope); +} diff --git a/img_processing/src/log_manager.cpp b/img_processing/src/log_manager.cpp index 918411af..1aeedb37 100644 --- a/img_processing/src/log_manager.cpp +++ b/img_processing/src/log_manager.cpp @@ -17,6 +17,7 @@ string LogManager::enumTypeToString(T enumType) {ErrorType::DETECTION_ERROR, "DETECTION_ERROR"}, {ErrorType::TRACKING_ERROR, "TRACKING_ERROR"}, {ErrorType::MODEL_ERROR, "MODEL_ERROR"}, + {ErrorType::SIZE_ERROR, "SIZE_ERROR"}, {InfoType::MEDIA_FINISH, "MEDIA_FINISH"}, {InfoType::ALERT_SENT, "ALERT_SENT"}, {InfoType::CHANGED, "CHANGED"}, @@ -28,6 +29,7 @@ string LogManager::enumTypeToString(T enumType) {InfoType::TRACKING, "TRACKING"}, {InfoType::DISTANCE, "DISTANCE"}, {InfoType::MODE, "MODE"}, + {DebugType::DRAW_PREV, "DRAW_PREV"}, {DebugType::PRINT, "PRINT"}}; auto it = enumTypeToStringMap.find(enumType); if (it != enumTypeToStringMap.end()) { diff --git a/img_processing/src/main.cpp b/img_processing/src/main.cpp index 728c7623..92f9cf00 100644 --- a/img_processing/src/main.cpp +++ b/img_processing/src/main.cpp @@ -1,15 +1,94 @@ #include +#include "nlohmann/json.hpp" #include "manager.h" #include "jsonUtils.h" +#include "log_manager.h" +#include "distance.h" +using json = nlohmann::json; using namespace std; using namespace cv; +void runOnVideo(Manager &manager, string videoPath, bool isTravel) +{ + // Convert Windows file path to WSL file path format + if (videoPath.length() >= 3 && videoPath[1] == ':') { + // Convert to lowercase + char driveLetter = tolower(static_cast(videoPath[0])); + videoPath = "/mnt/" + string(1, driveLetter) + videoPath.substr(2); + // Replace backslashes with forward slashes + replace(videoPath.begin(), videoPath.end(), '\\', '/'); + } + // open the video + VideoCapture capture(videoPath); + Mat frame = Mat::zeros(480, 640, CV_8UC3); + if (!capture.isOpened()) { + LogManager::logErrorMessage(ErrorType::VIDEO_ERROR, "video not found"); + throw runtime_error("video not found"); + return; + } + // run on video + while (1) { + capture >> frame; + if (frame.empty()) { + LogManager::logInfoMessage(InfoType::MEDIA_FINISH); + break; + } + int result = manager.processing(frame, isTravel); + if (result == -1) + return; + } +} + +void mainDemo(Manager &manager) +{ + string path; + int focalLength; + bool isTravel; + // Open the JSON file + std::ifstream file("../data.json"); + + if (!file.is_open()) { + LogManager::logErrorMessage(ErrorType::FILE_ERROR, + "Failed to open the file"); + throw runtime_error("Failed to open the file"); + return; + } + + // Read the content of the file into a JSON object + json jsonData; + file >> jsonData; + + // Check if the JSON data is an array + if (jsonData.is_array()) { + // Iterate over each object in the array + for (const auto &obj : jsonData) { + manager.setIterationCnt(1); + if (obj.find("path") != obj.end() && obj["path"].is_string()) { + path = obj["path"]; + } + + if (obj.find("focal_length") != obj.end() && + obj["focal_length"].is_number_integer()) { + focalLength = obj["focal_length"]; + } + + if (obj.find("is_travel") != obj.end() && + obj["is_travel"].is_boolean()) { + isTravel = obj["is_travel"]; + } + Distance *distance = manager.getDistance(); + distance->setFocalLength(focalLength); + runOnVideo(manager, path, isTravel); + } + } +} + int main() { int processID = readFromJson("ID"); Manager manager(processID); manager.init(); - manager.mainDemo(); + mainDemo(manager); return 0; } \ No newline at end of file diff --git a/img_processing/src/manager.cpp b/img_processing/src/manager.cpp index 918b6e3b..85ea1686 100644 --- a/img_processing/src/manager.cpp +++ b/img_processing/src/manager.cpp @@ -1,3 +1,5 @@ +#include +#include #include "manager.h" #include "alert.h" @@ -14,6 +16,16 @@ Manager::Manager(int processID) { } +Distance *Manager::getDistance() +{ + return &distance; +} + +void Manager::setIterationCnt(int cnt) +{ + iterationCnt = cnt; +} + void Manager::init() { string message = "Hello, I'm img_processing " + to_string(processID) + @@ -32,94 +44,38 @@ void Manager::init() LogManager::logErrorMessage(ErrorType::IMAGE_ERROR, "image not found"); return; } - Distance &distance = Distance::getInstance(calibrationImage); + distance.setFocalLength(calibrationImage); iterationCnt = 1; bool isCuda = false; detector.init(isCuda); dynamicTracker.init(); velocity.init(0.04); + laneDetector.init(); + longTime = 0; } -void Manager::mainDemo() +bool Manager::isDetect() { - string filePath = "../data.txt"; - // open the file - ifstream file(filePath); - if (!file.is_open()) { - LogManager::logErrorMessage(ErrorType::FILE_ERROR); - return; - } - string line; - // run over the file and read the lines - while (getline(file, line)) { - // intialize the iteration cnt - iterationCnt = 1; - istringstream iss(line); - string videoPath; - double focalLength; - // read the parameters - if (getline(iss, videoPath, '|') && iss >> focalLength) { - // Trim leading and trailing whitespaces from videoPath - videoPath.erase(0, videoPath.find_first_not_of(" \t\n\r\f\v")); - videoPath.erase(videoPath.find_last_not_of(" \t\n\r\f\v") + 1); - } - else { - LogManager::logErrorMessage(ErrorType::VIDEO_ERROR); - return; - } - // intialize focal length - Distance &distance = Distance::getInstance(); - distance.setFocalLength(focalLength); - runOnVideo(videoPath); - } - cout << "finish reading data"; -} - -void Manager::runOnVideo(string videoPath) -{ - // Convert Windows file path to WSL file path format - if (videoPath.length() >= 3 && videoPath[1] == ':') { - // Convert to lowercase - char driveLetter = tolower(static_cast(videoPath[0])); - videoPath = "/mnt/" + string(1, driveLetter) + videoPath.substr(2); - // Replace backslashes with forward slashes - replace(videoPath.begin(), videoPath.end(), '\\', '/'); - } - // open the video - VideoCapture capture(videoPath); - Mat frame = Mat::zeros(480, 640, CV_8UC3); - if (!capture.isOpened()) { - LogManager::logErrorMessage(ErrorType::VIDEO_ERROR, "video not found"); - throw runtime_error("video not found"); - return; - } - while (1) { - capture >> frame; - if (frame.empty()) { - LogManager::logInfoMessage(InfoType::MEDIA_FINISH); - break; - } - int result = processing(frame, true); - if (result == -1) - return; - } + if (!isTravel || iterationCnt == 1) + return true; + return false; } -bool Manager::isDetect(bool isTravel) +bool Manager::isResetTracker() { - if (!isTravel || iterationCnt == 1) + if (isTravel && iterationCnt == 1) return true; return false; } -bool Manager::isResetTracker(bool isTravel) +bool Manager::isTrack() { - if (isTravel && iterationCnt == 1) + if (isTravel && iterationCnt > 1) return true; return false; } -bool Manager::isTrack(bool isTravel) +bool Manager::isCalcVelocity() { if (isTravel && iterationCnt > 1) return true; @@ -128,117 +84,96 @@ bool Manager::isTrack(bool isTravel) int Manager::processing(const Mat &newFrame, bool isTravel) { - Distance &distance = Distance::getInstance(); + this->isTravel = isTravel; currentFrame = make_shared(newFrame); - if (isDetect(isTravel)) { + if (isDetect()) { // send the frame to detect detector.detect(this->currentFrame, isTravel); this->currentOutput = detector.getOutput(); } - if (isResetTracker(isTravel)) { + if (isResetTracker()) { // prepare the tracker dynamicTracker.startTracking(this->currentFrame, this->currentOutput); } - if (isTrack(isTravel)) { + if (isTrack()) { // send the frame to track dynamicTracker.tracking(this->currentFrame, this->currentOutput); } // add distance to detection objects distance.findDistance(this->currentOutput); - velocity.returnVelocities(this->currentOutput); + if (isCalcVelocity()) { + velocity.returnVelocities(this->currentOutput); + } +#ifdef DETECT_SUN + sunDetector.detectSun(this->currentFrame); +#endif // send allerts to main control - vector> alerts = alerter.sendAlerts(this->currentOutput); - sendAlerts(alerts); + if (isCalcVelocity()) { + vector> alerts = + alerter.sendAlerts(this->currentOutput); + sendAlerts(alerts); + } // update of the iterationCnt if (isTravel) { iterationCnt = iterationCnt % NUM_OF_TRACKING + 1; } - // visual - drawOutput(); - #ifdef SHOW_FRAMES - drawOutput(); - imshow("aaa", *currentFrame); - int key = cv::waitKey(1); - if (key == ESC) { - return -1; - } - return 1; - #endif +#ifdef LANE_DETECT + laneDetector.manageLaneDetector(this->currentFrame); +#endif +// visual +#ifdef SHOW_FRAMES + if (drawOutput() == 27) + return -1; + return 1; +#endif } -void Manager::drawOutput() +int Manager::drawOutput() { - for (ObjectInformation objectInformation : currentOutput) { - int topLeftX = objectInformation.position.x; - int topLeftY = objectInformation.position.y; - - // Draw rectangle around object - Scalar boxColor = - (objectInformation.distance < (alerter.MIN_LEGAL_DISTANCE)) - ? Scalar(0, 0, 255) - : Scalar(0, 255, 0); - rectangle(*currentFrame, objectInformation.position, boxColor, 2); - - // Define text for distance and velocity - std::stringstream ssDistance, ssVelocity; - ssDistance << std::fixed << std::setprecision(2) - << objectInformation.distance; - ssVelocity << std::fixed << std::setprecision(2) - << objectInformation.velocity; - - std::string distanceText = ssDistance.str(); - std::string velocityText = ssVelocity.str(); - - // Font properties - int fontFace = FONT_HERSHEY_SIMPLEX; - double fontScale = 0.6; - int thickness = 1; - int baseline = 0; - - // Calculate text sizes - Size distanceTextSize = getTextSize(distanceText, fontFace, fontScale, - thickness, &baseline); - Size velocityTextSize = getTextSize(velocityText, fontFace, fontScale, - thickness, &baseline); - - // Positions for the texts - Point distanceTextOrg(topLeftX + 5, topLeftY - velocityTextSize.height - - 7); // Above the object - Point velocityTextOrg(topLeftX + 5, topLeftY - 5); // Above the object - - // Draw outline for distance text - putText(*currentFrame, distanceText, distanceTextOrg, fontFace, - fontScale, Scalar(0, 0, 0), thickness + 2); - // Write the distance text - putText(*currentFrame, distanceText, distanceTextOrg, fontFace, - fontScale, Scalar(255, 255, 255), thickness); - - // Draw outline for velocity text - putText(*currentFrame, velocityText, velocityTextOrg, fontFace, - fontScale, Scalar(0, 0, 0), thickness + 2); - // Write the velocity text - putText(*currentFrame, velocityText, velocityTextOrg, fontFace, - fontScale, Scalar(255, 0, 0), thickness); - } + dynamicTracker.drawTracking(currentFrame, currentOutput); + distance.drawDistance(currentFrame, currentOutput); + if (isCalcVelocity()) + velocity.drawVelocity(currentFrame, currentOutput); +#ifdef DETECT_SUN + sunDetector.drowSun(currentFrame); +#endif +#ifdef LANE_DETECT + laneDetector.drawLane(currentFrame); +#endif // Legend int legendX = 10, legendY = 10; - putText(*currentFrame, "Legend:", Point(legendX, legendY), + + // Draw a black border around the legend + rectangle(*currentFrame, Point(legendX - 10, legendY - 10), + Point(legendX + 162, legendY + 72), Scalar(0, 0, 0), 2); + + // Draw a black rectangle as background for the legend + rectangle(*currentFrame, Point(legendX - 8, legendY - 8), + Point(legendX + 160, legendY + 70), Scalar(150, 150, 150), + FILLED); + + // Draw the legend text and colors + putText(*currentFrame, "Legend:", Point(legendX, legendY + 7), FONT_HERSHEY_SIMPLEX, 0.6, Scalar(255, 255, 255), 1); - rectangle(*currentFrame, Point(legendX, legendY + 10), - Point(legendX + 10, legendY + 30), Scalar(255, 255, 255), FILLED); - putText(*currentFrame, "Distance", Point(legendX + 15, legendY + 25), + rectangle(*currentFrame, Point(legendX, legendY + 17), + Point(legendX + 10, legendY + 37), Scalar(255, 255, 255), FILLED); + putText(*currentFrame, "Distance", Point(legendX + 15, legendY + 37), FONT_HERSHEY_SIMPLEX, 0.6, Scalar(255, 255, 255), 1); - rectangle(*currentFrame, Point(legendX, legendY + 35), - Point(legendX + 10, legendY + 55), Scalar(255, 0, 0), FILLED); - putText(*currentFrame, "velocity", Point(legendX + 15, legendY + 50), + rectangle(*currentFrame, Point(legendX, legendY + 47), + Point(legendX + 10, legendY + 62), Scalar(255, 255, 0), FILLED); + putText(*currentFrame, "Velocity", Point(legendX + 15, legendY + 62), FONT_HERSHEY_SIMPLEX, 0.6, Scalar(255, 255, 255), 1); + + imshow("Output", *currentFrame); + int key = waitKey(1); + return key; } void Manager::sendAlerts(vector> &alerts) diff --git a/img_processing/src/sun_detector.cpp b/img_processing/src/sun_detector.cpp new file mode 100644 index 00000000..d91f5e2d --- /dev/null +++ b/img_processing/src/sun_detector.cpp @@ -0,0 +1,74 @@ +#include "sun_detector.h" + +using namespace std; +using namespace cv; + +void SunDetector::detectSun(const std::shared_ptr &frame) +{ + isSun = false; + // Convert the frame to grayscale for easier processing + Mat image; + cvtColor(*frame, image, COLOR_BGR2GRAY); + // Calculate the histogram of the grayscale image + Mat histogram; + int histSize = 256; + float range[] = {0, 256}; + const float *histRange = {range}; + calcHist(&image, 1, 0, Mat(), histogram, 1, &histSize, &histRange); + // Compute the cumulative distribution function (CDF) from the histogram + Mat cdf; + histogram.copyTo(cdf); + for (int i = 1; i < histSize; i++) { + cdf.at(i) += cdf.at(i - 1); + } + cdf /= cdf.at(histSize - 1); // Normalize the CDF + // Determine a threshold based on the 95th percentile of pixel intensities + double minVal, maxVal; + minMaxLoc(image, &minVal, &maxVal); // Find min and max pixel intensities + Mat percentileThreshold; + threshold(image, percentileThreshold, maxVal * 0.95, 255, THRESH_BINARY); + int thresholdArea = cv::countNonZero(percentileThreshold); + // Detect contours in the thresholded image (for bright regions) + std::vector> contours; + findContours(percentileThreshold, contours, RETR_EXTERNAL, + CHAIN_APPROX_SIMPLE); + // Find the largest contour in terms of area (which could be the sun) + double maxArea = 0; + int maxAreaIdx = -1; + for (size_t i = 0; i < contours.size(); i++) { + double area = contourArea(contours[i]); + if (area > maxArea) { + maxArea = area; + maxAreaIdx = i; + } + } + // If a valid contour was found, proceed with further analysis + if (maxAreaIdx != -1) { + // Draw a circle around the largest contour to visualize it + minEnclosingCircle(contours[maxAreaIdx], center, radius); + // Calculate image gradients (Sobel) to analyze the smoothness of the region + Mat gradX, gradY; + Sobel(image, gradX, CV_32F, 1, 0); // Gradient in X direction + Sobel(image, gradY, CV_32F, 0, 1); // Gradient in Y direction + // Calculate the gradient magnitude (intensity of edges) + Mat gradientMag; + magnitude(gradX, gradY, gradientMag); // sqrt(gradX^2 + gradY^2) + // Compute the average gradient magnitude in the bright region (contour + // area) + Scalar meanGradient = mean(gradientMag, percentileThreshold); + // If the gradient magnitude is low, the area is likely smooth, like the sun + if (meanGradient[0] < 7 && + thresholdArea < 35000) // Adjust this threshold as needed + { + isSun = true; + } + } +} + +void SunDetector::drowSun(std::shared_ptr &frame) +{ + if (isSun) { + // Draw a green circle around the detected sun region + circle(*frame, center, static_cast(radius), Scalar(0, 255, 0), 2); + } +} diff --git a/img_processing/src/velocity.cpp b/img_processing/src/velocity.cpp index adbcbcd0..fce6fb08 100644 --- a/img_processing/src/velocity.cpp +++ b/img_processing/src/velocity.cpp @@ -33,7 +33,7 @@ float Velocity::averageDistanceChange(ObjectInformation obj) const return totalChange / (obj.prevDistances.size() - 1); } -//Function to update velocity while maintaining the last two velocities +// Function to update velocity while maintaining the last two velocities void Velocity::updateVelocity(float newVelocity, ObjectInformation &obj) { // If we have at least one previous velocity @@ -53,10 +53,12 @@ void Velocity::updateVelocity(float newVelocity, ObjectInformation &obj) float secondLastVelocity = obj.prevVelocities[obj.prevVelocities.size() - 2]; - // Check if the new velocity matches the sign of the second-last velocity + // Check if the new velocity matches the sign of the second-last + // velocity if ((newVelocity >= 0 && secondLastVelocity >= 0) || (newVelocity < 0 && secondLastVelocity < 0)) { - // If the new velocity's sign matches the second-last velocity, we accept it + // If the new velocity's sign matches the second-last velocity, we + // accept it obj.velocity = newVelocity; } else { @@ -65,7 +67,8 @@ void Velocity::updateVelocity(float newVelocity, ObjectInformation &obj) } } else { - // If there's only one previous velocity, we keep it as it’s unclear if the new one is valid + // If there's only one previous velocity, we keep it as it’s unclear if + // the new one is valid obj.velocity = lastVelocity; } } @@ -79,6 +82,46 @@ void Velocity::updateVelocity(float newVelocity, ObjectInformation &obj) // Keep only the last two velocities in the deque if (obj.prevVelocities.size() > MAX_PREV_VELOCITIES_SIZE) { obj.prevVelocities - .pop_front(); // Remove the oldest velocity if the deque exceeds the limit + .pop_front(); // Remove the oldest velocity if the deque + // exceeds the limit + } +} + +void Velocity::drawVelocity(const shared_ptr image, + const vector &objects) const +{ + // Font properties + int fontFace = FONT_HERSHEY_SIMPLEX; + double fontScale = 0.6; + int thickness = 2; + int baseline = 0; + + // Calculate text sizes + Size velocityTextSize = + getTextSize("velocity", fontFace, fontScale, thickness, &baseline); + + for (auto &obj : objects) { + // Check if velocity has a value + if (obj.velocity.has_value()) { + std::stringstream ssVelocity; + ssVelocity << std::fixed << std::setprecision(2) + << obj.velocity.value(); + + Point velocityTextOrg(obj.position.x + 5, obj.position.y - 7); + + // Draw outline for velocity text + putText(*image, ssVelocity.str(), velocityTextOrg, fontFace, + fontScale, Scalar(0, 0, 0), thickness + 3); + // Write the velocity text + putText(*image, ssVelocity.str(), velocityTextOrg, fontFace, + fontScale, Scalar(255, 255, 0), thickness); + } + else { + // Optional: Handle the case where velocity is not set + // For example, you can draw a placeholder or skip drawing. + Point velocityTextOrg(obj.position.x + 5, obj.position.y - 7); + putText(*image, "N/A", velocityTextOrg, fontFace, fontScale, + Scalar(255, 0, 0), thickness); // Draw "N/A" in red + } } } diff --git a/img_processing/tests/images/sun_in_street.mov b/img_processing/tests/images/sun_in_street.mov new file mode 100644 index 00000000..36888cd6 Binary files /dev/null and b/img_processing/tests/images/sun_in_street.mov differ diff --git a/img_processing/tests/test_distance.cpp b/img_processing/tests/test_distance.cpp index a11be2df..2ef1f5fa 100644 --- a/img_processing/tests/test_distance.cpp +++ b/img_processing/tests/test_distance.cpp @@ -1,10 +1,9 @@ #include #include #include "detector.h" -#include "manager.h" #include "distance.h" -#include "manager.h" #include "log_manager.h" +#include "manager.h" using namespace std; using namespace cv; @@ -21,8 +20,8 @@ TEST(DistanceTest, DistanceWithCalibration) throw runtime_error("Could not open or find the image"); } - Distance &distance = Distance::getInstance(calibrationImage); - + Distance distance; + distance.setFocalLength(calibrationImage); // Load a real image from file string imagePath2 = "../tests/images/parking_car.JPG"; Mat carImage; diff --git a/img_processing/tests/test_lane_detector.cpp b/img_processing/tests/test_lane_detector.cpp new file mode 100644 index 00000000..95075b8f --- /dev/null +++ b/img_processing/tests/test_lane_detector.cpp @@ -0,0 +1,42 @@ +#include +#include +#include +#include "lane_detector.h" +#include "log_manager.h" + +using namespace std; +using namespace cv; + +TEST(TLaneDetector, CheckRun) +{ + cout << "start" << endl; + string path = "/home/tirtza/train6/171.mov"; + VideoCapture cap(path); + + if (!cap.isOpened()) { + + LogManager::logErrorMessage(ErrorType::VIDEO_ERROR, "Could not load video"); + + throw runtime_error("Could not load video"); + } + + LaneDetector laneDetector; + + laneDetector.init(); + + while(true){ + + Mat frame; + cap >> frame; + + auto sharedFrame = make_shared(frame); + + laneDetector.manageLaneDetector(sharedFrame); + laneDetector.drawLane(sharedFrame); + + imshow("lane-detection",*sharedFrame); + if (waitKey() == 27) break; + } + + cap.release(); +} \ No newline at end of file diff --git a/img_processing/tests/test_manager.cpp b/img_processing/tests/test_manager.cpp index dfd47ec6..f5ec4ed2 100644 --- a/img_processing/tests/test_manager.cpp +++ b/img_processing/tests/test_manager.cpp @@ -6,10 +6,10 @@ using namespace std; using namespace cv; -TEST(ManagerTest, tryManager) -{ - int processID = readFromJson("ID"); - Manager manager(processID); - manager.init(); - manager.mainDemo(); -} \ No newline at end of file +// TEST(ManagerTest, tryManager) +// { +// int processID = readFromJson("ID"); +// Manager manager(processID); +// manager.init(); +// manager.mainDemo(); +// } \ No newline at end of file diff --git a/img_processing/tests/test_sun_detector.cpp b/img_processing/tests/test_sun_detector.cpp new file mode 100644 index 00000000..1fb9856c --- /dev/null +++ b/img_processing/tests/test_sun_detector.cpp @@ -0,0 +1,37 @@ +#include +#include +#include "sun_detector.h" + +using namespace std; +using namespace cv; + +TEST(SunDetectorTest, detectSun) +{ + VideoCapture capture("../tests/images/sun_in_street.mov"); + + if (!capture.isOpened()) { + // LogManager::logErrorMessage(ErrorType::VIDEO_ERROR, "video not found"); + // throw runtime_error("video not found"); + cout << "couldn't open video" << endl; + return; + } + Mat img; + SunDetector sd; + while (1) { + capture >> img; + + if (img.empty()) { + // LogManager::logInfoMessage(InfoType::MEDIA_FINISH); + cout << "media finish" << endl; + break; + } + shared_ptr frame = make_shared(img); + sd.detectSun(frame); + sd.drowSun(frame); + imshow("sun", *frame); + int key = waitKey(1); + if (key == 27) { + break; + } + } +} \ No newline at end of file diff --git a/img_processing/tests/test_velocity.cpp b/img_processing/tests/test_velocity.cpp index 25c4a671..f96ce461 100644 --- a/img_processing/tests/test_velocity.cpp +++ b/img_processing/tests/test_velocity.cpp @@ -1,11 +1,11 @@ +#include #include #include #include -#include -#include "velocity.h" -#include "dynamic_tracker.h" #include "detector.h" #include "distance.h" +#include "dynamic_tracker.h" +#include "velocity.h" using namespace std; using namespace cv; @@ -19,9 +19,8 @@ TEST(TVelocity, calculate_TVelocity) if (calibrationImage.empty()) { throw runtime_error("Could not open or find the image"); } - - Distance &distance = Distance::getInstance(calibrationImage); - + Distance distance; + distance.setFocalLength(calibrationImage); Detector detector; DynamicTracker tracker; Velocity velocity; @@ -58,7 +57,8 @@ TEST(TVelocity, calculate_TVelocity) Point textPosition((*trackingOutput)[i].position.x, (*trackingOutput)[i].position.y - 10); - // putText(*frame1, "Speed: " + std::to_string((*trackingOutput)[i].speed), textPosition, + // putText(*frame1, "Speed: " + + // std::to_string((*trackingOutput)[i].speed), textPosition, /// FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 0, 0), 2); putText( *frame1,