From 1c54d31ba5ea9434f03320f047bee31cf72fb81d Mon Sep 17 00:00:00 2001 From: MeravBenLulu Date: Tue, 8 Oct 2024 12:38:34 +0300 Subject: [PATCH 1/3] Img_processing: Refactor drawing logic-Each class now draws its own output --- img_processing/include/distance.h | 2 + img_processing/include/dynamic_tracker.h | 2 + img_processing/include/manager.h | 2 +- img_processing/include/velocity.h | 2 + img_processing/src/distance.cpp | 26 +++++++ img_processing/src/dynamic_tracker.cpp | 12 +++ img_processing/src/manager.cpp | 93 +++++++----------------- img_processing/src/velocity.cpp | 26 +++++++ 8 files changed, 99 insertions(+), 66 deletions(-) diff --git a/img_processing/include/distance.h b/img_processing/include/distance.h index e8913d51..96455a8d 100644 --- a/img_processing/include/distance.h +++ b/img_processing/include/distance.h @@ -11,6 +11,8 @@ class Distance { void findDistance(std::vector &objectInformation); static Distance &getInstance(const cv::Mat &image = cv::Mat()); void setFocalLength(double focalLength); + void drawDistance(std::shared_ptr image, + std::vector &objects); private: static Distance *instance; diff --git a/img_processing/include/dynamic_tracker.h b/img_processing/include/dynamic_tracker.h index 99de28ab..8b42ee4e 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(std::shared_ptr image, + std::vector &objects); private: std::shared_ptr frame; diff --git a/img_processing/include/manager.h b/img_processing/include/manager.h index 44a93e66..6745143b 100644 --- a/img_processing/include/manager.h +++ b/img_processing/include/manager.h @@ -37,7 +37,7 @@ class Manager { // Moves the current image to the prevFrame // and clears the memory of the currentFrame; void prepareForTheNext(); - void drawOutput(); + int drawOutput(); bool isDetect(bool isTravel); bool isResetTracker(bool isTravel); bool isTrack(bool isTravel); diff --git a/img_processing/include/velocity.h b/img_processing/include/velocity.h index 36a4f8ee..f02fd92a 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(std::shared_ptr image, + std::vector &objects); private: double frameTimeDiff; diff --git a/img_processing/src/distance.cpp b/img_processing/src/distance.cpp index 4fec4e92..bde9d56f 100644 --- a/img_processing/src/distance.cpp +++ b/img_processing/src/distance.cpp @@ -135,4 +135,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 image, + vector &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); + } } \ No newline at end of file diff --git a/img_processing/src/dynamic_tracker.cpp b/img_processing/src/dynamic_tracker.cpp index c1ed7881..0ef3446c 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(shared_ptr image, + vector &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); + } +} diff --git a/img_processing/src/manager.cpp b/img_processing/src/manager.cpp index 53e3c9a6..c91f37a3 100644 --- a/img_processing/src/manager.cpp +++ b/img_processing/src/manager.cpp @@ -159,82 +159,45 @@ int Manager::processing(const Mat &newFrame, bool isTravel) } // 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); - } + Distance &distance = Distance::getInstance(); + dynamicTracker.drawTracking(currentFrame, currentOutput); + distance.drawDistance(currentFrame, currentOutput); + 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> &alerts) diff --git a/img_processing/src/velocity.cpp b/img_processing/src/velocity.cpp index adbcbcd0..5dd9ea87 100644 --- a/img_processing/src/velocity.cpp +++ b/img_processing/src/velocity.cpp @@ -82,3 +82,29 @@ void Velocity::updateVelocity(float newVelocity, ObjectInformation &obj) .pop_front(); // Remove the oldest velocity if the deque exceeds the limit } } + +void Velocity::drawVelocity(shared_ptr image, + vector &objects) +{ + // 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) { + std::stringstream ssVelocity; + ssVelocity << std::fixed << std::setprecision(2) << obj.velocity; + + 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); + } +} From 1a6f5e4ed1a3b3de69e8d8d77a09d3d3e424772b Mon Sep 17 00:00:00 2001 From: MeravBenLulu Date: Tue, 8 Oct 2024 17:10:41 +0300 Subject: [PATCH 2/3] Img_processing: Refactor Distance and Velocity classes - Removed singleton from Distance class - Enhanced Velocity to support null values - Suppressed speed calculations and alerts when insufficient distance data is available --- img_processing/include/distance.h | 7 +--- img_processing/include/manager.h | 10 +++-- .../include/object_information_struct.h | 3 +- img_processing/src/alerter.cpp | 2 +- img_processing/src/distance.cpp | 20 +--------- img_processing/src/manager.cpp | 39 ++++++++++++------- img_processing/src/velocity.cpp | 35 +++++++++++------ img_processing/tests/test_distance.cpp | 4 +- img_processing/tests/test_velocity.cpp | 5 +-- 9 files changed, 65 insertions(+), 60 deletions(-) diff --git a/img_processing/include/distance.h b/img_processing/include/distance.h index 96455a8d..61757203 100644 --- a/img_processing/include/distance.h +++ b/img_processing/include/distance.h @@ -9,18 +9,13 @@ 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(std::shared_ptr image, std::vector &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); }; diff --git a/img_processing/include/manager.h b/img_processing/include/manager.h index 6745143b..dda2b45e 100644 --- a/img_processing/include/manager.h +++ b/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(); int drawOutput(); - bool isDetect(bool isTravel); - bool isResetTracker(bool isTravel); - bool isTrack(bool isTravel); + bool isDetect(); + bool isResetTracker(); + bool isTrack(); + bool isCalcVelocity(); + void sendAlerts(std::vector> &alerts); void runOnVideo(std::string videoPath); int readIdFromJson(const char *target); diff --git a/img_processing/include/object_information_struct.h b/img_processing/include/object_information_struct.h index 2558de24..69d91a55 100644 --- a/img_processing/include/object_information_struct.h +++ b/img_processing/include/object_information_struct.h @@ -3,6 +3,7 @@ #include #include "object_type_enum.h" +#include #define MAX_PREV_DISTANCES_SIZE 10 #define MAX_PREV_VELOCITIES_SIZE 2 @@ -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/src/alerter.cpp b/img_processing/src/alerter.cpp index 1ff035b8..d3946f2f 100644 --- a/img_processing/src/alerter.cpp +++ b/img_processing/src/alerter.cpp @@ -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 bde9d56f..ea8297fb 100644 --- a/img_processing/src/distance.cpp +++ b/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; diff --git a/img_processing/src/manager.cpp b/img_processing/src/manager.cpp index c91f37a3..890b562c 100644 --- a/img_processing/src/manager.cpp +++ b/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,31 +133,36 @@ 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); + } // 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) { @@ -166,10 +177,10 @@ int Manager::processing(const Mat &newFrame, bool isTravel) int Manager::drawOutput() { - Distance &distance = Distance::getInstance(); dynamicTracker.drawTracking(currentFrame, currentOutput); distance.drawDistance(currentFrame, currentOutput); - velocity.drawVelocity(currentFrame, currentOutput); + if (isCalcVelocity()) + velocity.drawVelocity(currentFrame, currentOutput); // Legend int legendX = 10, legendY = 10; diff --git a/img_processing/src/velocity.cpp b/img_processing/src/velocity.cpp index 5dd9ea87..b2e20c11 100644 --- a/img_processing/src/velocity.cpp +++ b/img_processing/src/velocity.cpp @@ -83,28 +83,41 @@ void Velocity::updateVelocity(float newVelocity, ObjectInformation &obj) } } -void Velocity::drawVelocity(shared_ptr image, - vector &objects) +void Velocity::drawVelocity(std::shared_ptr image, + std::vector &objects) { // 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) { - std::stringstream ssVelocity; - ssVelocity << std::fixed << std::setprecision(2) << obj.velocity; + // 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); + 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); + // 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/test_distance.cpp b/img_processing/tests/test_distance.cpp index a11be2df..e103b0d1 100644 --- a/img_processing/tests/test_distance.cpp +++ b/img_processing/tests/test_distance.cpp @@ -21,8 +21,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_velocity.cpp b/img_processing/tests/test_velocity.cpp index 25c4a671..35b88084 100644 --- a/img_processing/tests/test_velocity.cpp +++ b/img_processing/tests/test_velocity.cpp @@ -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; From 2198db0591ab47d2b3e4cfd605f62cbed4da9251 Mon Sep 17 00:00:00 2001 From: MeravBenLulu Date: Wed, 9 Oct 2024 18:07:06 +0300 Subject: [PATCH 3/3] Img_processing: Enhance velocity smoothing with EMA --- .../include/object_information_struct.h | 2 +- img_processing/include/velocity.h | 9 +-- img_processing/src/velocity.cpp | 68 ++++--------------- img_processing/tests/test_velocity.cpp | 53 ++++++--------- 4 files changed, 40 insertions(+), 92 deletions(-) diff --git a/img_processing/include/object_information_struct.h b/img_processing/include/object_information_struct.h index 69d91a55..b9bac2e3 100644 --- a/img_processing/include/object_information_struct.h +++ b/img_processing/include/object_information_struct.h @@ -6,7 +6,7 @@ #include #define MAX_PREV_DISTANCES_SIZE 10 -#define MAX_PREV_VELOCITIES_SIZE 2 +#define MAX_PREV_VELOCITIES_SIZE 10 struct ObjectInformation { int id; diff --git a/img_processing/include/velocity.h b/img_processing/include/velocity.h index f02fd92a..fef4a94b 100644 --- a/img_processing/include/velocity.h +++ b/img_processing/include/velocity.h @@ -2,6 +2,7 @@ #define __VELOCITY_H__ #include +#include #include "object_information_struct.h" class Velocity { @@ -9,13 +10,13 @@ class Velocity { Velocity() {} void returnVelocities(std::vector &objects); void init(double frameTimeDiff); - void drawVelocity(std::shared_ptr image, - std::vector &objects); + void drawVelocity(const std::shared_ptr image, + const std::vector &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__ \ No newline at end of file diff --git a/img_processing/src/velocity.cpp b/img_processing/src/velocity.cpp index b2e20c11..f7dff41b 100644 --- a/img_processing/src/velocity.cpp +++ b/img_processing/src/velocity.cpp @@ -18,11 +18,11 @@ void Velocity::calculateVelocity(ObjectInformation &object) float distanceAvg = averageDistanceChange(object); if (distanceAvg != -1) { float velocity = distanceAvg / this->frameTimeDiff; - updateVelocity(velocity, object); + smoothAndUpdateVelocity(velocity, object); } } -float Velocity::averageDistanceChange(ObjectInformation obj) const +float Velocity::averageDistanceChange(const ObjectInformation &obj) const { if (obj.prevDistances.size() < 2) return -1; @@ -33,58 +33,21 @@ float Velocity::averageDistanceChange(ObjectInformation obj) const return totalChange / (obj.prevDistances.size() - 1); } -//Function to update velocity while maintaining the last two velocities -void Velocity::updateVelocity(float newVelocity, ObjectInformation &obj) +void Velocity::smoothAndUpdateVelocity(float newVelocity, + ObjectInformation &obj) { - // If we have at least one previous velocity - if (!obj.prevVelocities.empty()) { - float lastVelocity = - obj.prevVelocities.back(); // Get the last velocity - // Check if the sign of the new velocity matches the last one - if ((newVelocity >= 0 && lastVelocity >= 0) || - (newVelocity < 0 && lastVelocity < 0)) { - // If the signs are the same, we accept the new velocity as valid - obj.velocity = newVelocity; - } - else { - // If the signs are different, we need to investigate further - if (obj.prevVelocities.size() > 1) { - // We look at the second-last velocity to check for consistency - float secondLastVelocity = - obj.prevVelocities[obj.prevVelocities.size() - 2]; - - // 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 - obj.velocity = newVelocity; - } - else { - // If the signs don’t match either, we keep the last velocity - obj.velocity = lastVelocity; - } - } - else { - // If there's only one previous velocity, we keep it as it’s unclear if the new one is valid - obj.velocity = lastVelocity; - } - } + if (obj.velocity.has_value()) { + // Adjust alpha for smoother or faster response + const float alpha = 0.17f; + obj.velocity = + (alpha * newVelocity) + ((1 - alpha) * obj.velocity.value()); } - else { - // If there are no previous velocities, just accept the new velocity + else obj.velocity = newVelocity; - } - // Add the new velocity to the deque for history tracking - obj.prevVelocities.push_back(newVelocity); - // 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 - } } -void Velocity::drawVelocity(std::shared_ptr image, - std::vector &objects) +void Velocity::drawVelocity(const std::shared_ptr image, + const std::vector &objects) const { // Font properties int fontFace = FONT_HERSHEY_SIMPLEX; @@ -112,12 +75,5 @@ void Velocity::drawVelocity(std::shared_ptr image, 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/test_velocity.cpp b/img_processing/tests/test_velocity.cpp index 35b88084..32428b59 100644 --- a/img_processing/tests/test_velocity.cpp +++ b/img_processing/tests/test_velocity.cpp @@ -30,46 +30,37 @@ TEST(TVelocity, calculate_TVelocity) VideoCapture capture("../tests/images/one_car.mp4"); Mat frame; capture.read(frame); - int cnt = 0; while (!frame.empty()) { shared_ptr f1 = make_shared(frame); - auto detectionOutput = make_shared>(); - auto trackingOutput = make_shared>(); + + // reset objects + auto output = vector(); detector.detect(f1, true); - *detectionOutput = detector.getOutput(); - *trackingOutput = detector.getOutput(); - tracker.startTracking(f1, *detectionOutput); + output = detector.getOutput(); + output = detector.getOutput(); + tracker.startTracking(f1, output); + + // after reset we not calc velocity + capture.read(frame); + if (frame.empty()) + return; + shared_ptr frame1 = make_shared(frame); + tracker.tracking(frame1, output); + distance.findDistance(output); - for (int i = 0; i < 10; i++) { + for (int i = 0; i < 9; i++) { capture.read(frame); - cnt++; if (frame.empty()) return; shared_ptr frame1 = make_shared(frame); - tracker.tracking(frame1, *trackingOutput); - distance.findDistance(*trackingOutput); - velocity.returnVelocities(*trackingOutput); - - for (int i = 0; i < (*trackingOutput).size(); i++) { - rectangle(*frame1, (*trackingOutput)[i].position, - Scalar(256, 0, 0), 2); - - Point textPosition((*trackingOutput)[i].position.x, - (*trackingOutput)[i].position.y - 10); - - // putText(*frame1, "Speed: " + std::to_string((*trackingOutput)[i].speed), textPosition, - /// FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 0, 0), 2); - putText( - *frame1, - "Speed: " + std::to_string((*trackingOutput)[i].distance), - textPosition, FONT_HERSHEY_SIMPLEX, 1, Scalar(255, 0, 0), - 2); - } - imshow("frame", *frame1); - waitKey(0); - cout << cnt << endl; + tracker.tracking(frame1, output); + distance.findDistance(output); + velocity.returnVelocities(output); + if(output.size()==1) + LogManager::logDebugMessage(DebugType::PRINT, + std::to_string(output[0].velocity.value())); + } capture.read(frame); - cnt++; } } \ No newline at end of file