ORB-SLAM3复现(ubuntu18)

1 坐标系与外参Intrinsic parameters

世界坐标系：Z轴指向重力矢量的相反方向，在纯视觉情况下，世界系设置为第一个相 机的姿态。

相机坐标系：采用常见的右下前（xyz）顺序。这里$C_1$即左目cam0，$C_2$即右目cam1。

IMU坐标系：也即Body坐标系

• 相机与body系转换

$$\begin{array}{rl}& {\mathbf{T}}_{{\mathsf{WC}}_1}={\mathbf{T}}_{\mathsf{WB}}{\mathbf{T}}_{{\mathsf{BC}}_1}\\ & {\mathbf{T}}_{{\mathsf{WC}}_2}={\mathbf{T}}_{\mathsf{WB}}{\mathbf{T}}_{{\mathsf{BC}}_1}{\mathbf{T}}_{{\mathsf{C}}_1{\mathsf{C}}_2}\end{array}$$

• 校准文件需要提供的外部参数有$T_{BC1}$和 $T_{C_1{C}_2}$，以双目EuRoC.yaml为例

Stereo.T_c1_c2: !!opencv-matrix
  rows: 4
  cols: 4
  dt: f
  data: [0.999997256477797,-0.002317135723275,-0.000343393120620,0.110074137800478,
         0.002312067192432,0.999898048507103,-0.014090668452683,-0.000156612054392,
         0.000376008102320,0.014089835846691,0.999900662638081,0.000889382785432,
         0,0,0,1.000000000000000]

# Transformation from camera 0 to body-frame (imu)
# 注意，如果是仅双目情况，只估计了左目位姿
IMU.T_b_c1: !!opencv-matrix
  rows: 4
  cols: 4
  dt: f
  data: [0.0148655429818, -0.999880929698, 0.00414029679422, -0.0216401454975,
         0.999557249008, 0.0149672133247, 0.025715529948, -0.064676986768,
         -0.0257744366974, 0.00375618835797, 0.999660727178, 0.00981073058949,
         0.0, 0.0, 0.0, 1.0]

2 内参Intrinsic parameters

​ 注意，有的双目相机提供的是相机基线b，有的直接提供的$T_{C_1{C}_2}$。

​ 这是因为当相机提供的是矫正过的立体图像时，意味着相机制造商或数据集已经对相机之间的畸变进行了矫正。在这种情况下，畸变已经被移除，相机图像上的点的位置更加准确。因此，在标定文件中，只需要提供基线 b，而不需要提供完整的$T_{C_1{C}_2}$。

2.1 相机内参

① 针孔模型Pinhole

​ 最常见的针孔模型，一般提供 the camera focal length and central point in pixe。以EuRoC.yaml为例

Camera.type: "PinHole"

# Camera calibration and distortion parameters (OpenCV) 
Camera1.fx: 458.654
Camera1.fy: 457.296
Camera1.cx: 367.215
Camera1.cy: 248.375

Camera1.k1: -0.28340811
Camera1.k2: 0.07395907
Camera1.p1: 0.00019359
Camera1.p2: 1.76187114e-05

​ ORB系统会使用OpenCV的stereorectify函数对左右图像进行内部矫正

② KannalaBrandt8模型

​ 一种用于描述广角和鱼眼相机镜头畸变的数学模型，Kannala-Brandt模型的8参数版本通常称为KannalaBrandt8，其中8个参数用于建模相机的畸变。对于KannalaBrandt8相机，由于其本身广角特性，不会进行内部矫正以避免失去分辨率和视场

​ 以RealSense_T265.yaml为例

Camera.type: "KannalaBrandt8"

# Left Camera calibration and distortion parameters (OpenCV)
Camera1.fx: 284.9501953125
Camera1.fy: 285.115295410156
Camera1.cx: 420.500213623047
Camera1.cy: 400.738098144531

# Kannala-Brandt distortion parameters
Camera1.k1: -0.00530046410858631
Camera1.k2: 0.0423333682119846
Camera1.k3: -0.03949885815382
Camera1.k4: 0.00682387687265873

③ Rectified相机

​ 这个模型适用于哪些已经去除畸变的数据，所以只需要$(f_x,f_y,c_x,c_y)$和相机基线b。以RealSense_D435i.yaml为例

Camera.type: "Rectified"

# Rectified Camera calibration (OpenCV)
Camera1.fx: 382.613
Camera1.fy: 382.613
Camera1.cx: 320.183
Camera1.cy: 236.455

Stereo.b: 0.0499585

​

2.2 IMU内参

​ 测量值 = 理想值 + 高斯噪声 + 偏差。注意，IMU测量的角速度和加速度都是在IMU坐标系下。
$$\begin{array}{rl}\tilde{\mathbf{a}}=& \mathbf{a}+{\eta }^a+{\mathbf{b}}^a\\ \tilde{\mathbf{\omega }}=& \omega +{\eta }^g+{\mathbf{b}}^g\end{array}$$

噪声

$$\begin{array}{rl}& {\eta }^a\sim \mathcal{N}(0,{\sigma }_a^2{\mathbf{I}}_3)\\ & {\eta }^g\sim \mathcal{N}(0,{\sigma }_g^2{\mathbf{I}}_3)\end{array}$$

​ ${\sigma }_a$ 和 ${\sigma }_g$ 是noise densities，IMU的数据手册中有详细说明，并且需要在标定文件中提供。

对于偏差

​ 假设它们遵循布朗运动。给定两个连续时刻 i和 i + 1，其特征如下
$$\begin{array}{rlrl}{\mathbf{b}}_{i+1}^a& ={\mathbf{b}}_i^a+{\mathbf{\eta }}_{\mathrm{rw}}^a& & \text{with }{\mathbf{\eta }}_{\mathrm{rw}}^a\sim \mathcal{N}(0,{\sigma }_{a,rw}^2{\mathbf{I}}_3)\\ {\mathbf{b}}_{i+1}^g& ={\mathbf{b}}_i^g+{\mathbf{\eta }}_{\mathrm{rw}}^g& & \text{with }{\mathbf{\eta }}_{\mathrm{rw}}^g\sim \mathcal{N}(0,{\sigma }_{g,rw}^2{\mathbf{I}}_3)\end{array}$$
​ 通常，将IMU制造商提供的随机游走标准差增加（比如乘以10）是一种常见做法，以考虑未建模的效应并提高IMU初始化的收敛性。

​ 以EuRoC.yaml为例（这里提供的是连续/离散，没有说，后面再看）

# IMU noise
IMU.NoiseGyro: 1.7e-04 # 1.6968e-04		σ_g
IMU.NoiseAcc: 2.0e-03 # 2.0000e-3		σ_a
IMU.GyroWalk: 1.9393e-05
IMU.AccWalk: 3.e-03 # 3.0000e-3
IMU.Frequency: 200.0

​ 以TUM-VI.yaml为例（连续时间下的方差单位）

# IMU noise (Use those from VINS-mono)
IMU.NoiseGyro: 0.00016 # 0.004 (VINS) # 0.00016 (TUM) # 0.00016    # rad/s^0.5 
IMU.NoiseAcc: 0.0028 # 0.04 (VINS) # 0.0028 (TUM) # 0.0028     # m/s^1.5
IMU.GyroWalk: 0.000022 # 0.000022 (VINS and TUM) rad/s^1.5
IMU.AccWalk: 0.00086 # 0.0004 (VINS) # 0.00086 # 0.00086    # m/s^2.5
IMU.Frequency: 200.0

​ 关于单位可参考高博新书P54页。

3 VI标定—外参

ORB给的文档Calibration_Tutorial,使用工具 Kalibr，后续有时间单独把这块上传

3.1 Visual calibration

视觉内参

3.2 Inertial calibration

cam-imu外参标定

4 编译

因为ORB-SLAM3有好几个版本，这里下载了v1.0

4.1 build.sh和build_ros.sh

# 依次编译各种库
echo "Building ROS nodes"

cd Examples/ROS/ORB_SLAM3
mkdir build
cd build
cmake .. -DROS_BUILD_TYPE=Release
make -j

​ 因为常见会报错的库已经被包含在包里，所以c++编译基本没报错，这里不再记录。

ps：在ubuntu20中安装第三方库sophus报错，sophus

/usr/include/eigen3/Eigen/src/Core/IO.h:122:39: error: ‘digits10’ is not a member of ‘Eigen::NumTraits<ceres::Jet<double, 3> >’ 122 | return NumTraits<Scalar>::digits10(); | ~~~~~~~~~~~~~~~~~~~~~~~~~~~^~

​ 问题的本质是sophus库版本和eigen不适配，首先尝试更换sophus库版本。改了之后会出现其它问题，因为原来程序中会引用到这里的文件，所以还需要改各种头文件，很是麻烦

4.2 ROS编译报错

4.2.1 报错1

​ 还是需要把功能包加入到ros路径，参考ORB-SLAM2中报错

gedit ~/.bashrc
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:PATH/ORB_SLAM3/Examples/ROS

chmod +x build_ros.sh
./build_ros.sh

4.2.2 报错2

cmake对应的一些警告

CMake Deprecation Warning at /opt/ros/melodic/share/ros/core/rosbuild/rosbuild.cmake:7 (cmake_policy):
  The OLD behavior for policy CMP0011 will be removed from a future version
  of CMake.

  The cmake-policies(7) manual explains that the OLD behaviors of all
  policies are deprecated and that a policy should be set to OLD only under
  specific short-term circumstances.  Projects should be ported to the NEW
  behavior and not rely on setting a policy to OLD.
Call Stack (most recent call first):
  CMakeLists.txt:2 (include)

	.....
# 这里其实不用管，只是一些警告

4.2.3 报错3

​ 关于sophus相应redefinition的错误，这些错误可能是由 Sophus 库的头文件重复引入导致的。之前安装sophus时候安装到系统里面了，这里ORB3又自带了相应的文件，可是每个头文件里面已经加上了预编译指令或#pragma once

/usr/local/include/sophus/common.hpp:174:8: error: redefinition of ‘struct Sophus::Constants<float>’ struct Constants<float>

​ 随机打开头文件查看，对于sophus库的引用，一个指明了ORB-SLAM3第三方库下的文件，一个没有指明，估计是这里引发的问题。c++里面引用自定义头文件和标准头文件格式不一样，一个用“”，另一个用括号<>。OBR-SLAM3里面关于sophus的引用比较混乱，有时候用“”，有时候又<>.

// GeometricTools.h		

#include <opencv2/core/core.hpp>
#include <sophus/se3.hpp>

// ORBmatcher.h			对于自定义的引用，改成
#include"sophus/sim3.hpp"

"Thirdparty/Sophus/sophus/sim3.hpp"		// 没有什么好的办法，都换为第三方库下面的，估计是两个sophus版本不一样引起

​ 估计报错提到的文件都有这个问题，修改完之后重新编译下c++版本，再编译ROS版本的

4.2.4 报错4

/home/wheeltec-client/WPJ/ORB_SLAM3-master/Examples_old/ROS/ORB_SLAM3/src/AR/ViewerAR.cc: In member function ‘ORB_SLAM3::Plane* ORB_SLAM3::ViewerAR::DetectPlane(cv::Mat, const std::vector<ORB_SLAM3::MapPoint*>&, int)’: /home/wheeltec-client/WPJ/ORB_SLAM3-master/Examples_old/ROS/ORB_SLAM3/src/AR/ViewerAR.cc:405:53: error: no matching function for call to ‘std::vector<cv::Mat>::push_back(Eigen::Vector3f)’ vPoints.push_back(pMP->GetWorldPos()); ^

报错显示：在ViewerAR.cc文件中类型不匹配错误，把Mat类型和Eigen::Vector3f混淆

/home/wheeltec-client/WPJ/ORB_SLAM3-master/Examples_old/ROS/ORB_SLAM3/src/AR/ViewerAR.cc:530:42: error: conversion from ‘Eigen::Vector3f {aka Eigen::Matrix<float, 3, 1>}’ to non-scalar type ‘cv::Mat’ requested cv::Mat Xw = pMP->GetWorldPos(); ~~~~~~~~~~~~~~~~^~

/home/wheeltec-client/WPJ/ORB_SLAM3-master/Examples_old/ROS/ORB_SLAM3/src/AR/ros_mono_ar.cc:151:41: error: conversion from ‘Sophus::SE3f {aka Sophus::SE3<float>}’ to non-scalar type ‘cv::Mat’ requested cv::Mat Tcw = mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec()); ~~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

ros_mono_ar.cc文件

^[[ACMakeFiles/MonoAR.dir/build.make:198: recipe for target 'CMakeFiles/MonoAR.dir/src/AR/ViewerAR.cc.o' failed
make[2]: *** [CMakeFiles/MonoAR.dir/src/AR/ViewerAR.cc.o] Error 1
make[2]: *** 正在等待未完成的任务....
CMakeFiles/MonoAR.dir/build.make:118: recipe for target 'CMakeFiles/MonoAR.dir/src/AR/ros_mono_ar.cc.o' failed

// 解决办法 ViewerAR.cc       ros_mono_ar.cc   添加对应头文件
#include"../../../include/Converter.h"

// 在这两个文件相应的位置修改下面的代码

//in line 151 of ros_mono_ar.cc
cv::Mat Tcw=ORB_SLAM3::Converter::toCvMat(mpSLAM->TrackMonocular(cv_ptr->image,cv_ptr->header.stamp.toSec()).matrix());



//in line 405 of ViewerAR.cc
vPoints.push_back(ORB_SLAM3::Converter::toCvMat(pMP->GetWorldPos()));


//in line 530 of ViewerAR.cc
cv::Mat Xw = ORB_SLAM3::Converter::toCvMat(pMP->GetWorldPos());

参考链接

4.3 CMakeLists.txt

c++

find_package(OpenCV REQUIRED)		# 注意把对应版本换为你自己版本，或者让编译器自己去找
find_package(Eigen3 REQUIRED)
find_package(Pangolin REQUIRED)
find_package(realsense2)

target_link_libraries(${PROJECT_NAME}
${OpenCV_LIBS}
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/Thirdparty/g2o/lib/libg2o.so
-lboost_serialization		# 链接到Boost库中的serialization模块
-lcrypto		# 链接到OpenSSL库中 
)

ROS

cmake_minimum_required(VERSION 2.4.6)
include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)

rosbuild_init()

LIST(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/../../../cmake_modules)

find_package(OpenCV QUIET)
find_package(Eigen3 REQUIRED)
find_package(Pangolin REQUIRED)

include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/../../../
${PROJECT_SOURCE_DIR}/../../../include
${PROJECT_SOURCE_DIR}/../../../include/CameraModels
${Pangolin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
)

set(LIBS 				# 这里没有引入sophus库，直接把libORB_SLAM3.so拿了过来
${OpenCV_LIBS} 
${EIGEN3_LIBS}
${Pangolin_LIBRARIES}
${PROJECT_SOURCE_DIR}/../../../Thirdparty/DBoW2/lib/libDBoW2.so
${PROJECT_SOURCE_DIR}/../../../Thirdparty/g2o/lib/libg2o.so
${PROJECT_SOURCE_DIR}/../../../lib/libORB_SLAM3.so
-lboost_system
)

# Node for monocular camera
rosbuild_add_executable(Mono
src/ros_mono.cc
)

target_link_libraries(Mono
${LIBS}
)

5 c++运行测试

​ 两个数据集的IMU都是连续时间下噪声参数，Euroc是针孔相机，TUM是鱼眼相机

5.1 Euroc数据集测试

# 数据，每个数据下都有对应的配置参数
└── mav0
    ├── cam0
    │   └── data
    │   └── sensor.yaml
    ├── cam1
    │   └── data
    ├── imu0
    ├── leica0
    └── state_groundtruth_estimate0

5.1.1 单目 + IMU

运行单目 和 单目+惯性的除了可执行文件命令是一致的

# 可执行文件 字典 配置文件 数据总路径(程序自己找imu和cam) 时间戳
./Examples/Monocular-Inertial/mono_inertial_euroc 
Vocabulary/ORBvoc.txt ./Examples/Monocular-Inertial/EuRoC.yaml /mnt/hgfs/dataset/EUROC/MH_05_difficult ./Examples/Monocular-Inertial/EuRoC_TimeStamps/MH05.txt #orb带了

5.1.2 双目

./Examples/Stereo/stereo_euroc Vocabulary/ORBvoc.txt Examples/Stereo/EuRoC.yaml /mnt/hgfs/dataset/EUROC/MH_05_difficult Examples/Stereo/EuRoC_TimeStamps/MH05.txt 

5.1.3 双目 + IMU

./Examples/Stereo-Inertial/stereo_inertial_euroc Vocabulary/ORBvoc.txt ./Examples/Stereo-Inertial/EuRoC.yaml /mnt/hgfs/dataset/EUROC/MH_05_difficult ./Examples/Stereo-Inertial/EuRoC_TimeStamps/MH05.txt 

这里没有运行成功

There are 1 cameras in the atlas
Camera 0 is pinhole
not IMU meas
not IMU meas
not enough acceleration

not enough acceleration
not enough acceleration
First KF:0; Map init KF:0
New Map created with 145 points
start VIBA 1
end VIBA 1
Not enough motion for initializing. Reseting...

5.2 TUM-VI数据集

​ TUM-VI dataset :两个鱼眼镜头和一个惯性传感器.这里IMU的噪声参数代码中声明参考了VINS-mono。

​ 下载的数据集没有带配置参数，但是官网提供了相应的相机校准文件和IMU标定参数。

# IMU noise (Use those from VINS-mono)
IMU.NoiseGyro: 0.00016 # rad/s^0.5 
IMU.NoiseAcc: 0.0028 # m/s^1.5
IMU.GyroWalk: 0.000022 # rad/s^1.5
IMU.AccWalk: 0.00086 # m/s^2.5
IMU.Frequency: 200.0

5.2.1 单目+IMU

# 可执行文件 字典 配置文件 图像路径 时间戳 imu路径 保存轨迹名
"Usage: ./mono_inertial_tum_vi path_to_vocabulary path_to_settings path_to_image_folder_1 path_to_times_file_1 path_to_imu_data_1 (trajectory_file_name)

# 作者已经把imu数据、时间戳放到下载的代码里了
./Examples/Monocular-Inertial/mono_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Monocular-Inertial/TUM-VI.yaml /mnt/hgfs/dataset/TUM/VIO/zip文件/dataset-corridor1_512_16/mav0/cam0/data Examples/Monocular-Inertial/TUM_TimeStamps/dataset-corridor1_512.txt Examples/Monocular-Inertial/TUM_IMU/dataset-corridor1_512.txt
    

5.2.2 双目 + IMU

# 可执行文件 字典 配置文件 cam0 cam1 时间戳 imu数据
./Examples/Stereo-Inertial/stereo_inertial_tum_vi Vocabulary/ORBvoc.txt Examples/Stereo-Inertial/TUM-VI.yaml /mnt/hgfs/dataset/TUM/VIO/zip文件/dataset-corridor1_512_16/mav0/cam0/data /mnt/hgfs/dataset/TUM/VIO/zip文件/dataset-corridor1_512_16/mav0/cam1/data Examples/Stereo-Inertial/TUM_TimeStamps/dataset-corridor1_512.txt Examples/Stereo-Inertial/TUM_IMU/dataset-corridor1_512.txt 

6 ROS测试（待补充）

参考