Img processing/improve velocity

img_processing/include/distance.h

@@ -9,16 +9,13 @@
 class Distance {
    public:
     void findDistance(std::vector<ObjectInformation> &objectInformation);
-    static Distance &getInstance(const cv::Mat &image = cv::Mat());
+    void setFocalLength(const cv::Mat &image);
     void setFocalLength(double focalLength);
+    void drawDistance(std::shared_ptr<cv::Mat> image,
+                      std::vector<ObjectInformation> &objects);
 
    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);
 };

img_processing/include/dynamic_tracker.h

@@ -12,6 +12,8 @@ class DynamicTracker {
                        const std::vector<ObjectInformation> &detectionOutput);
     void tracking(const std::shared_ptr<cv::Mat> &frame,
                   std::vector<ObjectInformation> &objectInformation);
+    void drawTracking(std::shared_ptr<cv::Mat> image,
+                      std::vector<ObjectInformation> &objects);
 
    private:
     std::shared_ptr<cv::Mat> frame;

img_processing/include/manager.h

@@ -29,18 +29,22 @@ class Manager {
     Detector detector;
     Velocity velocity;
     DynamicTracker dynamicTracker;
+    Distance distance;
     Alerter alerter;
     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<std::vector<uint8_t>> &alerts);
     void runOnVideo(std::string videoPath);
     int readIdFromJson(const char *target);

img_processing/include/object_information_struct.h

@@ -3,9 +3,10 @@
 
 #include <opencv2/opencv.hpp>
 #include "object_type_enum.h"
+#include <optional>
 
 #define MAX_PREV_DISTANCES_SIZE 10
-#define MAX_PREV_VELOCITIES_SIZE 2
+#define MAX_PREV_VELOCITIES_SIZE 10
 
 struct ObjectInformation {
     int id;
@@ -15,7 +16,7 @@ struct ObjectInformation {
     std::deque<float> prevDistances;
     float distance;
     std::deque<float> prevVelocities;
-    float velocity;
+    std::optional<float> velocity = std::nullopt;
 };
 
 #endif  // __OBJECT_INFORMATION_STRUCT_H__

img_processing/include/velocity.h

@@ -2,18 +2,21 @@
 #define __VELOCITY_H__
 
 #include <opencv2/opencv.hpp>
+#include <numeric>
 #include "object_information_struct.h"
 
 class Velocity {
    public:
     Velocity() {}
     void returnVelocities(std::vector<ObjectInformation> &objects);
     void init(double frameTimeDiff);
+    void drawVelocity(const std::shared_ptr<cv::Mat> image,
+                      const std::vector<ObjectInformation> &objects) const;
 
    private:
     double frameTimeDiff;
     void calculateVelocity(ObjectInformation &object);
-    float averageDistanceChange(ObjectInformation obj) const;
-    void updateVelocity(float newVelocity, ObjectInformation &obj);
+    float averageDistanceChange(const ObjectInformation &obj) const;
+    void smoothAndUpdateVelocity(float newVelocity, ObjectInformation &obj);
 };
 #endif  //__VELOCITY_H__

img_processing/src/alerter.cpp

@@ -31,7 +31,7 @@ vector<vector<uint8_t>> Alerter::sendAlerts(
         if (isSendAlert(objectInformation)) {
             vector<uint8_t> alertBuffer = makeAlertBuffer(
                 objectInformation.type, objectInformation.distance,
-                objectInformation.velocity);
+                objectInformation.velocity.value());
             alerts.push_back(alertBuffer);
         }
     }

img_processing/src/distance.cpp

@@ -10,28 +10,10 @@
 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;
@@ -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(shared_ptr<Mat> image,
+                            vector<ObjectInformation> &objects)
+{
+    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);
+    }
 }

img_processing/src/dynamic_tracker.cpp

@@ -56,3 +56,15 @@ void DynamicTracker::tracking(const shared_ptr<Mat> &frame,
         failedCount.erase(failedCount.begin() + idx);
     }
 }
+
+void DynamicTracker::drawTracking(shared_ptr<Mat> image,
+                                  vector<ObjectInformation> &objects)
+{
+    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);
+    }
+}

img_processing/src/manager.cpp

@@ -31,7 +31,7 @@ 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);
@@ -67,7 +67,6 @@ void Manager::mainDemo()
             return;
         }
         // intialize focal length
-        Distance &distance = Distance::getInstance();
         distance.setFocalLength(focalLength);
         runOnVideo(videoPath);
     }
@@ -104,21 +103,28 @@ void Manager::runOnVideo(string videoPath)
     }
 }
 
-bool Manager::isDetect(bool isTravel)
+bool Manager::isDetect()
 {
     if (!isTravel || iterationCnt == 1)
         return true;
     return false;
 }
 
-bool Manager::isResetTracker(bool isTravel)
+bool Manager::isResetTracker()
 {
     if (isTravel && iterationCnt == 1)
         return true;
     return false;
 }
 
-bool Manager::isTrack(bool isTravel)
+bool Manager::isTrack()
+{
+    if (isTravel && iterationCnt > 1)
+        return true;
+    return false;
+}
+
+bool Manager::isCalcVelocity()
 {
     if (isTravel && iterationCnt > 1)
         return true;
@@ -127,114 +133,82 @@ bool Manager::isTrack(bool isTravel)
 
 int Manager::processing(const Mat &newFrame, bool isTravel)
 {
-    Distance &distance = Distance::getInstance();
+    this->isTravel = isTravel;
     currentFrame = make_shared<Mat>(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);
+    }
 
     // send allerts to main control
-    vector<vector<uint8_t>> alerts = alerter.sendAlerts(this->currentOutput);
-    sendAlerts(alerts);
+    if (isCalcVelocity()) {
+        vector<vector<uint8_t>> alerts =
+            alerter.sendAlerts(this->currentOutput);
+        sendAlerts(alerts);
+    }
 
     // update of the iterationCnt
     if (isTravel) {
         iterationCnt = iterationCnt % NUM_OF_TRACKING + 1;
     }
 
     // visual
-    drawOutput();
-    imshow("aaa", *currentFrame);
-    int key = cv::waitKey(1);
-    if (key == 27) {
+    if (drawOutput() == 27)
         return -1;
-    }
     return 1;
 }
 
-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);
 
     // 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<vector<uint8_t>> &alerts)