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 實作

解決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 ***

SLAM第十二講實踐：【建圖】單目稠密圖重建實踐、RGB_D稠密建圖、從RGB_D稠密建圖點雲重建網格圖、八叉樹地圖實踐

2 RGB_D稠密建圖

這裡的話，我是用了vscode來做

如果不知道怎麼用vscode可以補充下這方面操作

SLAM第十二講實踐：【建圖】單目稠密圖重建實踐、RGB_D稠密建圖、從RGB_D稠密建圖點雲重建網格圖、八叉樹地圖實踐

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;



    // 定義點雲使用的格式：這裡用的是XYZRGB

    typedef 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 removal

        PointCloud::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 filter

    pcl::VoxelGrid<PointT> voxel_filter;

    double resolution = 0.03;

    voxel_filter.setLeafSize(resolution, resolution, resolution);       // resolution

    PointCloud::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)



# 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 編譯執行

然後點最下面一行小齒輪執行編譯，三角符号執行

SLAM第十二講實踐：【建圖】單目稠密圖重建實踐、RGB_D稠密建圖、從RGB_D稠密建圖點雲重建網格圖、八叉樹地圖實踐

這裡在build會生成一個map.pcd的檔案，使用pcl_viewer來觀察它

pcl_viewer /home/zhangyuanbo/Slam14_2/ch12/dense_RGBD/build/map.pcd

SLAM第十二講實踐：【建圖】單目稠密圖重建實踐、RGB_D稠密建圖、從RGB_D稠密建圖點雲重建網格圖、八叉樹地圖實踐

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 objects
    pcl::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 parameters
    gp3.setMu(2.5);
    gp3.setMaximumNearestNeighbors(100);
    gp3.setMaximumSurfaceAngle(M_PI / 4); // 45 degrees
    gp3.setMinimumAngle(M_PI / 18); // 10 degrees
    gp3.setMaximumAngle(2 * M_PI / 3); // 120 degrees
    gp3.setNormalConsistency(true);

    // Get result
    gp3.setInputCloud(surfels);
    gp3.setSearchMethod(tree);
    gp3.reconstruct(*triangles);

    return triangles;
}

int main(int argc, char **argv) {

    // Load the points
    PointCloudPtr 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 elements
    cout << "computing normals ... " << endl;
    double mls_radius = 0.05, polynomial_order = 2;
    auto surfels = reconstructSurface(cloud, mls_radius, polynomial_order);

    // Compute a greedy surface triangulation
    cout << "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

SLAM第十二講實踐：【建圖】單目稠密圖重建實踐、RGB_D稠密建圖、從RGB_D稠密建圖點雲重建網格圖、八叉樹地圖實踐

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 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<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)



# 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打開

SLAM第十二講實踐：【建圖】單目稠密圖重建實踐、RGB_D稠密建圖、從RGB_D稠密建圖點雲重建網格圖、八叉樹地圖實踐

可視化界面簡單，按下1鍵，可根據高度資訊着色

在右側有八叉樹地深度限制條,這裡可以調節地圖的分辨率

由于我們構造時使用的預設深度是 16 層,是以這裡顯示 16 層的話即最高分辨率,也就是每個小塊的邊長為 0.05米。

當我們将深度減少一層時,八叉樹的葉子節點往上提了一層,每個小塊的邊長就增加兩倍,變成 0.1 米。可以看到,我們能夠很容易地調節地圖分辨率以适應不同的場合。

Octomap還有一些可以探索的地方,例如,我們可以友善地查詢任意點的占據機率,以此設計在地圖中進行導航的方法。

SLAM第十二講實踐：【建圖】單目稠密圖重建實踐、RGB_D稠密建圖、從RGB_D稠密建圖點雲重建網格圖、八叉樹地圖實踐

目錄

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】吳恩達采訪 Geoffery Hinton

吳恩達機器學習筆記（3）

吳恩達j機器學習之過拟合

吳恩達機器學習(一) 介紹

Gsl在VS2008下的配置和使用

如何用Opencv求圖像的灰階投影曲線

opencv——圖像的灰階處理（線性變換/拉伸/直方圖/均衡化）

深度學習模型分析人類複雜疾病的準确性

疾病研究：重症肌無力

人工智能如何有效地運用于自然語言處理

新聞 | Mapbox 牽手阿裡，飛豬旅行上線六大城市地圖功能

[HTML5]自定義屬性 data-* 和 jQuery.data 詳解

2021-2025年中國運動療法（KT）帶行業市場供需與戰略研究報告

cs231n斯坦福基于卷積神經網絡的CV學習筆記（一）KNN和線性分類器/分類器損失/反向傳播一，KNN圖像分類算法二，線性分類器三，線性分類器損失四，反向傳播五，神經網絡

2021年危險化學品經營機關安全管理人員考試題庫及危險化學品經營機關安全管理人員考試技巧

無人機--飛控科普

SLAM第十二講實踐：【建圖】單目稠密圖重建實踐、RGB_D稠密建圖、 從RGB_D稠密建圖點雲重建網格圖、八叉樹地圖實踐

目錄

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

繼續閱讀

SLAM第十二講實踐：【建圖】單目稠密圖重建實踐、RGB_D稠密建圖、從RGB_D稠密建圖點雲重建網格圖、八叉樹地圖實踐