From d7314e3fc6a215d759e999c2e1b8db1b2f829d0a Mon Sep 17 00:00:00 2001 From: dimitris Date: Mon, 10 Oct 2016 13:33:06 +0300 Subject: [PATCH] split fusion synch cb to 2 --- chroma/src/chroma.cpp | 10 +- depth/src/depth.cpp | 7 +- fusion/include/fusion.hpp | 26 ++--- fusion/src/fusion.cpp | 199 ++++++++++++++++++-------------------- 4 files changed, 117 insertions(+), 125 deletions(-) diff --git a/chroma/src/chroma.cpp b/chroma/src/chroma.cpp index 9a258b8..f159c6b 100644 --- a/chroma/src/chroma.cpp +++ b/chroma/src/chroma.cpp @@ -16,15 +16,15 @@ Chroma_processing::Chroma_processing() { ROS_INFO_STREAM_NAMED("Chroma_processing","Subscribing at compressed topics \n"); - image_sub = it_.subscribe(image_topic, 1, + image_sub = it_.subscribe(image_topic, 10, &Chroma_processing::imageCb, this, image_transport::TransportHints("compressed")); } else - image_sub = it_.subscribe(image_topic, 1, &Chroma_processing::imageCb, this); + image_sub = it_.subscribe(image_topic, 10, &Chroma_processing::imageCb, this); - image_pub = it_.advertise(image_out_topic, 1); - image_pub_dif = it_.advertise(image_out_dif_topic, 1); + image_pub = it_.advertise(image_out_topic, 100); + image_pub_dif = it_.advertise(image_out_dif_topic, 100); } Chroma_processing::~Chroma_processing() @@ -63,7 +63,7 @@ void Chroma_processing::imageCb(const sensor_msgs::ImageConstPtr& msg) cur_rgb = (cv_ptr->image).clone(); // contrast fix - cur_rgb.convertTo(cur_rgb, -1, 4, 0); + cur_rgb.convertTo(cur_rgb, -1, 2, 0); // gamma correction gammaCorrection(cur_rgb); diff --git a/depth/src/depth.cpp b/depth/src/depth.cpp index bce5601..6c8766f 100644 --- a/depth/src/depth.cpp +++ b/depth/src/depth.cpp @@ -18,13 +18,13 @@ Depth_processing::Depth_processing() { ROS_INFO_STREAM_NAMED("Depth_processing","Subscribing at compressed topics \n"); - depth_sub = it_.subscribe(depth_topic, 1, + depth_sub = it_.subscribe(depth_topic, 10, &Depth_processing::depthCb, this, image_transport::TransportHints("compressedDepth")); } else - depth_sub = it_.subscribe(depth_topic, 1, &Depth_processing::depthCb, this); + depth_sub = it_.subscribe(depth_topic, 10, &Depth_processing::depthCb, this); - depth_pub = it_.advertise(depth_out_image_topic, 1); + depth_pub = it_.advertise(depth_out_image_topic, 100); } Depth_processing::~Depth_processing() @@ -173,6 +173,7 @@ detectBlobs(back_dif, back_dif_rects, 3); //Converting chroma values to meters grayToDepth(cur_depth, cur_depth, max_depth); + //Publish corrected depth image cv_ptr_depth->image = cur_depth; depth_pub.publish(cv_ptr_depth->toImageMsg()); diff --git a/fusion/include/fusion.hpp b/fusion/include/fusion.hpp index 72c9269..a9cf930 100644 --- a/fusion/include/fusion.hpp +++ b/fusion/include/fusion.hpp @@ -36,8 +36,9 @@ class Fusion_processing Fusion_processing(); ~Fusion_processing(); - - void callback(const sensor_msgs::Image::ConstPtr& chroma_msg, const sensor_msgs::Image::ConstPtr& chroma_dif_msg, const sensor_msgs::Image::ConstPtr& depth_msg); + + void chromaCb(const sensor_msgs::ImageConstPtr& msg); + void depthCb(const sensor_msgs::ImageConstPtr& msg); void writeCSV(People& collection, string path, ros::Time time); void publishResults(People& collection, ros::Time time); @@ -49,11 +50,15 @@ class Fusion_processing ros::NodeHandle nh_; ros::Publisher results_publisher; image_transport::ImageTransport it_; - - typedef image_transport::SubscriberFilter ImageSubscriber; - typedef message_filters::sync_policies::ApproximateTime MySyncPolicy; - message_filters::Synchronizer< MySyncPolicy > *sync; + image_transport::Subscriber image_sub; + image_transport::Subscriber depth_sub; + + Mat depth; + Mat back_Mat; + vector depth_storage; + vector< Rect_ > depth_rects; + string path_; string session_path; string image_topic; @@ -63,9 +68,6 @@ class Fusion_processing string csv_fields; string camera_frame; - vector depth_storage; - vector< Rect_ > depth_rects; - People people; bool playback_topics; @@ -73,6 +75,7 @@ class Fusion_processing bool create_directory; bool write_csv; bool has_image = false; + bool depth_available = false; int Hfield = 58; int Vfield = 45; @@ -86,7 +89,7 @@ class Fusion_processing int range = 2; //in pixels int verRange = 7; //in pixels int recR = 2; - + int counter = 0; long curTime ; float backFactor = 0.40; @@ -100,9 +103,6 @@ class Fusion_processing double recThreshold = 0.3; - - - Mat back_Mat; }; diff --git a/fusion/src/fusion.cpp b/fusion/src/fusion.cpp index e575085..e7466e5 100644 --- a/fusion/src/fusion.cpp +++ b/fusion/src/fusion.cpp @@ -27,15 +27,10 @@ Fusion_processing::Fusion_processing() depth_topic += "/compressedDepth"; } + image_sub = it_.subscribe(image_dif_topic, 10, &Fusion_processing::chromaCb, this); + depth_sub = it_.subscribe(depth_topic, 10, &Fusion_processing::depthCb, this); - ImageSubscriber *image_sub = new ImageSubscriber(it_, image_topic, 3 ); - ImageSubscriber *image_dif_sub = new ImageSubscriber(it_, image_dif_topic, 3 ); - ImageSubscriber *depth_sub = new ImageSubscriber(it_, depth_topic, 3 ); - - sync = new message_filters::Synchronizer< MySyncPolicy >( MySyncPolicy( 5 ), *image_sub, *image_dif_sub, *depth_sub ); - sync->registerCallback( boost::bind( &Fusion_processing::callback, this, _1, _2, _3) ); - - results_publisher = local_nh.advertise(results_topic, 1000); + results_publisher = local_nh.advertise(results_topic, 100); if(create_directory) { @@ -59,38 +54,15 @@ Fusion_processing::~Fusion_processing() } -/* Callback function to handle ROS messages, synchronously receives 3 messages - * - * PARAMETERS: - * - chroma_msg : ROS message that contains the RGB image and its metadata - * - chroma_dif_msg : ROS message that contains the differentiated RGB image and its metadata - * - depth_msg : ROS message that contains the depth image and its metadata - * - * - * RETURN -- - */ -void Fusion_processing::callback(const sensor_msgs::ImageConstPtr& chroma_msg, const sensor_msgs::ImageConstPtr& chroma_dif_msg, const sensor_msgs::ImageConstPtr& depth_msg) +void Fusion_processing::chromaCb(const sensor_msgs::ImageConstPtr& msg) { Mat fusion; - Mat chroma; - Mat chroma_dif; - Mat depth; vector< Rect_ > fusion_rects; - cv_bridge::CvImagePtr cv_ptr; cv_bridge::CvImagePtr cv_ptr_dif; - cv_bridge::CvImagePtr cv_ptr_depth; - - cv_ptr = cv_bridge::toCvCopy(chroma_msg, sensor_msgs::image_encodings::MONO8); - cv_ptr_dif = cv_bridge::toCvCopy(chroma_dif_msg, sensor_msgs::image_encodings::MONO8); - cv_ptr_depth = cv_bridge::toCvCopy(depth_msg, sensor_msgs::image_encodings::TYPE_32FC1); - - chroma = (cv_ptr->image).clone(); - chroma_dif = (cv_ptr_dif->image).clone(); - depth = (cv_ptr_depth->image).clone(); + cv_ptr_dif = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::MONO8); - //Fuse the gray and depth images - fusion = chroma_dif; + fusion = (cv_ptr_dif->image).clone(); cv::threshold(fusion, fusion, 100, 255, THRESH_BINARY); //Detect moving blobs @@ -114,7 +86,7 @@ void Fusion_processing::callback(const sensor_msgs::ImageConstPtr& chroma_msg, c //~ cout< 0) + end = people.tracked_boxes.size(); + for(int i = 0; i < end; i++) { - - Mat depth_rect = depth(rect); - - try + if(rect.width > 0 && depth_available) { - //Estimate its depth - people.tracked_pos[index].z = calculateDepth(depth_rect, people.tracked_boxes[index]); - } - catch(exception& e) - { - printf("%s %s", "Calculate depth failed: ", e.what()); - } - - //Filter the image according to the estimated depth and visualize it - Mat depth_filtered(depth.rows, depth.cols, CV_8UC1); - depth_filtered = Scalar(0); - - for(int i = 0; i < depth_rect.rows; i++) - { - float* cur = depth_rect.ptr(i); - for(int j = 0; j < depth_rect.cols; j++) - { - if(abs(cur[j] - people.tracked_pos[index].z) > 300) - cur[j] = 0; - } - } - depthToGray(depth_rect, depth_rect, min_depth, max_depth); - - depth_rect.copyTo(depth_filtered(rect)); - - if(display) - { - imshow("fusion", fusion); - moveWindow("fusion", 0, 0); - imshow("depth_filt", depth_filtered); - moveWindow("depth_filt", 645, 550); - imshow("chroma", chroma); - moveWindow("chroma", 0, 550); - waitKey(1); - } - - try - { - //Calculate real world position and height, distance moved - calculatePosition(people.tracked_boxes[index], people.tracked_pos[index]); - } - catch(exception& e) - { - printf("%s %s", "Calculate position failed: ", e.what()); + Mat depth_rect = depth(people.tracked_boxes[i]); + try + { + //Estimate its depth + people.tracked_pos[i].z = calculateDepth(depth_rect, people.tracked_boxes[i]); + } + catch(exception& e) + { + printf("%s %s", "Calculate depth failed: ", e.what()); + } + + + try + { + //Calculate real world position, height, distance moved + calculatePosition(people.tracked_boxes[i], people.tracked_pos[i]); + } + catch(exception& e) + { + printf("%s %s", "Calculate position failed: ", e.what()); + } } - + } + - //~ cout<<"X: "<(i); + //~ for(int j = 0; j < depth_rect.cols; j++) + //~ { + //~ if(abs(cur[j] - people.tracked_pos[index].z) > 300) + //~ cur[j] = 0; + //~ } + //~ } + //~ depthToGray(depth_rect, depth_rect, min_depth, max_depth); - ros::Time time = cv_ptr->header.stamp; + //~ depth_rect.copyTo(depth_filtered(people.tracked_boxes[index])); - //Write csv file - if(write_csv) - writeCSV(people, session_path, time); + //~ imshow("fusion", fusion); + //~ moveWindow("fusion", 0, 0); + //~ imshow("depth_filt", depth_filtered); + //~ moveWindow("depth_filt", 645, 550); + //~ imshow("chroma", chroma); + //~ moveWindow("chroma", 0, 550); + //~ waitKey(1); + } + //~ cout<<"X: "<header.stamp; + + //Write csv file + if(write_csv) + writeCSV(people, session_path, time); - //Publish results - publishResults(people, time); + //Publish results + publishResults(people, time); - - } } +void Fusion_processing::depthCb(const sensor_msgs::ImageConstPtr& msg) +{ + cv_bridge::CvImagePtr cv_ptr_depth; + depth_available = true; + cv_ptr_depth = cv_bridge::toCvCopy(msg, sensor_msgs::image_encodings::TYPE_32FC1); + depth = (cv_ptr_depth->image).clone(); +} + + + /* Function that writes creates a csv file and appends values to it * * PARAMETERS: @@ -223,12 +208,13 @@ void Fusion_processing::callback(const sensor_msgs::ImageConstPtr& chroma_msg, c */ void Fusion_processing::writeCSV(People& collection, string path, ros::Time time) { + ofstream storage(path + "/fusion.csv" ,ios::out | ios::app ); if (!collection.tracked_boxes.empty()) { - ofstream storage(path + "/fusion.csv" ,ios::out | ios::app ); for(int i = 0; i < collection.tracked_boxes.size() ; i++) { + Rect box = collection.tracked_boxes[i]; Position pos = collection.tracked_pos[i]; storage @@ -245,10 +231,15 @@ void Fusion_processing::writeCSV(People& collection, string path, ros::Time time <