diff --git a/easy_perception_deployment/CHANGELOG.rst b/easy_perception_deployment/CHANGELOG.rst index cff83b0..ebac7fd 100644 --- a/easy_perception_deployment/CHANGELOG.rst +++ b/easy_perception_deployment/CHANGELOG.rst @@ -21,3 +21,17 @@ Changelog for package easy_perception_deployment * Updated generateCountClassNames() in usecase_config.hpp to json parsing. * Fixed wrong if conditions for parsing incoming Use Case integer in Deploy.py. * Contributor(s): Bey Hao Yun + +0.2.0 (2022-08-05) +------------------- +* Removed all instances of infer_visualizes from P2OrtBase and P3OrtBase classes. +* Modified Visualize workflows for all Precision Levels and Use Case configuration to use infer_action. +* Renamed all instances of infer_action to infer. +* Removed debug statement printing onnx_model_path from epd_container.cpp. +* Transferred detection results visualization from P2OrtBase and P3OrtBase to EPDContainer class. +* Generified detection results visualization functions to minimize code verbosity. +* Modified default session_config.json. +* Modified default input_image_topic.json. +* Updated unit testing modules under test folder to reflect new infer function calls for P1OrtBase, P2OrtBase and P3OrtBase classes. +* Contributor(s): Bey Hao Yun + diff --git a/easy_perception_deployment/config/input_image_topic.json b/easy_perception_deployment/config/input_image_topic.json index 76e054a..02bd72d 100644 --- a/easy_perception_deployment/config/input_image_topic.json +++ b/easy_perception_deployment/config/input_image_topic.json @@ -1,3 +1,3 @@ { - "input_image_topic": "/camera/color/image_raw" -} \ No newline at end of file + "input_image_topic": "/virtual_camera/image_raw" +} diff --git a/easy_perception_deployment/config/session_config.json b/easy_perception_deployment/config/session_config.json index 2656403..ee3c27e 100644 --- a/easy_perception_deployment/config/session_config.json +++ b/easy_perception_deployment/config/session_config.json @@ -1,6 +1,6 @@ { - "path_to_label_list" : "/home/cardboardvoice/epd_workspace/5_linting_epd_ws/src/easy_perception_deployment/easy_perception_deployment/data/label_list/imagenet_classes.txt", - "path_to_model" : "/home/cardboardvoice/epd_workspace/5_linting_epd_ws/src/easy_perception_deployment/easy_perception_deployment/data/model/squeezenet1.1-7.onnx", + "path_to_label_list" : "./data/label_list/coco_classes.txt", + "path_to_model" : "./data/model/MaskRCNN-10.onnx", "useCPU" : "CPU", - "visualizeFlag" : "robot" -} \ No newline at end of file + "visualizeFlag" : "visualize" +} diff --git a/easy_perception_deployment/gui/main.py b/easy_perception_deployment/gui/main.py index a71314e..610c329 100755 --- a/easy_perception_deployment/gui/main.py +++ b/easy_perception_deployment/gui/main.py @@ -1,5 +1,5 @@ -# Copyright 2020 Advanced Remanufacturing and Technology Centre -# Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +# Copyright 2022 Advanced Remanufacturing and Technology Centre +# Copyright 2022 ROS-Industrial Consortium Asia Pacific Team # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/gui/run_test_gui.bash b/easy_perception_deployment/gui/run_test_gui.bash index 7d2cb20..9f01fd5 100755 --- a/easy_perception_deployment/gui/run_test_gui.bash +++ b/easy_perception_deployment/gui/run_test_gui.bash @@ -1,7 +1,7 @@ #!/bin/sh -# Copyright 2020 Advanced Remanufacturing and Technology Centre -# Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +# Copyright 2022 Advanced Remanufacturing and Technology Centre +# Copyright 2022 ROS-Industrial Consortium Asia Pacific Team # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/gui/test_gui.py b/easy_perception_deployment/gui/test_gui.py index 6e6cd67..95c9b3a 100644 --- a/easy_perception_deployment/gui/test_gui.py +++ b/easy_perception_deployment/gui/test_gui.py @@ -1,5 +1,5 @@ -# Copyright 2020 Advanced Remanufacturing and Technology Centre -# Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +# Copyright 2022 Advanced Remanufacturing and Technology Centre +# Copyright 2022 ROS-Industrial Consortium Asia Pacific Team # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/gui/trainer/P1Trainer.py b/easy_perception_deployment/gui/trainer/P1Trainer.py index 10155f8..d2b674f 100644 --- a/easy_perception_deployment/gui/trainer/P1Trainer.py +++ b/easy_perception_deployment/gui/trainer/P1Trainer.py @@ -1,4 +1,4 @@ -# Copyright 2020 ROS-Industrial Consortium Asia Pacific +# Copyright 2022 ROS-Industrial Consortium Asia Pacific # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/gui/trainer/P2Trainer.py b/easy_perception_deployment/gui/trainer/P2Trainer.py index a209b66..64d62ab 100644 --- a/easy_perception_deployment/gui/trainer/P2Trainer.py +++ b/easy_perception_deployment/gui/trainer/P2Trainer.py @@ -1,4 +1,4 @@ -# Copyright 2020 ROS-Industrial Consortium Asia Pacific +# Copyright 2022 ROS-Industrial Consortium Asia Pacific # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/gui/trainer/P3Trainer.py b/easy_perception_deployment/gui/trainer/P3Trainer.py index a55a565..fd97a5b 100644 --- a/easy_perception_deployment/gui/trainer/P3Trainer.py +++ b/easy_perception_deployment/gui/trainer/P3Trainer.py @@ -1,4 +1,4 @@ -# Copyright 2020 ROS-Industrial Consortium Asia Pacific +# Copyright 2022 ROS-Industrial Consortium Asia Pacific # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/gui/windows/Counting.py b/easy_perception_deployment/gui/windows/Counting.py index e190ccf..d2e7fdb 100644 --- a/easy_perception_deployment/gui/windows/Counting.py +++ b/easy_perception_deployment/gui/windows/Counting.py @@ -1,4 +1,4 @@ -# Copyright 2020 ROS-Industrial Consortium Asia Pacific +# Copyright 2022 ROS-Industrial Consortium Asia Pacific # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/gui/windows/Deploy.py b/easy_perception_deployment/gui/windows/Deploy.py index f4865b2..9b82b8a 100755 --- a/easy_perception_deployment/gui/windows/Deploy.py +++ b/easy_perception_deployment/gui/windows/Deploy.py @@ -1,4 +1,4 @@ -# Copyright 2020 ROS-Industrial Consortium Asia Pacific +# Copyright 2022 ROS-Industrial Consortium Asia Pacific # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/gui/windows/Main.py b/easy_perception_deployment/gui/windows/Main.py index 987c67a..987c305 100755 --- a/easy_perception_deployment/gui/windows/Main.py +++ b/easy_perception_deployment/gui/windows/Main.py @@ -1,4 +1,4 @@ -# Copyright 2020 ROS-Industrial Consortium Asia Pacific +# Copyright 2022 ROS-Industrial Consortium Asia Pacific # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/gui/windows/Tracking.py b/easy_perception_deployment/gui/windows/Tracking.py index 4174c9a..14c1bce 100644 --- a/easy_perception_deployment/gui/windows/Tracking.py +++ b/easy_perception_deployment/gui/windows/Tracking.py @@ -1,4 +1,4 @@ -# Copyright 2020 ROS-Industrial Consortium Asia Pacific +# Copyright 2022 ROS-Industrial Consortium Asia Pacific # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/gui/windows/Train.py b/easy_perception_deployment/gui/windows/Train.py index 9b17fd2..22d8818 100755 --- a/easy_perception_deployment/gui/windows/Train.py +++ b/easy_perception_deployment/gui/windows/Train.py @@ -1,4 +1,4 @@ -# Copyright 2020 ROS-Industrial Consortium Asia Pacific +# Copyright 2022 ROS-Industrial Consortium Asia Pacific # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/include/epd_utils_lib/easy_perception_deployment.hpp b/easy_perception_deployment/include/epd_utils_lib/easy_perception_deployment.hpp index 94afad7..bbf35d2 100644 --- a/easy_perception_deployment/include/epd_utils_lib/easy_perception_deployment.hpp +++ b/easy_perception_deployment/include/epd_utils_lib/easy_perception_deployment.hpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -364,9 +364,31 @@ void EasyPerceptionDeployment::process_localize_callback( // Initialize timer std::chrono::high_resolution_clock::time_point begin = std::chrono::high_resolution_clock::now(); + EPD::EPDObjectLocalization result = ortAgent_.p3_ort_session->infer( + img, + depth_img, + *camera_info, + camera_to_plane_distance_mm); + cv::Mat resultImg; if (ortAgent_.isVisualize()) { - resultImg = ortAgent_.p3_ort_session->infer_visualize(img, depth_img, *camera_info); + EPD::EPDObjectTracking converted_result(result.data_size); + converted_result.object_ids.clear(); + for (size_t i = 0; i < result.data_size; i++) { + EPD::EPDObjectTracking::LocalizedObject object; + object.name = result.objects[i].name; + object.roi = result.objects[i].roi; + object.mask = result.objects[i].mask; + object.length = result.objects[i].length; + object.breadth = result.objects[i].breadth; + object.height = result.objects[i].height; + object.segmented_pcl = result.objects[i].segmented_pcl; + object.axis = result.objects[i].axis; + + converted_result.objects.emplace_back(object); + } + + cv::Mat resultImg = ortAgent_.visualize(converted_result, img); sensor_msgs::msg::Image::SharedPtr output_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", resultImg).toImageMsg(); @@ -378,12 +400,6 @@ void EasyPerceptionDeployment::process_localize_callback( RCLCPP_INFO(this->get_logger(), "[-FPS-]= %f\n", 1000.0 / elapsedTime.count()); } else { - EPD::EPDObjectLocalization result = ortAgent_.p3_ort_session->infer_action( - img, - depth_img, - *camera_info, - camera_to_plane_distance_mm); - epd_msgs::msg::EPDObjectLocalization output_msg; output_msg.header = std_msgs::msg::Header(); @@ -484,22 +500,19 @@ void EasyPerceptionDeployment::process_tracking_callback( std::chrono::high_resolution_clock::time_point begin = std::chrono::high_resolution_clock::now(); cv::Mat resultImg; - if (ortAgent_.isVisualize()) { - resultImg = ortAgent_.p3_ort_session->infer_visualize( - img, - depth_img, - *camera_info, - ortAgent_.tracker_type, - ortAgent_.trackers, - ortAgent_.tracker_logs, - ortAgent_.tracker_results); - // DEBUG - // for (size_t i = 0; i < ortAgent_.tracker_results.size(); i++) { - // std::cout << "Tracked Object = [ " - // << ortAgent_.tracker_results[i].obj_tag - // << " ]" << std::endl; - // } + EPD::EPDObjectTracking result = ortAgent_.p3_ort_session->infer( + img, + depth_img, + *camera_info, + camera_to_plane_distance_mm, + ortAgent_.tracker_type, + ortAgent_.trackers, + ortAgent_.tracker_logs, + ortAgent_.tracker_results); + + if (ortAgent_.isVisualize()) { + resultImg = ortAgent_.visualize(result, img); sensor_msgs::msg::Image::SharedPtr output_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", resultImg).toImageMsg(); @@ -511,16 +524,6 @@ void EasyPerceptionDeployment::process_tracking_callback( RCLCPP_INFO(this->get_logger(), "[-FPS-]= %f\n", 1000.0 / elapsedTime.count()); } else { - EPD::EPDObjectTracking result = ortAgent_.p3_ort_session->infer_action( - img, - depth_img, - *camera_info, - camera_to_plane_distance_mm, - ortAgent_.tracker_type, - ortAgent_.trackers, - ortAgent_.tracker_logs, - ortAgent_.tracker_results); - epd_msgs::msg::EPDObjectTracking output_msg; output_msg.header = std_msgs::msg::Header(); @@ -535,13 +538,6 @@ void EasyPerceptionDeployment::process_tracking_callback( output_msg.ppy = camera_info->k.at(5); output_msg.fy = camera_info->k.at(4); - // DEBUG - // std::cout << "[ "; - // for (size_t i = 0; i < output_msg.object_ids.size(); i++) { - // std::cout << " " << output_msg.object_ids[i] << " "; - // } - // std::cout << " ]" << std::endl; - // Populate output_msg objects and roi_array for (size_t i = 0; i < result.data_size; i++) { epd_msgs::msg::LocalizedObject object; @@ -629,24 +625,40 @@ const } case 2: { + EPD::EPDObjectDetection result = ortAgent_.p2_ort_session->infer(img); + EPD::activateUseCase( + img, + result.bboxes, + result.classIndices, + result.scores, + result.masks, + ortAgent_.classNames, + ortAgent_.useCaseMode, + ortAgent_.countClassNames, + ortAgent_.template_color_path); + + EPD::EPDObjectDetection output_obj(result.bboxes.size()); + output_obj.bboxes = result.bboxes; + output_obj.classIndices = result.classIndices; + output_obj.scores = result.scores; + if (ortAgent_.isVisualize()) { - resultImg = ortAgent_.p2_ort_session->infer_visualize(img); + resultImg = ortAgent_.visualize(output_obj, img); sensor_msgs::msg::Image::SharedPtr output_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", resultImg).toImageMsg(); visual_pub->publish(*output_msg); } else { - EPD::EPDObjectDetection result = ortAgent_.p2_ort_session->infer_action(img); epd_msgs::msg::EPDObjectDetection output_msg; - for (size_t i = 0; i < result.data_size; i++) { - output_msg.class_indices.push_back(result.classIndices[i]); + for (size_t i = 0; i < output_obj.data_size; i++) { + output_msg.class_indices.push_back(output_obj.classIndices[i]); - output_msg.scores.push_back(result.scores[i]); + output_msg.scores.push_back(output_obj.scores[i]); sensor_msgs::msg::RegionOfInterest roi; - roi.x_offset = result.bboxes[i][0]; - roi.y_offset = result.bboxes[i][1]; - roi.width = result.bboxes[i][2] - result.bboxes[i][0]; - roi.height = result.bboxes[i][3] - result.bboxes[i][1]; + roi.x_offset = output_obj.bboxes[i][0]; + roi.y_offset = output_obj.bboxes[i][1]; + roi.width = output_obj.bboxes[i][2] - output_obj.bboxes[i][0]; + roi.height = output_obj.bboxes[i][3] - output_obj.bboxes[i][1]; roi.do_rectify = false; output_msg.bboxes.push_back(roi); } @@ -657,29 +669,49 @@ const } case 3: { + EPD::EPDObjectDetection result = ortAgent_.p3_ort_session->infer(img); + EPD::activateUseCase( + img, + result.bboxes, + result.classIndices, + result.scores, + result.masks, + ortAgent_.classNames, + ortAgent_.useCaseMode, + ortAgent_.countClassNames, + ortAgent_.template_color_path); + + EPD::EPDObjectDetection output_obj(result.bboxes.size()); + output_obj.bboxes = result.bboxes; + output_obj.classIndices = result.classIndices; + output_obj.scores = result.scores; + output_obj.masks = result.masks; + if (ortAgent_.isVisualize()) { - resultImg = ortAgent_.p3_ort_session->infer_visualize(img); + resultImg = ortAgent_.visualize(output_obj, img); sensor_msgs::msg::Image::SharedPtr output_msg = cv_bridge::CvImage(std_msgs::msg::Header(), "bgr8", resultImg).toImageMsg(); visual_pub->publish(*output_msg); } else { - EPD::EPDObjectDetection result = ortAgent_.p3_ort_session->infer_action(img); epd_msgs::msg::EPDObjectDetection output_msg; - for (size_t i = 0; i < result.data_size; i++) { - output_msg.class_indices.push_back(result.classIndices[i]); + for (size_t i = 0; i < output_obj.data_size; i++) { + output_msg.class_indices.push_back(output_obj.classIndices[i]); - output_msg.scores.push_back(result.scores[i]); + output_msg.scores.push_back(output_obj.scores[i]); sensor_msgs::msg::RegionOfInterest roi; - roi.x_offset = result.bboxes[i][0]; - roi.y_offset = result.bboxes[i][1]; - roi.width = result.bboxes[i][2] - result.bboxes[i][0]; - roi.height = result.bboxes[i][3] - result.bboxes[i][1]; + roi.x_offset = output_obj.bboxes[i][0]; + roi.y_offset = output_obj.bboxes[i][1]; + roi.width = output_obj.bboxes[i][2] - output_obj.bboxes[i][0]; + roi.height = output_obj.bboxes[i][3] - output_obj.bboxes[i][1]; roi.do_rectify = false; output_msg.bboxes.push_back(roi); sensor_msgs::msg::Image::SharedPtr mask = - cv_bridge::CvImage(std_msgs::msg::Header(), "32FC1", result.masks[i]).toImageMsg(); + cv_bridge::CvImage( + std_msgs::msg::Header(), + "32FC1", + output_obj.masks[i]).toImageMsg(); output_msg.masks.push_back(*mask); } p3_pub->publish(output_msg); diff --git a/easy_perception_deployment/include/epd_utils_lib/epd_container.cpp b/easy_perception_deployment/include/epd_utils_lib/epd_container.cpp index 6adb7b7..32016f9 100644 --- a/easy_perception_deployment/include/epd_utils_lib/epd_container.cpp +++ b/easy_perception_deployment/include/epd_utils_lib/epd_container.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -134,8 +134,6 @@ void EPDContainer::setModelConfigFile() onnx_model_path = obj["path_to_model"].asString(); class_label_path = obj["path_to_label_list"].asString(); - std::cout << "onnx_model_path = " << onnx_model_path << std::endl; - std::string visualizeFlag = obj["visualizeFlag"].asString(); if (visualizeFlag.compare("visualize") == 0) { @@ -230,4 +228,162 @@ void EPDContainer::setLabelList() infile.close(); } +cv::Mat EPDContainer::visualize( + const EPD::EPDObjectDetection result, + const cv::Mat input_image) +{ + cv::Scalar oneColor(0.0, 0.0, 255.0, 0.0); + cv::Mat output_image = input_image.clone(); + + bool noMasksFound = false; + cv::Mat curMask, finalMask; + if (result.masks.size() == 0) { + noMasksFound = true; + } + + for (size_t i = 0; i < result.bboxes.size(); ++i) { + const int curBbox[] = { + result.bboxes[i][0], + result.bboxes[i][1], + result.bboxes[i][2], + result.bboxes[i][3] + }; + + if (!noMasksFound) { + curMask = result.masks[i].clone(); + } + + // DEBUG patch. + if (curMask.empty() && !noMasksFound) { + continue; + } + + const cv::Scalar & curColor = oneColor; + const std::string curLabel = classNames[result.classIndices[i]]; + + cv::rectangle( + output_image, cv::Point(curBbox[0], curBbox[1]), + cv::Point(curBbox[2], curBbox[3]), curColor, 2); + + int baseLine = 0; + cv::Size labelSize = + cv::getTextSize(curLabel, cv::FONT_HERSHEY_COMPLEX, 0.35, 1, &baseLine); + cv::rectangle( + output_image, cv::Point( + curBbox[0], curBbox[1]), + cv::Point( + curBbox[0] + labelSize.width, + curBbox[1] + static_cast(1.3 * labelSize.height)), + curColor, -1); + + // Visualizing masks + const cv::Rect curBoxRect(cv::Point(curBbox[0], curBbox[1]), + cv::Point(curBbox[2], curBbox[3])); + + if (!noMasksFound) { + cv::resize(curMask, curMask, curBoxRect.size()); + // Assigning masks that exceed the maskThreshold. + finalMask = (curMask > 0.5); + } + + // Assigning coloredRoi with the bounding box. + cv::Mat coloredRoi = (0.3 * curColor + 0.7 * output_image(curBoxRect)); + coloredRoi.convertTo(coloredRoi, CV_8UC3); + + if (!noMasksFound) { + std::vector contours; + cv::Mat hierarchy, tempFinalMask; + finalMask.convertTo(tempFinalMask, CV_8U); + cv::findContours( + tempFinalMask, contours, hierarchy, cv::RETR_TREE, + cv::CHAIN_APPROX_SIMPLE); + cv::drawContours( + coloredRoi, contours, -1, cv::Scalar(0, 0, 255), 2, cv::LINE_8, + hierarchy, 100); + } + + if (!noMasksFound) { + coloredRoi.copyTo(output_image(curBoxRect), finalMask); + } + + cv::putText( + output_image, curLabel, + cv::Point(curBbox[0], curBbox[1] + labelSize.height), + cv::FONT_HERSHEY_COMPLEX, 0.35, cv::Scalar(255, 255, 255)); + } + + return output_image; +} + +cv::Mat EPDContainer::visualize( + const EPD::EPDObjectTracking result, + const cv::Mat input_image) +{ + cv::Scalar oneColor(0.0, 0.0, 255.0, 0.0); + + cv::Mat output_image = input_image.clone(); + for (size_t i = 0; i < result.objects.size(); ++i) { + const unsigned int curBbox[] = { + result.objects[i].roi.x_offset, + result.objects[i].roi.y_offset, + result.objects[i].roi.width + result.objects[i].roi.x_offset, + result.objects[i].roi.height + result.objects[i].roi.y_offset}; + cv::Mat curMask = result.objects[i].mask.clone(); + // DEBUG patch. + if (curMask.empty()) { + continue; + } + + const cv::Scalar & curColor = oneColor; + std::string curLabel = result.objects[i].name; + + cv::rectangle( + output_image, cv::Point(curBbox[0], curBbox[1]), + cv::Point(curBbox[2], curBbox[3]), curColor, 2); + + int baseLine = 0; + cv::Size labelSize = + cv::getTextSize(curLabel, cv::FONT_HERSHEY_COMPLEX, 0.35, 1, &baseLine); + cv::rectangle( + output_image, cv::Point(curBbox[0], curBbox[1]), + cv::Point( + curBbox[0] + labelSize.width, + curBbox[1] + static_cast(1.3 * labelSize.height)), + curColor, -1); + + if (result.object_ids.size() != 0) { + curLabel = curLabel + "_" + result.object_ids[i]; + } + + // Visualizing masks + const cv::Rect curBoxRect(cv::Point(curBbox[0], curBbox[1]), + cv::Point(curBbox[2], curBbox[3])); + cv::resize(curMask, curMask, curBoxRect.size()); + // Assigning masks that exceed the maskThreshold. + cv::Mat finalMask = (curMask > 0.5); + + // Assigning coloredRoi with the bounding box. + cv::Mat coloredRoi = (0.3 * curColor + 0.7 * output_image(curBoxRect)); + coloredRoi.convertTo(coloredRoi, CV_8UC3); + + std::vector contours; + cv::Mat hierarchy, tempFinalMask; + finalMask.convertTo(tempFinalMask, CV_8U); + cv::findContours( + tempFinalMask, contours, hierarchy, cv::RETR_TREE, + cv::CHAIN_APPROX_SIMPLE); + cv::drawContours( + coloredRoi, contours, -1, cv::Scalar(0, 0, 255), 2, cv::LINE_8, + hierarchy, 100); + + coloredRoi.copyTo(output_image(curBoxRect), finalMask); + + cv::putText( + output_image, curLabel, + cv::Point(curBbox[0], curBbox[1] + labelSize.height), + cv::FONT_HERSHEY_COMPLEX, 0.35, cv::Scalar(255, 255, 255)); + } + + return output_image; +} } // namespace EPD diff --git a/easy_perception_deployment/include/epd_utils_lib/epd_container.hpp b/easy_perception_deployment/include/epd_utils_lib/epd_container.hpp index d30996b..513a62f 100644 --- a/easy_perception_deployment/include/epd_utils_lib/epd_container.hpp +++ b/easy_perception_deployment/include/epd_utils_lib/epd_container.hpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -112,6 +112,14 @@ class EPDContainer */ void initORTSessionHandler(); + cv::Mat visualize( + const EPD::EPDObjectDetection result, + const cv::Mat input_image); + + cv::Mat visualize( + const EPD::EPDObjectTracking result, + const cv::Mat input_image); + private: /*! \brief A boolean to indicate that OrtBase object has been initialized.*/ bool hasInitialized; diff --git a/easy_perception_deployment/include/epd_utils_lib/image_viewer.hpp b/easy_perception_deployment/include/epd_utils_lib/image_viewer.hpp index e1500c9..07fd16b 100644 --- a/easy_perception_deployment/include/epd_utils_lib/image_viewer.hpp +++ b/easy_perception_deployment/include/epd_utils_lib/image_viewer.hpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/include/epd_utils_lib/message_utils.hpp b/easy_perception_deployment/include/epd_utils_lib/message_utils.hpp index 249091a..cf3d793 100644 --- a/easy_perception_deployment/include/epd_utils_lib/message_utils.hpp +++ b/easy_perception_deployment/include/epd_utils_lib/message_utils.hpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -38,7 +38,7 @@ class EPDObjectDetection { public: /*! \brief A vector of bounding boxes with xmin, ymin, xmax, ymax.*/ - std::vector> bboxes; + std::vector> bboxes; /*! \brief A vector of indices that indicate the numerical identities of corresponding bounding boxes of the same index. */ diff --git a/easy_perception_deployment/include/epd_utils_lib/usecase_config.hpp b/easy_perception_deployment/include/epd_utils_lib/usecase_config.hpp index 3651ba4..8b46424 100644 --- a/easy_perception_deployment/include/epd_utils_lib/usecase_config.hpp +++ b/easy_perception_deployment/include/epd_utils_lib/usecase_config.hpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,6 +21,7 @@ #include #include #include "opencv2/opencv.hpp" +#include "epd_utils_lib/message_utils.hpp" /*! \brief A collection of use-case filters, namely for parsing usecase_config.json, @@ -34,235 +35,28 @@ const unsigned int COLOR_MATCHING_MODE = 2; const unsigned int LOCALISATION_MODE = 3; const unsigned int TRACKING_MODE = 4; -const char PATH_TO_USECASE_CONFIG[] = PATH_TO_PACKAGE "/config/usecase_config.json"; - -/*! \brief A Getter function that parses the usecase_config.json if a Counting -usecaseMode is selected and populates a list of selected object names intended -to be counted. -*/ -inline std::vector generateCountClassNames() -{ - std::vector countClassNames; - - Json::Reader reader; - Json::Value obj; - std::ifstream ifs_1(PATH_TO_USECASE_CONFIG); - - if (ifs_1) { - try { - ifs_1 >> obj; - } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; - } - } else { - std::cerr << "File not found!" << std::endl; - } - - reader.parse(ifs_1, obj); - - Json::Value class_list = obj["class_list"]; - for (int index = 0; index < static_cast(class_list.size()); index++) { - countClassNames.emplace_back(class_list[index].asString()); - } - - ifs_1.close(); - - return countClassNames; -} - -/*! \brief A Mutator function that takes the base inference results from a P2 -inference engine and excludes any bounding boxes, classIndices and score -element that do not share the label of selected objects-to-be counted. -*/ -inline void count( - std::vector> & bboxes, - std::vector & classIndices, - std::vector & scores, - std::vector allClassNames) -{ - std::vector countClassNames = EPD::generateCountClassNames(); - - // Set max number of object to detect to 1000. - std::vector> local_bboxes; - std::vector local_classIndices; - std::vector local_scores; - - /*Iterate through bbboxes, classIndices and allClassNames - to count corresponding detected objects with the same labels. - */ - for (size_t i = 0; i < bboxes.size(); ++i) { - const auto & curBbox = bboxes[i]; - const uint64_t classIdx = classIndices[i]; - const float curScore = scores[i]; - const std::string curLabel = allClassNames.empty() ? - std::to_string(classIdx) : allClassNames[classIdx]; - - for (size_t j = 0; j < countClassNames.size(); j++) { - std::string countLabel = countClassNames[j]; - if (curLabel.compare(countLabel) == 0) { - local_bboxes.push_back(curBbox); - local_classIndices.push_back(classIdx); - local_scores.push_back(curScore); - } - } - } - - bboxes = local_bboxes; - classIndices = local_classIndices; - scores = local_scores; -} - -/*! \brief A Mutator function that takes the base inference results from a P2 -inference engine and excludes any bounding boxes, classIndices and score -element that is not similar enough to the template color.. -*/ -inline void matchColor( - const cv::Mat & img, - std::vector> & bboxes, - std::vector & classIndices, - std::vector & scores, - std::vector allClassNames) -{ - Json::Reader reader; - Json::Value obj; - std::ifstream ifs_1(PATH_TO_USECASE_CONFIG); - - if (ifs_1) { - try { - ifs_1 >> obj; - } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; - } - } else { - std::cerr << "File not found!" << std::endl; - } - - reader.parse(ifs_1, obj); - - std::string filepath_to_refcolor = obj["path_to_color_template"].asString(); - - ifs_1.close(); - - cv::Mat ref_color_image = cv::imread(filepath_to_refcolor, cv::IMREAD_COLOR); - cv::Mat hsv_base, hsv_test1; - cv::cvtColor(ref_color_image, hsv_base, cv::COLOR_BGR2HSV); - cv::Mat hist_base, hist_test1; - int h_bins = 50, s_bins = 60; - int histSize[] = {h_bins, s_bins}; - int channels[] = {0, 1}; - - // hue varies from 0 to 179, saturation from 0 to 255 - float h_ranges[] = {0, 180}; - float s_ranges[] = {0, 256}; - const float * ranges[] = {h_ranges, s_ranges}; - cv::calcHist(&hsv_base, 1, channels, cv::Mat(), hist_base, 2, histSize, ranges, true, false); - cv::normalize(hist_base, hist_base, 0, 1, cv::NORM_MINMAX, -1, cv::Mat()); - - double base_base; - cv::Mat croppedImage; - std::vector> local_bboxes; - std::vector local_classIndices; - std::vector local_scores; - - for (size_t i = 0; i < bboxes.size(); ++i) { - const auto & curBbox = bboxes[i]; - const uint64_t classIdx = classIndices[i]; - const float curScore = scores[i]; - const std::string curLabel = allClassNames.empty() ? - std::to_string(classIdx) : allClassNames[classIdx]; - - cv::Rect objectROI(cv::Point(curBbox[0], curBbox[1]), cv::Point(curBbox[2], curBbox[3])); - croppedImage = img(objectROI); - cv::cvtColor(croppedImage, hsv_test1, cv::COLOR_BGR2HSV); - cv::calcHist( - &hsv_test1, 1, channels, cv::Mat(), - hist_test1, 2, histSize, ranges, true, false); - cv::normalize( - hist_test1, hist_test1, 0, 1, - cv::NORM_MINMAX, -1, cv::Mat()); - - /* Can change 3rd arg in compareHist function call to [0,1,2,3], - [Correlation, Chi-square, Intersection, Bhattacharyya] - TODO(cardboardcode) Require benchmark to justify use of metric 0: Correlation.*/ - base_base = compareHist(hist_base, hist_test1, 0); - if (base_base > 0.8) { - local_bboxes.push_back(curBbox); - local_classIndices.push_back(classIdx); - local_scores.push_back(curScore); - } - } - - bboxes = local_bboxes; - classIndices = local_classIndices; - scores = local_scores; -} - -/*! \brief A Mutator function that takes the base inference results from a P2 -inference engine and excludes any bounding boxes, classIndices and score -element based on a selected use-case filter. -*/ -inline void activateUseCase( - const cv::Mat & img, - std::vector> & bboxes, - std::vector & classIndices, - std::vector & scores, - std::vector allClassNames) -{ - unsigned int useCaseMode = 0; - - Json::Reader reader; - Json::Value obj; - std::ifstream ifs_1(PATH_TO_USECASE_CONFIG); - - if (ifs_1) { - try { - ifs_1 >> obj; - } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; - } - } else { - std::cerr << "File not found!" << std::endl; - } - - reader.parse(ifs_1, obj); - - useCaseMode = obj["usecase_mode"].asInt(); - - ifs_1.close(); - - // If default CLASSIFICATION_MODE is selected, do not alter anything and return. - if (useCaseMode == EPD::CLASSIFICATION_MODE) { - return; - } else if (useCaseMode == EPD::COUNTING_MODE) { - printf("Use Case: [Counting] selected.\n"); - EPD::count(bboxes, classIndices, scores, allClassNames); - } else if (useCaseMode == EPD::COLOR_MATCHING_MODE) { - printf("Use Case: [Color-Matching] selected.\n"); - EPD::matchColor(img, bboxes, classIndices, scores, allClassNames); - } else { - throw std::runtime_error("Invalid Use Case. Can only be [0, 1, 2, 3, 4]."); - } -} - /*! \brief A Mutator function that takes the base inference results from a P3 inference engine and excludes any bounding boxes, classIndices and score element that do not share the label of selected objects-to-be counted. */ inline void count( - std::vector> & bboxes, + std::vector> & bboxes, std::vector & classIndices, std::vector & scores, std::vector & masks, - std::vector allClassNames) + std::vector allClassNames, + const std::vector countClassNames) { - std::vector countClassNames = EPD::generateCountClassNames(); + bool noMasksFound = false; + if (masks.size() == 0) { + noMasksFound = true; + } // Set max number of object to detect to 1000. - std::vector> local_bboxes; + std::vector> local_bboxes; std::vector local_classIndices; std::vector local_scores; std::vector local_masks; - /*Iterate through bbboxes, classIndices and allClassNames to count corresponding detected objects with the same labels. */ @@ -270,7 +64,10 @@ inline void count( const auto & curBbox = bboxes[i]; const uint64_t classIdx = classIndices[i]; const float curScore = scores[i]; - const cv::Mat curMask = masks[i]; + cv::Mat curMask; + if (!noMasksFound) { + curMask = masks[i]; + } const std::string curLabel = allClassNames.empty() ? std::to_string(classIdx) : allClassNames[classIdx]; @@ -280,7 +77,9 @@ inline void count( local_bboxes.push_back(curBbox); local_classIndices.push_back(classIdx); local_scores.push_back(curScore); - local_masks.push_back(curMask); + if (!noMasksFound) { + local_masks.push_back(curMask); + } } } } @@ -288,7 +87,9 @@ inline void count( bboxes = local_bboxes; classIndices = local_classIndices; scores = local_scores; - masks = local_masks; + if (!noMasksFound) { + masks = local_masks; + } } /*! \brief A Mutator function that takes the base inference results from a P3 @@ -297,32 +98,17 @@ element that is not similar enough to the template color.. */ inline void matchColor( const cv::Mat & img, - std::vector> & bboxes, + std::vector> & bboxes, std::vector & classIndices, std::vector & scores, std::vector & masks, - std::vector allClassNames) + std::vector allClassNames, + const std::string filepath_to_refcolor) { - Json::Reader reader; - Json::Value obj; - std::ifstream ifs_1(PATH_TO_USECASE_CONFIG); - - if (ifs_1) { - try { - ifs_1 >> obj; - } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; - } - } else { - std::cerr << "File not found!" << std::endl; + bool noMasksFound = false; + if (masks.size() == 0) { + noMasksFound = true; } - - reader.parse(ifs_1, obj); - - std::string filepath_to_refcolor = obj["path_to_color_template"].asString(); - - ifs_1.close(); - cv::Mat ref_color_image = cv::imread(filepath_to_refcolor, cv::IMREAD_COLOR); cv::Mat hsv_base, hsv_test1; cv::cvtColor(ref_color_image, hsv_base, cv::COLOR_BGR2HSV); @@ -340,7 +126,7 @@ inline void matchColor( double base_base; cv::Mat croppedImage; - std::vector> local_bboxes; + std::vector> local_bboxes; std::vector local_classIndices; std::vector local_scores; std::vector local_masks; @@ -349,7 +135,10 @@ inline void matchColor( const auto & curBbox = bboxes[i]; const uint64_t classIdx = classIndices[i]; const float curScore = scores[i]; - const cv::Mat curMask = masks[i]; + cv::Mat curMask; + if (!noMasksFound) { + curMask = masks[i]; + } const std::string curLabel = allClassNames.empty() ? std::to_string(classIdx) : allClassNames[classIdx]; @@ -371,14 +160,18 @@ inline void matchColor( local_bboxes.push_back(curBbox); local_classIndices.push_back(classIdx); local_scores.push_back(curScore); - local_masks.push_back(curMask); + if (!noMasksFound) { + local_masks.push_back(curMask); + } } } bboxes = local_bboxes; classIndices = local_classIndices; scores = local_scores; - masks = local_masks; + if (!noMasksFound) { + masks = local_masks; + } } /*! \brief A Mutator function that takes the base inference results from a P3 @@ -387,43 +180,24 @@ element based on a selected use-case filter. */ inline void activateUseCase( const cv::Mat & img, - std::vector> & bboxes, + std::vector> & bboxes, std::vector & classIndices, std::vector & scores, std::vector & masks, - std::vector allClassNames) + std::vector allClassNames, + const unsigned int useCaseMode, + const std::vector countClassNames, + const std::string filepath_to_refcolor) { - int useCaseMode = 3; - - Json::Reader reader; - Json::Value obj; - std::ifstream ifs_1(PATH_TO_USECASE_CONFIG); - - if (ifs_1) { - try { - ifs_1 >> obj; - } catch (const std::exception & e) { - std::cerr << e.what() << std::endl; - } - } else { - std::cerr << "File not found!" << std::endl; - } - - reader.parse(ifs_1, obj); - - useCaseMode = obj["usecase_mode"].asInt(); - - ifs_1.close(); - // If default CLASSIFICATION_MODE is selected, do not alter anything and return. if (useCaseMode == EPD::CLASSIFICATION_MODE) { return; } else if (useCaseMode == EPD::COUNTING_MODE) { printf("Use Case: [Counting] selected.\n"); - EPD::count(bboxes, classIndices, scores, masks, allClassNames); + EPD::count(bboxes, classIndices, scores, masks, allClassNames, countClassNames); } else if (useCaseMode == EPD::COLOR_MATCHING_MODE) { printf("Use Case: [Color-Matching] selected.\n"); - EPD::matchColor(img, bboxes, classIndices, scores, masks, allClassNames); + EPD::matchColor(img, bboxes, classIndices, scores, masks, allClassNames, filepath_to_refcolor); } else { throw std::runtime_error("Invalid Use Case. Can only be [0, 1, 2]."); } diff --git a/easy_perception_deployment/include/ort_cpp_lib/ort_base.cpp b/easy_perception_deployment/include/ort_cpp_lib/ort_base.cpp index c96b8bc..e3583d4 100644 --- a/easy_perception_deployment/include/ort_cpp_lib/ort_base.cpp +++ b/easy_perception_deployment/include/ort_cpp_lib/ort_base.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/include/ort_cpp_lib/ort_base.hpp b/easy_perception_deployment/include/ort_cpp_lib/ort_base.hpp index 4ee517c..0bf6ac4 100644 --- a/easy_perception_deployment/include/ort_cpp_lib/ort_base.hpp +++ b/easy_perception_deployment/include/ort_cpp_lib/ort_base.hpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/include/ort_cpp_lib/p1_ort_base.hpp b/easy_perception_deployment/include/ort_cpp_lib/p1_ort_base.hpp index cdb7f0c..c46bef2 100644 --- a/easy_perception_deployment/include/ort_cpp_lib/p1_ort_base.hpp +++ b/easy_perception_deployment/include/ort_cpp_lib/p1_ort_base.hpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/include/ort_cpp_lib/p2_ort_base.cpp b/easy_perception_deployment/include/ort_cpp_lib/p2_ort_base.cpp index f29c31f..551f874 100644 --- a/easy_perception_deployment/include/ort_cpp_lib/p2_ort_base.cpp +++ b/easy_perception_deployment/include/ort_cpp_lib/p2_ort_base.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -48,22 +48,11 @@ P2OrtBase::P2OrtBase( // Destructor P2OrtBase::~P2OrtBase() {} -// Mutator 4 -cv::Mat P2OrtBase::infer_visualize(const cv::Mat & inputImg) -{ - std::vector dst(3 * m_paddedH * m_paddedW); - - return this->infer_visualize( - inputImg, m_newW, m_newH, - m_paddedW, m_paddedH, m_ratio, - dst.data(), 0.5, cv::Scalar(102.9801, 115.9465, 122.7717)); -} - -EPD::EPDObjectDetection P2OrtBase::infer_action(const cv::Mat & inputImg) +EPD::EPDObjectDetection P2OrtBase::infer(const cv::Mat & inputImg) { std::vector dst(3 * m_paddedH * m_paddedW); - return this->infer_action( + return this->infer( inputImg, m_newW, m_newH, m_paddedW, m_paddedH, m_ratio, dst.data(), 0.5, cv::Scalar(102.9801, 115.9465, 122.7717)); @@ -96,71 +85,7 @@ void P2OrtBase::preprocess( } // Mutator 4 -cv::Mat P2OrtBase::infer_visualize( - const cv::Mat & inputImg, - int newW, - int newH, - int paddedW, - int paddedH, - float ratio, - float * dst, - float confThresh, - const cv::Scalar & meanVal) -{ - cv::Mat tmpImg; - cv::resize(inputImg, tmpImg, cv::Size(newW, newH)); - - tmpImg.convertTo(tmpImg, CV_32FC3); - tmpImg -= meanVal; - - cv::Mat paddedImg(paddedH, paddedW, CV_32FC3, cv::Scalar(0, 0, 0)); - tmpImg.copyTo(paddedImg(cv::Rect(0, 0, newW, newH))); - - this->preprocess(dst, paddedImg, paddedW, paddedH, 3); - - // boxes, labels, scores - auto inferenceOutput = (*this)({dst}); - - assert(inferenceOutput[1].second.size() == 1); - size_t nBoxes = inferenceOutput[1].second[0]; - - std::vector> bboxes; - std::vector classIndices; - // Remove scores for release - std::vector scores; - - bboxes.reserve(nBoxes); - classIndices.reserve(nBoxes); - scores.reserve(nBoxes); - - for (size_t i = 0; i < nBoxes; ++i) { - if (inferenceOutput[2].first[i] > confThresh) { - float xmin = inferenceOutput[0].first[i * 4 + 0] / ratio; - float ymin = inferenceOutput[0].first[i * 4 + 1] / ratio; - float xmax = inferenceOutput[0].first[i * 4 + 2] / ratio; - float ymax = inferenceOutput[0].first[i * 4 + 3] / ratio; - - xmin = std::max(xmin, 0); - ymin = std::max(ymin, 0); - xmax = std::min(xmax, inputImg.cols); - ymax = std::min(ymax, inputImg.rows); - - bboxes.emplace_back(std::array{xmin, ymin, xmax, ymax}); - classIndices.emplace_back(reinterpret_cast(inferenceOutput[1].first)[i]); - scores.emplace_back(inferenceOutput[2].first[i]); - } - } - - if (bboxes.size() == 0) { - return inputImg; - } - - EPD::activateUseCase(inputImg, bboxes, classIndices, scores, this->getClassNames()); - return visualize(inputImg, bboxes, classIndices, this->getClassNames()); -} - -// Mutator 4 -EPD::EPDObjectDetection P2OrtBase::infer_action( +EPD::EPDObjectDetection P2OrtBase::infer( const cv::Mat & inputImg, int newW, int newH, @@ -188,7 +113,7 @@ EPD::EPDObjectDetection P2OrtBase::infer_action( assert(inferenceOutput[1].second.size() == 1); size_t nBoxes = inferenceOutput[1].second[0]; - std::vector> bboxes; + std::vector> bboxes; std::vector classIndices; std::vector scores; @@ -198,17 +123,17 @@ EPD::EPDObjectDetection P2OrtBase::infer_action( for (size_t i = 0; i < nBoxes; ++i) { if (inferenceOutput[2].first[i] > confThresh) { - float xmin = inferenceOutput[0].first[i * 4 + 0] / ratio; - float ymin = inferenceOutput[0].first[i * 4 + 1] / ratio; - float xmax = inferenceOutput[0].first[i * 4 + 2] / ratio; - float ymax = inferenceOutput[0].first[i * 4 + 3] / ratio; + int xmin = inferenceOutput[0].first[i * 4 + 0] / ratio; + int ymin = inferenceOutput[0].first[i * 4 + 1] / ratio; + int xmax = inferenceOutput[0].first[i * 4 + 2] / ratio; + int ymax = inferenceOutput[0].first[i * 4 + 3] / ratio; - xmin = std::max(xmin, 0); - ymin = std::max(ymin, 0); - xmax = std::min(xmax, inputImg.cols); - ymax = std::min(ymax, inputImg.rows); + xmin = std::max(xmin, 0); + ymin = std::max(ymin, 0); + xmax = std::min(xmax, inputImg.cols); + ymax = std::min(ymax, inputImg.rows); - bboxes.emplace_back(std::array{xmin, ymin, xmax, ymax}); + bboxes.emplace_back(std::array{xmin, ymin, xmax, ymax}); classIndices.emplace_back(reinterpret_cast(inferenceOutput[1].first)[i]); scores.emplace_back(inferenceOutput[2].first[i]); } @@ -220,8 +145,6 @@ EPD::EPDObjectDetection P2OrtBase::infer_action( return output_msg; } - EPD::activateUseCase(inputImg, bboxes, classIndices, scores, this->getClassNames()); - EPD::EPDObjectDetection output_obj(bboxes.size()); output_obj.bboxes = bboxes; output_obj.classIndices = classIndices; @@ -230,46 +153,4 @@ EPD::EPDObjectDetection P2OrtBase::infer_action( return output_obj; } -cv::Mat P2OrtBase::visualize( - const cv::Mat & img, - const std::vector> & bboxes, - const std::vector & classIndices, - const std::vector & allClassNames = {}) -{ - assert(bboxes.size() == classIndices.size()); - if (!allClassNames.empty()) { - assert(allClassNames.size() > *std::max_element(classIndices.begin(), classIndices.end())); - } - - cv::Mat result = img.clone(); - - cv::Scalar allColors(255.0, 0.0, 0.0, 0.0); - - for (size_t i = 0; i < bboxes.size(); ++i) { - const auto & curBbox = bboxes[i]; - const uint64_t classIdx = classIndices[i]; - const cv::Scalar & curColor = allColors[classIdx]; - const std::string curLabel = allClassNames.empty() ? - std::to_string(classIdx) : allClassNames[classIdx]; - - cv::rectangle( - result, cv::Point(curBbox[0], curBbox[1]), - cv::Point(curBbox[2], curBbox[3]), curColor, 2); - - int baseLine = 0; - cv::Size labelSize = cv::getTextSize( - curLabel, - cv::FONT_HERSHEY_COMPLEX, 0.35, 1, &baseLine); - cv::rectangle( - result, cv::Point(curBbox[0], curBbox[1]), - cv::Point( - curBbox[0] + labelSize.width, curBbox[1] + - static_cast(1.3 * labelSize.height)), - curColor, -1); - cv::putText( - result, curLabel, cv::Point(curBbox[0], curBbox[1] + labelSize.height), - cv::FONT_HERSHEY_COMPLEX, 0.35, cv::Scalar(255, 255, 255)); - } - return result; -} } // namespace Ort diff --git a/easy_perception_deployment/include/ort_cpp_lib/p2_ort_base.hpp b/easy_perception_deployment/include/ort_cpp_lib/p2_ort_base.hpp index 9fc5fa6..cb848e1 100644 --- a/easy_perception_deployment/include/ort_cpp_lib/p2_ort_base.hpp +++ b/easy_perception_deployment/include/ort_cpp_lib/p2_ort_base.hpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -53,11 +53,8 @@ class P2OrtBase : public OrtBase /*! \brief A Destructor function*/ ~P2OrtBase(); /*! \brief A auxillary Mutator function that calls the internal overloading - infer_visualize function.*/ - cv::Mat infer_visualize(const cv::Mat & inputImg); - /*! \brief A auxillary Mutator function that calls the internal overloading - infer_action function.*/ - EPD::EPDObjectDetection infer_action(const cv::Mat & inputImg); + infer function.*/ + EPD::EPDObjectDetection infer(const cv::Mat & inputImg); /*! \brief A Getter function that gets the number of object names used for an ongoing session.*/ @@ -95,20 +92,8 @@ class P2OrtBase : public OrtBase const int numChannels) const; /*! \brief A Mutator function that runs a P2 Ort Session and gets P2 - inference result for visualization purposes.*/ - cv::Mat infer_visualize( - const cv::Mat & inputImg, - int newW, - int newH, - int paddedW, - int paddedH, - float ratio, - float * dst, - float confThresh, - const cv::Scalar & meanVal); - /*! \brief A Mutator function that runs a P2 Ort Session and gets P2 inference result for use by external agents.*/ - EPD::EPDObjectDetection infer_action( + EPD::EPDObjectDetection infer( const cv::Mat & inputImg, int newW, int newH, @@ -118,15 +103,6 @@ class P2OrtBase : public OrtBase float * dst, float confThresh, const cv::Scalar & meanVal); - - /*! \brief A Mutator function that takes P2 inference outputs and illustrates - derived bounding boxes with corresponding object labels for visualization - purposes.*/ - cv::Mat visualize( - const cv::Mat & img, - const std::vector> & bboxes, - const std::vector & classIndices, - const std::vector & allClassNames); }; } // namespace Ort diff --git a/easy_perception_deployment/include/ort_cpp_lib/p3_ort_base.cpp b/easy_perception_deployment/include/ort_cpp_lib/p3_ort_base.cpp index 5f1aba1..9d32239 100644 --- a/easy_perception_deployment/include/ort_cpp_lib/p3_ort_base.cpp +++ b/easy_perception_deployment/include/ort_cpp_lib/p3_ort_base.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -61,61 +61,18 @@ P3OrtBase::~P3OrtBase() {} // Mutator: Classification -cv::Mat P3OrtBase::infer_visualize(const cv::Mat & inputImg) +EPD::EPDObjectDetection P3OrtBase::infer(const cv::Mat & inputImg) { std::vector dst(3 * m_paddedH * m_paddedW); - return this->infer_visualize( + return this->infer( inputImg, m_newW, m_newH, m_paddedW, m_paddedH, m_ratio, dst.data(), 0.5, cv::Scalar(102.9801, 115.9465, 122.7717)); } // Mutator: Localization -cv::Mat P3OrtBase::infer_visualize( - const cv::Mat & inputImg, - const cv::Mat & depthImg, - sensor_msgs::msg::CameraInfo camera_info) -{ - std::vector dst(3 * m_paddedH * m_paddedW); - - return this->infer_visualize( - inputImg, depthImg, camera_info, m_newW, m_newH, - m_paddedW, m_paddedH, m_ratio, dst.data(), 0.5, - cv::Scalar(102.9801, 115.9465, 122.7717)); -} - -// Mutator: Tracking -cv::Mat P3OrtBase::infer_visualize( - const cv::Mat & inputImg, - const cv::Mat & depthImg, - sensor_msgs::msg::CameraInfo camera_info, - const std::string tracker_type, - std::vector> & trackers, - std::vector & tracker_logs, - std::vector & tracker_results) -{ - std::vector dst(3 * m_paddedH * m_paddedW); - - return this->infer_visualize( - inputImg, depthImg, camera_info, tracker_type, trackers, tracker_logs, - tracker_results, m_newW, m_newH, m_paddedW, m_paddedH, m_ratio, - dst.data(), 0.5, cv::Scalar(102.9801, 115.9465, 122.7717)); -} - -// Mutator: Classification -EPD::EPDObjectDetection P3OrtBase::infer_action(const cv::Mat & inputImg) -{ - std::vector dst(3 * m_paddedH * m_paddedW); - - return this->infer_action( - inputImg, m_newW, m_newH, - m_paddedW, m_paddedH, m_ratio, dst.data(), 0.5, - cv::Scalar(102.9801, 115.9465, 122.7717)); -} - -// Mutator: Localization -EPD::EPDObjectLocalization P3OrtBase::infer_action( +EPD::EPDObjectLocalization P3OrtBase::infer( const cv::Mat & inputImg, const cv::Mat & depthImg, sensor_msgs::msg::CameraInfo camera_info, @@ -123,14 +80,14 @@ EPD::EPDObjectLocalization P3OrtBase::infer_action( { std::vector dst(3 * m_paddedH * m_paddedW); - return this->infer_action( + return this->infer( inputImg, depthImg, camera_info, camera_to_plane_distance_mm, m_newW, m_newH, m_paddedW, m_paddedH, m_ratio, dst.data(), 0.5, cv::Scalar(102.9801, 115.9465, 122.7717)); } // Mutator: Tracking -EPD::EPDObjectTracking P3OrtBase::infer_action( +EPD::EPDObjectTracking P3OrtBase::infer( const cv::Mat & inputImg, const cv::Mat & depthImg, sensor_msgs::msg::CameraInfo camera_info, @@ -142,7 +99,7 @@ EPD::EPDObjectTracking P3OrtBase::infer_action( { std::vector dst(3 * m_paddedH * m_paddedW); - return this->infer_action( + return this->infer( inputImg, depthImg, camera_info, camera_to_plane_distance_mm, tracker_type, trackers, tracker_logs, tracker_results, m_newW, m_newH, m_paddedW, m_paddedH, m_ratio, dst.data(), 0.5, @@ -177,7 +134,7 @@ void P3OrtBase::preprocess( } // Mutator 4 -cv::Mat P3OrtBase::infer_visualize( +EPD::EPDObjectDetection P3OrtBase::infer( const cv::Mat & inputImg, int newW, int newH, @@ -205,7 +162,7 @@ cv::Mat P3OrtBase::infer_visualize( assert(inferenceOutput[1].second.size() == 1); size_t nBoxes = inferenceOutput[1].second[0]; - std::vector> bboxes; + std::vector> bboxes; std::vector classIndices; std::vector scores; std::vector masks; @@ -215,356 +172,43 @@ cv::Mat P3OrtBase::infer_visualize( scores.reserve(nBoxes); masks.reserve(nBoxes); - for (size_t i = 0; i < nBoxes; ++i) { if (inferenceOutput[2].first[i] > confThresh) { - float xmin = inferenceOutput[0].first[i * 4 + 0] / ratio; - float ymin = inferenceOutput[0].first[i * 4 + 1] / ratio; - float xmax = inferenceOutput[0].first[i * 4 + 2] / ratio; - float ymax = inferenceOutput[0].first[i * 4 + 3] / ratio; + int xmin = inferenceOutput[0].first[i * 4 + 0] / ratio; + int ymin = inferenceOutput[0].first[i * 4 + 1] / ratio; + int xmax = inferenceOutput[0].first[i * 4 + 2] / ratio; + int ymax = inferenceOutput[0].first[i * 4 + 3] / ratio; - xmin = std::max(xmin, 0); - ymin = std::max(ymin, 0); - xmax = std::min(xmax, inputImg.cols); - ymax = std::min(ymax, inputImg.rows); + xmin = std::max(xmin, 0); + ymin = std::max(ymin, 0); + xmax = std::min(xmax, inputImg.cols); + ymax = std::min(ymax, inputImg.rows); - bboxes.emplace_back(std::array{xmin, ymin, xmax, ymax}); + bboxes.emplace_back(std::array{xmin, ymin, xmax, ymax}); classIndices.emplace_back(reinterpret_cast(inferenceOutput[1].first)[i]); scores.emplace_back(inferenceOutput[2].first[i]); cv::Mat curMask(28, 28, CV_32FC1); memcpy( curMask.data, - inferenceOutput[3].first + i * 28 * 28, - 28 * 28 * sizeof(float)); - masks.emplace_back(curMask); - } - } - - if (bboxes.size() == 0) { - return inputImg; - } - - EPD::activateUseCase(inputImg, bboxes, classIndices, scores, masks, this->getClassNames()); - return visualize(inputImg, bboxes, classIndices, masks, this->getClassNames(), 0.5); -} - -// Mutator 5: Localization -cv::Mat P3OrtBase::infer_visualize( - const cv::Mat & inputImg, - const cv::Mat & depthImg, - sensor_msgs::msg::CameraInfo camera_info, - int newW, - int newH, - int paddedW, - int paddedH, - float ratio, - float * dst, - float confThresh, - const cv::Scalar & meanVal) -{ - cv::Mat tmpImg; - cv::resize(inputImg, tmpImg, cv::Size(newW, newH)); - - tmpImg.convertTo(tmpImg, CV_32FC3); - tmpImg -= meanVal; - - cv::Mat paddedImg(paddedH, paddedW, CV_32FC3, cv::Scalar(0, 0, 0)); - tmpImg.copyTo(paddedImg(cv::Rect(0, 0, newW, newH))); - - this->preprocess(dst, paddedImg, paddedW, paddedH, 3); - - // boxes, labels, scores, masks - auto inferenceOutput = (*this)({dst}); - - assert(inferenceOutput[1].second.size() == 1); - size_t nBoxes = inferenceOutput[1].second[0]; - - std::vector> bboxes; - std::vector classIndices; - std::vector scores; - std::vector masks; - - bboxes.reserve(nBoxes); - classIndices.reserve(nBoxes); - scores.reserve(nBoxes); - masks.reserve(nBoxes); - - - for (size_t i = 0; i < nBoxes; ++i) { - if (inferenceOutput[2].first[i] > confThresh) { - float xmin = inferenceOutput[0].first[i * 4 + 0] / ratio; - float ymin = inferenceOutput[0].first[i * 4 + 1] / ratio; - float xmax = inferenceOutput[0].first[i * 4 + 2] / ratio; - float ymax = inferenceOutput[0].first[i * 4 + 3] / ratio; - - xmin = std::max(xmin, 0); - ymin = std::max(ymin, 0); - xmax = std::min(xmax, inputImg.cols); - ymax = std::min(ymax, inputImg.rows); - - bboxes.emplace_back(std::array{xmin, ymin, xmax, ymax}); - classIndices.emplace_back(reinterpret_cast(inferenceOutput[1].first)[i]); - scores.emplace_back(inferenceOutput[2].first[i]); - - cv::Mat curMask(28, 28, CV_32FC1); - memcpy( - curMask.data, - inferenceOutput[3].first + i * 28 * 28, - 28 * 28 * sizeof(float)); - masks.emplace_back(curMask); - } - } - - if (bboxes.size() == 0) { - return inputImg; - } - - return localize_visualize( - inputImg, - depthImg, - camera_info, - bboxes, - classIndices, - masks, - this->getClassNames(), - 0.5); -} - -// Mutator 5: Tracking -cv::Mat P3OrtBase::infer_visualize( - const cv::Mat & inputImg, - const cv::Mat & depthImg, - sensor_msgs::msg::CameraInfo camera_info, - const std::string tracker_type, - std::vector> & trackers, - std::vector & tracker_logs, - std::vector & tracker_results, - int newW, - int newH, - int paddedW, - int paddedH, - float ratio, - float * dst, - float confThresh, - const cv::Scalar & meanVal) -{ - cv::Mat tmpImg; - cv::resize(inputImg, tmpImg, cv::Size(newW, newH)); - - tmpImg.convertTo(tmpImg, CV_32FC3); - tmpImg -= meanVal; - - cv::Mat paddedImg(paddedH, paddedW, CV_32FC3, cv::Scalar(0, 0, 0)); - tmpImg.copyTo(paddedImg(cv::Rect(0, 0, newW, newH))); - - this->preprocess(dst, paddedImg, paddedW, paddedH, 3); - - // boxes, labels, scores, masks - auto inferenceOutput = (*this)({dst}); - - assert(inferenceOutput[1].second.size() == 1); - size_t nBoxes = inferenceOutput[1].second[0]; - - std::vector> bboxes; - std::vector classIndices; - std::vector scores; - std::vector masks; - - bboxes.reserve(nBoxes); - classIndices.reserve(nBoxes); - scores.reserve(nBoxes); - masks.reserve(nBoxes); - - for (size_t i = 0; i < nBoxes; ++i) { - if (inferenceOutput[2].first[i] > confThresh) { - float xmin = inferenceOutput[0].first[i * 4 + 0] / ratio; - float ymin = inferenceOutput[0].first[i * 4 + 1] / ratio; - float xmax = inferenceOutput[0].first[i * 4 + 2] / ratio; - float ymax = inferenceOutput[0].first[i * 4 + 3] / ratio; - - xmin = std::max(xmin, 0); - ymin = std::max(ymin, 0); - xmax = std::min(xmax, inputImg.cols); - ymax = std::min(ymax, inputImg.rows); - - bboxes.emplace_back(std::array{xmin, ymin, xmax, ymax}); - classIndices.emplace_back(reinterpret_cast(inferenceOutput[1].first)[i]); - scores.emplace_back(inferenceOutput[2].first[i]); - - cv::Mat curMask(28, 28, CV_32FC1); - memcpy( - curMask.data, - inferenceOutput[3].first + i * 28 * 28, - 28 * 28 * sizeof(float)); - masks.emplace_back(curMask); - } - } - // DEBUG - // Evaluate detection results and tracking results. - tracking_evaluate(bboxes, inputImg, tracker_type, trackers, tracker_logs, tracker_results); - - if (bboxes.size() == 0) { - return inputImg; - } - - return tracking_visualize( - inputImg, - depthImg, - camera_info, - tracker_results, - bboxes, - classIndices, - masks, - this->getClassNames(), - 0.5); -} - -// Mutator 4 -EPD::EPDObjectDetection P3OrtBase::infer_action( - const cv::Mat & inputImg, - int newW, - int newH, - int paddedW, - int paddedH, - float ratio, - float * dst, - float confThresh, - const cv::Scalar & meanVal) -{ - cv::Mat tmpImg; - cv::resize(inputImg, tmpImg, cv::Size(newW, newH)); - - tmpImg.convertTo(tmpImg, CV_32FC3); - tmpImg -= meanVal; - - cv::Mat paddedImg(paddedH, paddedW, CV_32FC3, cv::Scalar(0, 0, 0)); - tmpImg.copyTo(paddedImg(cv::Rect(0, 0, newW, newH))); - - this->preprocess(dst, paddedImg, paddedW, paddedH, 3); - - // boxes, labels, scores, masks - auto inferenceOutput = (*this)({dst}); - - assert(inferenceOutput[1].second.size() == 1); - size_t nBoxes = inferenceOutput[1].second[0]; - - std::vector> bboxes; - std::vector classIndices; - std::vector scores; - std::vector masks; - - bboxes.reserve(nBoxes); - classIndices.reserve(nBoxes); - scores.reserve(nBoxes); - masks.reserve(nBoxes); - - for (size_t i = 0; i < nBoxes; ++i) { - if (inferenceOutput[2].first[i] > confThresh) { - float xmin = inferenceOutput[0].first[i * 4 + 0] / ratio; - float ymin = inferenceOutput[0].first[i * 4 + 1] / ratio; - float xmax = inferenceOutput[0].first[i * 4 + 2] / ratio; - float ymax = inferenceOutput[0].first[i * 4 + 3] / ratio; - - xmin = std::max(xmin, 0); - ymin = std::max(ymin, 0); - xmax = std::min(xmax, inputImg.cols); - ymax = std::min(ymax, inputImg.rows); - - bboxes.emplace_back(std::array{xmin, ymin, xmax, ymax}); - classIndices.emplace_back(reinterpret_cast(inferenceOutput[1].first)[i]); - scores.emplace_back(inferenceOutput[2].first[i]); - - cv::Mat curMask(28, 28, CV_32FC1); - memcpy( - curMask.data, - inferenceOutput[3].first + i * 28 * 28, - 28 * 28 * sizeof(float)); - masks.emplace_back(curMask); - } - } - - if (bboxes.size() == 0) { - EPD::EPDObjectDetection output_msg(0); - return output_msg; - } - - EPD::activateUseCase(inputImg, bboxes, classIndices, scores, masks, this->getClassNames()); - - EPD::EPDObjectDetection output_obj(bboxes.size()); - output_obj.bboxes = bboxes; - output_obj.classIndices = classIndices; - output_obj.scores = scores; - output_obj.masks = masks; - - return output_obj; -} - -cv::Mat P3OrtBase::visualize( - const cv::Mat & img, - const std::vector> & bboxes, - const std::vector & classIndices, - const std::vector & masks, - const std::vector & allClassNames = {}, - const float maskThreshold = 0.5) -{ - assert(bboxes.size() == classIndices.size()); - if (!allClassNames.empty()) { - assert(allClassNames.size() > *std::max_element(classIndices.begin(), classIndices.end())); - } - - cv::Scalar allColors(255.0, 0.0, 0.0, 0.0); - - cv::Mat result = img.clone(); - - for (size_t i = 0; i < bboxes.size(); ++i) { - const auto & curBbox = bboxes[i]; - const uint64_t classIdx = classIndices[i]; - cv::Mat curMask = masks[i].clone(); - const cv::Scalar & curColor = allColors; - const std::string curLabel = allClassNames.empty() ? - std::to_string(classIdx) : allClassNames[classIdx]; - - cv::rectangle( - result, cv::Point(curBbox[0], curBbox[1]), - cv::Point(curBbox[2], curBbox[3]), curColor, 2); - - int baseLine = 0; - cv::Size labelSize = cv::getTextSize( - curLabel, cv::FONT_HERSHEY_COMPLEX, - 0.35, 1, &baseLine); - cv::rectangle( - result, cv::Point(curBbox[0], curBbox[1]), - cv::Point( - curBbox[0] + labelSize.width, curBbox[1] + - static_cast(1.3 * labelSize.height)), - curColor, -1); - cv::putText( - result, curLabel, cv::Point(curBbox[0], curBbox[1] + labelSize.height), - cv::FONT_HERSHEY_COMPLEX, - 0.35, cv::Scalar(255, 255, 255)); - - // Visualize masks - const cv::Rect curBoxRect(cv::Point(curBbox[0], curBbox[1]), - cv::Point(curBbox[2], curBbox[3])); - - cv::resize(curMask, curMask, curBoxRect.size()); - - cv::Mat finalMask = (curMask > maskThreshold); - - cv::Mat coloredRoi = (0.3 * curColor + 0.7 * result(curBoxRect)); + inferenceOutput[3].first + i * 28 * 28, + 28 * 28 * sizeof(float)); + masks.emplace_back(curMask); + } + } - coloredRoi.convertTo(coloredRoi, CV_8UC3); + if (bboxes.size() == 0) { + EPD::EPDObjectDetection output_msg(0); + return output_msg; + } - std::vector contours; - cv::Mat hierarchy; - finalMask.convertTo(finalMask, CV_8U); + EPD::EPDObjectDetection output_obj(bboxes.size()); + output_obj.bboxes = bboxes; + output_obj.classIndices = classIndices; + output_obj.scores = scores; + output_obj.masks = masks; - cv::findContours(finalMask, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_SIMPLE); - cv::drawContours(coloredRoi, contours, -1, curColor, 5, cv::LINE_8, hierarchy, 100); - coloredRoi.copyTo(result(curBoxRect), finalMask); - } - return result; + return output_obj; } double P3OrtBase::findMedian(cv::Mat depthImg) @@ -622,420 +266,10 @@ double P3OrtBase::findMin(cv::Mat depthImg) return min; } -cv::Mat P3OrtBase::localize_visualize( - const cv::Mat & img, - const cv::Mat & depthImg, - sensor_msgs::msg::CameraInfo camera_info, - const std::vector> & bboxes, - const std::vector & classIndices, - const std::vector & masks, - const std::vector & allClassNames = {}, - const float maskThreshold = 0.5) -{ - assert(bboxes.size() == classIndices.size()); - if (!allClassNames.empty()) { - assert( - allClassNames.size() > - *std::max_element(classIndices.begin(), classIndices.end())); - } - - cv::Scalar allColors(0.0, 0.0, 255.0, 0.0); - - // // num of objects will be equal to number of bboxes - cv::Mat result = img.clone(); - for (size_t i = 0; i < bboxes.size(); ++i) { - const auto & curBbox = bboxes[i]; - const uint64_t classIdx = classIndices[i]; - cv::Mat curMask = masks[i].clone(); - const cv::Scalar & curColor = allColors[classIdx]; - const std::string curLabel = allClassNames.empty() ? - std::to_string(classIdx) : - allClassNames[classIdx]; - - cv::rectangle( - result, cv::Point(curBbox[0], curBbox[1]), - cv::Point(curBbox[2], curBbox[3]), curColor, 2); - - int baseLine = 0; - cv::Size labelSize = - cv::getTextSize(curLabel, cv::FONT_HERSHEY_COMPLEX, 0.35, 1, &baseLine); - cv::rectangle( - result, cv::Point( - curBbox[0], curBbox[1]), - cv::Point( - curBbox[0] + labelSize.width, - curBbox[1] + static_cast(1.3 * labelSize.height)), - curColor, -1); - cv::putText( - result, curLabel, - cv::Point(curBbox[0], curBbox[1] + labelSize.height), - cv::FONT_HERSHEY_COMPLEX, 0.35, cv::Scalar(255, 255, 255)); - - // Visualizing masks - const cv::Rect curBoxRect(cv::Point(curBbox[0], curBbox[1]), - cv::Point(curBbox[2], curBbox[3])); - - cv::resize(curMask, curMask, curBoxRect.size()); - - // Assigning masks that exceed the maskThreshold. - cv::Mat finalMask = (curMask > maskThreshold); - - // Assigning coloredRoi with the bounding box. - cv::Mat coloredRoi = (0.3 * curColor + 0.7 * result(curBoxRect)); - - coloredRoi.convertTo(coloredRoi, CV_8UC3); - - std::vector contours; - cv::Mat hierarchy; - cv::Mat tempFinalMask; - finalMask.convertTo(tempFinalMask, CV_8U); - // Generate red contour lines of segmentation mask. - cv::findContours( - tempFinalMask, contours, hierarchy, cv::RETR_TREE, - cv::CHAIN_APPROX_SIMPLE); - // Draw red contour lines on output image. - cv::drawContours( - coloredRoi, contours, -1, cv::Scalar(0, 0, 255), 5, cv::LINE_8, - hierarchy, 100); - - // For more details, refer to link below: - // https://tinyurl.com/y5qnnxud - float ppx = camera_info.k.at(2); - float fx = camera_info.k.at(0); - float ppy = camera_info.k.at(5); - float fy = camera_info.k.at(4); - - // Getting rotated rectangle and draw the major axis - std::vector minRect(contours.size()); - float obj_surface_depth; - float object_length, object_breadth, object_height; - cv::Point pt_a, pt_b, pt_c, pt_d; - cv::Point rotated_mid; - - // Getting only the largest contour - // The largest contour is the one which has the largest area. - // TODO(cardboardcode): Changed according to your use case. - double maxArea = 0; - int maxAreaContourId = 999; - for (unsigned int j = 0; j < contours.size(); j++) { - double newArea = cv::contourArea(contours[j]); - if (newArea > maxArea) { - maxArea = newArea; - maxAreaContourId = j; - } // End if - } // End for - unsigned int maxID = maxAreaContourId; - - for (unsigned int index = 0; index < contours.size(); index++) { - if (index != maxID) { - continue; - } - // Compute rotated rectangle based on contours - minRect[index] = cv::minAreaRect(cv::Mat(contours[index])); - cv::Point2f rect_points[4]; - // 4 points of the rotated rectangle - minRect[index].points(rect_points); - - // Mid points of the each side of the rotated rectangle - pt_a = (rect_points[0] + rect_points[3]) / 2; - pt_b = (rect_points[1] + rect_points[2]) / 2; - pt_c = (rect_points[0] + rect_points[1]) / 2; - pt_d = (rect_points[3] + rect_points[2]) / 2; - - // Add the top left corner to the coordinate in the small bbox - // For temporary, bboxes center - rotated_mid = (cv::Point(curBbox[0], curBbox[1]) + - cv::Point(curBbox[2], curBbox[3])) / 2; - - // Get coordinates of the object center - float table_depth = this->findMedian(depthImg) * 0.001; - obj_surface_depth = this->findMin(depthImg(curBoxRect)) * 0.001; - - float x = (rotated_mid.x - ppx) / fx * obj_surface_depth; - float y = (rotated_mid.y - ppy) / fy * obj_surface_depth; - - std::cout << "[-cam -> table-] = " << table_depth << - " meters" << std::endl; - std::cout << "[-cam -> obj_top-] = " << obj_surface_depth << - " meters" << std::endl; - - std::cout << "[-OBJ centroid x-] = " << x << std::endl; - std::cout << "[-OBJ centroid y-] = " << y << std::endl; - std::cout << "[-OBJ centroid z-] = " << obj_surface_depth + - (table_depth - obj_surface_depth) / 2 << std::endl; - - // Get estimated size of object - // Compare the length of 2 side of the rectangle, - // the longer side will be the major axis - if (cv::norm(rect_points[0] - rect_points[1]) > - cv::norm(rect_points[1] - rect_points[2])) - { - // Draws the major axis(red) - cv::line(coloredRoi, pt_a, pt_b, cv::Scalar(0, 0, 255), 2); - // Draws the minor axis (green) - cv::line(coloredRoi, pt_c, pt_d, cv::Scalar(0, 255, 0), 2); - // Calculates the length of the object - object_length = obj_surface_depth * sqrt( - pow((pt_a.x - pt_b.x) / fx, 2) + - pow( - (pt_a.y - pt_b.y) / fy, - 2)); - // Calculates the breadth of the object - object_breadth = obj_surface_depth * sqrt( - pow((pt_c.x - pt_d.x) / fx, 2) + - pow( - (pt_c.y - pt_d.y) / fy, - 2)); - - } else { - // Draw the major axis - cv::line(coloredRoi, pt_c, pt_d, cv::Scalar(0, 0, 255), 2); - // Draw the minor axis (green) - cv::line(coloredRoi, pt_a, pt_b, cv::Scalar(0, 255, 0), 2); - // Get object breadth and length - object_breadth = obj_surface_depth * sqrt( - pow((pt_a.x - pt_b.x) / fx, 2) + - pow((pt_a.y - pt_b.y) / fy, 2)); - object_length = obj_surface_depth * sqrt( - pow((pt_c.x - pt_d.x) / fx, 2) + - pow((pt_c.y - pt_d.y) / fy, 2)); - } - // Setting height of object - object_height = table_depth - obj_surface_depth; - - // TODO(cardboardcode): To provide optimized debug prints in future iterations. - std::cout << "[-OBJ Name-] = " << curLabel << std::endl; - std::cout << "[-OBJ Length-] = " << object_length << " meters" << std::endl; - std::cout << "[-OBJ Breadth-] = " << object_breadth << " meters" << std::endl; - std::cout << "[-OBJ Height-] = " << object_height << " meters" << std::endl; - - // Mark the center point with blue dot - cv::circle(coloredRoi, rotated_mid, 1, cv::Scalar(255, 0, 0), 1); - } - - // Push each Region-Of-Interest (ROI) in sequence - coloredRoi.copyTo(result(curBoxRect), finalMask); - } - - return result; -} - -cv::Mat P3OrtBase::tracking_visualize( - const cv::Mat & img, - const cv::Mat & depthImg, - sensor_msgs::msg::CameraInfo camera_info, - std::vector & tracker_results, - const std::vector> & bboxes, - const std::vector & classIndices, - const std::vector & masks, - const std::vector & allClassNames = {}, - const float maskThreshold = 0.5) -{ - assert(bboxes.size() == classIndices.size()); - if (!allClassNames.empty()) { - assert( - allClassNames.size() > - *std::max_element(classIndices.begin(), classIndices.end())); - } - - cv::Scalar allColors(0.0, 0.0, 255.0, 0.0); - cv::Scalar trackingColor(255.0, 0.0, 0.0, 0.0); - - cv::Mat result = img.clone(); - - for (size_t i = 0; i < tracker_results.size(); i++) { - cv::rectangle( - result, - tracker_results[i].obj_bounding_box, - trackingColor, - 4); - } - - for (size_t i = 0; i < bboxes.size(); ++i) { - const auto & curBbox = bboxes[i]; - const uint64_t classIdx = classIndices[i]; - cv::Mat curMask = masks[i].clone(); - const cv::Scalar & curColor = allColors[classIdx]; - std::string curLabel = allClassNames.empty() ? - std::to_string(classIdx) : - allClassNames[classIdx]; - - cv::rectangle( - result, cv::Point(curBbox[0], curBbox[1]), - cv::Point(curBbox[2], curBbox[3]), curColor, 2); - - int baseLine = 0; - cv::Size labelSize = - cv::getTextSize(curLabel, cv::FONT_HERSHEY_COMPLEX, 0.35, 1, &baseLine); - cv::rectangle( - result, cv::Point( - curBbox[0], curBbox[1]), - cv::Point( - curBbox[0] + labelSize.width, - curBbox[1] + static_cast(1.3 * labelSize.height)), - curColor, -1); - - // Update curLabel with tracker number tag for object. - for (size_t j = 0; j < tracker_results.size(); j++) { - if (tracker_results[j].obj_bounding_box.x == curBbox[0] && - tracker_results[j].obj_bounding_box.y == curBbox[1] && - tracker_results[j].obj_bounding_box.width == curBbox[2] - curBbox[0] && - tracker_results[j].obj_bounding_box.height == curBbox[3] - curBbox[1]) - { - curLabel = curLabel + "_" + std::string(tracker_results[j].obj_tag); - } - } - cv::putText( - result, curLabel, - cv::Point(curBbox[0], curBbox[1] + labelSize.height), - cv::FONT_HERSHEY_COMPLEX, 0.55, cv::Scalar(255, 255, 255)); - - // Visualizing masks - const cv::Rect curBoxRect(cv::Point(curBbox[0], curBbox[1]), - cv::Point(curBbox[2], curBbox[3])); - - cv::resize(curMask, curMask, curBoxRect.size()); - - // Assigning masks that exceed the maskThreshold. - cv::Mat finalMask = (curMask > maskThreshold); - - // Assigning coloredRoi with the bounding box. - cv::Mat coloredRoi = (0.3 * curColor + 0.7 * result(curBoxRect)); - - coloredRoi.convertTo(coloredRoi, CV_8UC3); - - std::vector contours; - cv::Mat hierarchy; - cv::Mat tempFinalMask; - finalMask.convertTo(tempFinalMask, CV_8U); - // Generate red contour lines of segmentation mask. - cv::findContours( - tempFinalMask, contours, hierarchy, cv::RETR_TREE, - cv::CHAIN_APPROX_SIMPLE); - // Draw red contour lines on output image. - cv::drawContours( - coloredRoi, contours, -1, cv::Scalar(0, 0, 255), 5, cv::LINE_8, - hierarchy, 100); - - // For more details, refer to link below: - // https://tinyurl.com/y5qnnxud - float fx = camera_info.k.at(0); - float fy = camera_info.k.at(4); - - // Getting rotated rectangle and draw the major axis - std::vector minRect(contours.size()); - float obj_surface_depth; - float object_length, object_breadth, object_height; - cv::Point pt_a, pt_b, pt_c, pt_d; - cv::Point rotated_mid; - - // Getting only the largest contour - // The largest contour is the one which has the largest area. - // TODO(cardboardcode): Changed according to your use case. - double maxArea = 0; - int maxAreaContourId = 999; - for (unsigned int j = 0; j < contours.size(); j++) { - double newArea = cv::contourArea(contours[j]); - if (newArea > maxArea) { - maxArea = newArea; - maxAreaContourId = j; - } // End if - } // End for - unsigned int maxID = maxAreaContourId; - - for (unsigned int index = 0; index < contours.size(); index++) { - if (index != maxID) { - continue; - } - // Compute rotated rectangle based on contours - minRect[index] = cv::minAreaRect(cv::Mat(contours[index])); - cv::Point2f rect_points[4]; - // 4 points of the rotated rectangle - minRect[index].points(rect_points); - - // Mid points of the each side of the rotated rectangle - pt_a = (rect_points[0] + rect_points[3]) / 2; - pt_b = (rect_points[1] + rect_points[2]) / 2; - pt_c = (rect_points[0] + rect_points[1]) / 2; - pt_d = (rect_points[3] + rect_points[2]) / 2; - - // Add the top left corner to the coordinate in the small bbox - // For temporary, bboxes center - rotated_mid = (cv::Point(curBbox[0], curBbox[1]) + - cv::Point(curBbox[2], curBbox[3])) / 2; - - // Get coordinates of the object center - float table_depth = this->findMedian(depthImg) * 0.001; - obj_surface_depth = this->findMin(depthImg(curBoxRect)) * 0.001; - - // std::cout << "table_depth = " << table_depth << std::endl; - // std::cout << "obj_surface_depth = " << obj_surface_depth << std::endl; - - // std::cout << "[-OBJ centroid x-] = " << x << std::endl; - // std::cout << "[-OBJ centroid y-] = " << y << std::endl; - // std::cout << "[-OBJ centroid z-] = " << obj_surface_depth + - // (table_depth - obj_surface_depth) / 2 << std::endl; - - // Get estimated size of object - // Compare the length of 2 side of the rectangle, - // the longer side will be the major axis - if (cv::norm(rect_points[0] - rect_points[1]) > - cv::norm(rect_points[1] - rect_points[2])) - { - // Draws the major axis(red) - cv::line(coloredRoi, pt_a, pt_b, cv::Scalar(0, 0, 255), 2); - // Draws the minor axis (green) - cv::line(coloredRoi, pt_c, pt_d, cv::Scalar(0, 255, 0), 2); - // Calculates the length of the object - object_length = obj_surface_depth * sqrt( - pow((pt_a.x - pt_b.x) / fx, 2) + - pow( - (pt_a.y - pt_b.y) / fy, - 2)); - // Calculates the breadth of the object - object_breadth = obj_surface_depth * sqrt( - pow((pt_c.x - pt_d.x) / fx, 2) + - pow( - (pt_c.y - pt_d.y) / fy, - 2)); - - } else { - // Draw the major axis - cv::line(coloredRoi, pt_c, pt_d, cv::Scalar(0, 0, 255), 2); - // Draw the minor axis (green) - cv::line(coloredRoi, pt_a, pt_b, cv::Scalar(0, 255, 0), 2); - // Get object breadth and length - object_breadth = obj_surface_depth * sqrt( - pow((pt_a.x - pt_b.x) / fx, 2) + - pow((pt_a.y - pt_b.y) / fy, 2)); - object_length = obj_surface_depth * sqrt( - pow((pt_c.x - pt_d.x) / fx, 2) + - pow((pt_c.y - pt_d.y) / fy, 2)); - } - // Setting height of object - object_height = table_depth - obj_surface_depth; - - // TODO(cardboardcode): To provide optimized debug prints in future iterations. - std::cout << "[-OBJ Name-] = " << curLabel << std::endl; - std::cout << "[-OBJ Length-] = " << object_length << std::endl; - std::cout << "[-OBJ Breadth-] = " << object_breadth << std::endl; - std::cout << "[-OBJ Height-] = " << object_height << std::endl; - - // Mark the center point with blue dot - cv::circle(coloredRoi, rotated_mid, 1, cv::Scalar(255, 0, 0), 1); - } - - // Push each Region-Of-Interest (ROI) in sequence - coloredRoi.copyTo(result(curBoxRect), finalMask); - } - - return result; -} - // DEBUG // A mutator function that will output an EPD::EPDObjectLocalization object that // contains all information required for Localization. -EPD::EPDObjectLocalization P3OrtBase::infer_action( +EPD::EPDObjectLocalization P3OrtBase::infer( const cv::Mat & inputImg, const cv::Mat & depthImg, sensor_msgs::msg::CameraInfo camera_info, @@ -1309,7 +543,7 @@ EPD::EPDObjectLocalization P3OrtBase::infer_action( // DEBUG // A mutator function that will output an EPD::EPDObjectTracking object that // contains all information required for Localization. -EPD::EPDObjectTracking P3OrtBase::infer_action( +EPD::EPDObjectTracking P3OrtBase::infer( const cv::Mat & inputImg, const cv::Mat & depthImg, sensor_msgs::msg::CameraInfo camera_info, @@ -1385,12 +619,12 @@ EPD::EPDObjectTracking P3OrtBase::infer_action( float maskThreshold = 0.5; cv::Mat result = inputImg.clone(); - assert(bboxes.size() == classIndices.size()); - if (!allClassNames.empty()) { - assert( - allClassNames.size() > - *std::max_element(classIndices.begin(), classIndices.end())); - } + // assert(bboxes.size() == classIndices.size()); + // if (!allClassNames.empty()) { + // assert( + // allClassNames.size() > + // *std::max_element(classIndices.begin(), classIndices.end())); + // } // If there is zero bounding boxes generated, return empty EPDObjectTracking object. if (bboxes.size() == 0) { diff --git a/easy_perception_deployment/include/ort_cpp_lib/p3_ort_base.hpp b/easy_perception_deployment/include/ort_cpp_lib/p3_ort_base.hpp index 9021aad..7fa5824 100644 --- a/easy_perception_deployment/include/ort_cpp_lib/p3_ort_base.hpp +++ b/easy_perception_deployment/include/ort_cpp_lib/p3_ort_base.hpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -57,41 +57,20 @@ class P3OrtBase : public OrtBase /*! \brief A Destructor function*/ ~P3OrtBase(); /*! \brief A auxillary Mutator function that calls the internal overloading - infer_visualize function.*/ - cv::Mat infer_visualize(const cv::Mat & inputImg); + infer function.*/ + EPD::EPDObjectDetection infer(const cv::Mat & inputImg); /*! \brief A auxillary Mutator function that calls the internal overloading - infer_visualize function with depth_msg and camera_info.*/ - cv::Mat infer_visualize( - const cv::Mat & inputImg, - const cv::Mat & depthImg, - sensor_msgs::msg::CameraInfo camera_info); - /*! \brief A auxillary Mutator function that calls the internal overloading - infer_visualize function with depth_msg and camera_info.*/ - cv::Mat infer_visualize( - const cv::Mat & inputImg, - const cv::Mat & depthImg, - sensor_msgs::msg::CameraInfo camera_info, - const std::string tracker_type, - std::vector> & trackers, - std::vector & tracker_logs, - std::vector & tracker_results); - - /*! \brief A auxillary Mutator function that calls the internal overloading - infer_action function.*/ - EPD::EPDObjectDetection infer_action(const cv::Mat & inputImg); - - /*! \brief A auxillary Mutator function that calls the internal overloading - infer_action function.*/ - EPD::EPDObjectLocalization infer_action( + infer function.*/ + EPD::EPDObjectLocalization infer( const cv::Mat & inputImg, const cv::Mat & depthImg, sensor_msgs::msg::CameraInfo camera_info, double camera_to_plane_distance_mm); /*! \brief A auxillary Mutator function that calls the internal overloading - infer_action function.*/ - EPD::EPDObjectTracking infer_action( + infer function.*/ + EPD::EPDObjectTracking infer( const cv::Mat & inputImg, const cv::Mat & depthImg, sensor_msgs::msg::CameraInfo camera_info, @@ -136,56 +115,9 @@ class P3OrtBase : public OrtBase const int64_t targetImgHeight, const int numChannels) const; - /*! \brief A Mutator function that runs a P3 Ort Session and gets P3 - inference result for visualization purposes.*/ - cv::Mat infer_visualize( - const cv::Mat & inputImg, - int newW, - int newH, - int paddedW, - int paddedH, - float ratio, - float * dst, - float confThresh, - const cv::Scalar & meanVal); - - /*! \brief A Mutator function that runs a P3 Ort Session and gets P3 - inference result for visualization purposes with localization results.*/ - cv::Mat infer_visualize( - const cv::Mat & inputImg, - const cv::Mat & depthImg, - sensor_msgs::msg::CameraInfo camera_info, - int newW, - int newH, - int paddedW, - int paddedH, - float ratio, - float * dst, - float confThresh, - const cv::Scalar & meanVal); - - /*! \brief A Mutator function that runs a P3 Ort Session and gets P3 - inference result for visualization purposes with localization results.*/ - cv::Mat infer_visualize( - const cv::Mat & inputImg, - const cv::Mat & depthImg, - sensor_msgs::msg::CameraInfo camera_info, - const std::string tracker_type, - std::vector> & trackers, - std::vector & tracker_logs, - std::vector & tracker_results, - int newW, - int newH, - int paddedW, - int paddedH, - float ratio, - float * dst, - float confThresh, - const cv::Scalar & meanVal); - /*! \brief A Mutator function that runs a P3 Ort Session and gets P3 inference result for use by external agents.*/ - EPD::EPDObjectDetection infer_action( + EPD::EPDObjectDetection infer( const cv::Mat & inputImg, int newW, int newH, @@ -198,7 +130,7 @@ class P3OrtBase : public OrtBase /*! \brief A Mutator function that runs a P3 Ort Session and gets P3 inference result with Localization results for use by external agents.*/ - EPD::EPDObjectLocalization infer_action( + EPD::EPDObjectLocalization infer( const cv::Mat & inputImg, const cv::Mat & depthImg, sensor_msgs::msg::CameraInfo camera_info, @@ -214,7 +146,7 @@ class P3OrtBase : public OrtBase /*! \brief A Mutator function that runs a P3 Ort Session and gets P3 inference result with Tracking results for use by external agents.*/ - EPD::EPDObjectTracking infer_action( + EPD::EPDObjectTracking infer( const cv::Mat & inputImg, const cv::Mat & depthImg, sensor_msgs::msg::CameraInfo camera_info, @@ -279,20 +211,6 @@ class P3OrtBase : public OrtBase const std::vector & masks, const std::vector & allClassNames, const float maskThreshold); - - /*! \brief A Mutator function that takes P3 inference outputs and illustrates - derived bounding boxes with corresponding object labels for visualization - purposes and tracked localization.*/ - cv::Mat tracking_visualize( - const cv::Mat & img, - const cv::Mat & depthImg, - sensor_msgs::msg::CameraInfo camera_info, - std::vector & tracker_results, - const std::vector> & bboxes, - const std::vector & classIndices, - const std::vector & masks, - const std::vector & allClassNames, - const float maskThreshold); }; } // namespace Ort diff --git a/easy_perception_deployment/package.xml b/easy_perception_deployment/package.xml index 69d79fb..18e2ac9 100644 --- a/easy_perception_deployment/package.xml +++ b/easy_perception_deployment/package.xml @@ -4,7 +4,7 @@ easy_perception_deployment 0.1.0 A ROS2 package that accelerates the training and deployment of CV models in industries - rosi + rosi Apache License 2.0 ament_cmake diff --git a/easy_perception_deployment/scripts/build_local.bash b/easy_perception_deployment/scripts/build_local.bash index 6f7fbf8..48979d5 100755 --- a/easy_perception_deployment/scripts/build_local.bash +++ b/easy_perception_deployment/scripts/build_local.bash @@ -1,7 +1,7 @@ #!/usr/bin/env bash -# Copyright 2020 Advanced Remanufacturing and Technology Centre -# Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +# Copyright 2022 Advanced Remanufacturing and Technology Centre +# Copyright 2022 ROS-Industrial Consortium Asia Pacific Team # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/scripts/create_desktop_shortcut.bash b/easy_perception_deployment/scripts/create_desktop_shortcut.bash index 1188a32..417b33a 100755 --- a/easy_perception_deployment/scripts/create_desktop_shortcut.bash +++ b/easy_perception_deployment/scripts/create_desktop_shortcut.bash @@ -1,7 +1,7 @@ #!/usr/bin/env bash -# Copyright 2020 Advanced Remanufacturing and Technology Centre -# Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +# Copyright 2022 Advanced Remanufacturing and Technology Centre +# Copyright 2022 ROS-Industrial Consortium Asia Pacific Team # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/scripts/install_dep_cpu.bash b/easy_perception_deployment/scripts/install_dep_cpu.bash index c7d32ba..faf4767 100755 --- a/easy_perception_deployment/scripts/install_dep_cpu.bash +++ b/easy_perception_deployment/scripts/install_dep_cpu.bash @@ -1,7 +1,7 @@ #!/usr/bin/env bash -# Copyright 2020 Advanced Remanufacturing and Technology Centre -# Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +# Copyright 2022 Advanced Remanufacturing and Technology Centre +# Copyright 2022 ROS-Industrial Consortium Asia Pacific Team # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/scripts/install_dep_gpu.bash b/easy_perception_deployment/scripts/install_dep_gpu.bash index 0bd8f42..2ca3c5c 100755 --- a/easy_perception_deployment/scripts/install_dep_gpu.bash +++ b/easy_perception_deployment/scripts/install_dep_gpu.bash @@ -1,7 +1,7 @@ #!/usr/bin/env bash -# Copyright 2020 Advanced Remanufacturing and Technology Centre -# Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +# Copyright 2022 Advanced Remanufacturing and Technology Centre +# Copyright 2022 ROS-Industrial Consortium Asia Pacific Team # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/scripts/run_local.bash b/easy_perception_deployment/scripts/run_local.bash index c48148d..9342d41 100755 --- a/easy_perception_deployment/scripts/run_local.bash +++ b/easy_perception_deployment/scripts/run_local.bash @@ -1,7 +1,7 @@ #!/usr/bin/env bash -# Copyright 2020 Advanced Remanufacturing and Technology Centre -# Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +# Copyright 2022 Advanced Remanufacturing and Technology Centre +# Copyright 2022 ROS-Industrial Consortium Asia Pacific Team # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/src/main.cpp b/easy_perception_deployment/src/main.cpp index c68b248..ea0a7c7 100644 --- a/easy_perception_deployment/src/main.cpp +++ b/easy_perception_deployment/src/main.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/test/p1u0a.cpp b/easy_perception_deployment/test/p1u0a.cpp index 2fc9e20..0969cac 100644 --- a/easy_perception_deployment/test/p1u0a.cpp +++ b/easy_perception_deployment/test/p1u0a.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/test/p2u1a.cpp b/easy_perception_deployment/test/p2u1a.cpp index 5bc0bdf..f64aeeb 100644 --- a/easy_perception_deployment/test/p2u1a.cpp +++ b/easy_perception_deployment/test/p2u1a.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -77,7 +77,7 @@ TEST(EPD_TestSuite, Test_P2Model_Counting_Action) ASSERT_EQ(!ortAgent_->p2_ort_session, false); - EPD::EPDObjectDetection result = ortAgent_->p2_ort_session->infer_action(frame); + EPD::EPDObjectDetection result = ortAgent_->p2_ort_session->infer(frame); ASSERT_NE(result.bboxes.size(), unsigned(0)); ASSERT_NE(result.classIndices.size(), unsigned(0)); ASSERT_NE(result.scores.size(), unsigned(0)); diff --git a/easy_perception_deployment/test/p2u1v.cpp b/easy_perception_deployment/test/p2u1v.cpp index d1ec214..912949d 100644 --- a/easy_perception_deployment/test/p2u1v.cpp +++ b/easy_perception_deployment/test/p2u1v.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,6 +21,7 @@ #include "gtest/gtest.h" #include "bits/stdc++.h" #include "epd_utils_lib/epd_container.hpp" +#include "epd_utils_lib/message_utils.hpp" std::string PATH_TO_SESSION_CONFIG(PATH_TO_PACKAGE "/config/session_config.json"); std::string PATH_TO_USECASE_CONFIG(PATH_TO_PACKAGE "/config/usecase_config.json"); @@ -79,7 +80,8 @@ TEST(EPD_TestSuite, Test_P2Model_Counting_Visualize) ASSERT_EQ(!ortAgent_->p2_ort_session, false); - cv::Mat output = ortAgent_->p2_ort_session->infer_visualize(frame); + EPD::EPDObjectDetection result = ortAgent_->p2_ort_session->infer(frame); + cv::Mat output = ortAgent_->visualize(result, frame); ASSERT_NE(output.cols, 0); ASSERT_NE(output.rows, 0); } diff --git a/easy_perception_deployment/test/p2u2a.cpp b/easy_perception_deployment/test/p2u2a.cpp index ab6e150..662abca 100644 --- a/easy_perception_deployment/test/p2u2a.cpp +++ b/easy_perception_deployment/test/p2u2a.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -77,7 +77,7 @@ TEST(EPD_TestSuite, Test_P2Model_ColorMatch_Action) ASSERT_EQ(!ortAgent_->p2_ort_session, false); - EPD::EPDObjectDetection result = ortAgent_->p2_ort_session->infer_action(frame); + EPD::EPDObjectDetection result = ortAgent_->p2_ort_session->infer(frame); ASSERT_NE(result.bboxes.size(), unsigned(0)); ASSERT_NE(result.classIndices.size(), unsigned(0)); ASSERT_NE(result.scores.size(), unsigned(0)); diff --git a/easy_perception_deployment/test/p3u1v.cpp b/easy_perception_deployment/test/p3u1v.cpp index 805df8c..6745f3e 100644 --- a/easy_perception_deployment/test/p3u1v.cpp +++ b/easy_perception_deployment/test/p3u1v.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,6 +21,7 @@ #include "gtest/gtest.h" #include "bits/stdc++.h" #include "epd_utils_lib/epd_container.hpp" +#include "epd_utils_lib/message_utils.hpp" std::string PATH_TO_SESSION_CONFIG(PATH_TO_PACKAGE "/config/session_config.json"); std::string PATH_TO_USECASE_CONFIG(PATH_TO_PACKAGE "/config/usecase_config.json"); @@ -79,7 +80,9 @@ TEST(EPD_TestSuite, Test_P3Model_Counting_Visualize) ASSERT_EQ(!ortAgent_->p3_ort_session, false); - cv::Mat output = ortAgent_->p3_ort_session->infer_visualize(frame); + EPD::EPDObjectDetection result = ortAgent_->p3_ort_session->infer(frame); + cv::Mat output = ortAgent_->visualize(result, frame); + ASSERT_NE(output.cols, 0); ASSERT_NE(output.rows, 0); } diff --git a/easy_perception_deployment/test/p3u2a.cpp b/easy_perception_deployment/test/p3u2a.cpp index 6b7a6a2..fe777ba 100644 --- a/easy_perception_deployment/test/p3u2a.cpp +++ b/easy_perception_deployment/test/p3u2a.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -76,7 +76,7 @@ TEST(EPD_TestSuite, Test_P3Model_ColorMatch_Action) ASSERT_EQ(!ortAgent_->p3_ort_session, false); - EPD::EPDObjectDetection result = ortAgent_->p3_ort_session->infer_action(frame); + EPD::EPDObjectDetection result = ortAgent_->p3_ort_session->infer(frame); ASSERT_NE(result.bboxes.size(), unsigned(0)); ASSERT_NE(result.classIndices.size(), unsigned(0)); ASSERT_NE(result.scores.size(), unsigned(0)); diff --git a/easy_perception_deployment/test/p3u2v.cpp b/easy_perception_deployment/test/p3u2v.cpp index 1e4603c..2e6fa21 100644 --- a/easy_perception_deployment/test/p3u2v.cpp +++ b/easy_perception_deployment/test/p3u2v.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,6 +21,7 @@ #include "gtest/gtest.h" #include "bits/stdc++.h" #include "epd_utils_lib/epd_container.hpp" +#include "epd_utils_lib/message_utils.hpp" std::string PATH_TO_SESSION_CONFIG(PATH_TO_PACKAGE "/config/session_config.json"); std::string PATH_TO_USECASE_CONFIG(PATH_TO_PACKAGE "/config/usecase_config.json"); @@ -78,7 +79,8 @@ TEST(EPD_TestSuite, Test_P3Model_ColorMatch_Visualize) ASSERT_EQ(!ortAgent_->p3_ort_session, false); - cv::Mat output = ortAgent_->p3_ort_session->infer_visualize(frame); + EPD::EPDObjectDetection result = ortAgent_->p3_ort_session->infer(frame); + cv::Mat output = ortAgent_->visualize(result, frame); ASSERT_NE(output.cols, 0); ASSERT_NE(output.rows, 0); diff --git a/easy_perception_deployment/test/p3u3a.cpp b/easy_perception_deployment/test/p3u3a.cpp index 16a4d2d..33a01d2 100644 --- a/easy_perception_deployment/test/p3u3a.cpp +++ b/easy_perception_deployment/test/p3u3a.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -83,7 +83,7 @@ TEST(EPD_TestSuite, Test_P3Model_Localize_Action) ASSERT_EQ(!ortAgent_->p3_ort_session, false); - EPD::EPDObjectLocalization result = ortAgent_->p3_ort_session->infer_action( + EPD::EPDObjectLocalization result = ortAgent_->p3_ort_session->infer( colored_img, depth_img, camera_info, diff --git a/easy_perception_deployment/test/p3u3v.cpp b/easy_perception_deployment/test/p3u3v.cpp index a465f71..b1e8ae8 100644 --- a/easy_perception_deployment/test/p3u3v.cpp +++ b/easy_perception_deployment/test/p3u3v.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -83,13 +83,32 @@ TEST(EPD_TestSuite, Test_P3Model_Localize_Visualize) ASSERT_EQ(!ortAgent_->p3_ort_session, false); - cv::Mat result = ortAgent_->p3_ort_session->infer_visualize( + EPD::EPDObjectLocalization result = ortAgent_->p3_ort_session->infer( colored_img, depth_img, - camera_info); - - ASSERT_NE(result.cols, 0); - ASSERT_NE(result.rows, 0); + camera_info, + 0.1); + + EPD::EPDObjectTracking converted_result(result.data_size); + converted_result.object_ids.clear(); + for (size_t i = 0; i < result.data_size; i++) { + EPD::EPDObjectTracking::LocalizedObject object; + object.name = result.objects[i].name; + object.roi = result.objects[i].roi; + object.mask = result.objects[i].mask; + object.length = result.objects[i].length; + object.breadth = result.objects[i].breadth; + object.height = result.objects[i].height; + object.segmented_pcl = result.objects[i].segmented_pcl; + object.axis = result.objects[i].axis; + + converted_result.objects.emplace_back(object); + } + + cv::Mat output = ortAgent_->visualize(converted_result, colored_img); + + ASSERT_NE(output.cols, 0); + ASSERT_NE(output.rows, 0); } int main(int argc, char ** argv) diff --git a/easy_perception_deployment/test/p3u4a.cpp b/easy_perception_deployment/test/p3u4a.cpp index 3bffab3..c074578 100644 --- a/easy_perception_deployment/test/p3u4a.cpp +++ b/easy_perception_deployment/test/p3u4a.cpp @@ -1,5 +1,5 @@ -// Copyright 2021 Advanced Remanufacturing and Technology Centre -// Copyright 2021 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -84,7 +84,7 @@ TEST(EPD_TestSuite, Test_P3Model_Tracking_Action) ASSERT_EQ(!ortAgent_->p3_ort_session, false); - EPD::EPDObjectTracking result = ortAgent_->p3_ort_session->infer_action( + EPD::EPDObjectTracking result = ortAgent_->p3_ort_session->infer( colored_img, depth_img, camera_info, diff --git a/easy_perception_deployment/test/p3u4v.cpp b/easy_perception_deployment/test/p3u4v.cpp index 2d26ec9..b10e19c 100644 --- a/easy_perception_deployment/test/p3u4v.cpp +++ b/easy_perception_deployment/test/p3u4v.cpp @@ -1,5 +1,5 @@ -// Copyright 2021 Advanced Remanufacturing and Technology Centre -// Copyright 2021 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -84,18 +84,19 @@ TEST(EPD_TestSuite, Test_P3Model_Tracking_Visualize) ASSERT_EQ(!ortAgent_->p3_ort_session, false); - cv::Mat result = ortAgent_->p3_ort_session->infer_visualize( + EPD::EPDObjectTracking result = ortAgent_->p3_ort_session->infer( colored_img, depth_img, camera_info, + 0.1, ortAgent_->tracker_type, ortAgent_->trackers, ortAgent_->tracker_logs, - ortAgent_->tracker_results - ); + ortAgent_->tracker_results); + cv::Mat output = ortAgent_->visualize(result, colored_img); - ASSERT_NE(result.cols, 0); - ASSERT_NE(result.rows, 0); + ASSERT_NE(output.cols, 0); + ASSERT_NE(output.rows, 0); } int main(int argc, char ** argv) diff --git a/easy_perception_deployment/test/test_easy_perception_deployment.cpp b/easy_perception_deployment/test/test_easy_perception_deployment.cpp index 76db2be..6425c1d 100644 --- a/easy_perception_deployment/test/test_easy_perception_deployment.cpp +++ b/easy_perception_deployment/test/test_easy_perception_deployment.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/test/test_init.cpp b/easy_perception_deployment/test/test_init.cpp index 206c47b..e1d69af 100644 --- a/easy_perception_deployment/test/test_init.cpp +++ b/easy_perception_deployment/test/test_init.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/easy_perception_deployment/test/test_invalid_usecase.cpp b/easy_perception_deployment/test/test_invalid_usecase.cpp index 92013d3..5ada6a2 100644 --- a/easy_perception_deployment/test/test_invalid_usecase.cpp +++ b/easy_perception_deployment/test/test_invalid_usecase.cpp @@ -1,5 +1,5 @@ -// Copyright 2020 Advanced Remanufacturing and Technology Centre -// Copyright 2020 ROS-Industrial Consortium Asia Pacific Team +// Copyright 2022 Advanced Remanufacturing and Technology Centre +// Copyright 2022 ROS-Industrial Consortium Asia Pacific Team // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License.