ROS运行ORB-SLAM2，D435i深度摄像头运行ORB-SLAM2，ROS编译的Syntax check of ORB_SLAM2/manifest.xml failed解决

一.运行ROS版的ORB-SLAM2

1. 将Examples/ROS/ORB_SLAM2路径添加到ROS_PACKAGE_PATH环境变量中。打开 .bashrc 文件并在末尾添加以下行。将 PATH 替换为克隆 ORB_SLAM2 的文件夹：

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM2/Examples/ROS
# 我的是下面这个路径：
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/cgm/ORB_SLAM2/Examples/ROS

2. 执行 build_ros.sh 脚本：

chmod +x build_ros.sh
./build_ros.sh

3. 报错Error from syntax check of ORB_SLAM2/manifest.xml

build.sh正常编译，但build_ros.sh编译不通过，报错如下：

[rosbuild] Building package ORB_SLAM2
[rosbuild] Error from syntax check of ORB_SLAM2/manifest.xml
Traceback (most recent call last):
  File "", line 1, in <module>
  File "/opt/ros/noetic/lib/python3/dist-packages/roslib/__init__.py", line 50, in <module>
    from roslib.launcher import load_manifest  # noqa: F401
  File "/opt/ros/noetic/lib/python3/dist-packages/roslib/launcher.py", line 42, in <module>
    import rospkg
ImportError: No module named rospkg
CMake Error at /opt/ros/noetic/share/ros/core/rosbuild/private.cmake:77 (message):
  [rosbuild] Syntax check of ORB_SLAM2/manifest.xml failed; aborting
Call Stack (most recent call first):
  /opt/ros/noetic/share/ros/core/rosbuild/public.cmake:174 (_rosbuild_check_manifest)
  CMakeLists.txt:4 (rosbuild_init)


-- Configuring incomplete, errors occurred!
See also "/home/cgm/ORB_SLAM2/Examples/ROS/ORB_SLAM2/build/CMakeFiles/CMakeOutput.log".
make: *** 没有指明目标并且找不到 makefile。 停止。

参考博客1

参考博客2

解决办法：

python版本原因，核心思想是让系统变量python指向python3而不是原来的python2；使用修改软连接的方式，让“python”指向python3；

在/usr/bin目录下ls -l 查看python软连接文件，发现python->python2

先删除python->python2软连接。再创建python3软连接

// 删除软连接
sudo rm -r link python
// 重新创建软连接
sudo ln -s python3  /usr/bin/python

最后检查python的软连接已经指向python3

lrwxrwxrwx 1 root             root                    7 9月  25 09:39  python -> python3

这是一个符号链接（也称为软链接）的长格式列表输出。

• lrwxrwxrwx: 这是文件的权限字符串。

  • l: 表示这是一个链接。
  • rwxrwxrwx: 这是权限部分，其中每三个字符代表一个权限组。第一组rwx是所有者的权限，第二组rwx是组的权限，第三组rwx是其他用户的权限。在这里，每组都有rwx权限，表示读（r）、写（w）和执行（x）权限。

• 1: 这是文件的硬链接数。对于符号链接，这通常是1。

• root: 这是文件的所有者。在这种情况下，文件的所有者是root用户。

• root: 这是文件的组。在这种情况下，文件所属的组也是root。

• 7: 这是文件的大小，以字节为单位。对于符号链接，这通常是链接目标的字符数。

• 9月 25 09:39: 这是文件的最后修改时间。

• python: 这是文件（或链接）的名称。

• ->: 这表示它是一个符号链接，后面跟着的是链接的目标。

• python3: 这是符号链接的目标。这意味着当您尝试运行或访问python时，您实际上是在运行或访问python3。

总之，这意味着在您的系统上，当您键入python并尝试执行它时，您实际上是在执行python3。这是通过创建一个指向python3的符号链接来实现的。

改之前，要使用python3

改之后，使用python就是python3

4.再次运行 ./build_ros.sh，错误：usleep在/home/cgm/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc中没有声明。

/home/cgm/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc:233:9: error: ‘usleep’ was not declared in this scope
  233 |         usleep(mT*1000);

• 打开/home/cgm/ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc文件。
• 在文件的顶部，添加#include 。
• 保存文件并重新尝试编译。

5. 报错

[ 77%] Linking CXX executable ../MonoAR
/usr/bin/ld: warning: libopencv_imgproc.so.4.2, needed by /opt/ros/noetic/lib/libcv_bridge.so, may conflict with libopencv_imgproc.so.3.4
/usr/bin/ld: CMakeFiles/MonoAR.dir/src/AR/ViewerAR.cc.o: undefined reference to symbol '_ZN2cv7putTextERKNS_17_InputOutputArrayERKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEENS_6Point_IiEEidNS_7Scalar_IdEEiib'
/usr/bin/ld: /usr/lib/x86_64-linux-gnu/libopencv_imgproc.so.4.2.0: error adding symbols: DSO missing from command line
collect2: error: ld returned 1 exit status
make[2]: *** [CMakeFiles/MonoAR.dir/build.make:278：../MonoAR] 错误 1
make[1]: *** [CMakeFiles/Makefile2:541：CMakeFiles/MonoAR.dir/all] 错误 2
make: *** [Makefile:130：all] 错误 2

这些错误指出了一个主要问题：您的系统上安装了多个版本的OpenCV库，并且在链接时发生了冲突。

解决：在/home/cgm/ORB_SLAM2/Examples/ROS/ORB_SLAM2/CMakeLists.txt中的

#这句
find_package(OpenCV 3.0 QUIET)
#改为下面两句
SET(OpenCV_DIR /usr/local/lib/cmake/opencv4/)   # 设置OpenCV_DIR
find_package(OpenCV QUIET)
#我的是报错了，你们的不一定会报错

其他人的解决办法ROS-noetic中OpenCV4和ORB_SLAM2中OpenCV3不匹配的问题

编译成功

Build type: Release
-- Using flag -std=c++11.
-- Configuring done
-- Generating done
-- Build files have been written to: /home/cgm/ORB_SLAM2/Examples/ROS/ORB_SLAM2/build
[  0%] Built target rospack_genmsg_libexe
[  0%] Built target rosbuild_precompile
[ 88%] Built target Mono
[ 88%] Built target RGBD
[100%] Built target Stereo
[100%] Built target MonoAR

二.Running RGB_D Node 运行 RGB_D 节点

对于来自话题 /camera/rgb/image_raw 和 /camera/depth_registered/image_raw 的 RGB-D 输入，运行节点 ORB_SLAM2/RGBD。您将需要提供词汇表文件和设置文件。

rosrun ORB_SLAM2 RGBD PATH_TO_VOCABULARY(词典路径) PATH_TO_SETTINGS_FILE（设置路径，默认的是Asus.yaml）

下载https://cvg.cit.tum.de/rgbd/dataset/freiburg3/rgbd_dataset_freiburg3_walking_xyz.bag

ORB SLAM2默认订阅的RGB-D图像主题（ /camera/rgb/image_raw 和 /camera/depth_registered/image_raw ）可能与你的bag文件中的主题不匹配。你可以使用以下命令查看bag文件中的主题：

cgm@cgm:~/下载$ rosbag info rgbd_dataset_freiburg3_walking_xyz.bag
path:         rgbd_dataset_freiburg3_walking_xyz.bag
version:      2.0
duration:     29.1s
start:        Jul 09 2012 23:05:13.48 (1341846313.48)
end:          Jul 09 2012 23:05:42.54 (1341846342.54)
size:         551.8 MB
messages:     10341
compression:  bz2 [1693/1693 chunks; 31.75%]
uncompressed:   1.7 GB @ 59.7 MB/s
compressed:   551.1 MB @ 19.0 MB/s (31.75%)
types:        sensor_msgs/CameraInfo         [c9a58c1b0b154e0e6da7578cb991d214]
              sensor_msgs/Image              [060021388200f6f0f447d0fcd9c64743]
              tf/tfMessage                   [94810edda583a504dfda3829e70d7eec]
              visualization_msgs/MarkerArray [90da67007c26525f655c1c269094e39f]
topics:       /camera/depth/camera_info    834 msgs    : sensor_msgs/CameraInfo        
              /camera/depth/image          833 msgs    : sensor_msgs/Image             
              /camera/rgb/camera_info      859 msgs    : sensor_msgs/CameraInfo        
              /camera/rgb/image_color      859 msgs    : sensor_msgs/Image             
              /cortex_marker_array        2900 msgs    : visualization_msgs/MarkerArray
              /tf                         4056 msgs    : tf/tfMessage

我们可以看到以下信息：

1. bag文件中的RGB图像主题是/camera/rgb/image_color，而深度图像主题是/camera/depth/image。
2. 在ros_rgbd.cc代码中，ORB SLAM2订阅的RGB图像主题是/camera/rgb/image_raw，深度图像主题是/camera/depth_registered/image_raw。

这里的主题名称不匹配，所以直接运行 cgm@cgm:~/ORB_SLAM2$ rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt /home/cgm/ORB_SLAM2/Examples/ROS/ORB_SLAM2/Asus.yamlORB 和 cgm@cgm:~/下载$ rosbag play rgbd_dataset_freiburg3_walking_xyz.bag 是没有接收到图像数据，就像这样：

为了解决这个问题，你需要修改ros_rgbd.cc中的主题名称，使其与bag文件中的主题名称匹配。具体修改如下：

将：

message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_raw", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "camera/depth_registered/image_raw", 1);

修改为：

message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/rgb/image_color", 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth/image", 1);

完成上述修改后，重新编译ORB SLAM2的ROS节点，并再次运行。然后，当你播放bag文件时，ORB SLAM2应该能够接收到图像数据并开始处理。

确保在运行ORB SLAM2节点之前先启动roscore，然后再在另一个终端中播放bag文件。

会有点卡顿

三.D435i深度摄像头运行ORB-SLAM2

1.修改一个文件

修改ORB_SLAM2/Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc文件

    // D435i 的 topic 是 /camera/color/image_raw 和 /camera/depth/image_rect_raw
    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, "/camera/color/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, "/camera/depth/image_rect_raw", 1);

2.重新编译 ./build_ros.sh

./build_ros.sh

3.查看相机内参

打开一个终端输入

roslaunch realsense2_camera rs_camera.launch

再打开一个终端输入

rostopic echo /camera/color/camera_info

查看到的相机内参如下:

  stamp: 
    secs: 1695614698
    nsecs: 941847086
  frame_id: "camera_color_optical_frame"
height: 720
width: 1280
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [909.855712890625, 0.0, 651.5874633789062, 0.0, 909.7683715820312, 381.3797302246094, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [909.855712890625, 0.0, 651.5874633789062, 0.0, 0.0, 909.7683715820312, 381.3797302246094, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
---

4.相机内参矩阵 ( K )

相机的内参矩阵通常表示为：

K = [ f x 0 c x 0 f y c y 0 0 1 ] K = \begin{bmatrix} fx & 0 & cx \\ 0 & fy & cy \\ 0 & 0 & 1 \\ \end{bmatrix} K= ​fx00​0fy0​cxcy1​ ​
其中：

• ( fx ) 和 ( fy )：相机的焦距，分别在图像的 x 和 y 方向上。这些值决定了图像上的点如何映射到相机坐标系中。
• ( cx ) 和 ( cy )：图像的主点坐标。主点是3D世界中的点投影到图像平面上的点。

从提供的 K 矩阵中，我们得到：

• ( fx = 910.0997314453125 )
• ( fy = 909.994873046875 )
• ( cx = 639.4933471679688 )
• ( cy = 359.3774108886719 )

5.创建配置文件

切换到ORB_SLAM2/Examples/ROS/ORB_SLAM2目录下
打开终端输入

cd ORB_SLAM2/Examples/ROS/ORB_SLAM2

新建MyD435i.yaml文件

在终端里输入

touch MyD435i.yaml

%YAML:1.0
 
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
 
# Camera calibration and distortion parameters (OpenCV) 
# rostopic echo /camera/color/camera_info中查看的是 K: [909.855712890625, 0.0, 651.5874633789062, 0.0, 909.7683715820312, 381.3797302246094, 0.0, 0.0, 1.0]
# 改为自己的相机的内参矩阵K
Camera.fx: 909.855712890625
Camera.fy: 909.7683715820312
Camera.cx: 651.5874633789062
Camera.cy: 381.3797302246094

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.p3: 0.0

Camera.width: 640
Camera.height: 480
# Camera frames per second 
Camera.fps: 30.0

# IR projector baseline times fx (aprox.)
# bf = baseline (in meters) * fx, D435i的 baseline = 50 mm 
Camera.bf: 50.0
 
# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1
 
# Close/Far threshold. Baseline times.
ThDepth: 40.0
 
# Deptmap values factor
DepthMapFactor: 1000.0
 
#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------
 
# ORB Extractor: Number of features per image
ORBextractor.nFeatures: 1000
 
# ORB Extractor: Scale factor between levels in the scale pyramid 	
ORBextractor.scaleFactor: 1.2
 
# ORB Extractor: Number of levels in the scale pyramid	
ORBextractor.nLevels: 8
 
# 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: 7
 
#--------------------------------------------------------------------------------------------
# Viewer Parameters
#--------------------------------------------------------------------------------------------
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
 
PointCloudMapping.Resolution: 0.01
meank: 50
thresh: 2.0

6.运行

roslaunch realsense2_camera rs_camera.launch

在ORB_SLAM2文件夹下再打开一个终端输入

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/MyD435i.yaml

即可运行成功