【RTAB-Map+VINS-Fusion+euroc】(Ubuntu 20.04)三维稠密重建-实践笔记

RTAB-Map+VINS-Fusion+euroc(Ubuntu 20.04)三维稠密重建-实践笔记

环境：Ubuntu 20.04+ROS Noetic

文章目录

- [RTAB-Map+VINS-Fusion+euroc(Ubuntu 20.04)三维稠密重建-实践笔记](#RTAB-Map+VINS-Fusion+euroc(Ubuntu 20.04)三维稠密重建-实践笔记)
- - - 官方参考文档-DockerFile
  - [1. Vins-Fusion编译](#1. Vins-Fusion编译)
  - - 可能遇到的bug
  - [2. RTAB-Map重新编译-支持vins-fusion](#2. RTAB-Map重新编译-支持vins-fusion)
  - - [2.0 环境配置](#2.0 环境配置)
    - [2.1 rtabmap编译](#2.1 rtabmap编译)
    - 可能遇到的bug:
    - [2.2 rtabmap-ros安装编译](#2.2 rtabmap-ros安装编译)
  - [3. rtabmap+EuRoC实验](#3. rtabmap+EuRoC实验)

记录使用RTAB-Map+VINS-Fusion实现三维稠密重建的实践过程、遇到的种种问题及解决方案备忘。

RTABMap支持VINS-Fusion、ORB_SLAM等多种算法，做实验时主要参考了参考文末的参考1，但其中的RTAB-Map+ORB_SLAM2步骤很详细，RTAB-Map+VINS-Fusion作者只是简单带过，实验时有很多迷惑的地方，能找到的其他人的相关实验参考也很少。官方文档DockerFile也是一个重要的参考文件，描述了基本的实验步骤，解决了我的一些问题。

总体来说，分三步：

1.下载编译VINS-Fusion（需切换分支打补丁）；

2.下载RTABMap，在支持VINS-Fusion的模式下编译；

编译RTABMap-ros。

官方参考文档-DockerFile

bash 复制代码

# Image: introlab3it/rtabmap:jfr2018

# Build this image:
# $ cd rtabmap
# $ docker build -t introlab3it/rtabmap:jfr2018 -f docker/jfr2018/latest/Dockerfile .

FROM ubuntu:16.04

WORKDIR /root/

# Install build dependencies
RUN apt-get update && apt-get install -y \
      libsqlite3-dev \
      libpcl-dev \
      git \
      cmake \
      libproj-dev \
      libqt5svg5-dev \
      libfreenect-dev \
      libopenni2-dev \
      libyaml-cpp-dev \
      software-properties-common \
      libgtk2.0-dev \
      pkg-config

# OpenCV (use version <= 3.1.0 for rtabmap 0.16.3 for a solvePnpRansac issue where we have worst ransac performance on newer opencv versions)
RUN git clone https://github.com/opencv/opencv_contrib.git
RUN git clone https://github.com/opencv/opencv.git
RUN cd opencv_contrib && \
    git checkout tags/3.1.0 && \
    cd && \
    cd opencv && \
    git checkout tags/3.1.0 && \
    mkdir build && \
    cd build && \
    cmake -DOPENCV_EXTRA_MODULES_PATH=/root/opencv_contrib/modules -DCMAKE_BUILD_TYPE=Release -DBUILD_SHARED_LIBS=ON -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF .. && \
    make -j4 && \
    make install && \
    cd && \
    rm -rf opencv opencv_contrib

# Install ROS to build other odometry approaches (depending on ROS)
# install packages
RUN apt-get update && apt-get install -q -y \
    dirmngr \
    gnupg2 \
    lsb-release \
    && rm -rf /var/lib/apt/lists/*

# setup keys
RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

# setup sources.list
RUN echo "deb http://packages.ros.org/ros/ubuntu `lsb_release -sc` main" > /etc/apt/sources.list.d/ros-latest.list

# install bootstrap tools
RUN apt-get update && apt-get install --no-install-recommends -y --allow-unauthenticated \
    python-rosdep \
    python-rosinstall \
    python-vcstools \
    && rm -rf /var/lib/apt/lists/*

# setup environment
ENV LANG C.UTF-8
ENV LC_ALL C.UTF-8

# bootstrap rosdep
RUN rosdep init \
    && rosdep update

# install ros packages
ENV ROS_DISTRO kinetic
RUN apt-get update && apt-get install -y \
    ros-${ROS_DISTRO}-ros-core=1.3.2-0* \
    && rm -rf /var/lib/apt/lists/*

# install ROS dependencies
RUN apt-get update && apt-get install -y ros-${ROS_DISTRO}-tf ros-${ROS_DISTRO}-pcl-ros ros-${ROS_DISTRO}-eigen-conversions ros-${ROS_DISTRO}-tf-conversions ros-${ROS_DISTRO}-random-numbers ros-${ROS_DISTRO}-image-transport libblas-dev liblapack-dev libsuitesparse-dev libvtk6.2 libyaml-cpp-dev wget libgoogle-glog-dev libatlas-base-dev ros-${ROS_DISTRO}-sophus ros-${ROS_DISTRO}-octomap libglew-dev

# g2o should be built to link on csparse installed system-wide
RUN git clone https://github.com/RainerKuemmerle/g2o.git
RUN cd g2o && \
    git checkout 20170730_git && \
    mkdir build && \
    cd build && \
    cmake -DBUILD_LGPL_SHARED_LIBS=ON -DG2O_BUILD_APPS=OFF -DBUILD_WITH_MARCH_NATIVE=OFF -DG2O_BUILD_EXAMPLES=OFF -DBUILD_SHARED_LIBS=ON -DCMAKE_BUILD_TYPE=Release .. && \
    make -j3 && \
    make install && \
    cd && \
    rm -r g2o

# GTSAM
RUN git clone https://bitbucket.org/gtborg/gtsam.git
RUN cd gtsam && \
    git checkout 4.0.0-alpha2 && \
    mkdir build && \
    cd build && \
    cmake -DMETIS_SHARED=ON -DGTSAM_BUILD_STATIC_LIBRARY=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON -DGTSAM_BUILD_TESTS=OFF -DGTSAM_BUILD_EXAMPLES_ALWAYS=OFF -DCMAKE_BUILD_TYPE=Release .. && \
    make -j3 && \
    make install && \
    cd && \
    rm -r gtsam

# libpointmatcher 
RUN git clone https://github.com/ethz-asl/libnabo.git
#commit Apr 25 2018
RUN cd libnabo && \
    git checkout 7e378f6765393462357b8b74d8dc8c5554542ae6 && \
    mkdir build && \
    cd build && \
    cmake -DCMAKE_BUILD_TYPE=Release .. && \
    make -j3 && \
    make install && \
    cd && \
    rm -r libnabo
RUN git clone https://github.com/ethz-asl/libpointmatcher.git
#commit Jan 19 2018
RUN cd libpointmatcher && \
    git checkout 00004bd41e44a1cf8de24ad87e4914760717cbcc && \
    mkdir build && \
    cd build && \
    cmake -DCMAKE_BUILD_TYPE=Release .. && \
    make -j3 && \
    make install && \
    cd && \
    rm -r libpointmatcher

# Clone dependencies
RUN mkdir -p catkin_ws/src
WORKDIR /root/catkin_ws/src
RUN /bin/bash -c '. /opt/ros/${ROS_DISTRO}/setup.bash; catkin_init_workspace'
# OPENCV ROS
RUN git clone https://github.com/ros-perception/vision_opencv.git && cd vision_opencv && git checkout kinetic
# make sure cv_bridge is rebuilt before other packages
RUN /bin/bash -c 'cd /root/catkin_ws;. /opt/ros/${ROS_DISTRO}/setup.bash; catkin_make -DCMAKE_BUILD_TYPE=Release; rm -rf build;'
# DVO
RUN git clone https://github.com/tum-vision/dvo_slam.git && cd dvo_slam && git checkout ${ROS_DISTRO}-devel && rm dvo_slam/package.xml && rm dvo_benchmark/package.xml && rm dvo_ros/package.xml
# VISO2
RUN git clone https://github.com/srv/viso2.git && cd viso2 && git checkout ${ROS_DISTRO}
# MSCKF-VIO: using a patched version to be used outside ros and c++14:
RUN git clone https://github.com/borongyuan/msckf_vio.git && cd msckf_vio && git checkout 916c917e5b481741c60732057f0141e8311962c1
# FOVIS
RUN git clone https://github.com/srv/libfovis.git && cd libfovis && git checkout 896acc8425e9fd7c5609153b8bad349ae1abbb50
# LOAM
RUN git clone https://github.com/laboshinl/loam_velodyne.git && cd loam_velodyne && git checkout a4c364a677647f2a35831439032dc5a58378b3fd
# FLOAM
RUN git clone https://github.com/wh200720041/floam.git && cd floam && git checkout 9fdb2ca60a2988c0db515c9c1567100fba8b58d8 && wget https://gist.githubusercontent.com/matlabbe/d8ab56e69146c18afbd1a3b05444c649/raw/f1f37ece5d5787c77f008cb89e9446fc20a40c1f/floam_as_library_support.patch && git apply --ignore-space-change --ignore-whitespace floam_as_library_support.patch
# VINS-Fusion
RUN git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git && cd VINS-Fusion && git checkout e72b5f7ae7437e8cf481cc0bcbfb5fe91bd3d640 && wget https://gist.githubusercontent.com/matlabbe/795ab37067367dca58bbadd8201d986c/raw/0657cc30fca6b1b61faeef42be9f3428861d2598/vins-fusion_e72b5f7.patch && git apply --ignore-space-change --ignore-whitespace vins-fusion_e72b5f7.patch
# OpenVINS (currently failing because not right ceres version is installed)
# RUN git clone https://github.com/rpng/open_vins.git && cd open_vins && git checkout 83ffb88ad35586d86bddc1e041094f4cd3d400df

WORKDIR /root
# OKVIS
RUN git clone https://github.com/ethz-asl/okvis.git && cd okvis && git checkout 1dce9129f22dd4d21d944788cd9da3a4341586aa && wget https://gist.githubusercontent.com/matlabbe/383be55b5cb682fea217d45ef9a37ef8/raw/bd4f6886eeba3800f0e5171e77abc5dd6dc8bfd9/okvis_1dce912_marchnative_disabled.patch && git apply --ignore-space-change --ignore-whitespace okvis_1dce912_marchnative_disabled.patch
# ORB_SLAM2
RUN git clone https://github.com/stevenlovegrove/Pangolin.git && cd Pangolin && git checkout v0.6
RUN git clone https://github.com/raulmur/ORB_SLAM2.git && cd ORB_SLAM2 && wget https://gist.githubusercontent.com/matlabbe/c10403c5d44af85cc3585c0e1c601a60/raw/48adf04098960d86ddf225f1a8c68af87bfcf56e/orbslam2_f2e6f51_marchnative_disabled.patch && git apply --ignore-space-change --ignore-whitespace orbslam2_f2e6f51_marchnative_disabled.patch

# for dvo, TBB is required (docker related issue)
# Download and install TBB
ENV TBB_RELEASE 2018_U2
ENV TBB_VERSION 2018_20171205
ENV TBB_DOWNLOAD_URL https://github.com/01org/tbb/releases/download/${TBB_RELEASE}/tbb${TBB_VERSION}oss_lin.tgz
ENV TBB_INSTALL_DIR /opt
RUN wget ${TBB_DOWNLOAD_URL} && \
	tar -C ${TBB_INSTALL_DIR} -xf tbb${TBB_VERSION}oss_lin.tgz && \
	rm tbb${TBB_VERSION}oss_lin.tgz && \
	sed -i "s%SUBSTITUTE_INSTALL_DIR_HERE%${TBB_INSTALL_DIR}/tbb${TBB_VERSION}oss%" ${TBB_INSTALL_DIR}/tbb${TBB_VERSION}oss/bin/tbbvars.* && \
        echo "source ${TBB_INSTALL_DIR}/tbb${TBB_VERSION}oss/bin/tbbvars.sh intel64" >> ~/.bashrc

RUN cd okvis && \
     mkdir build && \
     cd build && \
     cmake .. && \
     make -j3 && \
     make install && \
     mkdir -p /usr/local/lib/cmake && \
     mv /usr/local/lib/CMake/* /usr/local/lib/cmake/.

# Pangolin needed for ORB_SLAM2
RUN cd Pangolin && \
     mkdir build && \
     cd build && \
     cmake .. && \
     make -j3 && \
     make install && \
     cd && \
     rm -rf Pangolin

RUN cd ORB_SLAM2 && \
     cd Thirdparty/DBoW2 && \
     mkdir build && \
     cd build && \
     cmake .. -DCMAKE_BUILD_TYPE=Release && \
     make -j3 && \
     rm -rf * && \
     cd ../../g2o && \
     mkdir build && \
     cd build && \
     cmake .. -DCMAKE_BUILD_TYPE=Release && \
     make -j3 && \
     rm -rf * && \
     cd ../../../ && \
     cd Vocabulary && \
     tar -xf ORBvoc.txt.tar.gz && \
     cd .. && \
     mkdir build && \
     cd build && \
     cmake .. -DCMAKE_BUILD_TYPE=Release && \
     make -j3 && \
     rm -rf *

RUN mv okvis/config/config_fpga_p2_euroc.yaml .
RUN mv ORB_SLAM2/Vocabulary/ORBvoc.txt .
RUN cp catkin_ws/src/VINS-Fusion/config/euroc/* .
RUN cp catkin_ws/src/VINS-Fusion/config/kitti_odom/* .

# Catkin_make
WORKDIR /root/catkin_ws
RUN /bin/bash -c '. /opt/ros/${ROS_DISTRO}/setup.bash;. ${TBB_INSTALL_DIR}/tbb${TBB_VERSION}oss/bin/tbbvars.sh intel64; catkin_make -j3 -DCMAKE_BUILD_TYPE=Release; rm -rf build;'
RUN echo "source /root/catkin_ws/devel/setup.bash" >> ~/.bashrc

ENV LD_LIBRARY_PATH /opt/tbb2018_20171205oss/lib/intel64/gcc4.7:/root/ORB_SLAM2/lib:/root/ORB_SLAM2/Thirdparty/g2o/lib:/root/ORB_SLAM2/Thirdparty/DBoW2/lib:/root/catkin_ws/devel/lib:/opt/ros/${ROS_DISTRO}/lib:$LD_LIBRARY_PATH
ENV PATH /root/catkin_ws/devel/bin:/opt/ros/${ROS_DISTRO}/bin:$PATH
ENV ORB_SLAM2_ROOT_DIR /root/ORB_SLAM2

WORKDIR /root

# To make "source" working
RUN rm /bin/sh && ln -s /bin/bash /bin/sh

# Need cmake >=3.10
RUN apt update && apt install -y wget && apt-get clean && rm -rf /var/lib/apt/lists/ && \
    wget -nv https://github.com/Kitware/CMake/releases/download/v3.17.0/cmake-3.17.0-Linux-x86_64.tar.gz && \
    tar -xzf cmake-3.17.0-Linux-x86_64.tar.gz && \
    rm cmake-3.17.0-Linux-x86_64.tar.gz &&\
    ln -s ~/cmake-3.17.0-Linux-x86_64/bin/cmake ~/cmake

# Copy current source code
COPY . /root/rtabmap

# Patch for OpenCV 3.1
RUN cd rtabmap && git apply --ignore-space-change --ignore-whitespace docker/jfr2018/rtabmap_opencv310_backward_compatibility.patch

# Build RTAB-Map (using standard g2o, then a version for orbslam2, which uses its own g2o version)
RUN cp -r rtabmap rtabmap_os2 && cp -r rtabmap rtabmap_msckf && cp -r rtabmap rtabmap_loam

RUN source /root/catkin_ws/devel/setup.bash && \
    cd rtabmap && \
    cd build && \
    ~/cmake -DWITH_FOVIS=ON -DWITH_VISO2=ON -DWITH_DVO=ON -DWITH_OKVIS=ON -DWITH_VINS=ON -DWITH_OPENVINS=ON -DWITH_OPENGV=OFF .. && \
    make -j3 && \
    make install && \
    rm -rf * && \
    ldconfig
# Version with orb_slam2 (system g2o disabled)
RUN source /root/catkin_ws/devel/setup.bash && \
    cd rtabmap_os2 && \
    cd build && \
    ~/cmake -DWITH_G2O=OFF -DWITH_ORB_SLAM=ON -DWITH_OPENGV=OFF .. && \
    make -j3 && \
    rm -rf *

# rtabmap is crashing if LOAM and MSCKF dependencies are used at the same time, so split them
RUN source /root/catkin_ws/devel/setup.bash && \
    cd rtabmap_loam && \
    cd build && \
    ~/cmake  -DWITH_LOAM=ON -DWITH_FLOAM=ON -DWITH_OPENGV=OFF -DWITH_CERES=ON .. && \
    make -j3 && \
    rm -rf *
RUN source /root/catkin_ws/devel/setup.bash && \
    cd rtabmap_msckf && \
    git apply --ignore-space-change --ignore-whitespace docker/jfr2018/rtabmap_msckf_ubuntu_16_compatibility.patch && \
    cd build && \
    ~/cmake -DWITH_MSCKF_VIO=ON -DWITH_OPENGV=OFF .. && \
    make -j3 && \
    rm -rf *

WORKDIR /root

1. Vins-Fusion编译

VINS-Fusion官网

进入自己的工作空间src下，安装编译VINS-Fusion：

（0）环境配置，省略，可以找一下vins-fusion编译安装相关博客，本文的重点是rtabmap支持vins的安装配置。

（1）git源码并切换分支。若对代码进行了修改，切换分支会报错，需要提交修改或暂存修改。

bash 复制代码

git clone https://github.com/HKUST-Aerial-Robotics/VINS-Fusion.git 
cd VINS-Fusion 
git checkout e72b5f7ae7437e8cf481cc0bcbfb5fe91bd3d640

（2）下载补丁，将vins-fusion_e72b5f7.patch放在VINS-Fusion主目录下，打补丁

bash 复制代码

git apply --ignore-space-change --ignore-whitespace vins-fusion_e72b5f7.patch

（3）编译

bash 复制代码

catkin_make
source ~/catkin_ws/devel/setup.bash

编译完成后，运行以下命令，结果如下图，应该就没有问题了：

bash 复制代码

roslaunch vins vins_rviz.launch
rosrun vins vins_node  ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml.yaml
rosbag play MH_01_easy.bag

可能遇到的bug

ROS Noetic自带opencv4.2.0版本，vins-fusion需要opencv3，有两种方法：

（1）使用自带的opencv4编译vins-fusion，修改代码以兼容opencv4

参考下面文章的1.2部分：
Ubuntu20.04下运行VINS系列：VINS-Mono、VINS-Fusion和GVINS

（2）使用opencv3编译vins-fusion。若你安装了opencv3，使用opencv3编译vins-fusion仍然可能遇到一些报错，这是由cv_bridge引起的。经过我的反复编译，发现修改最少的情况下编译成功的方法是，使用非ROS Noetic自带opencv版本编译工程时，修改配置文件：

bash 复制代码

cd /opt/ros/noetic/share/cv_bridge/cmake
sudo gedit cv_bridgeConfig.cmake

将这部分的opencv4改为opencv，并修改set(libraries "cv_bridge;...")为自己的opencv库文件，我的opencv为3.4.5，就可以直接编译成功。（我第一次将opencv4改为opencv2，编译时仍然会报很多其他错误，需要再修改vins-fusion的头文件和代码中的一些内容）

90行左右：

bash 复制代码

#if(NOT "include;/usr/include/opencv4 " STREQUAL " ")
if(NOT "include;/usr/include/opencv " STREQUAL " ")
  set(cv_bridge_INCLUDE_DIRS "")
  #set(_include_dirs "include;/usr/include/opencv")
  set(_include_dirs "include;/usr/include/opencv4")

120行左右

bash 复制代码

#set(libraries "cv_bridge;/usr/lib/aarch64-linux-gnu/libopencv_calib3d.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_dnn.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_features2d.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_flann.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_highgui.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_ml.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_objdetect.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_photo.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_stitching.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_video.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_videoio.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_aruco.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_bgsegm.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_bioinspired.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_ccalib.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_datasets.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_dnn_objdetect.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_dnn_superres.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_dpm.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_face.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_freetype.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_fuzzy.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_hdf.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_hfs.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_img_hash.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_line_descriptor.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_optflow.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_phase_unwrapping.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_plot.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_quality.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_reg.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_rgbd.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_saliency.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_shape.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_stereo.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_structured_light.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_superres.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_surface_matching.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_text.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_tracking.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_videostab.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_viz.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_ximgproc.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_xobjdetect.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_xphoto.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_core.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_imgproc.so.4.2.0;/usr/lib/aarch64-linux-gnu/libopencv_imgcodecs.so.4.2.0")

set(libraries "cv_bridge;/usr/local/lib/libopencv_calib3d.so;/usr/local/lib/libopencv_features2d.so.3.4.5;/usr/local/lib/libopencv_imgproc.so.3.4;/usr/local/lib/libopencv_shape.so;/usr/local/lib/libopencv_videoio.so.3.4.5;/usr/local/lib/libopencv_calib3d.so.3.4;/usr/local/lib/libopencv_flann.so;/usr/local/lib/libopencv_imgproc.so.3.4.5;/usr/local/lib/libopencv_shape.so.3.4;/usr/local/lib/libopencv_video.so;/usr/local/lib/libopencv_calib3d.so.3.4.5;/usr/local/lib/libopencv_flann.so.3.4;/usr/local/lib/libopencv_ml.so;/usr/local/lib/libopencv_shape.so.3.4.5;/usr/local/lib/libopencv_video.so.3.4;/usr/local/lib/libopencv_core.so;/usr/local/lib/libopencv_flann.so.3.4.5;/usr/local/lib/libopencv_ml.so.3.4;/usr/local/lib/libopencv_stitching.so;/usr/local/lib/libopencv_video.so.3.4.5;/usr/local/lib/libopencv_core.so.3.4;/usr/local/lib/libopencv_highgui.so;/usr/local/lib/libopencv_ml.so.3.4.5;/usr/local/lib/libopencv_stitching.so.3.4;/usr/local/lib/libopencv_videostab.so;/usr/local/lib/libopencv_core.so.3.4.5;/usr/local/lib/libopencv_highgui.so.3.4;/usr/local/lib/libopencv_objdetect.so;/usr/local/lib/libopencv_stitching.so.3.4.5;/usr/local/lib/libopencv_videostab.so.3.4;/usr/local/lib/libopencv_dnn.so;/usr/local/lib/libopencv_highgui.so.3.4.5;/usr/local/lib/libopencv_objdetect.so.3.4;/usr/local/lib/libopencv_superres.so;/usr/local/lib/libopencv_videostab.so.3.4.5;/usr/local/lib/libopencv_dnn.so.3.4;/usr/local/lib/libopencv_imgcodecs.so;/usr/local/lib/libopencv_objdetect.so.3.4.5;/usr/local/lib/libopencv_superres.so.3.4;/usr/local/lib/libopencv_viz.so;/usr/local/lib/libopencv_dnn.so;/usr/local/lib/libopencv_dnn.so.3.4.5;/usr/local/lib/libopencv_imgcodecs.so.3.4;/usr/local/lib/libopencv_photo.so;/usr/local/lib/libopencv_superres.so.3.4.5;/usr/local/lib/libopencv_viz.so.3.4;/usr/local/lib/libopencv_features2d.so;/usr/local/lib/libopencv_imgcodecs.so.3.4.5;/usr/local/lib/libopencv_photo.so.3.4;/usr/local/lib/libopencv_videoio.so;/usr/local/lib/libopencv_viz.so.3.4.5")

修改cv_bridgeConfig.cmake配置文件后，Ubuntu20.04+ROS Noetic+opencv3下的vins-fusion工程直接编译成功。

此处解决问题参考了这篇文章，非常有用：
[替你踩遍坑]Ubuntu20.04+ROS Noetic+OPENCV3成功运行vins-fusion

2. RTAB-Map重新编译-支持vins-fusion

rtabmap安装官方参考：
rtabmap-github
rtabmap home
rtabmap installation

2.0 环境配置

（1）通过安装和卸载rtabmap，rtabmap-ros二进制文件，安装相关依赖

bash 复制代码

sudo apt-get install ros-noetic-rtabmap ros-noetic-rtabmap-ros
sudo apt-get remove ros-noetic-rtabmap ros-noetic-rtabmap-ros

下面两个包不安装好像也能成功

（2）（可选）g2o安装，一定要切换分支git checkout 9b41a4ea5a，否则后面会报关于 undefined reference to `g2o::的错。

bash 复制代码

git clone https://github.com/RainerKuemmerle/g2o/
cd g2o
git checkout 9b41a4ea5a
mkdir build
cd build
cmake -DBUILD_WITH_MARCH_NATIVE=OFF -DG2O_BUILD_APPS=OFF -DG2O_BUILD_EXAMPLES=OFF -DG2O_USE_OPENGL=OFF ..
make -j4
sudo make install

（3）（可选）安装GTSAM

bash 复制代码

git clone https://bitbucket.org/gtborg/gtsam.git
cd gtsam
mkdir build
cd build
cmake -DGTSAM_BUILD_WITH_MARCH_NATIVE=OFF -DGTSAM_USE_SYSTEM_EIGEN=ON ..
make -j4
sudo make install

2.1 rtabmap编译

注意：此处rtabmap和rtabmap_ros不要放在一个文件下，否则会导致出错。可以将rtabmap放在自己的一个目录中，rtabmap_ros放在工作空间下

bash 复制代码

git clone https://github.com/introlab/rtabmap.git rtabmap_vins
cd rtabmap/build
cmake -DCMAKE_INSTALL_PREFIX=~/catkin_ws/devel -DWITH_VINS=ON  ..
make -j4
sudo make install

命令如上：

（1）git rtabmap，命名为rtabmap_vins，进入到rtabmap/build文件夹中。

（2）执行命令，

bash 复制代码

cmake -DCMAKE_INSTALL_PREFIX=~/catkin_ws/devel -DWITH_VINS=ON  ..

其中，-DWITH_VINS=ON代表使得rtabmap支持vins-fusion。DCMAKE_INSTALL_PREFIX是指定的rtabmap安装位置，此处可以修改为自己已有的工作空间目录或和或新建一个rtabmap专用的目录（虽然有rtabmap和rtabmap_ros不能放到同一个文件夹下的说法，但亲测和rtabmap_ros安装在同一个工作空间不会导致错误）。

此时查看控制台输出，rtabmap能检测到vins：1. Found_vins 后有第一步编译好的vins-fusion工程路径（vins编译成功才能检测到）。2.With_VINS-Fusion=YES

(3) 编译：

bash 复制代码

make -j4
sudo make install

编译安装成功则进行下一步。

可能遇到的bug:

（a）Found_vins为空
编译报错 ：OdometryVINS.cpp:39:10: fatal error: estimator/estimator.h: 没有那个文件或目录。
解决方案：在rtamap的CMakeList中，大概790行，通过set(vins_DIR)给定自己编译好的VINS_Fusion绝对路径：

bash 复制代码

#set(vins_DIR mydir)
set(vins_DIR ~/catkin_ws)

（b）编译报错 ：/usr/include/flann/util/params.h:44:1: error: 'flann_algorithm_t' was not declared in this scope; did you mean 'cvflann::flann_algorithm_t'

这是由于PCL和opencv中的flann库冲突，参考文末的参考2和3。

解决方案 ：进入自己的opencv路径，将opencv下的flann文件夹重命名为flann2，对应的，打开features2d.hpp，flann.hpp，miniflann.hpp三个文件，将头文件中的include"xx/flann"修改为include"xx/flann2"。其中miniflann.hpp位于flann2文件夹下

bash 复制代码

cd /usr/local/include/opencv2     
sudo mv flann/ flann2/
sudo gedit features2d.hpp
sudo gedit flann.hpp
cd flann2
sudo gedit miniflann.hpp

示例：

（c）rtabmap_vins/guilib/src/CalibrationDialog.cpp:1899:46: error: cannot bind non-const lvalue reference of type 'cv::String&' to an rvalue of type 'cv::String'
解决方案 ：根据报错，非常量的左值不能和右值绑定，错误位于CalibrationDialog.cpp中，打开该文件修改报错的地方。这个地方查阅了很多资料，花费了很多时间，尝试过将左值定义为const类型、增加一个中间变量等方法都没有解决，因为相关部分的代码是和日志记录有关的，最后将相关部分注释掉，暂时解决问题，编译不再报错。待修正... 已修正。

更正：在这个问题上纠结痛苦了一周，一直以为是源代码的问题，修改代码、修改编译器版本、修改编译时的c++标准各种方法尝试了个遍，其实还是环境问题！！！最后在CMakeLists.txt中，设置opencv版本为4.2.0，不再报错（原始默认找到的为3.4.5）。

bash 复制代码

FIND_PACKAGE(OpenCV 4.2.0 ......)

2.2 rtabmap-ros安装编译

先cd切换到工作空间，此处可以修改为自己已有的工作空间目录或建立好的rtabmap专用的目录。

bash 复制代码

cd ~/catkin_ws
git clone https://github.com/introlab/rtabmap_ros.git src/rtabmap_ros
catkin_make

遇到的问题 ：

undefined reference to `g2o::OptimizableGraph::Edge::clone() const'

查了一些资料，说是因为ros安装了g2o，默认调用了ros里面的g2o的原因，给出的解决方案是进入g2o库的cmake_modules文件夹下，找到FINDG2O.cmake修改，不过对我没有用，参考了参考链接4 rtabmap的问答解决。

解决方案 ：重新编译g2o，g2o编译时切换分支git checkout 9b41a4ea5a

bash 复制代码

git clone https://github.com/RainerKuemmerle/g2o/
cd g2o
git checkout 9b41a4ea5a
mkdir build
cd build
cmake ..
make
sudo make install

rtabmap-ros catkin成功便进行下一步。

3. rtabmap+EuRoC实验

（1）测试rtabmap安装

分别打开两个终端执行命令：

bash 复制代码

roslaunch rtabmap_examples euroc_datasets.launch MH_seq:=true
rosbag play --clock MH_01_easy.bag

可能遇到的bug

报错[rtabmap/stereo_odometry-11] process has died，且rtabmap_viz无图像显示。

多版本opencv冲突问题，记不清怎么解决的了，好像是在编译时为rtabmap指定了opencv版本，noetic自带的4.2.0就可以。

在rtabmap的CMakeLists.txt中：

bash 复制代码

FIND_PACKAGE(OpenCV 4.2.0 REQUIRED QUIET COMPONENTS core calib3d imgproc highgui stitching photo video videoio OPTIONAL_COMPONENTS aruco objdetect xfeatures2d nonfree gpu cudafeatures2d cudaoptflow cudaimgproc)

（2）测试rtabmap+vins

使用euroc_stereo_config.yaml和euroc_stereo_imu_config.yaml进行VO和VIO测试。

分别打开两个终端执行命令：

bash 复制代码

roslaunch rtabmap_examples euroc_datasets.launch args:="Odom/Strategy 9 OdomVINS/ConfigPath ~/catkin_ws/src/VINS-Fusion/config/euroc/euroc_stereo_imu_config.yaml" MH_seq:=true raw_images_for_odom:=true
rosbag play --clock MH_01_easy.bag

问题：刚开始几秒有点云，后面会漂移，输出一直是Odom: quality=0, std dev=0.010000m|0.010000rad，最后得不到正常点云结果，rtabmap_rviz最终输出：

作者说是图像和IMU数据时间戳不一致时，新版本的rtabmap_ros会直接丢弃图像导致这个问题，切换到以前的版本就可以。

解决方案：

bash 复制代码

cd ~/catkin_ws/src/rtabmap_ros
git checkout 3eb0b47a55bd56ea6282e5fff157880dcdca753a
cd ..
catkin_make

bash 复制代码

rosbag play --clock -s 24 MH_01_easy.bag

最终结果，做出来纯双目看着反而比双目+IMU结果好一点，以下都是纯双目的结果。

结果保存在rtabmap.db中，可以使用rtabmap-databaseViewer打开查看。

bash 复制代码

rtabmap-databaseViewer ~/.ros/rtabmap.db

从rtamap.db导出点云

使用rtabmap-databaseViewer工具，Edit-View 3D Map查看点云；File-Export 3D Map导出.ply格式点云。
为期三周，三维稠密重建的实验终于得到结果了！！！中间rtabmap+vins的最后一个bug痛苦地卡了一周多，泪目。

参考：