Re: RTABMAP_ROS with ORBSLAM3
Posted by matlabbe on
URL: http://official-rtab-map-forum.206.s1.nabble.com/RTABMAP-ROS-with-ORBSLAM3-tp10153p10169.html
Hi,
The bag you shared has missing data. For example, the camera info topics are missing, /tf and /tf_static are missing (not sure if they should be there).
The stereo images are not "stereo rectified", so without calibration file, I cannot rectify them if these are the raw images. Here is an overlay of the two images (note how they are not aligned vertically):
Left and right separately:
I checked to download the first sequence of the dataset, but this command fails silently (with --debug, it says "cannot connect", maybe an aws account is really required to download):
Hopefully, I've found the dataset's Samples page. To rectify the images and create the calibration yaml files, I created that script:
First test with rtabmap standalone using stereo images as input (default parameters):
I tried your image with ORB_SLAM3, but I got this error after ORB_SAM3 initialized:
To launch rtabmap standalone, I launched your docker like this:
I didn't do more tests with the ros package because I don't have much more time to fix the dataset -> ros converter. If you are going to use ROS (if you want to use GPS, IMU and/or lidar), you may be able to rectify images before publishing them and also send camera_info for left and right cameras at the same time around here in their code (based on what I did in python above).
cheers,
Mathieu
URL: http://official-rtab-map-forum.206.s1.nabble.com/RTABMAP-ROS-with-ORBSLAM3-tp10153p10169.html
Hi,
The bag you shared has missing data. For example, the camera info topics are missing, /tf and /tf_static are missing (not sure if they should be there).
rosbag info pohang00_01_sample.bag
path: pohang00_01_sample.bag
version: 2.0
duration: 1:39s (99s)
start: Jul 01 2021 00:25:49.16 (1625124349.16)
end: Jul 01 2021 00:27:29.13 (1625124449.13)
size: 4.9 GB
messages: 23356
compression: none [2001/2001 chunks]
types: dataset_converter/PohangGPS [190edd4966029545b11fdf62ed1c900f]
geometry_msgs/PoseStamped [d3812c3cbc69362b77dc0b19b345f8f5]
sensor_msgs/CompressedImage [8f7a12909da2c9d3332d540a0977563f]
sensor_msgs/Imu [6a62c6daae103f4ff57a132d6f95cec2]
sensor_msgs/NavSatFix [2d3a8cd499b9b4a0249fb98fd05cfa48]
topics: /gps/gps_nav 480 msgs : sensor_msgs/NavSatFix
/gps/gps_nav_raw 480 msgs : dataset_converter/PohangGPS
/gx5/baseline 397 msgs : geometry_msgs/PoseStamped
/gx5/imu/data 10001 msgs : sensor_msgs/Imu
/gx5/imu_calib/data 9998 msgs : sensor_msgs/Imu
/stereo_cam/left_img/compressed 1000 msgs : sensor_msgs/CompressedImage
/stereo_cam/right_img/compressed 1000 msgs : sensor_msgs/CompressedImage
The stereo images are not "stereo rectified", so without calibration file, I cannot rectify them if these are the raw images. Here is an overlay of the two images (note how they are not aligned vertically):
Left and right separately:
I checked to download the first sequence of the dataset, but this command fails silently (with --debug, it says "cannot connect", maybe an aws account is really required to download):
aws s3 sync --no-sign-request s3://pohang-canal-dataset/pohang00 .
Hopefully, I've found the dataset's Samples page. To rectify the images and create the calibration yaml files, I created that script:
#!/usr/bin/python3
import cv2
import numpy as np
from numpy.linalg import inv
from scipy.spatial.transform import Rotation
import json
import os
from pathlib import Path
def save_calibration(yaml_filename, img_size, K, D, R, P):
cv_file = cv2.FileStorage(yaml_filename, cv2.FILE_STORAGE_WRITE)
cv_file.write("image_width", img_size[0])
cv_file.write("image_height", img_size[1])
cv_file.write("camera_matrix", K)
cv_file.write("distortion_model", "plumb_bob")
cv_file.write("distortion_coefficients", D)
cv_file.write("rectification_matrix", R)
cv_file.write("projection_matrix",P)
cv_file.release()
if __name__ == "__main__":
with open('calibration/intrinsics.json') as f:
intrinsics = json.load(f)
left = intrinsics["stereo_left"]
right = intrinsics["stereo_right"]
Kl = np.matrix([
[left["focal_length"], 0, left["cc_x"]],
[0, left["focal_length"], left["cc_y"]],
[0, 0, 1] ])
Dl = np.matrix(left["distortion_coefficients"])
Kr = np.matrix([
[right["focal_length"], 0, right["cc_x"]],
[0, right["focal_length"], right["cc_y"]],
[0, 0, 1] ])
Dr = np.matrix(right["distortion_coefficients"])
img_size = (right["image_width"],right["image_height"])
with open('calibration/extrinsics.json') as f:
extrinsics = json.load(f)
left = extrinsics["stereo_left"]
right = extrinsics["stereo_right"]
Tl = np.hstack((Rotation.from_quat(left["quaternion"]).as_matrix(), np.transpose(np.matrix(left["translation"]))))
Tr = np.hstack((Rotation.from_quat(right["quaternion"]).as_matrix(), np.transpose(np.matrix(right["translation"]))))
Tl = np.vstack((Tl, [0,0,0,1]))
Tr = np.vstack((Tr, [0,0,0,1]))
RT = np.dot(inv(Tr), Tl)
R = RT[0:3, 0:3]
T = RT[0:3, 3]
R1, R2, P1, P2, Q, validRoi1, validRoi2 = cv2.stereoRectify(Kl, Dl, Kr, Dr, img_size, R, T)
save_calibration("stereo/calib_left.yaml", img_size, Kl, Dl, R1, P1)
save_calibration("stereo/calib_right.yaml", img_size, Kr, Dr, R2, P2)
xmap1, ymap1 = cv2.initUndistortRectifyMap(Kl, Dl, R1, P1, img_size, cv2.CV_32FC1)
xmap2, ymap2 = cv2.initUndistortRectifyMap(Kr, Dr, R2, P2, img_size, cv2.CV_32FC1)
directory = "stereo/left_images"
output_directory = "stereo/rectified_left_images"
Path(output_directory).mkdir(parents=True, exist_ok=True)
for filename in os.listdir(directory):
f = os.path.join(directory, filename)
img = cv2.imread(f)
img_rectified = cv2.remap(img, xmap1, ymap1, cv2.INTER_LINEAR)
out = os.path.join(output_directory, filename)
cv2.imwrite(out, img_rectified)
directory = "stereo/right_images"
output_directory = "stereo/rectified_right_images"
Path(output_directory).mkdir(parents=True, exist_ok=True)
for filename in os.listdir(directory):
f = os.path.join(directory, filename)
img = cv2.imread(f)
img_rectified = cv2.remap(img, xmap2, ymap2, cv2.INTER_LINEAR)
out = os.path.join(output_directory, filename)
cv2.imwrite(out, img_rectified)
First test with rtabmap standalone using stereo images as input (default parameters):
I tried your image with ORB_SLAM3, but I got this error after ORB_SAM3 initialized:
ORB-SLAM3 Copyright (C) 2017-2020 Carlos Campos, Richard Elvira, Juan J. Gómez, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
ORB-SLAM2 Copyright (C) 2014-2016 Raúl Mur-Artal, José M.M. Montiel and Juan D. Tardós, University of Zaragoza.
This program comes with ABSOLUTELY NO WARRANTY;
This is free software, and you are welcome to redistribute it
under certain conditions. See LICENSE.txt.
Input sensor was set to: Stereo
Loading settings from /root/Documents/RTAB-Map/rtabmap_orbslam.yaml
-Loaded camera 1
-Loaded camera 2
Camera.newHeight optional parameter does not exist...
Camera.newWidth optional parameter does not exist...
-Loaded image info
-Loaded ORB settings
Viewer.imageViewScale optional parameter does not exist...
-Loaded viewer settings
System.LoadAtlasFromFile optional parameter does not exist...
System.SaveAtlasToFile optional parameter does not exist...
-Loaded Atlas settings
System.thFarPoints optional parameter does not exist...
-Loaded misc parameters
-Computed rectification maps
----------------------------------
SLAM settings:
-Camera 1 parameters (Pinhole): [ 1813.38 1813.38 1128.04 518.378 ]
-Camera 1 distortion parameters: [ 0 0 0 0 0 ]
-Camera 2 parameters (Pinhole: [ 1813.38 1813.38 1128.04 518.378 ]
-Camera 1 distortion parameters: [ 0 0 0 0 0 ]
-Original image size: [ 2048 , 1080 ]
-Current image size: [ 2048 , 1080 ]
-Camera 1 parameters after rectification: [ 1813.38 1813.38 1128.04 518.378 ]
-Sequence FPS: 10
-Stereo baseline: 0.843787
-Stereo depth threshold : 40
-Features per image: 1000
-ORB scale factor: 2
-ORB number of scales: 3
-Initial FAST threshold: 20
-Min FAST threshold: 7
Loading ORB Vocabulary. This could take a while...
Vocabulary loaded!
Initialization of Atlas from scratch
Creation of new map with id: 0
Creation of new map with last KF id: 0
Seq. Name:
There are 1 cameras in the atlas
Camera 0 is pinhole
terminate called after throwing an instance of 'cv::Exception'
what(): OpenCV(4.2.0) ../modules/imgproc/src/color.simd_helpers.hpp:92: error: (-2:Unspecified error) in function 'cv::impl::{anonymous}::CvtHelper<VScn, VDcn, VDepth, sizePolicy>::CvtHelper(cv::InputArray, cv::OutputArray, int) [with VScn = cv::impl::{anonymous}::Set<3, 4>; VDcn = cv::impl::{anonymous}::Set<1>; VDepth = cv::impl::{anonymous}::Set<0, 2, 5>; cv::impl::{anonymous}::SizePolicy sizePolicy = cv::impl::<unnamed>::NONE; cv::InputArray = const cv::_InputArray&; cv::OutputArray = const cv::_OutputArray&]'
> Invalid number of channels in input image:
> 'VScn::contains(scn)'
> where
> 'scn' is 1
Aborted (core dumped)
The issue was because the left image was fed with 3 channels (RGB) to ORB_SLAM3, which requires gray-scale image instead. It has been fixed in this commit. Here is a result with ORB_SLAM3:
To launch rtabmap standalone, I launched your docker like this:
XAUTH=/tmp/.docker.xauth touch $XAUTH xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge - docker run -it --rm \ --privileged \ --env="DISPLAY=$DISPLAY" \ --env="QT_X11_NO_MITSHM=1" \ --volume="/tmp/.X11-unix:/tmp/.X11-unix:rw" \ --env="XAUTHORITY=$XAUTH" \ --volume="$XAUTH:$XAUTH" \ -e NVIDIA_VISIBLE_DEVICES=all \ -e NVIDIA_DRIVER_CAPABILITIES=all \ --runtime=nvidia \ --network host \ -v ~/Downloads/pohang00_pier:/rtabmap/pohang00_pier \ rtabmap_orb_slam3 source /rtabmap/catkin_ws/devel/setup.bash rtabmapParameters to set (note that for timestamp.txt to be compatible, remove the second column in that file):
I didn't do more tests with the ros package because I don't have much more time to fix the dataset -> ros converter. If you are going to use ROS (if you want to use GPS, IMU and/or lidar), you may be able to rectify images before publishing them and also send camera_info for left and right cameras at the same time around here in their code (based on what I did in python above).
cheers,
Mathieu
| Free forum by Nabble | Edit this page |