SLAM第十二讲实践：【建图】单目稠密图重建实践、RGB_D稠密建图、 从RGB_D稠密建图点云重建网格图、八叉树地图实践
2024-05-13 15:01:57
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 <iostream>#include <fstream>using namespace std;#include <opencv2/core/core.hpp>#include <opencv2/highgui/highgui.hpp>#include <Eigen/Geometry>#include <boost/format.hpp> // for formating strings#include <pcl/point_types.h>#include <pcl/io/pcd_io.h>#include <pcl/filters/voxel_grid.h>#include <pcl/visualization/pcl_visualizer.h>#include <pcl/filters/statistical_outlier_removal.h>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 << "正在将图像转换为点云..." << endl;// 定义点云使用的格式：这里用的是XYZRGBtypedef pcl::PointXYZRGB PointT;typedef pcl::PointCloud<PointT> PointCloud;// 新建一个点云PointCloud::Ptr pointCloud(new PointCloud);for (int i = 0; i < 5; i++) {PointCloud::Ptr current(new PointCloud);cout << "转换图像中: " << i + 1 << endl;cv::Mat color = colorImgs[i];cv::Mat depth = depthImgs[i];Eigen::Isometry3d T = poses[i];for (int v = 0; v < color.rows; v++)for (int u = 0; u < color.cols; u++) {unsigned int d = depth.ptr<unsigned short>(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;PointT p;p.x = pointWorld[0];p.y = pointWorld[1];p.z = pointWorld[2];p.b = color.data[v * color.step + u * color.channels()];p.g = color.data[v * color.step + u * color.channels() + 1];p.r = color.data[v * color.step + u * color.channels() + 2];current->points.push_back(p);}// depth filter and statistical removalPointCloud::Ptr tmp(new PointCloud);pcl::StatisticalOutlierRemoval<PointT> statistical_filter;statistical_filter.setMeanK(50);statistical_filter.setStddevMulThresh(1.0);statistical_filter.setInputCloud(current);statistical_filter.filter(*tmp);(*pointCloud) += *tmp;}pointCloud->is_dense = false;cout << "点云共有" << pointCloud->size() << "个点." << endl;// voxel filterpcl::VoxelGrid<PointT> voxel_filter;double resolution = 0.03;voxel_filter.setLeafSize(resolution, resolution, resolution); // resolutionPointCloud::Ptr tmp(new PointCloud);voxel_filter.setInputCloud(pointCloud);voxel_filter.filter(*tmp);tmp->swap(*pointCloud);cout << "滤波之后，点云共有" << pointCloud->size() << "个点." << endl;pcl::io::savePCDFileBinary("map.pcd", *pointCloud);return 0;}
2.4 修改 CMakeLists.txt
cmake_minimum_required(VERSION 2.8)project(dense_RGBD)#set(CMAKE_BUILD_TYPE Release) 注意这里需要注释掉，不然没法在vscode中运行set(CMAKE_CXX_STANDARD 14)# opencvfind_package(OpenCV 3.1 REQUIRED)include_directories(${OpenCV_INCLUDE_DIRS})# eigeninclude_directories("/usr/include/eigen3/")# pclfind_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 <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/surface/surfel_smoothing.h>
#include <pcl/surface/mls.h>
#include <pcl/surface/gp3.h>
#include <pcl/surface/impl/mls.hpp>// typedefs
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointCloud<PointT>::Ptr PointCloudPtr;
typedef pcl::PointXYZRGBNormal SurfelT;
typedef pcl::PointCloud<SurfelT> SurfelCloud;
typedef pcl::PointCloud<SurfelT>::Ptr SurfelCloudPtr;SurfelCloudPtr reconstructSurface(const PointCloudPtr &input, float radius, int polynomial_order) {pcl::MovingLeastSquares<PointT, SurfelT> mls;pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);mls.setSearchMethod(tree);mls.setSearchRadius(radius);mls.setComputeNormals(true);mls.setSqrGaussParam(radius * radius);mls.setPolynomialFit(polynomial_order > 1);mls.setPolynomialOrder(polynomial_order);mls.setInputCloud(input);SurfelCloudPtr output(new SurfelCloud);mls.process(*output);return (output);
}pcl::PolygonMeshPtr triangulateMesh(const SurfelCloudPtr &surfels) {// Create search tree*pcl::search::KdTree<SurfelT>::Ptr tree(new pcl::search::KdTree<SurfelT>);tree->setInputCloud(surfels);// Initialize objectspcl::GreedyProjectionTriangulation<SurfelT> gp3;pcl::PolygonMeshPtr triangles(new pcl::PolygonMesh);// Set the maximum distance between connected points (maximum edge length)gp3.setSearchRadius(0.05);// Set typical values for the parametersgp3.setMu(2.5);gp3.setMaximumNearestNeighbors(100);gp3.setMaximumSurfaceAngle(M_PI / 4); // 45 degreesgp3.setMinimumAngle(M_PI / 18); // 10 degreesgp3.setMaximumAngle(2 * M_PI / 3); // 120 degreesgp3.setNormalConsistency(true);// Get resultgp3.setInputCloud(surfels);gp3.setSearchMethod(tree);gp3.reconstruct(*triangles);return triangles;
}int main(int argc, char **argv) {// Load the pointsPointCloudPtr cloud(new PointCloud);if (argc == 0 || pcl::io::loadPCDFile(argv[1], *cloud)) {cout << "failed to load point cloud!";return 1;}cout << "point cloud loaded, points: " << cloud->points.size() << endl;// Compute surface elementscout << "computing normals ... " << endl;double mls_radius = 0.05, polynomial_order = 2;auto surfels = reconstructSurface(cloud, mls_radius, polynomial_order);// Compute a greedy surface triangulationcout << "computing mesh ... " << endl;pcl::PolygonMeshPtr mesh = triangulateMesh(surfels);cout << "display mesh ... " << endl;pcl::visualization::PCLVisualizer vis;vis.addPolylineFromPolygonMesh(*mesh, "mesh frame");vis.addPolygonMesh(*mesh, "mesh");vis.resetCamera();vis.spin();
}
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 <iostream>#include <fstream>using namespace std;#include <opencv2/core/core.hpp>#include <opencv2/highgui/highgui.hpp>#include <octomap/octomap.h> // for octomap#include <eigen3/Eigen/Geometry>#include <boost/format.hpp> // for formating stringsint 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 treeoctomap::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 octomapfor (int v = 0; v < color.rows; v++)for (int u = 0; u < color.cols; u++) {unsigned int d = depth.ptr<unsigned short>(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)# opencvfind_package(OpenCV REQUIRED)include_directories(${OpenCV_INCLUDE_DIRS})# eigeninclude_directories("/usr/include/eigen3/")# pclfind_package(PCL REQUIRED)include_directories(${PCL_INCLUDE_DIRS})add_definitions(${PCL_DEFINITIONS})# octomapfind_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还有一些可以探索的地方,例如,我们可以方便地查询任意点的占据概率,以此设计在地图中进行导航的方法。
SLAM第十二讲实践：【建图】单目稠密图重建实践、RGB_D稠密建图、 从RGB_D稠密建图点云重建网格图、八叉树地图实践相关推荐
- CG基础教程-陈惟老师十二讲笔记
转自 麽洋TinyOcean:http://www.douban.com/people/Tinyocean/notes?start=50&type=note 因为看了陈惟十二讲视频没有课件,边 ...
- 服务器系统报错kernel-power,第十二讲、Linux服务器操作系统1.ppt
第十二讲.Linux服务器操作系统1.ppt 4.1引导与关闭系统 4.1.1 GRUB引导器 GRUB简介 1)grub?是一个多重启动管理器.grub是GRand?Unified?Bootload ...
- 运筹学与最优化方法_[公开课]运筹学之线性规划算法十二讲
运筹学之线性规划算法十二讲 这是最美好的时代,同样带来最无助的希望:这是最丰富多彩的时代,思想同样的匮乏:这是最互联互通的时代,一样找不到自己和同路的人:这是最光明的时代,我们依然要经过黑暗的摸索和摸 ...
- 计算机基础函数运用,计算机应用基础第十二讲：EXCEL中函数的实际运用.doc
文档介绍: 计算机应用基础第十二讲:EXCEL中函数的实际运用.doc计算机应用基础第十二讲:EXCEL中函数的实际运用课 题EXCEL屮函数的实际运用课型多媒体课授课时间第20周教学目的实例分析,掌 ...
- 陈力：传智播客古代 珍宝币 泡泡龙游戏开发第十二讲：盒子的定位方式
陈力:传智播客古代 珍宝币 泡泡龙游戏开发第十二讲:盒子的定位方式 摘要:通过前节<第十一讲:浮动>学习了贵阳网站建设中的DIV+CSS中盒子模型和浮动进行介绍.框模型是CSS的基础,本文 ...
- 【阿里聚安全·安全周刊】阿里双11技术十二讲直播预约|AWS S3配置错误曝光NSA陆军机密文件
原文链接:点击打开链接 摘要: 关键词:阿里双11技术十二讲直播丨雪人计划丨亚马逊AWS S3配置错误丨2018威胁预测丨MacOS漏洞丨智能风控平台MTEE3丨黑客窃取<权利的游戏>剧本 ...
- 【阿里聚安全·安全周刊】阿里双11技术十二讲直播预约|AWS S3配置错误曝光NSA陆军机密文件...
关键词:阿里双11技术十二讲直播丨雪人计划丨亚马逊AWS S3配置错误丨2018威胁预测丨MacOS漏洞丨智能风控平台MTEE3丨黑客窃取<权利的游戏>剧本|Android 8.1 本 ...
- 计算机系统组成怎么讲PPT,计算机组成原理第十二讲(存储子系统二)ppt课件.ppt
<计算机组成原理第十二讲(存储子系统二)ppt课件.ppt>由会员分享,提供在线免费全文阅读可下载,此文档格式为ppt,更多相关<计算机组成原理第十二讲(存储子系统二)ppt课件.p ...
- 【预告：直播回顾资料下载】2017阿里巴巴双11技术十二讲，历数双11精彩技术干货
点击有惊喜 历届双11,阿里以其前瞻性的视角和创新技术一直致力于为大众递交诚意满满的答卷,大浪淘沙后最终沉淀下来的都是技术的烁金.叹为观止的数字中流转着这样的人.故事和技术,想要详细了解2017阿里双 ...
最新文章
- 使用Vscode进行远程炼丹
- 【图文讲解】TCP为啥要3次握手和4次挥手？握两次手不行吗？
- 解决COOKIES存储中文乱码的问题
- 快逸报表API直接生成v4统计图
- C++的三大特性：封装，继承，多态
- iPhoneX 序列适配方案
- js鼠标移动到指定位置_Python: pyautogui模块之鼠标控制
- Lerna 运行流程剖析
- 访问量大如何增加服务器,服务器流量过大原因及解决方法
- 为什么不使能中断，中断标志位也会被置一
- HTML5如何把圆分成六等分,Photoshop怎么把一个圆64等分？
- 使用 React-Sketchapp
- CV：计算机视觉技最强学习路线之CV简介(传统视觉技术/相关概念)、早期/中期/近期应用领域(偏具体应用)、经典CNN架构(偏具体算法)概述、常用工具/库/框架/产品、环境安装、常用数据集、编程技巧
- 计算机限制打开移动硬盘,移动硬盘一接到电脑上就显示“一个USB设备超过其集线器端口的电源限制”...
- 微信公众号网页授权40029错误,小程序微信支付前后端逻辑？ (微信授权支付之 (篇一))
- git 安装windows错误处理
- java1000字节是多少汉字_面试官：Java 中有几种基本数据类型是什么？各自占用多少字节？...
- 【ACP】复合材料铺层受压案例(附源文件)
- 人生苦短，我用Python--爬虫模拟登陆教务处并且保存数据到本地
- 2023年4月的12篇AI论文推荐