前言
-
往期内容：
-
本教材将贯穿nav2的全部内容，使用ROS2和C++实现一些仿真乃至实车中常见的建图和路径规划算法，例如
cartographer,ORB-SLAM,RRT,hybrid-astar。我们将注重与原理讲解和代码实现，去详细讲解每一步的配置过程和代码复现细节。 -
同理本教材默认大家有一些基础的
ROS2和C++的编程基础，故不对一些基础部分进行详细说明。 -
本教程使用的环境：
ROS2 humbleubuntu 22.04 LTS
-
本文涉及到部分相机内外参和深度相机相关的基础知识，可以去观看本人之前写的一些文章：
- 上手一个RGBD深度相机：从原理到实践–ROS noetic+Astra S（上）：解读深度测距原理和内外参推导
- 上手一个RGBD深度相机：从原理到实践–ROS noetic+Astra S（中）：RGB相机的标定和使用
- 上手一个RGBD深度相机：从原理到实践–ROS noetic+Astra S（下）：关于深度相机的标定由于项目原因相机被收走了没有来得及写标定的内容，后续再借到新的相机我再补充
-
上一节我们成功在ROS2环境中编译并配置了
ORB-SLAM3-ROS2,并成功使用了自己电脑的摄像头进行了单目摄像头建图的功能，本期我们就来在仿真中配置，并实现复杂仿真地图中中的单目摄像机ORB-SLAM3建图 -
效果如下
-
借助网上开源的gazebo地图 
] -
]
1 深度相机gazebo仿真插件-libgazebo_ros_camera
1-1 介绍
libgazebo_ros_camera是一个用于 Gazebo 模拟环境的插件，它能够发布原始图像和相机信息，适用于普通的相机传感器。此外，该插件还可以配置为发布原始深度图像、点云和深度相机传感器的相机信息。它还可以被配置为多相机传感器。
1-2 配置说明
- 我们打开我们的机器人
urdf描述文件qingzhou_base.xacro,为机器人添加摄像头的描述：
<!-- Camera -->
<link name="camera_link">
<visual>
<geometry>
<box size="0.05 0.07 0.07"/>
</geometry>
<origin rpy="0 0 0" xyz="0 0 -0.01"/>
<material name="rgba(255,255,255,1)">
<color rgba="1 1 1 1"/>
</material>
</visual>
</link>
<joint name="camera_joint" type="fixed">
<parent link="chassis_link"/>
<child link="camera_link"/>
<origin rpy="0 0 0" xyz="0.4 0 0"/>
</joint>
<link name="camera_optional_link"></link>
<joint name="camera_optional_joint" type="fixed">
<parent link="camera_link"/>
<child link="camera_optional_link"/>
<origin rpy="${-pi/2} 0 ${-pi/2}" xyz="0.4 0 0"/>
</joint>
- 上述代码将创建一个长方体位于车体前方用于代表机器人的摄像头组建
] - 注意这里我们创建了一个额外的
link为camera_optional_link,这是由于libgazebo_ros_camera深度相机组建的默认拍摄深度图的坐标系是沿着z轴向上的，如果设置为camera_joint的话拍摄到的深度图像将会是往上的（这个我们后面会展示），因此我们这里设置一个额外的坐标系camera_optional_link，进行两次旋转，使得最终捕获的深度图像是水平朝前的。 - 同时我们在机器人的仿真描述文件
qingzhou_gazebo_core.xacro中描述插件
<gazebo reference="camera_link">
<material>Gazebo/Black</material>
<mu1>1.0</mu1>
<mu2>1.0</mu2>
</gazebo>
<gazebo reference="camera_link">
<sensor name="camera" type="depth">
<pose>0 0 0 0 0 0 0</pose>
<visualize>true</visualize>
<update_rate>30</update_rate>
<camera>
<horizontal_fov>2</horizontal_fov>
<image>
<format>R8G8B8</format>
<width>640</width>
<height>480</height>
</image>
<distortion>
<k1>0.0</k1>
<k2>0.0</k2>
<k3>0.0</k3>
<p1>0.0</p1>
<p2>0.0</p2>
<center>0.5 0.5</center>
</distortion>
<clip>
<near>.001</near>
<far>10</far>
</clip>
</camera>
<plugin name="depth_camera" filename="libgazebo_ros_camera.so">
<frame_name>camera_optional_link</frame_name>
</plugin>
</sensor>
</gazebo>
-
下面是一些关键的参数说明：
-
horizontal_fov：相机的水平视野（Field of View），单位是弧度。在这个例子中，视野是 2 弧度。 -
format：图像的格式。R8G8B8表示每个像素的红、绿、蓝颜色分量都是 8 位。 -
width和height：图像的宽度和高度，单位是像素。 -
k1,k2,k3,p1,p2：这些是畸变系数，用于校正镜头的径向和切向畸变。 -
center：图像中心的坐标，通常为(0.5, 0.5)，表示图像中心的像素坐标。 -
near和far：近裁剪平面和远裁剪平面的距离，单位是米。在这个例子中，近裁剪平面距离相机 0.001 米，远裁剪平面距离相机 10 米。 -
frame_name：这是传感器在 ROS 中的帧名称。在这个例子中，帧名称是camera_optional_link。 -
我们将上述内容写入文件,打开
launch.py文件(建议这个时候把雷达的仿真插件在qingzhou_gazebo_core.xacro中注释，否则可能会由于性能卡到爆炸)，然后我们运行。 -
正常打开没有任何变化
-
我们查看新增加的话题
-
可以看到相机新增加了几个新的话题，我们使用
ros2 topic echo查看他们具体的内容
1-3 /camera/camera_info
/camera/camera_info：发布的是相机的内参和外参信息，包括焦距、光学中心、图像尺寸以及相机相对于世界坐标系的位姿。
header:
stamp:
sec: 493
nanosec: 9000000
frame_id: camera_optional_link
height: 480
width: 640
distortion_model: plumb_bob
d:
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
k:
- 205.46963709898583
- 0.0
- 320.5
- 0.0
- 205.46963709898583
- 240.5
- 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:
- 205.46963709898583
- 0.0
- 320.5
- -0.0
- 0.0
- 205.46963709898583
- 240.5
- 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
height和width：表示图像的高度和宽度，单位是像素。distortion_model：表示畸变模型，plumb_bob是一种畸变模型。
d：表示畸变系数，用于校正图像畸变。
k：表示相机内参矩阵的焦距和光学中心，用于描述相机成像的几何特性。- 关于相机内参可以看我之前的文章上手一个RGBD深度相机：从原理到实践–ROS noetic+Astra S（上）：解读深度测距原理和内外参推导
r：表示旋转矩阵，也就是外参矩阵，用于描述相机坐标系与世界坐标系之间的旋转关系。p：表示投影矩阵，用于将相机坐标系中的点投影到图像坐标系中。binning_x和binning_y：表示图像的像素合并，0 表示没有合并。roi：表示感兴趣区域（Region of Interest），用于指定图像的一部分进行处理。x_offset和y_offset：表示感兴趣区域的左上角坐标。height和width：表示感兴趣区域的高度和宽度。do_rectify：表示是否对感兴趣区域进行畸变校正。
1-5 其他话题以及RVIZ2可视化
-
/camera/depth/camera_info：类似于/camera/camera_info，但这个话题专门用于深度相机。它包含了深度相机的内参和外参信息，这些信息对于处理深度图像和理解场景的深度信息是必要的。- 这里echo的结果和上述
/camera/camera_info是一样的
- 这里echo的结果和上述
-
/camera/depth/image_raw:发布的是深度相机的原始深度图像。深度图像通常是一个灰度图像，其中每个像素的值代表该像素点到相机的距离。这些数据可以用于各种应用，如障碍物检测、机器人导航和三维重建。 -
/camera/image_raw:发布的是普通相机的原始彩色图像。彩色图像包含了 RGB 信息，可以用于图像识别、物体检测和其他计算机视觉任务。 -
/camera/points：发布的是由深度图像生成的点云数据。点云是一个三维坐标点的集合，每个点代表深度图像中的一个像素在三维空间中的位置。点云数据对于三维建模、场景理解和机器人感知非常有用 -
对此，我们可以使用RVIZ2进行可视化：
-
为了更方便看清楚，我们把车开到地图后面有障碍物的地方
-
然后我们根据话题，在
RVIZ2中添加ADD下属三个组建，分别选择对应的三个话题 -
-
/camera/depth/image_raw:发布的是深度相机的原始深度图像。 -
/camera/image_raw:发布的是普通相机的原始彩色图像 -
/camera/points：发布的是由深度图像生成的点云数据。
-
如上，我们得到三个图像和点云信息。
1-6 问题描述和解决方法
- 聪明的你一定发现了，上述配置有两个纯疑点:
camera_optional_link什么作用，看不懂斯密达- 为啥深度相机画面一片黑，且
RVIZ2报错
1-6-1 camera_optional_link的作用
- 那我们一个个来看，先来看看
camera_optional_link的作用，我们把
<plugin name="depth_camera" filename="libgazebo_ros_camera.so">
<frame_name>camera_optional_link</frame_name>
</plugin>
- 改成
<plugin name="depth_camera" filename="libgazebo_ros_camera.so">
<frame_name>camera_link</frame_name>
</plugin>
-
然后打开rviz2一看，天塌了亚，这也就是为前面说的
-
libgazebo_ros_camera深度相机组建的默认拍摄深度图的坐标系是沿着z轴向上的，如果设置为camera_joint的话拍摄到的深度图像将会是往上的（这个我们后面会展示），因此我们这里设置一个额外的坐标系camera_optional_link，进行两次旋转，使得最终捕获的深度图像是水平朝前的。
1-6-2 RVIZ2深度图像不可视-[INFO] [1730445634.422698525] [rviz2]: Stereo is NOT SUPPORTED
[INFO] [1730445634.422698525] [rviz2]: Stereo is NOT SUPPORTED立体视觉是一种技术，通过使用两个或多个相机来获取深度信息。它通常用于创建三维点云或进行深度感知。如果rviz2不支持立体视觉，那么它可能无法正确显示或处理来自立体相机系统的数据。- 那怎么办，看不了了吗，错误的。我们另辟蹊径：
ros2 run image_tools showimage image:=/camera/depth/image_raw
-
输出如下画面,这这这，对吗？
-
对的，这是因为现在车前面啥也没有，我们开到障碍物区域,这就看的到了!
2 ORB角点以及为什么要引入复杂环境仿真进行ORB-SLAM3建图
2-1 简单仿真直接建图失败问题描述
-
上述插件配置成功后，相信激动的你颤抖的手毫不犹豫的将上述捕获到的摄像头画面直接输入
ORB-SLAM3-ROS2的mono的节点之中，也就是直接把上节我们使用电脑摄像头的接口的图像订阅改称自己的话题。
-
然后尝试使用上节所教的节点启动指令去建图
source ./install/setup.bash
ros2 run orbslam3 mono ./src/orbslam3_ros2/vocabulary/ORBvoc.txt ./src/orbslam3_ros2/config/monocular/TUM1.yaml
-
然后这时候你会得到：
-
上述画面就这样，一动不动，停在那里，即使你摆弄小车，画面也没有接受到。
-
不服输的你又打开了上一节为讲到的捕获自己电脑画面的节点，发现却可以正常建图，这时候是不是心态崩了阿 ？不要慌，我们看看为什么
2-2 ORB角点以及ORB-SLAM3
ORB（Oriented FAST and Rotated BRIEF）角点是计算机视觉中的一种特征点检测和描述方法，它是由David G. Lowe在2009年提出的。ORB角点的主要特点包括：- FAST角点检测：ORB使用改进的FAST（Features from Accelerated Segment Test）算法来检测图像中的角点。FAST算法通过比较像素与其周围像素的亮度来判断一个像素是否是角点。
- 旋转的BRIEF描述子：ORB使用BRIEF（Binary Robust Independent Elementary Features）描述子来描述角点的特征。BRIEF描述子是一组二进制特征，通过比较图像中不同位置的像素强度来生成。ORB对BRIEF描述子进行了旋转，以提高其在不同视角下的鲁棒性。
- 尺度不变性：ORB通过在图像的多个尺度上进行角点检测来提高尺度不变性。
- 效率：ORB算法设计得非常高效，适合实时应用。
- 相较于
SIFU角点（SuperPoint）和SIFT角点（Scale-Invariant Feature Transform）,ORB角点在保证实时性的同时，还能提供较好的特征点检测和描述性能。在速度上可谓是遥遥领先。 - 而本次的主角
ORB-SLAM3是一个基于ORB角点的单目、双目和RGB-D相机的SLAM（Simultaneous Localization and Mapping）系统。ORB-SLAM3利用ORB角点进行以下关键步骤：- 前端视觉里程计：
ORB-SLAM3使用ORB角点来跟踪相机运动和周围环境的变化。通过在连续帧中检测和匹配ORB角点，系统可以估计相机的运动轨迹。 - 后端优化：在后端，
ORB-SLAM3使用图优化技术来最小化累积误差，提高位置估计的准确性。ORB角点的匹配和重投影误差是优化过程中的关键因素。 - 回环检测：
ORB-SLAM3使用ORB角点来检测回环，即相机返回到之前访问过的位置。通过识别和匹配ORB角点，系统可以纠正累积的漂移误差。 - 建图：
ORB-SLAM3使用ORB角点来构建环境地图。通过在关键帧中提取和匹配ORB角点，系统可以创建一个三维点云地图。
- 前端视觉里程计：
2-3 问题分析
- 我们可以使用简单的
opencv-python代码进行ORB角点的展示，我们分别使用两张图，他们分别是- 前几节我们画的地图
- 现实世界复杂的图片
- 代码很简单如下
import numpy as np
import cv2
def main():
img=cv2.imread('1.png')
orb = cv2.ORB_create()
kp = orb.detect(img, None)
kp, des = orb.compute(img, kp)
img2 = cv2.drawKeypoints(img, kp, None, color=(0, 255, 0), flags=0)
cv2.imshow('frame',img2)
cv2.waitKey(0)
cv2.destroyAllWindows()
if __name__ =='__main__':
main()
-
orb = cv2.ORB_create()创建了一个ORB（Oriented FAST and Rotated BRIEF）对象，用于检测图像中的特征点。 -
kp = orb.detect(img, None)使用ORB对象检测图像img中的特征点，并将检测到的关键点（keypoints）存储在变量kp中。 -
kp, des = orb.compute(img, kp)使用ORB对象计算图像img中关键点kp的特征描述子（descriptors），并将描述子存储在变量des中。 -
最后在图上画出来。这是仿真的
-
这是现实的
-
大家不难看出，简单仿真中的可提取
ORB角点数量过少，因此ORB-SLAM3无法正常工作，因此如果要在仿真中使用到ORB-SLAM3,建立复杂的仿真环境是有必要的。
3 复杂仿真环境的配置
3-1 介绍warehouse_simulation_toolkit
-
熟悉blender的朋友们可能蠢蠢欲动，建个房间还不简单，但是今天这里我就偷懒，使用别人开元的内容进行配置。
-
我这里用的是一个复杂的gazebo仿真工厂环境链接
-
-
由于原工程是适配与
ROS1的NAV1的，因此我们下载后需要修改，只使用其模型相关的内容，我们下面一步步说。
3-2 修改为ROS2支持
- 我们把源码拉到工作空间的
src目录下、
cd qingzhou_sim/src
git clone https://github.com/wh200720041/warehouse_simulation_toolkit.git
-
下载后我们需要修改源码中的
CMakeLists和package.xml以适配ROS2
-
CMakeLists.txt
cmake_minimum_required(VERSION 3.8)
project(warehouse_simulation)
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
# find dependencies
find_package(ament_cmake REQUIRED)
# uncomment the following section in order to fill in
# further dependencies manually.
# find_package(<dependency> REQUIRED)
if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
# the following line skips the linter which checks for copyrights
# comment the line when a copyright and license is added to all source files
set(ament_cmake_copyright_FOUND TRUE)
# the following line skips cpplint (only works in a git repo)
# comment the line when this package is in a git repo and when
# a copyright and license is added to all source files
set(ament_cmake_cpplint_FOUND TRUE)
ament_lint_auto_find_test_dependencies()
endif()
install(DIRECTORY
launch
urdf
meshes
models
param
rviz
worlds
DESTINATION
share/${PROJECT_NAME}
)
ament_package()
package.xml
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>warehouse_simulation</name>
<version>0.0.0</version>
<description>TODO: Package description</description>
<maintainer email="zhlucifer@todo.todo">zhlucifer</maintainer>
<license>TODO: License declaration</license>
<buildtool_depend>ament_cmake</buildtool_depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<exec_depend>joint_state_publisher_gui</exec_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>
- 修改后我们打开launch文件夹，新建文件
warehouse_simulation.launch.py
# Author: Addison Sears-Collins
# Date: September 23, 2021
# Description: Load a world file into Gazebo.
# https://automaticaddison.com
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# Set the path to the Gazebo ROS package
pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
# Set the path to this package.
pkg_share = FindPackageShare(package='warehouse_simulation').find('warehouse_simulation')
# Set the path to the world file
world_file_name = 'warehouse.world'
world_path = os.path.join(pkg_share, 'worlds', world_file_name)
# Set the path to the SDF model files.
gazebo_models_path = os.path.join(pkg_share, 'models')
os.environ["GAZEBO_MODEL_PATH"] = gazebo_models_path
########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ##############
# Launch configuration variables specific to simulation
headless = LaunchConfiguration('headless')
use_sim_time = LaunchConfiguration('use_sim_time')
use_simulator = LaunchConfiguration('use_simulator')
world = LaunchConfiguration('world')
declare_simulator_cmd = DeclareLaunchArgument(
name='headless',
default_value='False',
description='Whether to execute gzclient')
declare_use_sim_time_cmd = DeclareLaunchArgument(
name='use_sim_time',
default_value='true',
description='Use simulation (Gazebo) clock if true')
declare_use_simulator_cmd = DeclareLaunchArgument(
name='use_simulator',
default_value='True',
description='Whether to start the simulator')
declare_world_cmd = DeclareLaunchArgument(
name='world',
default_value=world_path,
description='Full path to the world model file to load')
# Specify the actions
# Start Gazebo server
start_gazebo_server_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
condition=IfCondition(use_simulator),
launch_arguments={'world': world}.items())
# Start Gazebo client
start_gazebo_client_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')),
condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])))
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_use_simulator_cmd)
ld.add_action(declare_world_cmd)
# Add any actions
ld.add_action(start_gazebo_server_cmd)
ld.add_action(start_gazebo_client_cmd)
return ld
-
修改上述文件后编译（tools是我写的一些插件，大家不用管他）
-
然后我们运行
source ./install/setup.bash
ros2 launch warehouse_simulation warehouse_simulation.launch.py
- 我们可以看到如下画面，两个人还在来回走动
3-3 加入我们的机器人
-
上述仿真配置成功后，我们就可以加入我们的机器人了
-
我们打开之前的仿真功能包，新建两个文件
-
qingzhou_gazebo_robot.launch.py单独用于加入机器人仿真的文件
#!/usr/bin/python3
# -*- coding: utf-8 -*-
import os
from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.actions import IncludeLaunchDescription
from launch.conditions import IfCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
import xacro
def generate_launch_description():
use_sim_time_arg = DeclareLaunchArgument(
"use_sim_time", default_value="true", description="Use simulation clock if true"
)
urdf_path = os.path.join(
get_package_share_directory("qingzhou_gazebo"), "urdf", "qingzhou_sim.xacro"
)
robot_state_publisher_node = Node(
package="robot_state_publisher",
executable="robot_state_publisher",
name="robot_state_publisher",
output="screen",
parameters=[
{
"use_sim_time": LaunchConfiguration("use_sim_time"),
"robot_description": xacro.process_file(urdf_path).toxml(),
}
],
)
gazebo_spawn_entity_node = Node(
name="urdf_spawner",
package="gazebo_ros",
executable="spawn_entity.py",
arguments=["-topic", "robot_description", "-entity", "qingzhou"],
output="screen",
)
return LaunchDescription(
[
use_sim_time_arg,
gazebo_spawn_entity_node,
robot_state_publisher_node,
]
)
qingzhou_gazebo_warehouse.launch.py完整复杂环境和导入下车的文件
# Author: Addison Sears-Collins
# Date: September 23, 2021
# Description: Load a world file into Gazebo.
# https://automaticaddison.com
import os
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription
from launch.conditions import IfCondition, UnlessCondition
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch.substitutions import Command, LaunchConfiguration, PythonExpression
from launch_ros.actions import Node
from launch_ros.substitutions import FindPackageShare
def generate_launch_description():
# Set the path to the Gazebo ROS package
pkg_gazebo_ros = FindPackageShare(package='gazebo_ros').find('gazebo_ros')
# Set the path to this package.
pkg_share = FindPackageShare(package='warehouse_simulation').find('warehouse_simulation')
pkg_qingzhou_gazebo= FindPackageShare(package='qingzhou_gazebo').find('qingzhou_gazebo')
# Set the path to the world file
world_file_name = 'warehouse.world'
world_path = os.path.join(pkg_share, 'worlds', world_file_name)
# Set the path to the SDF model files.
gazebo_models_path = os.path.join(pkg_share, 'models')
os.environ["GAZEBO_MODEL_PATH"] = gazebo_models_path
########### YOU DO NOT NEED TO CHANGE ANYTHING BELOW THIS LINE ##############
# Launch configuration variables specific to simulation
headless = LaunchConfiguration('headless')
use_sim_time = LaunchConfiguration('use_sim_time')
use_simulator = LaunchConfiguration('use_simulator')
world = LaunchConfiguration('world')
declare_simulator_cmd = DeclareLaunchArgument(
name='headless',
default_value='False',
description='Whether to execute gzclient')
declare_use_sim_time_cmd = DeclareLaunchArgument(
name='use_sim_time',
default_value='true',
description='Use simulation (Gazebo) clock if true')
declare_use_simulator_cmd = DeclareLaunchArgument(
name='use_simulator',
default_value='True',
description='Whether to start the simulator')
declare_world_cmd = DeclareLaunchArgument(
name='world',
default_value=world_path,
description='Full path to the world model file to load')
# Specify the actions
# Start Gazebo server
start_gazebo_server_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzserver.launch.py')),
condition=IfCondition(use_simulator),
launch_arguments={'world': world}.items())
# Start Gazebo client
start_gazebo_client_cmd = IncludeLaunchDescription(
PythonLaunchDescriptionSource(os.path.join(pkg_gazebo_ros, 'launch', 'gzclient.launch.py')),
condition=IfCondition(PythonExpression([use_simulator, ' and not ', headless])))
add_qingzhou_cmd=IncludeLaunchDescription(PythonLaunchDescriptionSource(os.path.join(pkg_qingzhou_gazebo, 'launch', 'qingzhou_gazebo_robot.launch.py')),)
# Create the launch description and populate
ld = LaunchDescription()
# Declare the launch options
ld.add_action(declare_simulator_cmd)
ld.add_action(declare_use_sim_time_cmd)
ld.add_action(declare_use_simulator_cmd)
ld.add_action(declare_world_cmd)
# Add any actions
ld.add_action(start_gazebo_server_cmd)
ld.add_action(add_qingzhou_cmd)
ld.add_action(start_gazebo_client_cmd)
return ld
-
编译运行,小车成功被加载
-
打开RVIZ2，可以看到正常的图像
-
遥控向前，同时使用
ros2 run image_tools showimage image:=/camera/depth/image_raw可视化深度图像
-
至此我们就完成了全部配置，下面就是直接把图像输入
ORB-SLAM3
4 复杂仿真环境的mono单目ORB-SLAM3
4-1 相机标定文件
-
在正式开始之前，我们需要配置相机标定的文件,上一节中
ros2 run orbslam3 mono ./src/orbslam3_ros2/vocabulary/ORBvoc.txt ./src/orbslam3_ros2/config/monocular/TUM1.yaml中我们载入了一个默认的参数文件!
-
这一节中，我们需要根据我们创建的仿真相机去配置新的文件
qingzhou_cam.yaml
%YAML:1.0
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
Camera.type: "PinHole"
# Camera calibration and distortion parameters (OpenCV)
Camera.fx: 205.46963709898583
Camera.fy: 205.46963709898583
Camera.cx: 320.5
Camera.cy: 240.5
Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.k3: 0.0
Camera.width: 640
Camera.height: 480
# Camera frames per second
Camera.fps: 30.0
# IR projector baseline times fx (aprox.)
Camera.bf: 40.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: 10.0
# Deptmap values factor
DepthMapFactor: 5000.0 # 1.0 for ROS_bag
#--------------------------------------------------------------------------------------------
# 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
- 我们来看看参数把
4-1-1 相机参数
Camera.type：指定相机类型，这里设置为"PinHole"，表示针孔相机模型。
Camera.fx: 205.46963709898583
Camera.fy: 205.46963709898583
Camera.cx: 320.5
Camera.cy: 240.5
Camera.fx和Camera.fy：相机焦距，分别表示x轴和y轴上的焦距。Camera.cx和Camera.cy：相机主点坐标，表示图像中心的像素坐标。
Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.k3: 0.0
Camera.k1、Camera.k2、Camera.p1、Camera.p2和Camera.k3：相机畸变系数，用于校正图像畸变。Camera.width和Camera.height：相机图像的宽度和高度。Camera.fps：相机帧率，表示每秒捕获的图像数量。Camera.bf：红外投影仪基线乘以焦距（fx）的近似值，用于深度计算。（这里后续和深度标定一起说）Camera.RGB：图像的颜色顺序，0表示BGR，1表示RGB。如果图像是灰度图，则该参数被忽略。ThDepth：深度阈值，表示基线乘以深度。（这里后续和深度标定一起说）DepthMapFactor：深度图因子，用于将深度值转换为像素值。
4-1-2 ORB特征提取器参数
ORBextractor.nFeatures：每张图像中提取的特征点数量。ORBextractor.scaleFactor：尺度金字塔中每层之间的尺度因子。ORBextractor.nLevels：尺度金字塔中的层数。ORBextractor.iniThFAST和ORBextractor.minThFAST：FAST角点检测的初始阈值和最小阈值。
4-1-3 可视化Viewer参数
Viewer.KeyFrameSize：关键帧的大小。Viewer.KeyFrameLineWidth：关键帧线条的宽度。Viewer.GraphLineWidth：图形线条的宽度。Viewer.PointSize：点的尺寸。Viewer.CameraSize：相机的尺寸。Viewer.CameraLineWidth：相机线条的宽度。Viewer.ViewpointX、Viewer.ViewpointY、Viewer.ViewpointZ：查看器的视角坐标。Viewer.ViewpointF：查看器的视角焦距。
4-2 ORB-SLAM3-ROS2 MONO单目建图正式启动
- 仿真启动
source ./install/setup.bash
ros2 launch qingzhou_gazebo qingzhou_gazebo_warehouse.launch.py
- RVIZ2启动
rviz
- 深度可可视化启动
source /opt/ros/humble/setup.bash
ros2 run image_tools showimage --ros-args --remap image:=/camera/depth/image_raw
- 程序启动!!!
source ./install/setup.bash
ros2 run orbslam3 mono ./src/orbslam3_ros2/vocabulary/ORBvoc.txt ./src/orbslam3_ros2/config/qingzhou_cam.yaml
-
稍等片刻或者移动车车，我们就有画面了
-
我们键盘控制小车，可以发现小车的路径被可视化
-
我们转一圈，发现建图完成闭环，至此，我们就完成了
ORB-SLAM3-ROS2的mono单目配置
5 小节和扩展
-
本节咱们将了如何使用复杂环境对单目相机
MONO进行ORB-SLAM3基础建图 -
可以看到
ORB-SLAM3下还有很多的内容
-
关于
深度相机基于RGBD的ORB-SLAM3以及如何把建立的地图转换为ROS2导航支持的栅格地图，这里我就先卖个关子，我们留到后续一起搞。 -
如有错误，欢迎指出！！！！感谢大家的支持！！！
扫一扫
358
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
