Visibility Estimator

.gitignore

@@ -0,0 +1,32 @@
+# Prerequisites
+*.d
+
+# Compiled Object files
+*.slo
+*.lo
+*.o
+*.obj
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Compiled Dynamic libraries
+*.so
+*.dylib
+*.dll
+
+# Fortran module files
+*.mod
+*.smod
+
+# Compiled Static libraries
+*.lai
+*.la
+*.a
+*.lib
+
+# Executables
+*.exe
+*.out
+*.app

VisibilityEstimator/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.23)
+project(VisibilityEstimator)
+
+set(CMAKE_CXX_STANDARD 23)
+set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} "${CMAKE_SOURCE_DIR}/cmake/")
+
+find_package(OpenCV REQUIRED)
+include_directories(${OpenCV_INCLUDE_DIRS})
+
+add_executable(VisibilityEstimator
+        src/main.cpp
+        src/contourDetector.cpp
+        src/contourDetector.h
+        src/contourDrawer.cpp
+        src/contourDrawer.h
+        src/contourMerger.cpp
+        src/contourMerger.h
+        src/contourOrdering.cpp
+        src/contourOrdering.h
+        src/distanceEstimator.cpp
+        src/distanceEstimator.h
+        src/edgeDetector.cpp
+        src/edgeDetector.h
+        src/imageFilter.cpp
+        src/imageFilter.h
+        src/inversePerspectiveMapping.cpp
+        src/inversePerspectiveMapping.h)
+
+set(OpenCV_LIBS
+        opencv_highgui
+        opencv_imgproc
+        opencv_imgcodecs)
+
+target_link_libraries(VisibilityEstimator ${OpenCV_LIBS})

VisibilityEstimator/src/contourDetector.cpp

@@ -0,0 +1,29 @@
+#include "opencv2/imgproc.hpp"
+#include "opencv2/imgcodecs.hpp"
+
+#include "contourDetector.h"
+#include "imageFilter.h"
+#include "contourMerger.h"
+
+namespace edge_detector
+{
+    std::vector<std::vector<cv::Point>> ContourDetector::detectContours(const cv::Mat& image)
+    {
+        std::vector<std::vector<cv::Point>> contours;
+
+        cv::Mat cannyImage = ImageFilter::applyCannyOperator(image);
+        findContours(cannyImage, contours, cv::RETR_LIST, cv::CHAIN_APPROX_NONE);
+
+        if (contours.empty())
+        {
+            return contours;
+        }
+
+        auto mergedContours = ContourMerger::connectContours(contours, image.cols, image.rows);
+
+        cannyImage.release();
+        contours.clear();
+
+        return mergedContours;
+    }
+}

VisibilityEstimator/src/contourDetector.h

@@ -0,0 +1,20 @@
+#pragma once
+
+#include "opencv2/opencv.hpp"
+
+namespace edge_detector
+{
+    /**
+     * @brief Class that provides a method for detecting contours on the image.
+    */
+    class ContourDetector
+    {
+    public:
+        /**
+         * @brief Detects contours using Canny, Suzuki algorithm. And them applies to them algorithm for merging.
+         * @param image is the image on which contours should be detected.
+         * @return Vector of detected contours.
+        */
+        static std::vector<std::vector<cv::Point>> detectContours(const cv::Mat& image);
+    };
+}

VisibilityEstimator/src/contourDrawer.cpp

@@ -0,0 +1,24 @@
+#include "opencv2/imgproc.hpp"
+
+#include "contourDrawer.h"
+
+namespace edge_detector
+{
+    void ContourDrawer::colorContours(const cv::Mat& frame, const std::vector<std::vector<cv::Point>>& contours)
+    {
+        for (int idx = 0; idx < contours.size(); idx++)
+        {
+            cv::Scalar color(rand() & 255, rand() & 255, rand() & 255);
+            drawContours(frame, contours, idx, color, 1, 8);
+        }
+    }
+
+    void ContourDrawer::drawHorizontalLine(const cv::Mat& frame, cv::Point point)
+    {
+        cv::Point p1(0, point.y);
+        cv::Point p2(frame.cols, point.y);
+        int thickness = 1;
+
+        line(frame, p1, p2, cv::Scalar(0, 0, 255), thickness, cv::LINE_4);
+    }
+}

VisibilityEstimator/src/contourDrawer.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#include "opencv2/opencv.hpp"
+
+namespace edge_detector
+{
+    /**
+     * @brief Class for debugging purposes. Provides a set of methods for drawing contours on the image.
+    */
+    class ContourDrawer
+    {
+    public:
+        static void colorContours(const cv::Mat& frame, const std::vector<std::vector<cv::Point>>& contours);
+
+        static void drawHorizontalLine(const cv::Mat& frame, cv::Point point);
+    };
+}

VisibilityEstimator/src/contourMerger.cpp

@@ -0,0 +1,145 @@
+#include "opencv2/imgcodecs.hpp"
+
+#include <filesystem>
+#include <algorithm>
+#include <set>
+#include <utility>
+
+#include "contourOrdering.h"
+#include "contourMerger.h"
+
+namespace edge_detector
+{
+    std::vector<std::vector<cv::Point>> ContourMerger::connectContours(
+        const std::vector<std::vector<cv::Point>>& contours,
+        int imageSizeX,
+        int imageSizeY)
+    {
+        // Making matrix where points are -1 if they don't belong to any contour, and they can be positive,
+        // if they belong to any contour; and positive value is the index of the contour they belong to.
+        std::vector<std::vector<int>> indexMatrix(imageSizeX, std::vector(imageSizeY, -1));
+        for (int contour = 0; contour < contours.size(); contour++)
+        {
+            for (int pointIndex = 0; pointIndex < contours[contour].size(); pointIndex++)
+            {
+                indexMatrix[contours[contour][pointIndex].x][contours[contour][pointIndex].y] = contour;
+            }
+        }
+
+        // Sorting by values the dictionary where key is contour index, value is contour size.
+        auto sortedContoursIndices = ContourOrdering::sortContoursByTheirSizesDescending(contours);
+
+        // Merging contours.
+        std::vector<std::vector<cv::Point>> newContours(contours);
+        for (int i = 0; i < (sortedContoursIndices.size() < 15 ? sortedContoursIndices.size() : 15); i++)
+        {
+            newContours = mergeContours(newContours, indexMatrix, sortedContoursIndices[i], contours[sortedContoursIndices[i]]);
+        }
+
+        indexMatrix.clear();
+        sortedContoursIndices.clear();
+
+        return removeZeroSizeContours(newContours);
+    }
+
+    std::set<int> ContourMerger::findNearestContours(
+        const std::vector<std::vector<cv::Point>>& contours,
+        int mainContourIndex,
+        const std::vector<std::vector<int>>& contourIndexMatrix,
+        cv::Point point)
+    {
+        std::set<int> foundContours;
+        const int neighborhoodSize = 5;
+
+        for (int xDiff = -neighborhoodSize; xDiff <= neighborhoodSize; xDiff++)
+        {
+            for (int yDiff = -neighborhoodSize; yDiff <= neighborhoodSize; yDiff++)
+            {
+                if (point.x + xDiff <= 0 || point.y + yDiff <= 0
+                    || point.x + xDiff >= contourIndexMatrix.size() || point.y + yDiff >= contourIndexMatrix[0].size())
+                {
+                    continue;
+                }
+                auto contourIndex = contourIndexMatrix[point.x + xDiff][point.y + yDiff];
+                if (contourIndex != -1 && !contours[contourIndex].empty() && contourIndex != mainContourIndex)
+                {
+                    foundContours.insert(contourIndex);
+                }
+            }
+        }
+
+        return foundContours;
+    }
+
+    // Returns vector where some contours have been merged.
+    std::vector<std::vector<cv::Point>> ContourMerger::mergeContours(
+        const std::vector<std::vector<cv::Point>>& contours,
+        const std::vector<std::vector<int>>& indexMatrix,
+        int mainContourIndex,
+        const std::vector<cv::Point>& mainContour)
+    {
+        auto pointWithMaxY = ContourOrdering::findPointWithMaxY(std::move(mainContour));
+
+        auto nearestContours = findNearestContours(contours, mainContourIndex, indexMatrix, pointWithMaxY);
+
+        if (nearestContours.empty())
+        {
+            return contours;
+        }
+
+        std::vector<cv::Point> zeroSizeVector;
+        std::vector<std::vector<cv::Point>> mergedContours;
+        mergedContours.reserve(contours.size() - nearestContours.size());
+        for (auto i = 0; i < std::ssize(contours); i++)
+        {
+            if (i == mainContourIndex)
+            {
+                auto sizeOfMergedContour = std::ssize(contours[mainContourIndex]);
+                for (auto contourIndex : nearestContours)
+                {
+                    sizeOfMergedContour += std::ssize(contours[contourIndex]);
+                }
+
+                std::vector<cv::Point> newContour;
+                newContour.reserve(sizeOfMergedContour);
+                newContour.insert(newContour.end(), contours[mainContourIndex].begin(), contours[mainContourIndex].end());
+                for (auto neighbor : nearestContours)
+                {
+                    newContour.insert(newContour.end(), contours[neighbor].begin(), contours[neighbor].end());
+                }
+
+                mergedContours.push_back(newContour);
+
+                continue;
+            }
+
+            if (nearestContours.find(i) != nearestContours.end())
+            {
+                mergedContours.push_back(zeroSizeVector);
+                continue;
+            }
+
+            mergedContours.push_back(contours[i]);
+        }
+
+        return mergeContours(mergedContours, indexMatrix, mainContourIndex, mergedContours[mainContourIndex]);
+    }
+
+    std::vector<std::vector<cv::Point>> ContourMerger::removeZeroSizeContours(std::vector<std::vector<cv::Point>> contours)
+    {
+        auto iter = contours.begin();
+        while (iter != contours.end())
+        {
+            if (iter->empty())
+            {
+                iter->clear();
+                iter = contours.erase(iter);
+                continue;
+            }
+
+            advance(iter, 1);
+        }
+
+        return contours;
+    }
+}

VisibilityEstimator/src/contourMerger.h

@@ -0,0 +1,59 @@
+#pragma once
+
+#include "opencv2/imgproc.hpp"
+
+#include <string>
+#include <set>
+
+namespace edge_detector
+{
+    /**
+     * @brief Class that provides a set of methods for connecting broken contours.
+    */
+    class ContourMerger
+    {
+    public:
+        /**
+         * @brief Applies contour merging algorithm to the longest contours.
+         * @param contours Vector of contours to merge.
+         * @param imageSizeX is the image width.
+         * @param imageSizeY is the image height.
+         * @return Vector of connected contours.
+        */
+        static std::vector<std::vector<cv::Point>> connectContours(
+            const std::vector<std::vector<cv::Point>>& contours,
+            int imageSizeX,
+            int imageSizeY);
+
+        /**
+         * @brief Finds nearest contours in square neighborhood 5x5 pixels around the given point.
+         * @param contours Vector of contours.
+         * @param mainContourIndex Index of the contour to which the point belongs.
+         * @param contourIndexMatrix Matrix which elements are contour indices.
+         * @param point Point around which the search is made.
+         * @return Set of nearest contours.
+        */
+        static std::set<int> findNearestContours(
+            const std::vector<std::vector<cv::Point>>& contours,
+            int mainContourIndex,
+            const std::vector<std::vector<int>>& contourIndexMatrix,
+            cv::Point point);
+
+        /**
+         * @brief Finds the highest point of a contour and merges origin contour with contours found around this point.
+         * @param contours Vector of contours.
+         * @param indexMatrix Matrix which elements are contour indices.
+         * @param mainContourIndex Index of contour that should be merged with nearest contours.
+         * @param mainContour Contour that should be merged with nearest contours
+         * @return Vector of connected contours.
+        */
+        static std::vector<std::vector<cv::Point>> mergeContours(
+            const std::vector<std::vector<cv::Point>>& contours,
+            const std::vector<std::vector<int>>& indexMatrix,
+            int mainContourIndex,
+            const std::vector<cv::Point>& mainContour);
+
+    private:
+        static std::vector<std::vector<cv::Point>> removeZeroSizeContours(std::vector<std::vector<cv::Point>> contours);
+    };
+}