博主创建了一个科研互助群Q:772356582,欢迎大家加入讨论。
博主的合并代码
git@github.com:huashu996/VINS-FUSION-ESDFmap.git
一、D435i深度相机配置
1.1 SDK+ROS
参考我之前的博客,步骤和所遇见的问题已经写的很详细了
Jetson配置realsense D435i SDK以及realsense-ros_桦树无泪的博客-CSDN博客
1.2 相机标定参数
1、相机内参
通过rostopic的camera/info获取
-
header:标准消息头
seq:序列ID,连续递增的ID号
stamp:两个时间戳
frame_id:与此数据相关联的帧ID
height:图像尺寸,height代表高度,(height*width)相机的分辨率,以像素为单位
width:图像尺寸,width代表宽度,(height*width)相机分辨率,以像素为单位
-
distortion_model:指定了相机畸变模型,对于大多数相机,"plumb_bob"简单的径向和切向畸变模型就足够了
-
D(distortion_parameters):畸变参数也叫失真系数,取决于畸变模型,(k1, k2, t1, t2, k3)
-
K:相机内参矩阵,使用焦距(fx, fy)和主点坐标(cx, cy),单位为像素,内参矩阵可以将相机坐标中的3D点投影到2D像素坐标,数据顺序(fx,cx,fy,cy,1)
-
R:旋转矩阵,将相机坐标系统对准理想的立体图像平面,使两张立体图像中的极线平行,仅对双目相机有效
-
P:投影矩阵,左边3*3矩阵是相机的内参矩阵,将相机坐标中的3D点投影到2D像素坐标,可能与相机内参K不同。对于单目相机Tx = Ty = 0。对于双目相机,Tx和Ty有所不同。
-
binning_x:图像下采样参数,水平方向
-
binning_y:图像下采样参数,竖直方向
-
(width / binning_x) x (height / binning_y)
-
下采样:binning_x = binning_y > 1。缩小图像,生成对应图像的缩略图,使得图像符合显示区域的大小。
-
roi:感兴趣区域定义,即完整图像上的一个矩形子窗口
2.相机外参标定
Ubuntu 18.04 ———(Intel RealSense D435i)安装kalibr + 双目视觉与IMU标定(2022年)_ubuntu18.04安装kalibr并对d435i相机进行标定_@曾记否的博客-CSDN博客
二、VINS-FUSION环境配置
2.1 重要环境配置如下
ubuntu18.04
ROS
pcl 1.7
opencv3
ceres-solver-1.14.0
整体环境配置还算简单要求不严格,间要说明以下。
-
ROS
sudo apt-get install ros-melodic-cv-bridge ros-melodic-tf ros-melodic-message-filters ros-melodic-image-transport ros-melodic-octomap*
-
Ceres
sudo apt-get install liblapack-dev libsuitesparse-dev libcxsparse3 libgflags-dev libgoogle-glog-dev libeigen3-dev libgtest-dev
git clone https://github.com/ceres-solver/ceres-solver.git
cd ceres-solver/
mkdir build
cd build
cmake ..
make
sudo make install
-
代码安装
cd ~/catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git
cd ../
catkin_make
source ~/catkin_ws/devel/setup.bash
三、实际测试运行
3.1 D435I launch修改
为了启动IMU,双目摄像头
<launch>
<arg name="serial_no" default=""/>
<arg name="usb_port_id" default=""/>
<arg name="device_type" default=""/>
<arg name="json_file_path" default=""/>
<arg name="camera" default="camera"/>
<arg name="tf_prefix" default="$(arg camera)"/>
<arg name="external_manager" default="false"/>
<arg name="manager" default="realsense2_camera_manager"/>
<arg name="fisheye_width" default="640"/>
<arg name="fisheye_height" default="480"/>
<arg name="enable_fisheye" default="false"/>
<arg name="depth_width" default="640"/>
<arg name="depth_height" default="480"/>
<arg name="enable_depth" default="true"/>
<arg name="infra_width" default="640"/>
<arg name="infra_height" default="480"/>
<arg name="enable_infra1" default="true"/>
<arg name="enable_infra2" default="true"/>
<arg name="color_width" default="640"/>
<arg name="color_height" default="480"/>
<arg name="enable_color" default="true"/>
<arg name="fisheye_fps" default="30"/>
<arg name="depth_fps" default="30"/>
<arg name="infra_fps" default="30"/>
<arg name="color_fps" default="30"/>
<arg name="gyro_fps" default="200"/>
<arg name="accel_fps" default="250"/>
<arg name="enable_gyro" default="true"/>
<arg name="enable_accel" default="true"/>
<arg name="enable_pointcloud" default="false"/>
<arg name="pointcloud_texture_stream" default="RS2_STREAM_COLOR"/>
<arg name="pointcloud_texture_index" default="0"/>
<arg name="enable_sync" default="true"/>
<arg name="align_depth" default="true"/>
<arg name="publish_tf" default="true"/>
<arg name="tf_publish_rate" default="0"/>
<arg name="filters" default=""/>
<arg name="clip_distance" default="-2"/>
<arg name="linear_accel_cov" default="0.01"/>
<arg name="initial_reset" default="false"/>
<arg name="unite_imu_method" default="linear_interpolation"/>
<arg name="topic_odom_in" default="odom_in"/>
<arg name="calib_odom_file" default=""/>
<arg name="publish_odom_tf" default="true"/>
<arg name="allow_no_texture_points" default="false"/>
<arg name="emitter_enable" default="false"/>
<!-- rosparam set /camera/stereo_module/emitter_enabled false -->
<rosparam>
/camera/stereo_module/emitter_enabled: 0
</rosparam>
<rosparam if="$(arg emitter_enable)">
/camera/stereo_module/emitter_enabled: 1
</rosparam>
<group ns="$(arg camera)">
<include file="$(find realsense2_camera)/launch/includes/nodelet.launch.xml">
<arg name="tf_prefix" value="$(arg tf_prefix)"/>
<arg name="external_manager" value="$(arg external_manager)"/>
<arg name="manager" value="$(arg manager)"/>
<arg name="serial_no" value="$(arg serial_no)"/>
<arg name="usb_port_id" value="$(arg usb_port_id)"/>
<arg name="device_type" value="$(arg device_type)"/>
<arg name="json_file_path" value="$(arg json_file_path)"/>
<arg name="enable_pointcloud" value="$(arg enable_pointcloud)"/>
<arg name="pointcloud_texture_stream" value="$(arg pointcloud_texture_stream)"/>
<arg name="pointcloud_texture_index" value="$(arg pointcloud_texture_index)"/>
<arg name="enable_sync" value="$(arg enable_sync)"/>
<arg name="align_depth" value="$(arg align_depth)"/>
<arg name="fisheye_width" value="$(arg fisheye_width)"/>
<arg name="fisheye_height" value="$(arg fisheye_height)"/>
<arg name="enable_fisheye" value="$(arg enable_fisheye)"/>
<arg name="depth_width" value="$(arg depth_width)"/>
<arg name="depth_height" value="$(arg depth_height)"/>
<arg name="enable_depth" value="$(arg enable_depth)"/>
<arg name="color_width" value="$(arg color_width)"/>
<arg name="color_height" value="$(arg color_height)"/>
<arg name="enable_color" value="$(arg enable_color)"/>
<arg name="infra_width" value="$(arg infra_width)"/>
<arg name="infra_height" value="$(arg infra_height)"/>
<arg name="enable_infra1" value="$(arg enable_infra1)"/>
<arg name="enable_infra2" value="$(arg enable_infra2)"/>
<arg name="fisheye_fps" value="$(arg fisheye_fps)"/>
<arg name="depth_fps" value="$(arg depth_fps)"/>
<arg name="infra_fps" value="$(arg infra_fps)"/>
<arg name="color_fps" value="$(arg color_fps)"/>
<arg name="gyro_fps" value="$(arg gyro_fps)"/>
<arg name="accel_fps" value="$(arg accel_fps)"/>
<arg name="enable_gyro" value="$(arg enable_gyro)"/>
<arg name="enable_accel" value="$(arg enable_accel)"/>
<arg name="publish_tf" value="$(arg publish_tf)"/>
<arg name="tf_publish_rate" value="$(arg tf_publish_rate)"/>
<arg name="filters" value="$(arg filters)"/>
<arg name="clip_distance" value="$(arg clip_distance)"/>
<arg name="linear_accel_cov" value="$(arg linear_accel_cov)"/>
<arg name="initial_reset" value="$(arg initial_reset)"/>
<arg name="unite_imu_method" value="$(arg unite_imu_method)"/>
<arg name="topic_odom_in" value="$(arg topic_odom_in)"/>
<arg name="calib_odom_file" value="$(arg calib_odom_file)"/>
<arg name="publish_odom_tf" value="$(arg publish_odom_tf)"/>
<arg name="allow_no_texture_points" value="$(arg allow_no_texture_points)"/>
</include>
</group>
</launch>
3.2 单目+IMU
修改配置文件VINS-FUSION/src/VINS-Fusion-RGBD/config/realsense/realsense_d435i_config.yaml
主要修改相机的外参矩阵、内参矩阵、topic,不修改也能运行,漂移大。
roslaunch realsense2_camera rs_camera_vins.launch
roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_d435i_config.yaml
3.1 双目+IMU
修改配置文件VINS-FUSION/src/VINS-Fusion-RGBD/config/realsense/realsense_stereo_imu_config.yaml
主要修改两个相机的外参矩阵、内参矩阵、topic。
roslaunch realsense2_camera rs_camera_vins.launch
roslaunch vins vins_rviz.launch
rosrun vins vins_node ~/catkin_ws/src/VINS-Fusion/config/realsense_d435i/realsense_stereo_imu_config.yaml
四、ESDF建图
4.1 下载代码编译
cd ~/catkin_ws/src
git clone https://github.com/HKUST-Aerial-Robotics/FIESTA
cd ../
catkin_make
source ~/catkin_ws/devel/setup.bash
4.2 修改代码
-
1.修改launch
<launch>
<node pkg="fiesta" type="test_fiesta" name="fiesta" output="screen"
required="true">
<param name="resolution" value="0.05"/>
<param name="update_esdf_every_n_sec" value="0.1"/>
<!--Hash Table only-->
<param name="reserved_size" value="1000000"/>
<!--Array only-->
<param name="lx" value="-10.0"/>
<param name="ly" value="-10.0"/>
<param name="lz" value="-1"/>
<param name="rx" value="+10.0"/>
<param name="ry" value="+10.0"/>
<param name="rz" value="+3"/>
<!-- raycasting parameters -->
<param name="min_ray_length" value="0.5"/>
<param name="max_ray_length" value="5.0"/>
<!--If hash table is used, make sure the value is 0. In array implementation, 0 means no other thread.-->
<param name="ray_cast_num_thread" value="0"/>
<!-- needed when using depth image-->
<param name="center_x" value="381.91"/>
<param name="center_y" value="241.553"/>
<param name="focal_x" value="609.738"/>
<param name="focal_y" value="608.557"/>
<!-- probabilistic grid map -->
<param name="p_hit" value="0.70"/>
<param name="p_miss" value="0.35"/>
<param name="p_min" value="0.12"/>
<param name="p_max" value="0.97"/>
<param name="p_occ" value="0.80"/>
<!-- global / local -->
<param name="global_map" value="true"/>
<param name="global_update" value="true"/>
<param name="global_vis" value="true"/>
<param name="radius_x" value="3.0"/>
<param name="radius_y" value="3.0"/>
<param name="radius_z" value="1.5"/>
<!--depth_filter -->
<param name="use_depth_filter" value="true"/>
<param name="depth_filter_tolerance" value="0.1"/>
<param name="depth_filter_max_dist" value="10.0"/>
<param name="depth_filter_min_dist" value="0.1"/>
<!--unit: pixel-->
<param name="depth_filter_margin" value="0"/>
<!-- visulization -->
<!--0 for no visulize-->
<param name="visualize_every_n_updates" value="10"/>
<param name="slice_vis_max_dist" value="2.0"/>
<!-- relative to the lz if array is used, relative to the origin if hash table is used -->
<param name="slice_vis_level" value="1.6"/>
<param name="vis_lower_bound" value="0"/>
<param name="vis_upper_bound" value="+10"/>
<!-- subsribe source -->
<remap from="~depth" to="/camera/depth/image_rect_raw"/>
<remap from="~transform" to="/vins_estimator/camera_pose"/>
</node>
<node name="rvizvisualisation" pkg="rviz" type="rviz" output="log" args="-d $(find fiesta)/demo.rviz" />
</launch>
2.修改主函数
FIESTA-master/src/FIESTA/test/test_fiesta.cpp,默认是点云输入格式。
#include "Fiesta.h"
int main(int argc, char **argv) {
ros::init(argc, argv, "FIESTA");
ros::NodeHandle node("~");
//fiesta::Fiesta<sensor_msgs::PointCloud2::ConstPtr, geometry_msgs::TransformStamped::ConstPtr> esdf_map(node);fiesta::Fiesta<sensor_msgs::Image::ConstPtr, nav_msgs::Odometry::ConstPtr> esdf_map(node);
fiesta::Fiesta<sensor_msgs::Image::ConstPtr, nav_msgs::Odometry::ConstPtr> esdf_map(node);
ros::spin();
return 0;
}
4.3 运行
roslaunch fiesta D435i.launch
运行效果图
双目文章来源:https://www.toymoban.com/news/detail-467825.html
单目文章来源地址https://www.toymoban.com/news/detail-467825.html
到了这里,关于工程(十一)——NUC11+D435i+VINS-FUSION+ESDF建图(github代码)的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!