草庐IT

工程（十一）——NUC11+D435i+VINS-FUSION+ESDF建图（github代码）

桦树无泪 2025-05-12 原文

博主的合并代码

git@github.com:huashu996/VINS-FUSION-ESDFmap.git

一、D435i深度相机配置

1.1 SDK+ROS

参考我之前的博客，步骤和所遇见的问题已经写的很详细了

https://blog.csdn.net/HUASHUDEYANJING/article/details/129323834?spm=1001.2014.3001.5501

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.相机外参标定

https://blog.csdn.net/qq_38364548/article/details/124917067

二、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

运行效果图

双目

单目

mdashVINS-FUSION3461namec++视觉slamlinux

有关工程（十一）——NUC11+D435i+VINS-FUSION+ESDF建图（github代码）的更多相关文章

  1. 语法类似于 GitHub Flavored Markdown 的 Ruby markdown 解释器？ - 2

    我使用Jekyll运行博客，并认为我会解决RedcarpetMarkdown解释器，因为它是developedandusedbyGitHub.好吧，我只是碰巧遇到了一个错误，去检查问题，然后foundthis.Maintainersays,"Asyouprobablyhavenoticed(harharharhar)Idon'thavetimetomaintainRedcarpetanymore.It'snotapriorityforme(IfindMarkdownthoroughlyboring)andit'snotapriorityforGitHub,becausewenolong

  2. ruby - vagrant 从 github 安装插件 - 2

    我们正在使用Vagrant进行部署，我们最终希望将此集群部署在Rackspace上。vagrant-rackspace插件是一个自然的选择，但它有一些错误，这些错误未包含在最新的0.1.1版本中(notablythatvagrantprovisiondoesn'twork)。我已经在我的personalfork中解决了这个问题通过合并其他人的工作来对存储库进行改造。是否可以从github安装vagrant插件？显而易见的事情没有奏效:[unix]$vagrantplugininstallvagrant-rackspace--plugin-sourcehttps://github.com

  3. ruby - 如何让 GitHub 页面使用 master 分支？ - 2

    我有一个使用Jekyll托管在GitHub上的静态网站。问题是，我真的不需要master分支，因为存储库唯一包含的是网站。这样我就必须gitcheckoutgh-pages，然后gitmergemaster，然后gitpushorigingh-pages。有什么简单的方法可以摆脱gh-pages分支并直接从master推送？ 最佳答案 Theproblemis,Idon'treallyneedthemasterbranch,astheonlythingtherepositorycontainsisthewebsite.Isthere

  4. ruby - 警告 : PATH set to RVM ruby but GEM_HOME and/or GEM_PATH not set, 请参阅 : https://github. com/wayneeseguin/rvm/issues/3212 - 2

    我每次打开终端时都会收到这个错误:警告:PATH设置为RVMruby​​但未设置GEM_HOME和/或GEM_PATH，请参阅:https://github.com/wayneeseguin/rvm/issues/3212这是在我最近安装zsh(oh-my-zsh)后开始发生的我不知道如何设置GEM_HOME和/或GEM_PATH的路径。 最佳答案 我也面临同样的问题，更改.zshrc中的以下行，exportPATH="/usr/local/heroku/bin:.........."到exportPATH="$PATH:/usr/

  5. ruby - github api v3 创建问题消息未找到 - 2

    当我尝试创建一个github问题时，它给出消息未找到回复。以及如何发送身份验证header。因为创建问题需要用户登录或验证curl-XPOST-i-d'{"title":"my-new-repo","body":"mynewissuedescription"}'https://api.github.com/repos/barterli/barter.li/issuesHTTP/1.1404NotFoundServer:GitHub.comDate:Wed,19Feb201407:11:33GMTContent-Type:application/json;charset=utf-8Sta

  6. ruby - 如何在 github 操作中捆绑安装私有(private) gem - 2

    我想通过github操作在gem上运行rspec(称之为priv_gem_a)。priv_gem_a依赖于私有(private)存储库中的另一个gem(称之为priv_gem_b)。但是，由于权限无效，我无法捆绑安装priv_gem_b。错误:Fetchinggemmetadatafromhttps://rubygems.org/..........Fetchinggit@github.com:myorg/priv_gem_bHostkeyverificationfailed.fatal:Couldnotreadfromremoterepository.Pleasemakesureyo

  7. ruby - 如何使用 webhooks 从 Gitlab 推送到 Github - 2

    如果我只能找到正确的手册，我的Google-fu就会让我失望，因为这看起来很明显。我有一个由我们的托管服务提供商安装的Gitlab服务器Gitlab服务器有很多项目。对于其中一些项目，我希望Gitlab每次从本地客户端推送到Gitlab时自动推送到远程存储库(在本例中为Github)。像这样:客户端-->gitlab-->github任何标签和分支也应该被推送。AFAICT我有3个选择:用两个Remote配置本地客户端，同时推送到Gitlab和Github。我想避免这种情况，因为开发人员。在Gitlab服务器上的存储库中添加一个gitpost-receiveHook。这将是最灵活的(我

  8. ruby - 使用 overcommit 和 Github Desktop 时出错 - 2

    我在我的项目中使用过度使用gem(https://github.com/brigade/overcommit)，当我使用GithubDesktopforosx时，我得到这些错误:ThisrepositorycontainshooksinstalledbyOvercommit,buttheovercommitgemisnotinstalled.Installitwithgeminstallovercommit.(1)gem已安装，它可以在终端中运行。我想这是因为我使用rvm而GithubDesktop不知道rvm。有人知道如何解决这个问题吗？ 最佳答案

  9. ruby - Jekyll:如何在 GitHub 页面上使用自定义插件？ - 2

    事实证明，由于securityconcerns，自定义ruby​​插件在GitHub页面上不起作用。.我正在尝试将插件(thisone)添加到我的Jekyll项目的_plugins文件夹中，但是当我将它部署到GitHub时，它会被忽略。问题:有没有办法解决这个问题？有没有人找到解决办法？注意:显然我可以在本地生成html文件并将它们提交到我的存储库。但这不是我想要的。 最佳答案 如果你想让Jekyll站点像在本地一样运行，比如让自定义插件正常工作，这里有一种非常方便的方式来构建和部署Jekyll站点到GithubPages。?AGi

  10. ruby - git:从 bitbucket 导出并导入 github(带提交) - 2

    我在bitbucket上创建了一个私有(private)git存储库并提交了代码。现在我想导出所有(提交、代码、历史记录)并将其导入github上的gitrepo。有没有办法做到这一点？谢谢 最佳答案 在本地检查所有内容到您的计算机和gitpull。创建一个github存储库将此存储库添加为您的第二个远程(“使用gitremote添加githubURL”)推送到第二个Remote 关于ruby-git:从bitbucket导出并导入github(带提交)，我们在StackOverflow

随机推荐