From 1c81d558a8f92623afa5a6760caf8dd9bd15ece6 Mon Sep 17 00:00:00 2001 From: admin1 Date: Mon, 15 Aug 2022 09:54:33 +0300 Subject: [PATCH] created rosnode to render images --- ov_eval/launch/record.launch | 6 +- ov_msckf/CMakeLists.txt | 2 +- ov_msckf/cmake/ROS1.cmake | 7 ++ ov_msckf/launch/display.rviz | 28 +++++- ov_msckf/launch/subscribe.launch | 22 +++- ov_msckf/src/imrender_combine.cpp | 117 ++++++++++++++++++++++ ov_msckf/src/ros/ROS1Visualizer.cpp | 149 ++++++++++++++++++---------- ov_msckf/src/ros/ROS1Visualizer.h | 13 ++- 8 files changed, 277 insertions(+), 67 deletions(-) create mode 100644 ov_msckf/src/imrender_combine.cpp diff --git a/ov_eval/launch/record.launch b/ov_eval/launch/record.launch index bfb2494..9bf947f 100644 --- a/ov_eval/launch/record.launch +++ b/ov_eval/launch/record.launch @@ -3,11 +3,11 @@ - + - + @@ -20,7 +20,7 @@ - + diff --git a/ov_msckf/CMakeLists.txt b/ov_msckf/CMakeLists.txt index 205bedf..67ac539 100644 --- a/ov_msckf/CMakeLists.txt +++ b/ov_msckf/CMakeLists.txt @@ -10,7 +10,7 @@ if (NOT OpenCV_FOUND) endif () find_package(Boost REQUIRED COMPONENTS system filesystem thread date_time) find_package(Ceres REQUIRED) -include_directories(/home/pi/work_drivecast/slams/ORB-SLAM3_Linux/Thirdparty/Sophus) +include_directories(/home/admin1/podmivan/SLAM/ORB_SLAM3/Thirdparty/Sophus) message(STATUS "OPENCV: " ${OpenCV_VERSION} " | BOOST: " ${Boost_VERSION} " | CERES: " ${Ceres_VERSION}) # If we will compile with aruco support diff --git a/ov_msckf/cmake/ROS1.cmake b/ov_msckf/cmake/ROS1.cmake index 561d938..7109877 100644 --- a/ov_msckf/cmake/ROS1.cmake +++ b/ov_msckf/cmake/ROS1.cmake @@ -126,6 +126,13 @@ if (catkin_FOUND AND ENABLE_ROS) RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) + add_executable(image_render_combine src/imrender_combine.cpp) + target_link_libraries(image_render_combine ov_msckf_lib ${thirdparty_libraries}) + install(TARGETS image_render_combine + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION $[CATKIN_PACKAGE_BIN_DESTINATION}) + endif () add_executable(run_simulation src/run_simulation.cpp) diff --git a/ov_msckf/launch/display.rviz b/ov_msckf/launch/display.rviz index cb8f471..1fc756d 100644 --- a/ov_msckf/launch/display.rviz +++ b/ov_msckf/launch/display.rviz @@ -52,6 +52,22 @@ Visualization Manager: Plane Cell Count: 80 Reference Frame: Value: true + - Class: rviz_camera_stream/CameraPub + Enabled: true + Frame Rate: 10 + Image Topic: /ov_msckf/rendered + Camera Info Topic: /ov_msckf/camera0_info + Name: CameraPub1 + Queue Size: 2 + Value: true + Visibility: + Axes: true + CameraPub2: true + Grid: true + Image1: true + Image2: true + TF: true + Value: true - Class: rviz/Image Enabled: true Image Topic: /ov_msckf/trackhist @@ -61,10 +77,9 @@ Visualization Manager: Name: Image Tracks Normalize Range: true Queue Size: 2 - Transport Hint: compressed + Transport Hint: raw Unreliable: false Value: true - Camera Info Topic: /ov_msckf/camera_info - Class: rviz/Image Enabled: true Image Topic: /ov_msckf/loop_depth_colored @@ -77,7 +92,6 @@ Visualization Manager: Transport Hint: compressed Unreliable: false Value: true - - Camera Info Topic: /ov_msckf/camera_info - Alpha: 1 Buffer Length: 1 Class: rviz/Path @@ -291,6 +305,14 @@ Visualization Manager: TF: true Value: true Zoom Factor: 1 + - Class: rviz/Marker + Enabled: true + Marker Topic: /ov_msckf/vis_markers + Name: Marker + Namespaces: + {} + Queue Size: 100 + Value: true Enabled: true Global Options: Background Color: 44; 44; 44 diff --git a/ov_msckf/launch/subscribe.launch b/ov_msckf/launch/subscribe.launch index 8f2e1d6..751978a 100644 --- a/ov_msckf/launch/subscribe.launch +++ b/ov_msckf/launch/subscribe.launch @@ -16,8 +16,8 @@ - - + + @@ -28,6 +28,7 @@ + @@ -65,6 +66,23 @@ + + + + + + diff --git a/ov_msckf/src/imrender_combine.cpp b/ov_msckf/src/imrender_combine.cpp new file mode 100644 index 0000000..f553051 --- /dev/null +++ b/ov_msckf/src/imrender_combine.cpp @@ -0,0 +1,117 @@ +// +// Created by Podmogilnyi Ivan on 11.08.2022. +// +#include +#include + +#include +#include +#include +#include +#include + +std::vector sensor_images; +ros::Publisher pub_res, pub_mask, pub_recv_curr_image; + +void rendered_image_callback(sensor_msgs::Image msg){ + cv::Mat image, mask; + cv_bridge::CvImagePtr bridge_img = cv_bridge::toCvCopy(msg, "bgr8"); + image = bridge_img->image; + ROS_INFO_STREAM("The image encoding is: " << bridge_img->encoding); + mask = image.clone(); + cv::cvtColor(mask, mask, cv::COLOR_BGR2GRAY); + + // 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; + } + } + + if (found_flag){ + ROS_INFO_STREAM("imrender: 1"); + ROS_INFO_STREAM("imrender: 1.2"); + cv::Mat masked(image.rows, image.cols, CV_8UC3, cv::Scalar(0, 0, 0)); + masked = current_sensor_image.clone(); + ROS_INFO_STREAM("imrender: 2" << image.rows << " " << image.cols << " " << current_sensor_image.rows << + " " << current_sensor_image.cols << " " << masked.rows << " " << masked.cols); + //copy the non zero pixels to masked from image. + for (int i=0;i(i, j) != 0){ + masked.at(i, j) = image.at(i, j); + } + } + } + + ROS_INFO_STREAM("current image type: " << current_sensor_image.type()); + ROS_INFO_STREAM("rendered image type: " << image.type()); + ROS_INFO_STREAM("masked image type: " << masked.type()); +// 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"; + cv::imwrite(fn, masked); + + std_msgs::Header header; + header.stamp = msg.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(); + ROS_INFO_STREAM("imrender: 4"); + pub_res.publish(*masked_msg); + pub_mask.publish(*mask_msg); + ROS_INFO_STREAM("imrender: 5"); +// cv::imwrite(fn, masked); + } + else{ + ROS_INFO("Not found an image with the appropriate timestamp."); + } +} + +void sensor_image_callback(sensor_msgs::Image msg){ + sensor_images.push_back(msg); + ROS_INFO_STREAM("The sensor_images size is: " << sensor_images.size()); + ROS_INFO_STREAM("The sensor_image seq number is: " << msg.header.seq); + +// cv::Mat image; +// cv_bridge::CvImagePtr bridge_msg = cv_bridge::toCvCopy(msg, "bgr8"); +// ROS_INFO_STREAM("imrender: 6" << std::endl << "received image encoding is: " << msg.encoding); +// current_sensor_image = bridge_msg->image; +// ROS_INFO_STREAM("imrender: 7" << std::endl << "the number of channels in opencv image: " << current_sensor_image.channels()); +// if (current_sensor_image.channels() == 1){ +// cv::cvtColor(current_sensor_image, current_sensor_image, cv::COLOR_GRAY2BGR); +// } +//// cv::imshow("currr_image", current_sensor_image); +//// cv::waitKey(1); +// auto current_sensor_image_msg = cv_bridge::CvImage(msg.header, "bgr8", current_sensor_image).toImageMsg(); +// pub_recv_curr_image.publish(*current_sensor_image_msg); +// ROS_INFO_STREAM("imrender: 8"); +} + +int main(int argc, char* argv[]){ + ros::init(argc, argv, "image_render_combine"); + + ros::NodeHandle nh; + sensor_images = {}; + + ROS_INFO("Hello world!"); + + 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); + pub_res = nh.advertise("/ov_msckf/combined", 2); + pub_mask = nh.advertise("/ov_msckf/mask", 2); + pub_recv_curr_image = nh.advertise("/ov_msckf/recv_curr_image", 2); + + + ros::spin(); +} diff --git a/ov_msckf/src/ros/ROS1Visualizer.cpp b/ov_msckf/src/ros/ROS1Visualizer.cpp index cd6a136..a6ebb8e 100644 --- a/ov_msckf/src/ros/ROS1Visualizer.cpp +++ b/ov_msckf/src/ros/ROS1Visualizer.cpp @@ -57,6 +57,7 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ // Our tracking image it_pub_tracks = it.advertise("/ov_msckf/trackhist", 2); + sensor_left_pub = it.advertise("/ov_msckf/sensor_left", 2); PRINT_DEBUG("Publishing: %s\n", it_pub_tracks.getTopic().c_str()); // Groundtruth publishers @@ -75,6 +76,23 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ // Added by PI on 27.07.2022 // В какой топик публикуется ? + sensor_left_id = 0; + + pub_aligned_traj = nh->advertise("/ov_msckf/pathaligned", 2); + + fT_aligned.open("T_aligned_history.txt"); + if (!fT_aligned.is_open()){ + ROS_INFO("error opening file!"); + } + else{ + ROS_INFO("file open success!"); + std::string curr_path = std::filesystem::current_path(); + std::string output_str = "current path is: " + curr_path; + ROS_INFO(output_str.c_str()); + + } + fT_aligned << "ts mT_aligned" << std::endl; + subs_T_align = nh->subscribe("/ov_msckf/T_align", 1000, &ROS1Visualizer::T_align_callback, this); std::ifstream fileTraj; @@ -116,6 +134,7 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ // } pub_camera_info = nh->advertise("/ov_msckf/camera_info", 2); + pub_cam0_info = nh->advertise("/ov_msckf/cam0_info", 2); vis_pub = nh->advertise("/ov_msckf/vis_markers", 0); // pub_camera_info = nh->advertise("/ov_mskcf/camera_info", 2); // pub_camera_info_trackhist = nh->advertise("/ov_msckf/trackhist", 2); @@ -149,60 +168,35 @@ ROS1Visualizer::ROS1Visualizer(std::shared_ptr nh, std::shared_ // {240, 90, 0.02}, {80, 110, 0.02}, // {280, 110, 0.02}, {100, 130, 0.02}, // {320, 130, 0.02}, {320, 150, 0.02}}; - float yCubesVisPos = -3.0; + float yCubesVisPos = -6.0; float skew = 5.0; cubeVisPoints = { - {-33.451, 231.074, yCubesVisPos}, - {-74.510, 141.228, yCubesVisPos}, - {-114.119, -45.468, yCubesVisPos}, - {96.947, -52.157, yCubesVisPos}, - {-183.654, 605.40, yCubesVisPos}, - {228.287, 146.678, yCubesVisPos}, - {363.666, 1769.109, yCubesVisPos}, - {366.259, 1810.029, yCubesVisPos}, - {241.346, 186.492, yCubesVisPos}, - {424.114, 1789.749, yCubesVisPos}, - {588.224, 322.999, yCubesVisPos}, - {616.916, 1919.684, yCubesVisPos}, - {845.480, 518.359, yCubesVisPos}, - {1027.410, 713.207, yCubesVisPos}, - {1027.410, 733.207, yCubesVisPos}, - {1101.795, 667.008, yCubesVisPos}, - {1100.890, 778.080, yCubesVisPos}, - {1182.285, 788.335, yCubesVisPos}, - {1593.721, 1111.198, yCubesVisPos}, - {1635.912, 1542.791, yCubesVisPos}, - {1936.860, 1898.863, yCubesVisPos}, - {1865.827, 2190.633, yCubesVisPos}, - {1603.437, 2262.750, yCubesVisPos}, + {71.215, 32.694, yCubesVisPos}, +// {81.668, 26.227, yCubesVisPos}, + {84.455, 41.731, yCubesVisPos}, +// {90.726, 32.825, yCubesVisPos}, + {109.541, 60.415, yCubesVisPos}, +// {149.264, 69.490, yCubesVisPos}, + {150.645, 89.474, yCubesVisPos}, +// {198.294, 97.113, yCubesVisPos}, + {251.391, 154.108, yCubesVisPos}, +// {310.030, 190.208, yCubesVisPos}, + {330.554, 183.244, yCubesVisPos}, + {746.086, 450.563, yCubesVisPos-1}, +// {792.608, 527.140, yCubesVisPos}, + {883.669, 517.444, yCubesVisPos-1}, + {957.811, 652.638, yCubesVisPos-1}, + {1086.045, 669.317, yCubesVisPos-2}, + {1125.310, 728.214, yCubesVisPos-2}, + {731.996, 425.021, yCubesVisPos-1}, -// {307.369, 150.830, yCubesVisPos}, -// {336.567, 223.124, yCubesVisPos}, -// {406.839, 224.973, yCubesVisPos}, -// {473.202, 296.579, yCubesVisPos}, -// {663.727, 382.915, yCubesVisPos}, -// {706.092, 446.382, yCubesVisPos}, -// {773.465, 464.975, yCubesVisPos}, -// {815.571, 524.754, yCubesVisPos}, -// {862.600, 560.769, yCubesVisPos}, -// {987.206, 590.868, yCubesVisPos}, -// {1018.318, 661.753, yCubesVisPos} - - {239.974, 72.267, yCubesVisPos}, - {285.534, 147.012, yCubesVisPos}, - {379.105, 103.735, yCubesVisPos}, - {377.834, 190.505, yCubesVisPos}, - {589.754, 207.650, yCubesVisPos}, - {583.577, 299.558, yCubesVisPos}, - {695.422, 271.515, yCubesVisPos}, - {683.414, 342.212, yCubesVisPos}, - {803.130, 295.558, yCubesVisPos}, - {901.909, 445.780, yCubesVisPos}, - {104.461, 397.006, yCubesVisPos}, - {110.683, 534.233, yCubesVisPos}, - {119.329, 502.730, yCubesVisPos}, - {238.588, 137.661, yCubesVisPos} + {1186.527, 700.189, yCubesVisPos-2}, +// {1205.432, 681.457, yCubesVisPos}, + {1184.375, 735.604, yCubesVisPos-2}, +// {1232.697, 715.647, yCubesVisPos}, + {1244.767, 784.257, yCubesVisPos-2} }; + cashedCubeVisPoints = cubeVisPoints; // End of added by PI // option `to enable publishing of global to IMU transformation @@ -696,6 +690,9 @@ void ROS1Visualizer::publish_state() { for (size_t i = 0; i < poses_imu.size(); i += std::floor((double)poses_imu.size() / 16384.0) + 1) { arrIMU.poses.push_back(poses_imu.at(i)); } + // Changed by PI + curr_path = arrIMU; + pub_pathimu.publish(arrIMU); // Move them forward in time @@ -721,22 +718,38 @@ void ROS1Visualizer::publish_images() { // Publish it_pub_tracks.publish(exl_msg); + // publish the left image with the visualizations + cv::Range rows(0, img_history.rows); + cv::Range cols(0, img_history.cols / 2); + cv::Mat cropped_image = img_history(rows, cols); + sensor_msgs::ImagePtr sensor_left_msg = cv_bridge::CvImage(header, "bgr8", cropped_image).toImageMsg(); + sensor_left_msg->header.seq = sensor_left_id++; + sensor_left_pub.publish(sensor_left_msg); + ROS_INFO("Image publish success!"); + bool is_fisheye = (std::dynamic_pointer_cast(_app->get_params().camera_intrinsics.at(0)) != nullptr); sensor_msgs::CameraInfo cameraparams; cameraparams.header = header; cameraparams.header.frame_id = "cam0"; cameraparams.distortion_model = is_fisheye ? "equidistant" : "plumb_bob"; Eigen::VectorXd cparams = _app->get_state()->_cam_intrinsics.at(0)->value(); + std::stringstream ss; + ss << cparams; + std::string str_to_output = "cparams are: "+ ss.str(); + ROS_INFO(str_to_output.c_str()); cameraparams.D = {cparams(4), cparams(5), cparams(6), cparams(7)}; cameraparams.K = {cparams(0), 0, cparams(2), 0, cparams(1), cparams(3), 0, 0, 1}; cameraparams.P = {cparams(0), 0, cparams(2), 0, 0, cparams(1), cparams(3), 0, 0, 0, 1, 0}; PRINT_DEBUG("cameraparams.P: %d", cameraparams.P.data()); + ROS_INFO("cameraparams.P: %d", cameraparams.P.data()); camInfoGlob = sensor_msgs::CameraInfo(); camInfoGlob = cameraparams; - // Added by PI - pub_camera_info.publish(camInfoGlob); + pub_camera_info.publish(camInfoGlob); + camInfoGlob.height = exl_msg->height; + camInfoGlob.width = exl_msg->width / 2; + pub_cam0_info.publish(camInfoGlob); for (auto & cubeVisPoint : cubeVisPoints){ geometry_msgs::Point tempPoint; @@ -1149,12 +1162,17 @@ void ROS1Visualizer::T_align_callback(const geometry_msgs::TransformStamped &msg std::cout << "3" << std::endl; for (int i=0;i #include #include +#include #include #include @@ -162,18 +163,24 @@ protected: // Added by PI on 27.07.2022 // image_transport::CameraPublisher pub_camera_info, pub_camera_info_trackhist; image_transport::CameraPublisher camPub; + image_transport::Publisher sensor_left_pub; visualization_msgs::Marker Marker; - std::vector> cubeVisPoints; + std::vector> cubeVisPoints, cashedCubeVisPoints; image_transport::CameraSubscriber camSub; sensor_msgs::CameraInfoPtr camInfoPtrGlob; - ros::Publisher pub_camera_info; + ros::Publisher pub_camera_info, pub_cam0_info; ros::Publisher vis_pub; sensor_msgs::CameraInfo camInfoGlob; sensor_msgs::ImagePtr exl_msg_global; ros::Subscriber sub_camera_info, subs_T_align; - ros::Publisher pub_poserec; + ros::Publisher pub_poserec, pub_aligned_traj; + nav_msgs::Path curr_path; + + std::ofstream fT_aligned; std::vector> loadedTrajectory; + + int sensor_left_id; // End of adding by PI // Our subscribers and camera synchronizers