SLAM第十二讲实践：【建图】单目稠密图重建实践、RGB_D稠密建图、 从RGB_D稠密建图点云重建网格图、八叉树地图实践
2022.10.20 bionic
目录
-
- 1 单目稠密图重建实践
-
- 1.1 修改CMakeLists.txt
- 1.2 实现
- 2 RGB_D稠密建图
-
- 2.1 c_cpp_properties.json
- 2.2 launch.json
- 2.3 修改pointcloud_mapping.cpp
- 2.4 修改 CMakeLists.txt
- 2.5 编译执行
- 3 从RGB_D稠密建图点云重建网格图
-
- 3.1 修改surfel_mapping.cpp
- 3.2 修改CMakeLists.txt
- 4 八叉树地图
-
- 4.1 安装octomap
- 4.2 修改octomap_mapping.cpp
- 4.3 修改Cmake
1 单目稠密图重建实践
本工程采用的数据集是使用REMODE的测试数据集。它提供了一架无人集采集的单目俯视图像，共有200张，同时提供了每张图像的真实位姿数据集自取
remode_test_data.zip
数据集下载
链接: link
提取码：lrjl
1.1 修改CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
project(dense_monocular)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_STANDARD 14)
############### dependencies ######################
# Eigen
include_directories("/usr/include/eigen3")
# OpenCV
find_package(OpenCV 3.1 REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# Sophus
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
set(THIRD_PARTY_LIBS
${OpenCV_LIBS}
${Sophus_LIBRARIES} fmt)
add_executable(dense_mapping dense_mapping.cpp)
target_link_libraries(dense_mapping ${THIRD_PARTY_LIBS})
1.2 实现
./dense_mapping ../../test_data/
解决Failed to load module canberra-gtk-module错误
sudo apt-get install libcanberra-gtk-module
再次执行上述代码
Average squared error = 0.297042, average error: -0.0441052
*** loop 21 ***
Average squared error = 0.296148, average error: -0.0433047
*** loop 22 ***
Average squared error = 0.295283, average error: -0.0425199
*** loop 23 ***
Average squared error = 0.294497, average error: -0.0418232
*** loop 24 ***
Average squared error = 0.293742, average error: -0.041222
*** loop 25 ***
Average squared error = 0.293078, average error: -0.0406909
*** loop 26 ***
Average squared error = 0.292565, average error: -0.0402602
*** loop 27 ***
Average squared error = 0.292156, average error: -0.039893
*** loop 28 ***
Average squared error = 0.29175, average error: -0.0395667
*** loop 29 ***
Average squared error = 0.291299, average error: -0.039213
*** loop 30 ***
Average squared error = 0.290664, average error: -0.0387603
*** loop 31 ***
Average squared error = 0.290207, average error: -0.0384232
*** loop 32 ***
Average squared error = 0.289564, average error: -0.0379961
*** loop 33 ***
Average squared error = 0.289188, average error: -0.0377423
*** loop 34 ***
Average squared error = 0.288831, average error: -0.0373817
*** loop 35 ***
Average squared error = 0.28817, average error: -0.0369678
*** loop 36 ***
Average squared error = 0.28776, average error: -0.0366581
*** loop 37 ***
Average squared error = 0.287351, average error: -0.0363875
*** loop 38 ***
Average squared error = 0.286935, average error: -0.0361054
*** loop 39 ***
Average squared error = 0.286412, average error: -0.0357438
*** loop 40 ***
2 RGB_D稠密建图
这里的话，我是用了vscode来做
如果不知道怎么用vscode可以补充下这方面操作
2.1 c_cpp_properties.json
{
"configurations": [
{
"name": "Linux",
"includePath": [
"${workspaceFolder}/**"
],
"defines": [],
"compilerPath": "/usr/bin/clang",
"cStandard": "c17",
"cppStandard": "c++14",
"intelliSenseMode": "linux-clang-x64"
}
],
"version": 4
}
2.2 launch.json
{
// 使用 IntelliSense 了解相关属性。
// 悬停以查看现有属性的描述。
// 欲了解更多信息，请访问: https://go.microsoft.com/fwlink/?linkid=830387
"version": "0.2.0",
"configurations": [
{
"name": "(gdb) 启动",
"type": "cppdbg",
"request": "launch",
"program": "${workspaceFolder}/build/debug_test",
"args": ["debug_test"],
"stopAtEntry": false,
"cwd": "${fileDirname}",
"environment": [],
"externalConsole": false,
"MIMode": "gdb",
"setupCommands": [
{
"description": "为 gdb 启用整齐打印",
"text": "-enable-pretty-printing",
"ignoreFailures": true
},
{
"description": "将反汇编风格设置为 Intel",
"text": "-gdb-set disassembly-flavor intel",
"ignoreFailures": true
}
]
}
]
}
把cpp文件全部放进src目录下
2.3 修改pointcloud_mapping.cpp
#include 2.4 修改 CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
project(dense_RGBD)
#set(CMAKE_BUILD_TYPE Release) 注意这里需要注释掉，不然没法在vscode中运行
set(CMAKE_CXX_STANDARD 14)
# opencv
find_package(OpenCV 3.1 REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# eigen
include_directories("/usr/include/eigen3/")
# pcl
find_package(PCL REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
add_definitions(${PCL_DEFINITIONS})
# octomap
#find_package(octomap REQUIRED)
#include_directories(${OCTOMAP_INCLUDE_DIRS})
add_executable(pointcloud_mapping src/pointcloud_mapping.cpp)
target_link_libraries(pointcloud_mapping ${OpenCV_LIBS} ${PCL_LIBRARIES})
2.5 编译执行
然后点最下面一行小齿轮执行编译，三角符号执行
这里在build会生成一个map.pcd的文件，使用pcl_viewer来观察它
pcl_viewer /home/zhangyuanbo/Slam14_2/ch12/dense_RGBD/build/map.pcd
3 从RGB_D稠密建图点云重建网格图
3.1 修改surfel_mapping.cpp
//
// Created by gaoxiang on 19-4-25.
//
#include 3.2 修改CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
#set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS "-std=c++11 -O2")
# opencv
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# eigen
include_directories("/usr/include/eigen3/")
# pcl
find_package(PCL REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
add_definitions(${PCL_DEFINITIONS})
add_executable(surfel_mapping src/surfel_mapping.cpp)
target_link_libraries(surfel_mapping ${OpenCV_LIBS} ${PCL_LIBRARIES})
编译后这里直接在终端 build内
./surfel_mapping map.pcd
4 八叉树地图
4.1 安装octomap
sudo apt-get install liboctomap-dev octovis
4.2 修改octomap_mapping.cpp
ifstream fin("../data/pose.txt");
boost::format fmt("../data/%s/%d.%s"); //图像文件格式
#include
#include
using namespace std;
#include
#include
#include // for octomap
#include
#include // for formating strings
int main(int argc, char **argv) {
vector<cv::Mat> colorImgs, depthImgs; // 彩色图和深度图
vector<Eigen::Isometry3d> poses; // 相机位姿
ifstream fin("../data/pose.txt");
if (!fin) {
cerr << "cannot find pose file" << endl;
return 1;
}
for (int i = 0; i < 5; i++) {
boost::format fmt("../data/%s/%d.%s");
colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str()));
depthImgs.push_back(cv::imread((fmt % "depth" % (i + 1) % "png").str(), -1)); // 使用-1读取原始图像
double data[7] = {0};
for (int i = 0; i < 7; i++) {
fin >> data[i];
}
Eigen::Quaterniond q(data[6], data[3], data[4], data[5]);
Eigen::Isometry3d T(q);
T.pretranslate(Eigen::Vector3d(data[0], data[1], data[2]));
poses.push_back(T);
}
// 计算点云并拼接
// 相机内参
double cx = 319.5;
double cy = 239.5;
double fx = 481.2;
double fy = -480.0;
double depthScale = 5000.0;
cout << "正在将图像转换为 Octomap ..." << endl;
// octomap tree
octomap::OcTree tree(0.01); // 参数为分辨率
for (int i = 0; i < 5; i++) {
cout << "转换图像中: " << i + 1 << endl;
cv::Mat color = colorImgs[i];
cv::Mat depth = depthImgs[i];
Eigen::Isometry3d T = poses[i];
octomap::Pointcloud cloud; // the point cloud in octomap
for (int v = 0; v < color.rows; v++)
for (int u = 0; u < color.cols; u++) {
unsigned int d = depth.ptr(v)[u]; // 深度值
if (d == 0) continue; // 为0表示没有测量到
Eigen::Vector3d point;
point[2] = double(d) / depthScale;
point[0] = (u - cx) * point[2] / fx;
point[1] = (v - cy) * point[2] / fy;
Eigen::Vector3d pointWorld = T * point;
// 将世界坐标系的点放入点云
cloud.push_back(pointWorld[0], pointWorld[1], pointWorld[2]);
}
// 将点云存入八叉树地图，给定原点，这样可以计算投射线
tree.insertPointCloud(cloud, octomap::point3d(T(0, 3), T(1, 3), T(2, 3)));
}
// 更新中间节点的占据信息并写入磁盘
tree.updateInnerOccupancy();
cout << "saving octomap ... " << endl;
tree.writeBinary("octomap.bt");
return 0;
}
4.3 修改Cmake
cmake_minimum_required(VERSION 2.8)
project(octomap_mapping)
#set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_STANDARD 14)
# opencv
find_package(OpenCV REQUIRED)
include_directories(${OpenCV_INCLUDE_DIRS})
# eigen
include_directories("/usr/include/eigen3/")
# pcl
find_package(PCL REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})
add_definitions(${PCL_DEFINITIONS})
# octomap
find_package(octomap REQUIRED)
include_directories(${OCTOMAP_INCLUDE_DIRS})
add_executable(octomap_mapping src/octomap_mapping.cpp)
target_link_libraries(octomap_mapping ${OpenCV_LIBS} ${PCL_LIBRARIES} ${OCTOMAP_LIBRARIES})
执行后会在工程主路径下生成一个octomap.bt文件
然后可以使用octovis打开
可视化界面简单，按下1键，可根据高度信息着色
在右侧有八叉树地深度限制条,这里可以调节地图的分辨率
由于我们构造时使用的默认深度是 16 层,所以这里显示 16 层的话即最高分辨率,也就是每个小块的边长为 0.05米。
当我们将深度减少一层时,八叉树的叶子节点往上提了一层,每个小块的边长就增加两倍,变成 0.1 米。可以看到,我们能够很容易地调节地图分辨率以适应不同的场合。Octomap还有一些可以探索的地方,例如,我们可以方便地查询任意点的占据概率,以此设计在地图中进行导航的方法。