diff --git a/img_processing/include/distance.h b/img_processing/include/distance.h index e8913d51..61757203 100644 --- a/img_processing/include/distance.h +++ b/img_processing/include/distance.h @@ -9,16 +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/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..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(); - 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); 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/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/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 4fec4e92..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; @@ -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 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..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) { @@ -159,82 +170,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); - } + 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> &alerts) diff --git a/img_processing/src/velocity.cpp b/img_processing/src/velocity.cpp index adbcbcd0..b2e20c11 100644 --- a/img_processing/src/velocity.cpp +++ b/img_processing/src/velocity.cpp @@ -82,3 +82,42 @@ void Velocity::updateVelocity(float newVelocity, ObjectInformation &obj) .pop_front(); // Remove the oldest velocity if the deque exceeds the limit } } + +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) { + // 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/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;