pi/openvins_linux
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

implemented frame synchronization and frame skew to match rendered image with sensor image"

Browse Source 
;
This commit is contained in:
admin1
2022-08-16 12:44:20 +03:00
parent 1c81d558a8
commit f8843bf0fc
2 changed files with 75 additions and 36 deletions
Download Patch File Download Diff File Expand all files Collapse all files

52
ov_msckf/src/imrender_combine.cpp
View File

@@ -3,6 +3,9 @@
//
#include <ros/ros.h>
#include <sensor_msgs/Image.h>
#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/core.hpp>
@@ -13,9 +16,9 @@
std::vector<sensor_msgs::Image> sensor_images;
ros::Publisher pub_res, pub_mask, pub_recv_curr_image;
void rendered_image_callback(sensor_msgs::Image msg){
void rendered_image_callback(sensor_msgs::Image::ConstPtr sMsg, sensor_msgs::Image::ConstPtr rMsg){
cv::Mat image, mask;
cv_bridge::CvImagePtr bridge_img = cv_bridge::toCvCopy(msg, "bgr8");
cv_bridge::CvImagePtr bridge_img = cv_bridge::toCvCopy(rMsg, "bgr8");
image = bridge_img->image;
ROS_INFO_STREAM("The image encoding is: " << bridge_img->encoding);
mask = image.clone();
@@ -23,17 +26,26 @@ void rendered_image_callback(sensor_msgs::Image msg){
// find the appropriate image from msgs
cv::Mat current_sensor_image;
ROS_INFO_STREAM("The message sequence number: " << msg.header.seq);
ROS_INFO_STREAM("The sensor_message sequence number: " << sensor_images[sensor_images.size()-1].header.seq);
bool found_flag = false;
for (int i=sensor_images.size()-1;i>0;i--){
if (i == msg.header.seq){
cv_bridge::CvImagePtr bridge_sensor_img = cv_bridge::toCvCopy(sensor_images[i], "bgr8");
current_sensor_image = bridge_sensor_img->image;
found_flag = true;
break;
}
}
cv_bridge::CvImagePtr bridge_sensor_img = cv_bridge::toCvCopy(sMsg, "bgr8");
current_sensor_image = bridge_sensor_img->image;
ROS_INFO_STREAM("The message sequence number: " << rMsg->header.seq);
ROS_INFO_STREAM("The sensor_message sequence number: " << sMsg->header.seq);
ROS_INFO_STREAM("The message stamp is: " << std::fixed << std::setprecision(9) << rMsg->header.stamp.toSec());
ROS_INFO_STREAM("The sensor_message stamp is: " << std::fixed << std::setprecision(9) << sMsg->header.stamp.toSec());
bool found_flag = true;
// since there's the exact match in the timestamps, we don't need to find the closest
// timestamp, therefore, no need to use the Synchronized algorithm from message_filters in ROS.
// for (int i=sensor_images.size()-1;i>=0;i--){
// if (sensor_images[i].header.stamp == msg.header.stamp){
// cv_bridge::CvImagePtr bridge_sensor_img = cv_bridge::toCvCopy(sensor_images[i], "bgr8");
// current_sensor_image = bridge_sensor_img->image;
// found_flag = true;
// break;
// }
// }
if (found_flag){
ROS_INFO_STREAM("imrender: 1");
@@ -57,11 +69,11 @@ void rendered_image_callback(sensor_msgs::Image msg){
// cv::bitwise_and(current_sensor_image, image, masked, mask);
ROS_INFO_STREAM("imrender: 3");
std::string fn = std::to_string(msg.header.stamp.toSec()) + "_cam0.png";
std::string fn = std::to_string(rMsg->header.stamp.toSec()) + "_cam0.png";
cv::imwrite(fn, masked);
std_msgs::Header header;
header.stamp = msg.header.stamp;
header.stamp = rMsg->header.stamp;
header.frame_id = "cam0";
sensor_msgs::ImagePtr masked_msg = cv_bridge::CvImage(header, "bgr8", masked).toImageMsg();
sensor_msgs::ImagePtr mask_msg = cv_bridge::CvImage(header, "mono8", mask).toImageMsg();
@@ -96,6 +108,8 @@ void sensor_image_callback(sensor_msgs::Image msg){
// ROS_INFO_STREAM("imrender: 8");
}
typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image> sync_pol;
int main(int argc, char* argv[]){
ros::init(argc, argv, "image_render_combine");
@@ -106,8 +120,12 @@ int main(int argc, char* argv[]){
ros::Subscriber sub_rend_image, sub_sensor_image;
sub_sensor_image = nh.subscribe("/stereo/left/image_raw", 1000, &sensor_image_callback);
sub_rend_image = nh.subscribe("/ov_msckf/rendered", 1000, &rendered_image_callback);
// sub_sensor_image = nh.subscribe("/stereo/left/image_raw", 1000, &sensor_image_callback);
// sub_rend_image = nh.subscribe("/ov_msckf/rendered", 1000, &rendered_image_callback);
auto sensor_image_sub = std::make_shared<message_filters::Subscriber<sensor_msgs::Image>>(nh, "/stereo/left/image_raw", 1);
auto rend_image_sub = std::make_shared<message_filters::Subscriber<sensor_msgs::Image>>(nh, "/ov_msckf/rendered", 1);
auto sync = std::make_shared<message_filters::Synchronizer<sync_pol>>(sync_pol(10), *sensor_image_sub, *rend_image_sub);
sync->registerCallback(boost::bind(&rendered_image_callback, _1, _2));
pub_res = nh.advertise<sensor_msgs::Image>("/ov_msckf/combined", 2);
pub_mask = nh.advertise<sensor_msgs::Image>("/ov_msckf/mask", 2);
pub_recv_curr_image = nh.advertise<sensor_msgs::Image>("/ov_msckf/recv_curr_image", 2);