视觉十四讲:第十二讲_八叉树地图

1.简介

把三维空间建模为许多小方块，如果把每一个小方块的每一个面平均切为两片，那么这个小方块就会变为同样大小的八个小方块，这个步骤不断重复，直到最后的方块大小达到建模的最高精度，这个过程，就是一颗八叉树。

上图显示了大立方体不断均分为八块，知道变成最小的方块为止，整个大方块可以看作根节点，最小的方块可以看作叶子节点。
在八叉树中，当某个方块的一个子节点都被占据或者不被占据时，就没必要展开这个节点，例如一开始地图为空白，那么只需要一个根节点，不需要完整的树，当向地图中添加信息时，由于实际的物体经常连在一起，空白的地方也经常连在一起，所以大多数八叉树节点不需要展开到叶子层面，这就大大减小了存储空间。
对于占据和空白来说，我们选择用概率的形式表达这件事情，设 (yin R) 为概率对数值，x为0到1之间的概率：


可以看到，当y从负无穷变到正无穷时，x相应地从0变到了1。当y取0的时候，x取到了0.5。因此，我们不妨设y来表达节点是否被占据，当不断观测到占据时，让y增加一个值，否则就减小一个值。用数学形式来说，设某节点为n，观测数据为z。那么从开始到t时刻某点的概率对数值为 (L(n|z_{1:t}))，那么t+1时刻为：

有了对数概率，我们就可以根据RGB-D数据，更新整个八叉树地图了，假设我们在RGB-D图像中观测到了某个像素带有深度d，这说明一件事：我们在深度值对应的空间点上观测到了一个占据数据，并且，从相机光心出发，到这个点的线段上，是没有物体的，利用这个信息，就可以很好的对八叉树地图进行更新。

#include <iostream>
#include <fstream>

using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>

#include <octomap/octomap.h>    // for octomap 

#include <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;
}

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

# octomap 
find_package(octomap REQUIRED)
include_directories(${OCTOMAP_INCLUDE_DIRS})

add_executable(octomap_mapping octomap_mapping.cpp)
target_link_libraries(octomap_mapping ${OpenCV_LIBS} ${PCL_LIBRARIES} ${OCTOMAP_LIBRARIES})