From 2e105168ce44d4972cff22aa0efccdba308f0921 Mon Sep 17 00:00:00 2001 From: aPR0T0 Date: Wed, 20 Sep 2023 00:00:50 +0800 Subject: [PATCH] [ H264 Codec ] : Resolves h264 video encoding problems --- Examples/ROS/ORB_SLAM3/Tello.yaml | 101 ++++++++++++++++++ Examples/ROS/ORB_SLAM3/src/ros_mono.cc | 13 +-- .../ROS/ORB_SLAM3/src/ros_mono_inertial.cc | 4 +- .../ROS/ORB_SLAM3/src/ros_stereo_inertial.cc | 2 +- get_map.sh | 3 + 5 files changed, 114 insertions(+), 9 deletions(-) create mode 100644 Examples/ROS/ORB_SLAM3/Tello.yaml create mode 100755 get_map.sh diff --git a/Examples/ROS/ORB_SLAM3/Tello.yaml b/Examples/ROS/ORB_SLAM3/Tello.yaml new file mode 100644 index 0000000000..7d013d8005 --- /dev/null +++ b/Examples/ROS/ORB_SLAM3/Tello.yaml @@ -0,0 +1,101 @@ +%YAML:1.0 + +# Camera calibration and distortion parameters (OpenCV) +Camera.type: "PinHole" + +Camera.fx: 924.873180 +Camera.fy: 923.504522 +Camera.cx: 486.997346 +Camera.cy: 364.308527 + +# data: [ 924.873180, 0.000000 , 486.997346, +# 0.000000 , 923.504522, 364.308527, +# 0.000000 , 0.000000 , 1.000000 ] + + # [ fx 0 cx] + # [ 0 fy cy] + # [ 0 0 1 ] + + +Camera.k1: -0.034749 +Camera.k2: 0.071514 +Camera.p1: 0.000363 +Camera.p2: 0.003131 +Camera.k3: 0.0 + +# data: [-0.034749, 0.071514, 0.000363, 0.003131, 0.000000] + + +Camera.width: 960 +Camera.height: 720 + +# Camera frames per second +# Camera.fps: 60.0 +Camera.fps: 30.0 + +# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale) +Camera.RGB: 1 + +#-------------------------------------------------------------------------------------------- +# ORB Parameters +#-------------------------------------------------------------------------------------------- + +# ORB Extractor: Number of features per image +ORBextractor.nFeatures: 10000 + +# ORB Extractor: Scale factor between levels in the scale pyramid +ORBextractor.scaleFactor: 1.15 + +# ORB Extractor: Number of levels in the scale pyramid +ORBextractor.nLevels: 10 + +# ORB Extractor: Fast threshold +# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response. +# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST +# You can lower these values if your images have low contrast +ORBextractor.iniThFAST: 20 +ORBextractor.minThFAST: 14 + + +# image_width: 960 +#hanged the resolution of the cameras so as to have the biggest resolution they can. The results seem similar for the camera matrix, but the distorti +#image_height: 720 +# camera_name: narrow_stereo +# camera_matrix: +# rows: 3 +# cols: 3 +# data: [ 924.873180, 0.000000 , 486.997346, +# 0.000000 , 923.504522, 364.308527, +# 0.000000 , 0.000000 , 1.000000 ] + + # [ fx 0 cx] + # [ 0 fy cy] + # [ 0 0 1 ] +# distortion_model: plumb_bob +# distortion_coefficients: +# rows: 1 +# cols: 5 +# data: [-0.034749, 0.071514, 0.000363, 0.003131, 0.000000] +# rectification_matrix: +# rows: 3 +# cols: 3 +# data: [ 1.000000, 0.000000, 0.000000, +# 0.000000, 1.000000, 0.000000, +# 0.000000, 0.000000, 1.000000] +# projection_matrix: +# rows: 3 +# cols: 4 +# data: [ 921.967102, 0.000000 , 489.492281, 0.000000, +# 0.000000 , 921.018890, 364.508536, 0.000000, +# 0.000000 , 0.000000 , 1.000000 , 0.000000] + +Viewer.KeyFrameSize: 0.05 +Viewer.KeyFrameLineWidth: 1 +Viewer.GraphLineWidth: 0.9 +Viewer.PointSize:2 +Viewer.CameraSize: 0.08 +Viewer.CameraLineWidth: 3 +Viewer.ViewpointX: 0 +Viewer.ViewpointY: -0.7 +Viewer.ViewpointZ: -1.8 +Viewer.ViewpointF: 500 diff --git a/Examples/ROS/ORB_SLAM3/src/ros_mono.cc b/Examples/ROS/ORB_SLAM3/src/ros_mono.cc index 6a65cf208b..a689294f41 100644 --- a/Examples/ROS/ORB_SLAM3/src/ros_mono.cc +++ b/Examples/ROS/ORB_SLAM3/src/ros_mono.cc @@ -18,8 +18,9 @@ #include #include +#include #include -#include +#include #include #include "../../../include/System.h" @@ -30,7 +31,7 @@ class ImageGrabber public: ImageGrabber(ORB_SLAM3::System* pSLAM) : mpSLAM(pSLAM) {} - void GrabImage(const sensor_msgs::ImageConstPtr& msg); + void GrabImage(const sensor_msgs::CompressedImageConstPtr& msg); ORB_SLAM3::System* mpSLAM; }; @@ -53,7 +54,7 @@ int main(int argc, char** argv) ImageGrabber igb(&SLAM); ros::NodeHandle nodeHandler; - ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw", 1, &ImageGrabber::GrabImage, &igb); + ros::Subscriber sub = nodeHandler.subscribe("/camera/image_raw/compressed", 1, &ImageGrabber::GrabImage, &igb); ros::spin(); @@ -68,13 +69,13 @@ int main(int argc, char** argv) return 0; } -void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg) +void ImageGrabber::GrabImage(const sensor_msgs::CompressedImageConstPtr& msg) { // Copy the ros image message to cv::Mat. cv_bridge::CvImageConstPtr cv_ptr; try { - cv_ptr = cv_bridge::toCvShare(msg); + cv_ptr = cv_bridge::toCvCopy(msg,image_transport::ImageEncodings::BGR8 ); } catch (cv_bridge::Exception& e) { @@ -83,4 +84,4 @@ void ImageGrabber::GrabImage(const sensor_msgs::ImageConstPtr& msg) } mpSLAM->TrackMonocular(cv_ptr->image, cv_ptr->header.stamp.toSec()); -} +} \ No newline at end of file diff --git a/Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc b/Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc index 96eaf1b229..d1fd2972a7 100644 --- a/Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc +++ b/Examples/ROS/ORB_SLAM3/src/ros_mono_inertial.cc @@ -92,8 +92,8 @@ int main(int argc, char **argv) ImageGrabber igb(&SLAM,&imugb,bEqual); // Maximum delay, 5 seconds - ros::Subscriber sub_imu = n.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb); - ros::Subscriber sub_img0 = n.subscribe("/camera/image_raw", 100, &ImageGrabber::GrabImage,&igb); + ros::Subscriber sub_imu = n.subscribe("/tello/imu", 1000, &ImuGrabber::GrabImu, &imugb); + ros::Subscriber sub_img0 = n.subscribe("/tello/image_raw", 100, &ImageGrabber::GrabImage,&igb); std::thread sync_thread(&ImageGrabber::SyncWithImu,&igb); diff --git a/Examples/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc b/Examples/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc index ff2f49e872..93ab981988 100644 --- a/Examples/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc +++ b/Examples/ROS/ORB_SLAM3/src/ros_stereo_inertial.cc @@ -132,7 +132,7 @@ int main(int argc, char **argv) } // Maximum delay, 5 seconds - ros::Subscriber sub_imu = n.subscribe("/imu", 1000, &ImuGrabber::GrabImu, &imugb); + ros::Subscriber sub_imu = n.subscribe("/tello/imu", 1000, &ImuGrabber::GrabImu, &imugb); ros::Subscriber sub_img_left = n.subscribe("/camera/left/image_raw", 100, &ImageGrabber::GrabImageLeft, &igb); ros::Subscriber sub_img_right = n.subscribe("/camera/right/image_raw", 100, &ImageGrabber::GrabImageRight, &igb); diff --git a/get_map.sh b/get_map.sh new file mode 100755 index 0000000000..1412c8cd7a --- /dev/null +++ b/get_map.sh @@ -0,0 +1,3 @@ +# Don't Forget to connect the tello drone first with the camera topic + +rosrun ORB_SLAM3 Mono ~/ORB_SLAM3/Vocabulary/ORBvoc.txt ~/ORB_SLAM3/Examples/ROS/ORB_SLAM3/Tello.yaml