works with basic InfooImage

Author: MoscowskyAnton

CMakeLists.txt

@@ -90,7 +90,8 @@ set( EOD_SRC
     src/lib/types/Tracker.cpp
     src/lib/types/Relationship.cpp    
     src/lib/types/Filtering.cpp    
-    src/lib/types/Clusterization.cpp    
+    src/lib/types/Clusterization.cpp   
+    src/lib/types/InfoImage.cpp
     # lib utils
     src/lib/utils/geometry_utils.cpp
     src/lib/utils/drawing_utils.cpp

launch/extended_object_detection_oakd_example.launch

@@ -19,7 +19,7 @@
         <remap from="depth/image_raw" to="/rgb_stereo_publisher/stereo/depth"/>
         <remap from="depth/info" to="/rgb_stereo_publisher/stereo/camera_info"/>
 
-        <rosparam param="selectedOnStartSimple">[]</rosparam>
+        <rosparam param="selectedOnStartSimple">[40]</rosparam>
         <rosparam param="selectedOnStartComplex">[-1]</rosparam>
 
     </node>

src/extended_object_detection_node/eod_node.cpp

@@ -2,12 +2,13 @@
 #include <cstdint>
 #include <sensor_msgs/image_encodings.h>
 #include <geometry_msgs/Vector3.h>
-#include <geometry_msgs/Point.h>
+//#include <geometry_msgs/Point.h>
 
-geometry_msgs::Vector3 getUnitTranslation(geometry_msgs::Point point){
+
+geometry_msgs::Vector3 getUnitTranslation(cv::Point point, const cv::Mat& K){
     geometry_msgs::Vector3 unit_translate;
-    unit_translate.x = (point.x - cx) / fx;
-    unit_translate.y = (point.y - cy) / fy;
+    unit_translate.x = (point.x - K.at<double>(0,2)) / K.at<double>(0,0);
+    unit_translate.y = (point.y - K.at<double>(1,2)) / K.at<double>(1,1);
     unit_translate.z = 1;
     return unit_translate;
 }
@@ -59,10 +60,8 @@ EOD_ROS::EOD_ROS(ros::NodeHandle nh, ros::NodeHandle nh_p){
             }        
         }        
     }
-    ROS_INFO("Selected to detect on start %i objects", selected_simple_objects.size());
-    
-    
-    
+    ROS_INFO("Selected to detect on start %li objects", selected_simple_objects.size());
+            
     // set up publishers
     simple_objects_pub_ = nh_p.advertise<extended_object_detection::SimpleObjectArray>("simple_objects",1);
 
@@ -91,6 +90,16 @@ bool EOD_ROS::check_time(ros::Time stamp){
     return (stamp - prev_detected_time).toSec() > rate_limit_sec;
 }
 
+cv::Mat EOD_ROS::getK(const sensor_msgs::CameraInfoConstPtr& info_msg){
+    cv::Mat K = cv::Mat::zeros(3, 3, CV_64F);
+    for (size_t i=0; i<3; i++) {
+        for (size_t j=0; j<3; j++) {
+            K.at<double>(i, j) = info_msg->K[i*3+j];
+        }
+    }
+    return K;
+}
+
 void EOD_ROS::rgb_info_cb(const sensor_msgs::ImageConstPtr& rgb_image, const sensor_msgs::CameraInfoConstPtr& rgb_info){
     ROS_INFO("Get Image!");
 
@@ -107,9 +116,12 @@ void EOD_ROS::rgb_info_cb(const sensor_msgs::ImageConstPtr& rgb_image, const sen
     }
     catch (cv_bridge::Exception& e){
         ROS_ERROR("Could not convert from '%s' to 'bgr8'.", rgb_image->encoding.c_str());
+        return;
     }
 
-    detect(rgb, cv::Mat(), rgb_image->header);
+    eod::InfoImage ii = eod::InfoImage(rgb, getK(rgb_info), cv::Mat() );    
+    detect(ii, eod::InfoImage(), rgb_image->header);
+    //detect(rgb, cv::Mat(), rgb_image->header);
 }
 
 void EOD_ROS::rgbd_info_cb(const sensor_msgs::ImageConstPtr& rgb_image, const sensor_msgs::CameraInfoConstPtr& rgb_info, const sensor_msgs::ImageConstPtr& depth_image, const sensor_msgs::CameraInfoConstPtr& depth_info){
@@ -128,6 +140,7 @@ void EOD_ROS::rgbd_info_cb(const sensor_msgs::ImageConstPtr& rgb_image, const se
     }
     catch (cv_bridge::Exception& e){
         ROS_ERROR("Could not convert from '%s' to 'bgr8'.", rgb_image->encoding.c_str());
+        return;
     }
 
     cv::Mat depth;    
@@ -141,10 +154,15 @@ void EOD_ROS::rgbd_info_cb(const sensor_msgs::ImageConstPtr& rgb_image, const se
         ROS_ERROR_THROTTLE(5, "Depth image has unsupported encoding [%s]", depth_image->encoding.c_str());
     }
 
-    detect(rgb, depth, rgb_image->header);
+    eod::InfoImage ii_rgb = eod::InfoImage(rgb, getK(rgb_info), cv::Mat() );
+    eod::InfoImage ii_depth = eod::InfoImage(depth, getK(depth_info), cv::Mat() );        
+    
+    detect(ii_rgb, ii_depth, rgb_image->header);
 }
 
-void EOD_ROS::detect(const cv::Mat& rgb, const cv::Mat& depth, std_msgs::Header header){
+//void EOD_ROS::detect(const cv::Mat& rgb, const cv::Mat& depth, std_msgs::Header header){
+void EOD_ROS::detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std_msgs::Header header){
+    ROS_INFO("Detecting...");
     prev_detected_time = ros::Time::now();
 
     cv::Mat image_to_draw;
@@ -157,10 +175,10 @@ void EOD_ROS::detect(const cv::Mat& rgb, const cv::Mat& depth, std_msgs::Header
 
     // detect simple objects
     for (auto& s_it : selected_simple_objects){
-        s_it->Identify(rgb, depth, frame_sequence);
-        
-        add_data_to_simple_msg(s_it, simples_msg);
-        
+        //ROS_INFO("Identifiyng...");
+        s_it->Identify(rgb, depth, frame_sequence);        
+        //ROS_INFO("Adding...");
+        add_data_to_simple_msg(&(*s_it), simples_msg, rgb.K);
     }
 
     simples_msg.header = header;
@@ -172,29 +190,30 @@ void EOD_ROS::detect(const cv::Mat& rgb, const cv::Mat& depth, std_msgs::Header
     frame_sequence++;    
 }
 
-void EOD_ROS::add_data_to_simple_msg(const eod::SimpleObject* so, extended_object_detection::SimpleObjectArray& msg){    
+void EOD_ROS::add_data_to_simple_msg( eod::SimpleObject* so, extended_object_detection::SimpleObjectArray& msg, const cv::Mat& K){    
     for(auto& eoi : so->objects){
-        msg.objects.push_back(eoi_to_base_object(so, &eoi));
+        msg.objects.push_back(eoi_to_base_object(so, &eoi, K));
     }            
 }
 
-extended_object_detection::BaseObject EOD_ROS::eoi_to_base_object(const eod::SimpleObject* so, const eod::ExtendedObjectInfo* eoi){
+extended_object_detection::BaseObject EOD_ROS::eoi_to_base_object( eod::SimpleObject* so,  eod::ExtendedObjectInfo* eoi, const cv::Mat& K){
+    //ROS_INFO("Forming...");
     extended_object_detection::BaseObject bo;
     // common
     bo.type_id = so->ID;
     bo.type_name = so->name;
     bo.score = eoi->total_score;
 
-    for( auto const& exi : eoi->extracted_info){
+    for( auto& exi : eoi->extracted_info){
         bo.extracted_info.keys.push_back(exi.first);
         bo.extracted_info.values.push_back(exi.second);        
     }
 
     // rect
     bo.rect.left_bottom.x = eoi->x;
     bo.rect.left_bottom.y = eoi->y;
-    bo.rect.right_bottom.x = eoi->x + eoi->width;
-    bo.rect.right_bottom.y = eoi->y + eoi->height;
+    bo.rect.right_up.x = eoi->x + eoi->width;
+    bo.rect.right_up.y = eoi->y + eoi->height;
 
     // transform
     if( eoi->tvec.size() > 0 ){
@@ -203,7 +222,7 @@ extended_object_detection::BaseObject EOD_ROS::eoi_to_base_object(const eod::Sim
         bo.transform.translation.z = eoi->tvec[0][2];
     }
     else{
-        bo.transform.translation = getUnitTranslation(ext_obj->getCenter());      
+        bo.transform.translation = getUnitTranslation(eoi->getCenter(), K);      
     }
 
 

src/extended_object_detection_node/eod_node.h

@@ -66,10 +66,12 @@ class EOD_ROS{
     void rgbd_info_cb(const sensor_msgs::ImageConstPtr& image, const sensor_msgs::CameraInfoConstPtr& info, const sensor_msgs::ImageConstPtr& depth_image, const sensor_msgs::CameraInfoConstPtr& depth_info);
 
     // functions
-    void detect(const cv::Mat& rgb, const cv::Mat& depth, std_msgs::Header header);
+    //void detect(const cv::Mat& rgb, const cv::Mat& depth, std_msgs::Header header);
+    void detect(const eod::InfoImage& rgb, const eod::InfoImage& depth, std_msgs::Header header);
     bool check_time(ros::Time stamp);
-    void add_data_to_simple_msg(const eod::SimpleObject*, extended_object_detection::SimpleObjectArray& msg);
-    extended_object_detection::BaseObject eoi_to_base_object(const eod::SimpleObject* so, const eod::ExtendedObjectInfo* eoi);
+    void add_data_to_simple_msg( eod::SimpleObject*, extended_object_detection::SimpleObjectArray& msg, const cv::Mat& K);
+    extended_object_detection::BaseObject eoi_to_base_object( eod::SimpleObject* so, eod::ExtendedObjectInfo* eoi, const cv::Mat& K);
+    cv::Mat getK(const sensor_msgs::CameraInfoConstPtr& info_msg);
 
     // EOD
     eod::ObjectBase * object_base;