ORB-SLAM2实时稠密地图，解决运行报段错误（核心已转储）运行数据集时出现段错误，出现可视化界面后闪退（添加实时彩色点云地图+保存点云地图）

一.ORB-SLAM2实时稠密地图ORBSLAM2_with_pointcloud_map

高翔的稠密建图仓库

1. git clone https://github.com/gaoxiang12/ORBSLAM2_with_pointcloud_map.git

2. 去ORB SLAM2里拷贝Vocabulary到/home/cgm/ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified文件夹下

3. 删除一些build文件夹

删除ORB_SLAM2_modified/Thirdparty/DBoW2/build、ORB_SLAM2_modified/Thirdparty/g2o/build以及ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/build这3个 build 文件夹

4. 尝试运行./build.sh看看报什么错，再解决；

chmod +x build.sh
./build.sh
1
2

5. 报错如下：

/usr/include/pcl-1.10/pcl/pcl_config.h:7:4: error: #error PCL requires C++14 or above
7 | #error PCL requires C++14 or above

**原因：**错误消息 PCL requires C++14 or above 表明您正在使用的点云库 (PCL) 需要至少 C++14 的 C++ 标准版本才能编译。您的项目可能使用较旧的 C++ 标准，从而导致此问题。
**解决：**修改ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/CMakeLists.txt文件-std=c++11换成-std=c++14

下面的这是原来的


# # Check C++11 or C++0x support
# include(CheckCXXCompilerFlag)
# CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
# CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
# if(COMPILER_SUPPORTS_CXX11)
#    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
#    add_definitions(-DCOMPILEDWITHC11)
#    message(STATUS "Using flag -std=c++11.")
# elseif(COMPILER_SUPPORTS_CXX0X)
#    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
#    add_definitions(-DCOMPILEDWITHC0X)
#    message(STATUS "Using flag -std=c++0x.")
# else()
#    message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
# endif()

修改为：


# Check C++14 or C++0x support  好像PCL1.10版本需要C++14
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++14" COMPILER_SUPPORTS_CXX14)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX14)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++14")
   add_definitions(-DCOMPILEDWITHC14)
   message(STATUS "Using flag -std=c++14.")
elseif(COMPILER_SUPPORTS_CXX0X)
   set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
   add_definitions(-DCOMPILEDWITHC0X)
   message(STATUS "Using flag -std=c++0x.")
else()
   message(FATAL_ERROR "The compiler ${CMAKE_CXX_COMPILER} has no C++14 support. Please use a different C++ compiler.")
endif()
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

6. 修改ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/CMakeLists.txt文件之后再次运行./build.sh看看报什么错，

/usr/include/c++/9/bits/stl_map.h:122:71: error: static assertion failed: std::map must have the same value_type as its allocator
  122 |       static_assert(is_same<typename _Alloc::value_type, value_type>::value,
1
2

解决办法： 打开LoopClosing.h，将

typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
        Eigen::aligned_allocator<std::pair<const KeyFrame*, g2o::Sim3> > > KeyFrameAndPose;
        
替换为

typedef map<KeyFrame*,g2o::Sim3,std::less<KeyFrame*>,
        Eigen::aligned_allocator<std::pair<KeyFrame *const, g2o::Sim3> > > KeyFrameAndPose;
1
2
3
4
5
6
7

7. 报错：

/home/cgm/ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/Examples/Monocular/mono_tum.cc:81:22: error: ‘std::chrono::monotonic_clock’ has not been declared
81 | std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
ORB_SLAM安装问题error: ‘std::chrono::monotonic_clock’ has not been declared

解决办法：将代码中所有使用 `std::chrono::monotonic_clock` 的地方替换为 `std::chrono::steady_clock`。
1

8. 成功编译截图

9. 运行TUM数据集

使用如下命令./rgbd_tum path_to_vocabulary path_to_settings path_to_sequence path_to_association
我的命令如下:

./bin/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM1.yaml /home/cgm/DataSet/TUM_Dataset/rgbd_dataset_freiburg1_xyz Examples/RGB-D/associations/fr1_xyz.txt
1

10. 报错分析

(1) 编译出现参数未声明问题，参考代码中有些未在头文件中声明，要完整声明才能正确编译。
(2) 运行数据集时出现段错误，出现可视化界面后闪退

ORB Extractor Parameters: 
- Number of Features: 1000
- Scale Levels: 8
- Scale Factor: 1.2
- Initial Fast Threshold: 20
- Minimum Fast Threshold: 7

Depth Threshold (Close/Far Points): 3.09294

-------
Start processing sequence ...
Images in the sequence: 792

New map created with 834 points
receive a keyframe, id = 1
generate point cloud for kf 1, size=25153
show global map, size=14971
receive a keyframe, id = 2
generate point cloud for kf 2, size=25597
段错误 (核心已转储)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

• 有的说：将其中的PCL 1.7 REQUIRED中的1.7删掉（我的是1.10.0，不用删）

find_package(Eigen3 3.1.0 REQUIRED)
find_package(Pangolin REQUIRED)
# adding for point cloud viewer and mapper
# find_package( PCL 1.7 REQUIRED )#修改这行
find_package( PCL REQUIRED )#改为这行
message("PCL version: " ${PCL_VERSION})#增加这行代码可查看PCL版本信息
1
2
3
4
5
6

注意：我的PCL版本是1.10.0，我没有改动这个代码

• 出现这个原因是你没有把 -march=native 删干净，你是手动删除的 -march=native
• 出现这个原因是你没有把 -march=native 删干净，你是手动删除的 -march=native
• 出现这个原因是你没有把 -march=native 删干净，你是手动删除的 -march=native
• -出现这个原因是你没有把 -march=native 删干净，你是手动删除的 -march=native
• 出现这个原因是你没有把 -march=native 删干净，你是手动删除的 -march=native
• 出现这个原因是你没有把 -march=native 删干净，你是手动删除的 -march=native

如果手动删的话，要删除这四个CMakeLists.txt文件里的-march=native

• ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/CMakeLists.txt
• ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/CMakeLists.txt
• ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/Thirdparty/DBoW2/CMakeLists.txt
• ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/Thirdparty/g2o/CMakeLists.txt

建议使用 CTRL+SHIFT+F进行搜索全替换,将所有的-march=native替换为空格

成功运行截图（灰色点云）

11. 使用ORB-SLAM2保存彩色点云地图

ORB-SLAM2是一个用于实时单目、双目和RGB-D相机SLAM的流行开源库。如果您想要修改ORB-SLAM2以保存彩色点云地图，以下是一些您需要进行的修改步骤。

• 步骤1: 在Tracking.h中添加成员变量

在ORB-SLAM2的include/Tracking.h文件中，您需要添加以下成员变量，以保存当前帧的彩色图像：

// Current Frame
Frame mCurrentFrame;
cv::Mat mImRGB; // 添加这行
cv::Mat mImGray;
cv::Mat mImDepth;
1
2
3
4
5

• 步骤2: 在Tracking.cc中修改代码

在ORB-SLAM2的src/Tracking.cc文件中，需要修改两个地方。

第一处修改

cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB, const cv::Mat &imD, const double &timestamp)
{
    mImRGB = imRGB; // 添加这行
    mImGray = imRGB;
    mImDepth = imD;

1
2
3
4
5
6

第二处修改

// insert Key Frame into point cloud viewer
//mpPointCloudMapping->insertKeyFrame( pKF, this->mImGray, this->mImDepth );
mpPointCloudMapping->insertKeyFrame( pKF, this->mImRGB, this->mImDepth ); // 修改地方

1
2
3
4

• 步骤3: 保存彩色点云地图

修改ORB-SLAM2的src/pointcloudmapping.cc文件，在其中调用PCL库的 pcl::io::savePCDFileBinary 函数来保存点云地图。

首先，添加以下头文件：

#include 

1
2

然后，在 void PointCloudMapping::viewer() 函数中，大约在第123行附近加入保存地图的命令：

for (size_t i = lastKeyframeSize; i < N; i++)
{
    PointCloud::Ptr p = generatePointCloud(keyframes[i], colorImgs[i], depthImgs[i]);
    *globalMap += *p;
}

 // pcl::io::savePCDFileBinary("vslam.pcd", *globalMap); // 只需要加入这一句
 //我用的下面的
 // 存储点云
 string save_path = "./resultPointCloudFile.pcd";
 pcl::io::savePCDFile(save_path, *globalMap);
 cout << "save pcd files to :  " << save_path << endl;

1
2
3
4
5
6
7
8
9
10
11
12
13

• 步骤4: 重新编译程序

修改代码后，您需要重新编译以使更改生效。

• 步骤5: 安装PCL工具并查看生成的文件

为了查看保存的彩色点云地图，您需要安装PCL工具，并使用pcl_viewer工具来查看生成的文件。您可以使用以下命令来安装和查看：

# 安装PCL工具
sudo apt-get install pcl-tools

# 查看保存的点云地图文件
pcl_viewer vslam.pcd

1
2
3
4
5
6

现在，您已经修改了ORB-SLAM2以保存彩色点云地图，并且可以使用PCL工具来查看生成的地图文件。

按住shift+鼠标滚轮可以上下移动点云；
按住ctrl+鼠标坐标可以顺时针逆时针拖动点云；

高动态环境的数据集rgbd_dataset_freiburg3_walking_xyz

./bin/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM3.yaml /home/cgm/DataSet/TUM_Dataset/rgbd_dataset_freiburg3_walking_xyz/ Examples/RGB-D/associations/fr3_walking_xyz.txt 
1
2
3

二. 使用D435i进行稠密建图

1.建立ROS工作空间并初始化 ~/catkin_pointcloudmap_ws/src

mkdir -p ~/catkin_pointcloudmap_ws/src
cd ~/catkin_pointcloudmap_ws/src
catkin_init_workspace
cd ..
catkin_make
echo "source ~/catkin_pointcloudmap_ws/devel/setup.bash" >> ~/.bashrc
source ~/.bashrc
echo $ROS_PACKAGE_PATH
1
2
3
4
5
6
7
8

把ORB_SLAM2_modified移动到catkin_pointcloudmap_ws/src里

删除旧的构建文件和目录：首先，你需要删除build目录以及其中的所有文件。这将删除旧的CMake缓存和生成的文件。

cd src/ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/
rm -r build
1
2

2. 编译./build_ros.sh

在刚刚的目录下返回到ORB_SLAM2_modified$

cd ..
cd ..
cd ..
cgm@cgm:~/catkin_pointcloudmap_ws/src/ORB_SLAM2_modified$ ./build_ros.sh 
1
2
3
4

• 报错：1

/home/cgm/catkin_pointcloudmap_ws/src/ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/…/…/…/include/pointcloudmapping.h:25:10:
fatal error: pcl/common/transforms.h: 没有那个文件或目录 25 | #include

• 解决办法：
  修改/home/cgm/catkin_pointcloudmap_ws/src/ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/CMakeLists.txt，增加PCL相关信息

find_package(Eigen3 3.1.0 REQUIRED)
find_package(Pangolin REQUIRED)
# # adding for point cloud viewer and mapper # new
find_package( PCL REQUIRED )# new
message(STATUS "PCL include dirs: ${PCL_INCLUDE_DIRS}")# new
message("PCL version: " ${PCL_VERSION})

include_directories(
${PROJECT_SOURCE_DIR}
${PROJECT_SOURCE_DIR}/../../../
${PROJECT_SOURCE_DIR}/../../../include
${Pangolin_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}# new
)

add_definitions( ${PCL_DEFINITIONS} )# new
link_directories( ${PCL_LIBRARY_DIRS} )# new
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

• 再次编译后报错：2

1. /usr/bin/ld: warning: libopencv_imgproc.so.4.2, needed by /opt/ros/noetic/lib/libcv_bridge.so, may conflict with libopencv_imgproc.so.3.4
2. /usr/bin/ld: CMakeFiles/MonoAR.dir/src/AR/ViewerAR.cc.o: undefined reference to symbol '_ZN2cv7putTextERKNS_17_InputOutputArrayERKNSt7__cxx1112basic_stringIcSt11char_traitsIcESaIcEEENS_6Point_IiEEidNS_7Scalar_IdEEiib'
3. /usr/bin/ld: /usr/lib/x86_64-linux-gnu/libopencv_imgproc.so.4.2.0: error adding symbols: DSO missing from command line

这些错误提示表明你的系统上安装了多个版本的OpenCV，而且它们之间存在冲突。ROS Noetic似乎需要OpenCV 4.2，但ORB_SLAM2可能是针对OpenCV 3.4编译的。

• 解决

find_package(OpenCV 4.2 QUIET) # change 3.0 to 4.2
1

• 再次编译后报错：3

/home/cgm/catkin_pointcloudmap_ws/src/ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/…/…/…/include/System.h:29,
from /home/cgm/catkin_pointcloudmap_ws/src/ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc:36:
/home/cgm/catkin_pointcloudmap_ws/src/ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/…/…/…/include/ORBextractor.h:26:10:
fatal error: opencv/cv.h: 没有那个文件或目录 26 | #include
| ^~~~~~~~~~~~~

• 解决

// #include 
#include
#include 
1
2
3

3.查看相机内参

打开一个终端输入

roslaunch realsense2_camera rs_camera.launch
1

再打开一个终端输入

rostopic echo /camera/color/camera_info
1

查看到的相机内参如下:

  stamp: 
    secs: 1695614698
    nsecs: 941847086
  frame_id: "camera_color_optical_frame"
height: 720
width: 1280
distortion_model: "plumb_bob"
D: [0.0, 0.0, 0.0, 0.0, 0.0]
K: [909.855712890625, 0.0, 651.5874633789062, 0.0, 909.7683715820312, 381.3797302246094, 0.0, 0.0, 1.0]
R: [1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0]
P: [909.855712890625, 0.0, 651.5874633789062, 0.0, 0.0, 909.7683715820312, 381.3797302246094, 0.0, 0.0, 0.0, 1.0, 0.0]
binning_x: 0
binning_y: 0
roi: 
  x_offset: 0
  y_offset: 0
  height: 0
  width: 0
  do_rectify: False
---

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

4.相机内参矩阵 ( K )

相机的内参矩阵通常表示为：

$$K=\left[\begin{array}{ccc}fx& 0& cx\\ 0& fy& cy\\ 0& 0& 1\end{array}\right]$$
其中：

• ( fx ) 和 ( fy )：相机的焦距，分别在图像的 x 和 y 方向上。这些值决定了图像上的点如何映射到相机坐标系中。
• ( cx ) 和 ( cy )：图像的主点坐标。主点是3D世界中的点投影到图像平面上的点。

从提供的 K 矩阵中，我们得到：

• ( fx = 910.0997314453125 )
• ( fy = 909.994873046875 )
• ( cx = 639.4933471679688 )
• ( cy = 359.3774108886719 )

5.创建配置文件

切换到/home/cgm/catkin_pointcloudmap_ws/src/ORB_SLAM2_modified/Examples/ROS目录下
打开终端输入

cd /home/cgm/catkin_pointcloudmap_ws/src/ORB_SLAM2_modified/Examples/ROS
1

新建MyD435i.yaml文件

在终端里输入

touch MyD435i.yaml

%YAML:1.0
 
#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------
 
# Camera calibration and distortion parameters (OpenCV) 
# rostopic echo /camera/color/camera_info中查看的是 K: [909.855712890625, 0.0, 651.5874633789062, 0.0, 909.7683715820312, 381.3797302246094, 0.0, 0.0, 1.0]
# 改为自己的相机的内参矩阵K
Camera.fx: 909.855712890625
Camera.fy: 909.7683715820312
Camera.cx: 651.5874633789062
Camera.cy: 381.3797302246094

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0
Camera.p3: 0.0

Camera.width: 640
Camera.height: 480
# Camera frames per second 
Camera.fps: 30.0

# IR projector baseline times fx (aprox.)
# bf = baseline (in meters) * fx, D435i的 baseline = 50 mm 
Camera.bf: 50.0
 
# 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
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
 
PointCloudMapping.Resolution: 0.01
meank: 50
thresh: 2.0


``



## 6.运行

```bash
roslaunch realsense2_camera rs_camera.launch
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85

在ORB_SLAM2文件夹下再打开一个终端输入

rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/MyD435i.yaml
1

即可运行成功

7.问题:

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/cgm/ORB_SLAM2/Examples/ROS
source ~/catkin_pointcloudmap_ws/devel/setup.bash
这两个的作用给一样的吗
1
2
3

不完全一样，但它们都与ROS的包路径有关。我会详细解释它们的作用：

1. export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/cgm/ORB_SLAM2/Examples/ROS: export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/cgm/ORB_SLAM2/Examples/ROS ：
   • 这行命令是将/home/cgm/ORB_SLAM2/Examples/ROS添加到现有的ROS_PACKAGE_PATH环境变量的末尾。
   • ROS_PACKAGE_PATH是一个环境变量，它告诉ROS在哪里查找包。当你尝试使用如roscd、roslaunch等命令时，ROS会在这些路径中查找相关的包。
   • 通过这行命令，你确保ROS可以在/home/cgm/ORB_SLAM2/Examples/ROS路径下找到ORB_SLAM2包。
2. source ~/catkin_pointcloudmap_ws/devel/setup.bash: source ~/catkin_pointcloudmap_ws/devel/setup.bash ：
   • 这行命令是为了设置与catkin_pointcloudmap_ws工作空间相关的环境变量，包括更新ROS_PACKAGE_PATH以包含该工作空间中的包。
   • 当你构建一个catkin工作空间后，devel/setup.bash脚本会被生成。运行这个脚本会设置各种环境变量，使得你可以运行、查找和使用该工作空间中的ROS包。
   • 这也意味着，当你source这个setup.bash脚本时，它可能会修改或覆盖你之前设置的ROS_PACKAGE_PATH。

总结：第一行命令是手动修改ROS_PACKAGE_PATH，而第二行命令是通过source一个脚本来自动设置与特定catkin工作空间相关的环境变量。如果你在source工作空间之前手动修改了ROS_PACKAGE_PATH，那么这个修改可能会被source命令覆盖或修改。因此，通常建议先source工作空间，然后再进行任何手动的路径添加。