一.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
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()
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,
解决办法: 打开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;
7. 报错:
/home/cgm/ORBSLAM2_with_pointcloud_map/ORB_SLAM2_modified/Examples/Monocular/mono_tum.cc:81:22: error
: ‘std::chrono::monotonic_clo
ck’ 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`。
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
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
段错误 (核心已转储)
- 有的说:将其中的
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版本信息
注意:我的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;
- 步骤2: 在Tracking.cc中修改代码
在ORB-SLAM2的src/Tracking.cc
文件中,需要修改两个地方。
第一处修改
cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB, const cv::Mat &imD, const double ×tamp)
{
mImRGB = imRGB; // 添加这行
mImGray = imRGB;
mImDepth = imD;
第二处修改
// insert Key Frame into point cloud viewer
//mpPointCloudMapping->insertKeyFrame( pKF, this->mImGray, this->mImDepth );
mpPointCloudMapping->insertKeyFrame( pKF, this->mImRGB, this->mImDepth ); // 修改地方
- 步骤3: 保存彩色点云地图
修改ORB-SLAM2的src/pointcloudmapping.cc
文件,在其中调用PCL库的 pcl::io::savePCDFileBinary
函数来保存点云地图。
首先,添加以下头文件:
#include <pcl/io/pcd_io.h>
然后,在 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;
- 步骤4: 重新编译程序
修改代码后,您需要重新编译以使更改生效。
- 步骤5: 安装PCL工具并查看生成的文件
为了查看保存的彩色点云地图,您需要安装PCL工具,并使用pcl_viewer
工具来查看生成的文件。您可以使用以下命令来安装和查看:
# 安装PCL工具
sudo apt-get install pcl-tools
# 查看保存的点云地图文件
pcl_viewer vslam.pcd
现在,您已经修改了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
二. 使用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
把ORB_SLAM2_modified
移动到catkin_pointcloudmap_ws/src
里
删除旧的构建文件和目录:首先,你需要删除build
目录以及其中的所有文件。这将删除旧的CMake缓存和生成的文件。
cd src/ORB_SLAM2_modified/Examples/ROS/ORB_SLAM2/
rm -r build
2. 编译./build_ros.sh
在刚刚的目录下返回到ORB_SLAM2_modified$
cd ..
cd ..
cd ..
cgm@cgm:~/catkin_pointcloudmap_ws/src/ORB_SLAM2_modified$ ./build_ros.sh
- 报错: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
<pcl/common/transforms.h>
- 解决办法:
修改/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
- 再次编译后报错:2
/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
/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'
/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
- 再次编译后报错: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 <opencv/cv.h>
| ^~~~~~~~~~~~~
- 解决
// #include <opencv/cv.h>
#include<opencv2/imgproc/imgproc_c.h>
#include <opencv2/highgui/highgui_c.h>
3.查看相机内参
打开一个终端输入
roslaunch realsense2_camera rs_camera.launch
再打开一个终端输入
rostopic echo /camera/color/camera_info
查看到的相机内参如下:
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
---
4.相机内参矩阵 ( K )
相机的内参矩阵通常表示为:
K
=
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
K = \begin{bmatrix} fx & 0 & cx \\ 0 & fy & cy \\ 0 & 0 & 1 \\ \end{bmatrix}
K=
fx000fy0cxcy1
其中:
- ( 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
新建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
在ORB_SLAM2
文件夹下再打开一个终端输入
rosrun ORB_SLAM2 RGBD Vocabulary/ORBvoc.txt Examples/ROS/ORB_SLAM2/MyD435i.yaml
即可运行成功
7.问题:
export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:/home/cgm/ORB_SLAM2/Examples/ROS
source ~/catkin_pointcloudmap_ws/devel/setup.bash
这两个的作用给一样的吗
不完全一样,但它们都与ROS的包路径有关。我会详细解释它们的作用:
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
包。
- 这行命令是将
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
工作空间,然后再进行任何手动的路径添加。