[ H264 Codec ] : Resolves h264 video encoding problems

Author: aPR0T0

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

Examples/ROS/ORB_SLAM3/src/ros_mono.cc

@@ -18,8 +18,9 @@
 
 #include <iostream>
 #include <ros/ros.h>
+#include <sensor_msgs/CompressedImage.h>
 #include <cv_bridge/cv_bridge.h>
-#include <sensor_msgs/Image.h>
+#include <image_transport/image_encoding.h>
 #include <opencv2/core.hpp>
 #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());
-}
+}

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);
 

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);
 

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