Intel RealSense SDK 2.0 é um pacote de desenvolvimento de plataforma cruzada, incluindo ferramentas básicas de câmera, como realsense-viewer, e também fornece interfaces ricas para desenvolvimento secundário, incluindo ROS, python, Matlab, node.js, LabVIEW, OpenCV, PCL, .NET etc.
A câmera usada desta vez é a D435i
, que pode fornecer profundidade e imagens RGB, e possui IMU.
Este processo consiste em usar a câmera D435i para construção de nuvem de pontos SLAM.
Instale o Intel RealSense SDK e visualize os dados da câmeraLinux
/Ubuntu - RealSense SDK 2.0 Construction Guide
Depois de instalada, visualize a profundidade da câmera e as imagens RGB
realsense-viewer
Em seguida, baixe o pacote de funções do driver da estrutura ROS que aciona cada câmera do intel
ROS wrapper para dispositivos Intel® RealSense™
Quando a instalação for concluída, você poderá começar a ver os dados da câmera
Continuamos a configurar o SMAL aqui.
Antes de iniciar a câmera, precisamos definir o arquivo rs_camera.launch no rospack realsense2_camera
O primeiro é conseguir a sincronização temporal de diferentes dados do sensor (profundidade, RGB, IMU), ou seja, ter o mesmo carimbo de data / hora; o último
adicionará uma série de rostopic, dos quais estamos mais preocupados que /camera/aligned_depth_to_color/image_rawa imagem de profundidade aqui esteja alinhada com a imagem RGB
<arg name="enable_sync" default="true"/>
<arg name="align_depth" default="true"/>
Depois de concluído, você pode iniciar a câmera com o seguinte comando
roslaunch realsense2_camera rs_camera.launch
A chave é que /camera/color/image_raw e /camera/aligned_depth_to_color/image_raw
correspondem à imagem RGB e à imagem de profundidade, respectivamente
Em seguida Examples/RGB-D, crie um novo RealSense.yamlarquivo de parâmetro chamado,
então edite
#--------------------------------------------------------------------------------------------
# Camera calibration and distortion parameters (OpenCV)
Camera.fx: 909.3504638671875
Camera.fy: 908.5739135742188
Camera.cx: 645.5443725585938
Camera.cy: 369.3554992675781
Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.k3: 0.0
Camera.width: 1280
Camera.height: 720
#Camera frames per second
Camera.fps: 30.0
#IR projector baseline times fx (aprox.)
Camera.bf: 46.01
#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，将深度像素值转化为实际距离，原来单位是 mm，转化成 m
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
Dentre eles, contém os parâmetros internos da câmera, que podem ser obtidos através do seguinte, e os parâmetros de cada câmera podem ser diferentes
rostopic echo /camera/color/camera_info
A principal coisa a se prestar atenção no yaml aqui é
# 相机标定和畸变参数（OpenCV）
Para modificar os dados de acordo com o tópico [/camera/color/camera_info],
os parâmetros podem ser verificados em relação ao URL para visualizar as informações específicas do parâmetro
http://docs.ros.org/en/melodic/api/sensor_msgs/html /msg/CameraInfo.html
Esses são os seguintes parâmetros
Camera.fx: 909.3504638671875
Camera.fy: 908.5739135742188
Camera.cx: 645.5443725585938
Camera.cy: 369.3554992675781
Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.k3: 0.0
Camera.width: 1280
Camera.height: 720
Salve e saia quando terminar
então modifique src/pointcloudmapping.cco arquivo
1. Perto da linha 128
voxel.setLeafSize (0.02f, 0.02f, 0.02f); //Modified place 1 调节点云密度
2, próximo à linha 75
p.y = ( m - kf->cy) * p.z / kf->fy;
// p.y = - ( m - kf->cy) * p.z / kf->fy; //Modified place2 与原先添加了负号,将原本颠倒的点云地图上下翻转，方便观察
//【后续测试显示的pcd完全无法使用。不进行上下翻转】
p.r = color.ptr<uchar>(m)[n*3]; //Modified place3 修改颜色显示
p.g = color.ptr<uchar>(m)[n*3+1];
p.b = color.ptr<uchar>(m)[n*3+2];
#########2023年04月17日08:54:17 UTC+8:00#########
##########Devido à ambiguidade do original captura de tela, Substitua a nova imagem #########
Em seguida, compile ORB_SLAM2_modified
./build.sh
./build_ros.sh
Teste após compilar
Inicie o nó mestre
roscore
iniciar câmera
roslaunch realsense2_camera rs_camera.launch
Construa um mapa e encontre um erro
rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.bin Examples/RGB-D/RealSense.yaml /camera/rgb/image_raw:=/camera/color/image_raw /camera/depth_registered/image_raw:=/camera/aligned_depth_to_color/image_raw
Após a pesquisa, pode ser o arquivo yaml . Opencv4.0 precisa ser modificado para ler o erro do arquivo yaml erro: (-49: Código de erro desconhecido -49) O arquivo de entrada está vazio na função 'abrir'
Precisa adicionar no topo do arquivo yaml
%YAML:1.0
repetir
rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.bin Examples/RGB-D/RealSense.yaml /camera/rgb/image_raw:=/camera/color/image_raw /camera/depth_registered/image_raw:=/camera/aligned_depth_to_color/image_raw
Após CTRL+C, você pode ver o arquivo pcd de saída
Ver arquivos pcd salvos
pcl_viewer vslam.pcd
Até o momento, o processo de implementação do ORB SLAM 2 baseado na câmera de profundidade Intel RealSense D435i foi bem-sucedido
No entanto, como as instruções para iniciar o SLAM são um pouco complicadas, use roslauch para otimizar
o primeiro é iniciar o pacote offline
Quando o comando iniciado não for inserido por completo, o terminal exibirá o uso usado
Usage: rosrun ORB_SLAM2 RGBD path_to_vocabulary path_to_settings
Primeiro crie um pacote de funções chamado slam
cd ~/catkin_ws/src
catkin_create_pkg slam std_msgs rospy roscpp
Em seguida, crie um arquivo de inicialização neste pacote de recursos, nomeei-o para rgbd.launch
edição
<launch>
<!--定义全局参数-->
<arg name="rgb_image" default="/camera/rgb/image_raw"/>
<arg name="path_to_vacabulary" default="/home/heying/ORB_SLAM2_modified/Vocabulary/ORBvoc.bin"/>
<arg name="path_to_settings" default="/home/heying/ORB_SLAM2_modified/Examples/RGB-D/TUM1_ROS.yaml"/>
<!--播放离线包-->
<!-- 注意这里bag文件的路径必须为绝对路径-->
<node pkg="rosbag" type="play" name="player" output="screen"
args=" /home/heying/ORB_SLAM2_modified/Examples/datasets/rgbd_dataset_freiburg1_xyz.bag">
<remap from="/camera/rgb/image_color" to="/camera/rgb/image_raw" />
<remap from="/camera/depth/image" to="/camera/depth_registered/image_raw" />
</node>
<!--启动ORB-SLAM2 RGBD-->
<node name ="RGBD" pkg="ORB_SLAM2" type="RGBD"
args="$(arg path_to_vacabulary) $(arg path_to_settings)" respawn="true" output="screen">
<!--remap from="/camera/image_raw" to="$(arg rgb_image)"/-->
</node>
</launch>
teste após a conclusão
roslaunch slam rgbd.launch
Início normal
câmera ao vivo
Crie um novo arquivo de lançamento para iniciar a câmera e construir um mapa
touch camera.launch
gedit camera.launch
então escreva
O primeiro é o parâmetro global, não digite o comando completo primeiro, você pode ver que o método de uso é gerado path_to_vocabularye a soma que sai path_to_settingsé o parâmetro global que precisa ser definido
rosrun ORB_SLAM2 RGBD
<launch>
<!--定义全局参数-->
<arg name = "path_to_vocabulary" default = "/home/heying/ORB_SLAM2_modified/Vocabulary/ORBvoc.bin"/>
<arg name = "path_to_settings" default = "/home/heying/ORB_SLAM2_modified/Examples/RGB-D/RealSense.yaml"/>
<!--include file="$(find realsense2_camera)/launch/rs_camera.launch"/-->
<!--启动ORB-SLAM2 RGBD-->
<node pkg = "ORB_SLAM2" type ="RGBD" name = "RGBD_camera" args="$(arg path_to_vocabulary) $(arg path_to_settings)" respawn="true" output="screen">
<remap from="/camera/rgb/image_raw" to="/camera/color/image_raw" />
<remap from="/camera/depth_registered/image_raw" to="/camera/aligned_depth_to_color/image_raw" />
</node>
</launch>
Teste
o início do nó mestre
roscore
Em seguida, verifique a conexão de hardware da câmera
iniciar câmera
roslaunch realsense2_camera rs_camera.launch
Iniciar mapeamento
roslaunch slam camera.launch
A inicialização é normal e o efeito é perfeito.
O arquivo pcd será gerado após o programa CTRL+C
Veja o efeito
pcl_viewer vslam.pcd
O efeito é bom.
Até agora, o processo de otimização do comando de inicialização do uso do Intel D435i para construir o mapa de nuvem de pontos SLAM e o mapa de câmera slam terminou.