第12讲 建图

最新推荐文章于 2023-10-18 17:35:50 发布
最新推荐文章于 2023-10-18 17:35:50 发布
阅读量478 收藏
点赞数
分类专栏： 《视觉SLAM十四讲》代码研读 文章标签： slam 计算机视觉
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/YMWM_/article/details/117426065
版权
本文详细介绍了利用单目稠密重建技术估计图像深度，构建点云地图，并进一步通过法线计算和网格化生成三维表面模型，最后利用八叉树结构建立空间地图的过程。通过实验展示了不同阶段的3D重建结果，包括点云、网格和八叉树地图。
摘要由CSDN通过智能技术生成

目录

1 单目稠密重建

  给定200张图像，同时已知它们的轨迹真值，求第1张图像的深度图。注意这200张图与第1张图都有重叠的观测。
  cpp文件内容为，

#include <iostream>
#include <chrono>
#include <vector>
#include <fstream>

using namespace std;

#include <boost/timer.hpp>

// for sophus
#include <sophus/se3.hpp>

using Sophus::SE3d;

// for eigen
#include <Eigen/Core>
#include <Eigen/Geometry>

using namespace Eigen;

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

using namespace cv;

/**********************************************
* 本程序演示了单目相机在已知轨迹下的稠密深度估计
* 使用极线搜索 + NCC 匹配的方式，与书本的 12.2 节对应
* 请注意本程序并不完美，你完全可以改进它——我其实在故意暴露一些问题(这是借口)。
***********************************************/

// ------------------------------------------------------------------
// parameters
const int boarder = 20;         // 边缘宽度
const int width = 640;          // 图像宽度
const int height = 480;         // 图像高度
const double fx = 481.2f;       // 相机内参
const double fy = -480.0f;
const double cx = 319.5f;
const double cy = 239.5f;
const int ncc_window_size = 3;    // NCC 取的窗口半宽度
const int ncc_area = (2 * ncc_window_size + 1) * (2 * ncc_window_size + 1); // NCC窗口面积
const double min_cov = 0.1;     // 收敛判定：最小方差
const double max_cov = 10;      // 发散判定：最大方差

// ------------------------------------------------------------------
// 重要的函数
/// 从 REMODE 数据集读取数据
bool readDatasetFiles(
    const string &path,
    vector<string> &color_image_files,
    vector<SE3d> &poses,
    cv::Mat &ref_depth
);

/**
 * 根据新的图像更新深度估计
 * @param ref           参考图像
 * @param curr          当前图像
 * @param T_C_R         参考图像到当前图像的位姿
 * @param depth         深度
 * @param depth_cov     深度方差
 * @return              是否成功
 */
bool update(
    const Mat &ref,
    const Mat &curr,
    const SE3d &T_C_R,
    Mat &depth,
    Mat &depth_cov2
);

/**
 * 极线搜索
 * @param ref           参考图像
 * @param curr          当前图像
 * @param T_C_R         位姿
 * @param pt_ref        参考图像中点的位置
 * @param depth_mu      深度均值
 * @param depth_cov     深度方差
 * @param pt_curr       当前点
 * @param epipolar_direction  极线方向
 * @return              是否成功
 */
bool epipolarSearch(
    const Mat &ref,
    const Mat &curr,
    const SE3d &T_C_R,
    const Vector2d &pt_ref,
    const double &depth_mu,
    const double &depth_cov,
    Vector2d &pt_curr,
    Vector2d &epipolar_direction
);

/**
 * 更新深度滤波器
 * @param pt_ref    参考图像点
 * @param pt_curr   当前图像点
 * @param T_C_R     位姿
 * @param epipolar_direction 极线方向
 * @param depth     深度均值
 * @param depth_cov2    深度方向
 * @return          是否成功
 */
bool updateDepthFilter(
    const Vector2d &pt_ref,
    const Vector2d &pt_curr,
    const SE3d &T_C_R,
    const Vector2d &epipolar_direction,
    Mat &depth,
    Mat &depth_cov2
);

/**
 * 计算 NCC 评分
 * @param ref       参考图像
 * @param curr      当前图像
 * @param pt_ref    参考点
 * @param pt_curr   当前点
 * @return          NCC评分
 */
double NCC(const Mat &ref, const Mat &curr, const Vector2d &pt_ref, const Vector2d &pt_curr);

// 双线性灰度插值
inline double getBilinearInterpolatedValue(const Mat &img, const Vector2d &pt) {
    uchar *d = &img.data[int(pt(1, 0)) * img.step + int(pt(0, 0))];
    double xx = pt(0, 0) - floor(pt(0, 0));
    double yy = pt(1, 0) - floor(pt(1, 0));
    return ((1 - xx) * (1 - yy) * double(d[0]) +
            xx * (1 - yy) * double(d[1]) +
            (1 - xx) * yy * double(d[img.step]) +
            xx * yy * double(d[img.step + 1])) / 255.0;
}

// ------------------------------------------------------------------
// 一些小工具
// 显示估计的深度图
void plotDepth(const Mat &depth_truth, const Mat &depth_estimate);

// 像素到相机坐标系
inline Vector3d px2cam(const Vector2d px) {
    return Vector3d(
        (px(0, 0) - cx) / fx,
        (px(1, 0) - cy) / fy,
        1
    );
}

// 相机坐标系到像素
inline Vector2d cam2px(const Vector3d p_cam) {
    return Vector2d(
        p_cam(0, 0) * fx / p_cam(2, 0) + cx,
        p_cam(1, 0) * fy / p_cam(2, 0) + cy
    );
}

// 检测一个点是否在图像边框内
inline bool inside(const Vector2d &pt) {
    return pt(0, 0) >= boarder && pt(1, 0) >= boarder
           && pt(0, 0) + boarder < width && pt(1, 0) + boarder <= height;
}

// 显示极线匹配
void showEpipolarMatch(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_curr);

// 显示极线
void showEpipolarLine(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_min_curr,
                      const Vector2d &px_max_curr);

/// 评测深度估计
void evaluateDepth(const Mat &depth_truth, const Mat &depth_estimate);
// ------------------------------------------------------------------


int main(){
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();

    string path = "../test_data";

    // 从数据集读取数据
    vector<string> color_image_files;
    vector<SE3d> poses_TWC;
    Mat ref_depth;
    bool ret = readDatasetFiles(path, color_image_files, poses_TWC, ref_depth);
    if (ret == false) {
        cout << "Reading image files failed!" << endl;
        return -1;
    }
    cout << "read total " << color_image_files.size() << " files." << endl;

    // 第一张图
    Mat ref = imread(color_image_files[0], 0);                // gray-scale image
    SE3d pose_ref_TWC = poses_TWC[0];
    double init_depth = 3.0;    // 深度初始值
    double init_cov2 = 3.0;     // 方差初始值
    Mat depth(height, width, CV_64F, init_depth);             // 深度图
    Mat depth_cov2(height, width, CV_64F, init_cov2);         // 深度图方差

    for (int index = 1; index < color_image_files.size(); index++) {
        cout << "*** loop " << index << " ***" << endl;
        Mat curr = imread(color_image_files[index], 0);
        if (curr.data == nullptr) continue;
        SE3d pose_curr_TWC = poses_TWC[index];
        SE3d pose_T_C_R = pose_curr_TWC.inverse() * pose_ref_TWC;   // 坐标转换关系： T_C_W * T_W_R = T_C_R
        update(ref, curr, pose_T_C_R, depth, depth_cov2);
        evaluateDepth(ref_depth, depth);
        plotDepth(ref_depth, depth);
        imshow("image", curr);
        waitKey(1);
    }

    cout << "estimation returns, saving depth map ..." << endl;
    imwrite("depth.png", depth);
    depth.convertTo(depth, CV_16U, 1000.0);  //depthScale = 1000.0，计算深度时记得除以这个值！
    imwrite("CV_16U.png", depth);
    cout << "done." << endl;

    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> timeUsed = chrono::duration_cast<chrono::duration<double>> (t2 - t1);
    cout << "执行程序耗时" << timeUsed.count() / 60.0 << "分钟！" << endl;

    return 0;
}

/**
 * 从REMODE数据集中读取数据
 * @param path  数据集文件路径
 * @param color_image_files  彩色图向量
 * @param poses  位姿真值向量，以T_WC形式存储
 * @param ref_depth  第1张图深度真值
 */
bool readDatasetFiles(
        const string &path,
        vector<string> &color_image_files,
        vector<SE3d> &poses,
        cv::Mat &ref_depth)
{
    ifstream fin(path + "/first_200_frames_traj_over_table_input_sequence.txt");
    if(!fin) return false;
    while(!fin.eof()){
        //数据格式：图像文件名 tx, ty, tz, qx, qy, qz, qw
        string image;
        fin >> image;

        if(image == "") break;  //解决“eof指向文件结尾的下一位”的设定带来的麻烦

        double data[7];
        for(double &d : data) fin >> d;
        if(image != "scene_107.png")  //并不存在scene_107.png图片
            color_image_files.push_back(path + string("/images/") + image);
        poses.push_back(SE3d(Quaterniond(data[6], data[3], data[4], data[5]), Vector3d(data[0], data[1], data[2])));
    }
    fin.close();

    fin.open(path + "/depthmaps/scene_000.depth");
    ref_depth = cv::Mat(height, width, CV_64F);
    if(!fin) return false;
    for(int y = 0; y < height; y++){
        for(int x = 0; x < width; x++){
            double depth = 0;
            fin >> depth;
            ref_depth.ptr<double>(y)[x] = depth / 100.0;
        }
    }
    fin.close();

    return true;
}

// 对整个深度图进行更新
bool update(const Mat &ref, const Mat &curr, const SE3d &T_C_R, Mat &depth, Mat &depth_cov2) {
    for (int x = boarder; x < width - boarder; x++)
        for (int y = boarder; y < height - boarder; y++) {
            // 遍历每个像素
            if (depth_cov2.ptr<double>(y)[x] < min_cov || depth_cov2.ptr<double>(y)[x] > max_cov) // 深度已收敛或发散
                continue;
            // 在极线上搜索 (x,y) 的匹配
            Vector2d pt_curr;
            Vector2d epipolar_direction;
            bool ret = epipolarSearch(
                ref,
                curr,
                T_C_R,
                Vector2d(x, y),
                depth.ptr<double>(y)[x],
                sqrt(depth_cov2.ptr<double>(y)[x]),
                pt_curr,
                epipolar_direction
            );

            if (ret == false) // 匹配失败
                continue;

            // 取消该注释以显示匹配
            // showEpipolarMatch(ref, curr, Vector2d(x, y), pt_curr);

            // 匹配成功，更新深度图
            updateDepthFilter(Vector2d(x, y), pt_curr, T_C_R, epipolar_direction, depth, depth_cov2);
        }
    return true;
}

// 极线搜索
// 方法见书 12.2 12.3 两节
bool epipolarSearch(
    const Mat &ref, const Mat &curr,
    const SE3d &T_C_R, const Vector2d &pt_ref,
    const double &depth_mu, const double &depth_cov,
    Vector2d &pt_curr, Vector2d &epipolar_direction) {
    Vector3d f_ref = px2cam(pt_ref);
    f_ref.normalize();
    Vector3d P_ref = f_ref * depth_mu;    // 参考帧的 P 向量

    Vector2d px_mean_curr = cam2px(T_C_R * P_ref); // 按深度均值投影的像素
    double d_min = depth_mu - 3 * depth_cov, d_max = depth_mu + 3 * depth_cov;
    if (d_min < 0.1) d_min = 0.1;
    Vector2d px_min_curr = cam2px(T_C_R * (f_ref * d_min));    // 按最小深度投影的像素
    Vector2d px_max_curr = cam2px(T_C_R * (f_ref * d_max));    // 按最大深度投影的像素

    Vector2d epipolar_line = px_max_curr - px_min_curr;    // 极线（线段形式）
    epipolar_direction = epipolar_line;        // 极线方向
    epipolar_direction.normalize();
    double half_length = 0.5 * epipolar_line.norm();    // 极线线段的半长度
    if (half_length > 100) half_length = 100;   // 我们不希望搜索太多东西

    // 取消此句注释以显示极线（线段）
    // showEpipolarLine( ref, curr, pt_ref, px_min_curr, px_max_curr );

    // 在极线上搜索，以深度均值点为中心，左右各取半长度
    double best_ncc = -1.0;
    Vector2d best_px_curr;
    for (double l = -half_length; l <= half_length; l += 0.7) { // l+=sqrt(2)
        Vector2d px_curr = px_mean_curr + l * epipolar_direction;  // 待匹配点
        if (!inside(px_curr))
            continue;
        // 计算待匹配点与参考帧的 NCC
        double ncc = NCC(ref, curr, pt_ref, px_curr);
        if (ncc > best_ncc) {
            best_ncc = ncc;
            best_px_curr = px_curr;
        }
    }
    if (best_ncc < 0.85f)      // 只相信 NCC 很高的匹配
        return false;
    pt_curr = best_px_curr;
    return true;
}

double NCC(
    const Mat &ref, const Mat &curr,
    const Vector2d &pt_ref, const Vector2d &pt_curr) {
    // 零均值-归一化互相关
    // 先算均值
    double mean_ref = 0, mean_curr = 0;
    vector<double> values_ref, values_curr; // 参考帧和当前帧的均值
    for (int x = -ncc_window_size; x <= ncc_window_size; x++)
        for (int y = -ncc_window_size; y <= ncc_window_size; y++) {
            double value_ref = double(ref.ptr<uchar>(int(y + pt_ref(1, 0)))[int(x + pt_ref(0, 0))]) / 255.0;
            mean_ref += value_ref;

            double value_curr = getBilinearInterpolatedValue(curr, pt_curr + Vector2d(x, y));
            mean_curr += value_curr;

            values_ref.push_back(value_ref);
            values_curr.push_back(value_curr);
        }

    mean_ref /= ncc_area;
    mean_curr /= ncc_area;

    // 计算 Zero mean NCC
    double numerator = 0, demoniator1 = 0, demoniator2 = 0;
    for (int i = 0; i < values_ref.size(); i++) {
        double n = (values_ref[i] - mean_ref) * (values_curr[i] - mean_curr);
        numerator += n;
        demoniator1 += (values_ref[i] - mean_ref) * (values_ref[i] - mean_ref);
        demoniator2 += (values_curr[i] - mean_curr) * (values_curr[i] - mean_curr);
    }
    return numerator / sqrt(demoniator1 * demoniator2 + 1e-10);   // 防止分母出现零
}

bool updateDepthFilter(
    const Vector2d &pt_ref,
    const Vector2d &pt_curr,
    const SE3d &T_C_R,
    const Vector2d &epipolar_direction,
    Mat &depth,
    Mat &depth_cov2) {
    // 不知道这段还有没有人看
    // 用三角化计算深度
    SE3d T_R_C = T_C_R.inverse();
    Vector3d f_ref = px2cam(pt_ref);
    f_ref.normalize();
    Vector3d f_curr = px2cam(pt_curr);
    f_curr.normalize();

    // 方程
    // d_ref * f_ref = d_cur * ( R_RC * f_cur ) + t_RC
    // f2 = R_RC * f_cur
    // 转化成下面这个矩阵方程组
    // => [ f_ref^T f_ref, -f_ref^T f2 ] [d_ref]   [f_ref^T t]
    //    [ f_2^T f_ref, -f2^T f2    ] [d_cur] = [f2^T t   ]
    Vector3d t = T_R_C.translation();
    Vector3d f2 = T_R_C.so3() * f_curr;
    Vector2d b = Vector2d(t.dot(f_ref), t.dot(f2));
    Matrix2d A;
    A(0, 0) = f_ref.dot(f_ref);
    A(0, 1) = -f_ref.dot(f2);
    A(1, 0) = -A(0, 1);
    A(1, 1) = -f2.dot(f2);
    Vector2d ans = A.inverse() * b;
    Vector3d xm = ans[0] * f_ref;           // ref 侧的结果
    Vector3d xn = t + ans[1] * f2;          // cur 结果
    Vector3d p_esti = (xm + xn) / 2.0;      // P的位置，取两者的平均
    double depth_estimation = p_esti.norm();   // 深度值

    // 计算不确定性（以一个像素为误差）
    Vector3d p = f_ref * depth_estimation;
    Vector3d a = p - t;
    double t_norm = t.norm();
    double a_norm = a.norm();
    double alpha = acos(f_ref.dot(t) / t_norm);
    double beta = acos(-a.dot(t) / (a_norm * t_norm));
    Vector3d f_curr_prime = px2cam(pt_curr + epipolar_direction);
    f_curr_prime.normalize();
    double beta_prime = acos(f_curr_prime.dot(-t) / t_norm);
    double gamma = M_PI - alpha - beta_prime;
    double p_prime = t_norm * sin(beta_prime) / sin(gamma);
    double d_cov = p_prime - depth_estimation;
    double d_cov2 = d_cov * d_cov;

    // 高斯融合
    double mu = depth.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))];
    double sigma2 = depth_cov2.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))];

    double mu_fuse = (d_cov2 * mu + sigma2 * depth_estimation) / (sigma2 + d_cov2);
    double sigma_fuse2 = (sigma2 * d_cov2) / (sigma2 + d_cov2);

    depth.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = mu_fuse;
    depth_cov2.ptr<double>(int(pt_ref(1, 0)))[int(pt_ref(0, 0))] = sigma_fuse2;

    return true;
}

// 后面这些太简单我就不注释了（其实是因为懒）
void plotDepth(const Mat &depth_truth, const Mat &depth_estimate) {
    imshow("depth_truth", depth_truth * 0.4);
    imshow("depth_estimate", depth_estimate * 0.4);
    imshow("depth_error", depth_truth - depth_estimate);
    waitKey(1);
}

/**
 * 向屏幕中输出深度估计图和深度真值图之间的均方根误差
 * @param depth_truth 深度图真值
 * @param depth_estimate 深度图估计值
 */
void evaluateDepth(
        const Mat &depth_truth,
        const Mat &depth_estimate)
{
    double rmse = 0;
    int cnt_depth_data = 0;
    for(int y = boarder; y < depth_truth.rows - boarder; y++)
        for(int x = boarder; x < depth_truth.cols - boarder; x++){
            double error = depth_truth.ptr<double>(y)[x] - depth_estimate.ptr<double>(y)[x];
            rmse += error * error;
            cnt_depth_data++;
        }
    rmse = sqrt(rmse / cnt_depth_data);
    cout << "深度估计图和深度真值图之间的均方根误差为：" << rmse << "米！" << endl;
}

void showEpipolarMatch(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_curr) {
    Mat ref_show, curr_show;
    cv::cvtColor(ref, ref_show, CV_GRAY2BGR);
    cv::cvtColor(curr, curr_show, CV_GRAY2BGR);

    cv::circle(ref_show, cv::Point2f(px_ref(0, 0), px_ref(1, 0)), 5, cv::Scalar(0, 0, 250), 2);
    cv::circle(curr_show, cv::Point2f(px_curr(0, 0), px_curr(1, 0)), 5, cv::Scalar(0, 0, 250), 2);

    imshow("ref", ref_show);
    imshow("curr", curr_show);
    waitKey(1);
}

void showEpipolarLine(const Mat &ref, const Mat &curr, const Vector2d &px_ref, const Vector2d &px_min_curr,
                      const Vector2d &px_max_curr) {

    Mat ref_show, curr_show;
    cv::cvtColor(ref, ref_show, CV_GRAY2BGR);
    cv::cvtColor(curr, curr_show, CV_GRAY2BGR);

    cv::circle(ref_show, cv::Point2f(px_ref(0, 0), px_ref(1, 0)), 5, cv::Scalar(0, 255, 0), 2);
    cv::circle(curr_show, cv::Point2f(px_min_curr(0, 0), px_min_curr(1, 0)), 5, cv::Scalar(0, 255, 0), 2);
    cv::circle(curr_show, cv::Point2f(px_max_curr(0, 0), px_max_curr(1, 0)), 5, cv::Scalar(0, 255, 0), 2);
    cv::line(curr_show, Point2f(px_min_curr(0, 0), px_min_curr(1, 0)), Point2f(px_max_curr(0, 0), px_max_curr(1, 0)),
             Scalar(0, 255, 0), 1);

    imshow("ref", ref_show);
    imshow("curr", curr_show);
    waitKey(1);
}

  CMakeLists.txt文件内容为，

set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_CXX_FLAGS "-std=c++11 -march=native -O3")

include_directories("/usr/include/eigen3")

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})

find_package(Sophus REQUIRED)
include_directories(${Sophus_DIRECTORIES})

find_package(fmt)

set(LIBS ${OpenCV_LIBRARIES} ${Sophus_LIBRARIES})

add_executable(main dense_mapping.cpp)
target_link_libraries(main ${LIBS} fmt::fmt)

  程序运行结果为，

read total 200 files.
*** loop 1 ***
深度估计图和深度真值图之间的均方根误差为：1.35752米！
*** loop 2 ***
深度估计图和深度真值图之间的均方根误差为：1.19225米！
*** loop 3 ***
深度估计图和深度真值图之间的均方根误差为：1.02735米！
*** loop 4 ***
深度估计图和深度真值图之间的均方根误差为：0.937914米！
*** loop 5 ***
深度估计图和深度真值图之间的均方根误差为：0.674211米！
*** loop 6 ***
深度估计图和深度真值图之间的均方根误差为：0.630013米！
*** loop 7 ***
深度估计图和深度真值图之间的均方根误差为：0.603759米！
*** loop 8 ***
深度估计图和深度真值图之间的均方根误差为：0.591725米！
*** loop 9 ***
深度估计图和深度真值图之间的均方根误差为：0.583338米！
*** loop 10 ***
深度估计图和深度真值图之间的均方根误差为：0.575829米！
*** loop 11 ***
深度估计图和深度真值图之间的均方根误差为：0.570151米！
*** loop 12 ***
深度估计图和深度真值图之间的均方根误差为：0.564941米！
*** loop 13 ***
深度估计图和深度真值图之间的均方根误差为：0.561238米！
*** loop 14 ***
深度估计图和深度真值图之间的均方根误差为：0.557061米！
*** loop 15 ***
深度估计图和深度真值图之间的均方根误差为：0.554082米！
*** loop 16 ***
深度估计图和深度真值图之间的均方根误差为：0.551189米！
*** loop 17 ***
深度估计图和深度真值图之间的均方根误差为：0.549086米！
*** loop 18 ***
深度估计图和深度真值图之间的均方根误差为：0.547147米！
*** loop 19 ***
深度估计图和深度真值图之间的均方根误差为：0.545983米！
*** loop 20 ***
深度估计图和深度真值图之间的均方根误差为：0.545016米！
*** loop 21 ***
深度估计图和深度真值图之间的均方根误差为：0.544195米！
*** loop 22 ***
深度估计图和深度真值图之间的均方根误差为：0.5434米！
*** loop 23 ***
深度估计图和深度真值图之间的均方根误差为：0.542676米！
*** loop 24 ***
深度估计图和深度真值图之间的均方根误差为：0.54198米！
*** loop 25 ***
深度估计图和深度真值图之间的均方根误差为：0.541366米！
*** loop 26 ***
深度估计图和深度真值图之间的均方根误差为：0.540892米！
*** loop 27 ***
深度估计图和深度真值图之间的均方根误差为：0.540514米！
*** loop 28 ***
深度估计图和深度真值图之间的均方根误差为：0.540138米！
*** loop 29 ***
深度估计图和深度真值图之间的均方根误差为：0.539721米！
*** loop 30 ***
深度估计图和深度真值图之间的均方根误差为：0.539133米！
*** loop 31 ***
深度估计图和深度真值图之间的均方根误差为：0.538708米！
*** loop 32 ***
深度估计图和深度真值图之间的均方根误差为：0.538111米！
*** loop 33 ***
深度估计图和深度真值图之间的均方根误差为：0.537762米！
*** loop 34 ***
深度估计图和深度真值图之间的均方根误差为：0.53743米！
*** loop 35 ***
深度估计图和深度真值图之间的均方根误差为：0.536814米！
*** loop 36 ***
深度估计图和深度真值图之间的均方根误差为：0.536432米！
*** loop 37 ***
深度估计图和深度真值图之间的均方根误差为：0.536051米！
*** loop 38 ***
深度估计图和深度真值图之间的均方根误差为：0.535663米！
*** loop 39 ***
深度估计图和深度真值图之间的均方根误差为：0.535175米！
*** loop 40 ***
深度估计图和深度真值图之间的均方根误差为：0.534822米！
*** loop 41 ***
深度估计图和深度真值图之间的均方根误差为：0.534336米！
*** loop 42 ***
深度估计图和深度真值图之间的均方根误差为：0.533915米！
*** loop 43 ***
深度估计图和深度真值图之间的均方根误差为：0.533362米！
*** loop 44 ***
深度估计图和深度真值图之间的均方根误差为：0.532897米！
*** loop 45 ***
深度估计图和深度真值图之间的均方根误差为：0.532432米！
*** loop 46 ***
深度估计图和深度真值图之间的均方根误差为：0.531942米！
*** loop 47 ***
深度估计图和深度真值图之间的均方根误差为：0.531231米！
*** loop 48 ***
深度估计图和深度真值图之间的均方根误差为：0.530775米！
*** loop 49 ***
深度估计图和深度真值图之间的均方根误差为：0.53034米！
*** loop 50 ***
深度估计图和深度真值图之间的均方根误差为：0.529729米！
*** loop 51 ***
深度估计图和深度真值图之间的均方根误差为：0.529396米！
*** loop 52 ***
深度估计图和深度真值图之间的均方根误差为：0.528988米！
*** loop 53 ***
深度估计图和深度真值图之间的均方根误差为：0.528659米！
*** loop 54 ***
深度估计图和深度真值图之间的均方根误差为：0.528417米！
*** loop 55 ***
深度估计图和深度真值图之间的均方根误差为：0.528293米！
*** loop 56 ***
深度估计图和深度真值图之间的均方根误差为：0.528063米！
*** loop 57 ***
深度估计图和深度真值图之间的均方根误差为：0.528007米！
*** loop 58 ***
深度估计图和深度真值图之间的均方根误差为：0.527839米！
*** loop 59 ***
深度估计图和深度真值图之间的均方根误差为：0.527631米！
*** loop 60 ***
深度估计图和深度真值图之间的均方根误差为：0.527462米！
*** loop 61 ***
深度估计图和深度真值图之间的均方根误差为：0.527323米！
*** loop 62 ***
深度估计图和深度真值图之间的均方根误差为：0.52716米！
*** loop 63 ***
深度估计图和深度真值图之间的均方根误差为：0.526912米！
*** loop 64 ***
深度估计图和深度真值图之间的均方根误差为：0.526764米！
*** loop 65 ***
深度估计图和深度真值图之间的均方根误差为：0.526588米！
*** loop 66 ***
深度估计图和深度真值图之间的均方根误差为：0.526494米！
*** loop 67 ***
深度估计图和深度真值图之间的均方根误差为：0.52631米！
*** loop 68 ***
深度估计图和深度真值图之间的均方根误差为：0.526171米！
*** loop 69 ***
深度估计图和深度真值图之间的均方根误差为：0.526037米！
*** loop 70 ***
深度估计图和深度真值图之间的均方根误差为：0.525873米！
*** loop 71 ***
深度估计图和深度真值图之间的均方根误差为：0.525728米！
*** loop 72 ***
深度估计图和深度真值图之间的均方根误差为：0.525578米！
*** loop 73 ***
深度估计图和深度真值图之间的均方根误差为：0.525371米！
*** loop 74 ***
深度估计图和深度真值图之间的均方根误差为：0.525226米！
*** loop 75 ***
深度估计图和深度真值图之间的均方根误差为：0.525093米！
*** loop 76 ***
深度估计图和深度真值图之间的均方根误差为：0.524924米！
*** loop 77 ***
深度估计图和深度真值图之间的均方根误差为：0.524764米！
*** loop 78 ***
深度估计图和深度真值图之间的均方根误差为：0.524624米！
*** loop 79 ***
深度估计图和深度真值图之间的均方根误差为：0.524487米！
*** loop 80 ***
深度估计图和深度真值图之间的均方根误差为：0.524392米！
*** loop 81 ***
深度估计图和深度真值图之间的均方根误差为：0.524308米！
*** loop 82 ***
深度估计图和深度真值图之间的均方根误差为：0.524115米！
*** loop 83 ***
深度估计图和深度真值图之间的均方根误差为：0.523944米！
*** loop 84 ***
深度估计图和深度真值图之间的均方根误差为：0.52373米！
*** loop 85 ***
深度估计图和深度真值图之间的均方根误差为：0.523455米！
*** loop 86 ***
深度估计图和深度真值图之间的均方根误差为：0.52315米！
*** loop 87 ***
深度估计图和深度真值图之间的均方根误差为：0.522543米！
*** loop 88 ***
深度估计图和深度真值图之间的均方根误差为：0.521579米！
*** loop 89 ***
深度估计图和深度真值图之间的均方根误差为：0.520613米！
*** loop 90 ***
深度估计图和深度真值图之间的均方根误差为：0.519782米！
*** loop 91 ***
深度估计图和深度真值图之间的均方根误差为：0.518495米！
*** loop 92 ***
深度估计图和深度真值图之间的均方根误差为：0.518038米！
*** loop 93 ***
深度估计图和深度真值图之间的均方根误差为：0.517217米！
*** loop 94 ***
深度估计图和深度真值图之间的均方根误差为：0.516438米！
*** loop 95 ***
深度估计图和深度真值图之间的均方根误差为：0.515655米！
*** loop 96 ***
深度估计图和深度真值图之间的均方根误差为：0.514897米！
*** loop 97 ***
深度估计图和深度真值图之间的均方根误差为：0.514478米！
*** loop 98 ***
深度估计图和深度真值图之间的均方根误差为：0.513979米！
*** loop 99 ***
深度估计图和深度真值图之间的均方根误差为：0.513558米！
*** loop 100 ***
深度估计图和深度真值图之间的均方根误差为：0.513126米！
*** loop 101 ***
深度估计图和深度真值图之间的均方根误差为：0.512669米！
*** loop 102 ***
深度估计图和深度真值图之间的均方根误差为：0.512351米！
*** loop 103 ***
深度估计图和深度真值图之间的均方根误差为：0.511821米！
*** loop 104 ***
深度估计图和深度真值图之间的均方根误差为：0.511339米！
*** loop 105 ***
深度估计图和深度真值图之间的均方根误差为：0.511061米！
*** loop 106 ***
深度估计图和深度真值图之间的均方根误差为：0.510834米！
*** loop 107 ***
深度估计图和深度真值图之间的均方根误差为：0.510656米！
*** loop 108 ***
深度估计图和深度真值图之间的均方根误差为：0.510535米！
*** loop 109 ***
深度估计图和深度真值图之间的均方根误差为：0.510396米！
*** loop 110 ***
深度估计图和深度真值图之间的均方根误差为：0.510186米！
*** loop 111 ***
深度估计图和深度真值图之间的均方根误差为：0.510066米！
*** loop 112 ***
深度估计图和深度真值图之间的均方根误差为：0.509987米！
*** loop 113 ***
深度估计图和深度真值图之间的均方根误差为：0.509756米！
*** loop 114 ***
深度估计图和深度真值图之间的均方根误差为：0.509733米！
*** loop 115 ***
深度估计图和深度真值图之间的均方根误差为：0.509674米！
*** loop 116 ***
深度估计图和深度真值图之间的均方根误差为：0.509639米！
*** loop 117 ***
深度估计图和深度真值图之间的均方根误差为：0.509229米！
*** loop 118 ***
深度估计图和深度真值图之间的均方根误差为：0.509096米！
*** loop 119 ***
深度估计图和深度真值图之间的均方根误差为：0.50897米！
*** loop 120 ***
深度估计图和深度真值图之间的均方根误差为：0.508936米！
*** loop 121 ***
深度估计图和深度真值图之间的均方根误差为：0.508311米！
*** loop 122 ***
深度估计图和深度真值图之间的均方根误差为：0.508166米！
*** loop 123 ***
深度估计图和深度真值图之间的均方根误差为：0.508066米！
*** loop 124 ***
深度估计图和深度真值图之间的均方根误差为：0.508053米！
*** loop 125 ***
深度估计图和深度真值图之间的均方根误差为：0.508031米！
*** loop 126 ***
深度估计图和深度真值图之间的均方根误差为：0.508032米！
*** loop 127 ***
深度估计图和深度真值图之间的均方根误差为：0.508012米！
*** loop 128 ***
深度估计图和深度真值图之间的均方根误差为：0.507986米！
*** loop 129 ***
深度估计图和深度真值图之间的均方根误差为：0.507988米！
*** loop 130 ***
深度估计图和深度真值图之间的均方根误差为：0.507926米！
*** loop 131 ***
深度估计图和深度真值图之间的均方根误差为：0.507919米！
*** loop 132 ***
深度估计图和深度真值图之间的均方根误差为：0.507908米！
*** loop 133 ***
深度估计图和深度真值图之间的均方根误差为：0.507866米！
*** loop 134 ***
深度估计图和深度真值图之间的均方根误差为：0.507854米！
*** loop 135 ***
深度估计图和深度真值图之间的均方根误差为：0.507834米！
*** loop 136 ***
深度估计图和深度真值图之间的均方根误差为：0.507783米！
*** loop 137 ***
深度估计图和深度真值图之间的均方根误差为：0.507752米！
*** loop 138 ***
深度估计图和深度真值图之间的均方根误差为：0.507717米！
*** loop 139 ***
深度估计图和深度真值图之间的均方根误差为：0.507685米！
*** loop 140 ***
深度估计图和深度真值图之间的均方根误差为：0.507635米！
*** loop 141 ***
深度估计图和深度真值图之间的均方根误差为：0.507601米！
*** loop 142 ***
深度估计图和深度真值图之间的均方根误差为：0.507565米！
*** loop 143 ***
深度估计图和深度真值图之间的均方根误差为：0.50755米！
*** loop 144 ***
深度估计图和深度真值图之间的均方根误差为：0.507502米！
*** loop 145 ***
深度估计图和深度真值图之间的均方根误差为：0.507477米！
*** loop 146 ***
深度估计图和深度真值图之间的均方根误差为：0.507465米！
*** loop 147 ***
深度估计图和深度真值图之间的均方根误差为：0.507461米！
*** loop 148 ***
深度估计图和深度真值图之间的均方根误差为：0.507424米！
*** loop 149 ***
深度估计图和深度真值图之间的均方根误差为：0.507415米！
*** loop 150 ***
深度估计图和深度真值图之间的均方根误差为：0.507406米！
*** loop 151 ***
深度估计图和深度真值图之间的均方根误差为：0.507405米！
*** loop 152 ***
深度估计图和深度真值图之间的均方根误差为：0.507401米！
*** loop 153 ***
深度估计图和深度真值图之间的均方根误差为：0.5074米！
*** loop 154 ***
深度估计图和深度真值图之间的均方根误差为：0.507355米！
*** loop 155 ***
深度估计图和深度真值图之间的均方根误差为：0.5073米！
*** loop 156 ***
深度估计图和深度真值图之间的均方根误差为：0.50727米！
*** loop 157 ***
深度估计图和深度真值图之间的均方根误差为：0.507231米！
*** loop 158 ***
深度估计图和深度真值图之间的均方根误差为：0.507094米！
*** loop 159 ***
深度估计图和深度真值图之间的均方根误差为：0.507018米！
*** loop 160 ***
深度估计图和深度真值图之间的均方根误差为：0.506917米！
*** loop 161 ***
深度估计图和深度真值图之间的均方根误差为：0.506841米！
*** loop 162 ***
深度估计图和深度真值图之间的均方根误差为：0.506743米！
*** loop 163 ***
深度估计图和深度真值图之间的均方根误差为：0.506689米！
*** loop 164 ***
深度估计图和深度真值图之间的均方根误差为：0.50665米！
*** loop 165 ***
深度估计图和深度真值图之间的均方根误差为：0.506615米！
*** loop 166 ***
深度估计图和深度真值图之间的均方根误差为：0.506541米！
*** loop 167 ***
深度估计图和深度真值图之间的均方根误差为：0.506507米！
*** loop 168 ***
深度估计图和深度真值图之间的均方根误差为：0.506512米！
*** loop 169 ***
深度估计图和深度真值图之间的均方根误差为：0.506476米！
*** loop 170 ***
深度估计图和深度真值图之间的均方根误差为：0.506492米！
*** loop 171 ***
深度估计图和深度真值图之间的均方根误差为：0.506468米！
*** loop 172 ***
深度估计图和深度真值图之间的均方根误差为：0.50647米！
*** loop 173 ***
深度估计图和深度真值图之间的均方根误差为：0.506439米！
*** loop 174 ***
深度估计图和深度真值图之间的均方根误差为：0.506371米！
*** loop 175 ***
深度估计图和深度真值图之间的均方根误差为：0.506373米！
*** loop 176 ***
深度估计图和深度真值图之间的均方根误差为：0.506363米！
*** loop 177 ***
深度估计图和深度真值图之间的均方根误差为：0.506348米！
*** loop 178 ***
深度估计图和深度真值图之间的均方根误差为：0.506333米！
*** loop 179 ***
深度估计图和深度真值图之间的均方根误差为：0.506327米！
*** loop 180 ***
深度估计图和深度真值图之间的均方根误差为：0.506308米！
*** loop 181 ***
深度估计图和深度真值图之间的均方根误差为：0.506311米！
*** loop 182 ***
深度估计图和深度真值图之间的均方根误差为：0.506306米！
*** loop 183 ***
深度估计图和深度真值图之间的均方根误差为：0.506289米！
*** loop 184 ***
深度估计图和深度真值图之间的均方根误差为：0.506287米！
*** loop 185 ***
深度估计图和深度真值图之间的均方根误差为：0.506274米！
*** loop 186 ***
深度估计图和深度真值图之间的均方根误差为：0.506263米！
*** loop 187 ***
深度估计图和深度真值图之间的均方根误差为：0.506269米！
*** loop 188 ***
深度估计图和深度真值图之间的均方根误差为：0.506267米！
*** loop 189 ***
深度估计图和深度真值图之间的均方根误差为：0.506277米！
*** loop 190 ***
深度估计图和深度真值图之间的均方根误差为：0.506269米！
*** loop 191 ***
深度估计图和深度真值图之间的均方根误差为：0.506279米！
*** loop 192 ***
深度估计图和深度真值图之间的均方根误差为：0.506288米！
*** loop 193 ***
深度估计图和深度真值图之间的均方根误差为：0.506286米！
*** loop 194 ***
深度估计图和深度真值图之间的均方根误差为：0.506286米！
*** loop 195 ***
深度估计图和深度真值图之间的均方根误差为：0.506289米！
*** loop 196 ***
深度估计图和深度真值图之间的均方根误差为：0.506291米！
*** loop 197 ***
深度估计图和深度真值图之间的均方根误差为：0.506288米！
*** loop 198 ***
深度估计图和深度真值图之间的均方根误差为：0.50628米！
*** loop 199 ***
深度估计图和深度真值图之间的均方根误差为：0.506286米！
estimation returns, saving depth map ...
done.
执行程序耗时22.083分钟！

注其中updateDepthFilter()函数中的公式推导见我的上一篇博客。由程序输出结果可知，最终的深度估计图和深度真值图之间的均方根误差为0.506286米。第1张彩色图像的深度真值图和最终的深度估计图如下，
在这里插入图片描述在这里插入图片描述

2 点云地图

  给定5张彩色图和其对应的深度图信息，求这5张图的点云地图。
  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()
{
    vector<cv::Mat> colorImgs, depthImgs;  //彩色图和深度图
    vector<Eigen::Isometry3d> poses;

    ifstream fin("../data/pose.txt");
    if(!fin)
    {
        cerr << "不能打开文件pose.txt！" << 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];
        for(auto &d : data) fin >> d;
        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, cy = 239.5, fx = 481.2, fy = -480.0;
    double depthScale = 5000.0;
    cout << "正在将图像转换为点云..." << endl;

    //点云定义使用的格式：这里用的是XYZRGB
    typedef pcl::PointXYZRGB PointT;
    typedef pcl::PointCloud<PointT> PointCloud;

    //新建一个点云
    PointCloud::Ptr pointCloud(new PointCloud);  //创建了一个点云pointCloud
    for(int i = 0; i < 5; i++)
    {
        PointCloud::Ptr current(new PointCloud);  //创建了一个点云current
        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) / fx * point[2];
                point[1] = (v - cy) / fy * point[2];
                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);  //将点存入点云current中
            }
        //利用统计滤波器去除孤立点
        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;

    //利用体素网络滤波器进行降采样
    pcl::VoxelGrid<PointT> voxel_filter;
    double resolution = 0.03;
    voxel_filter.setLeafSize(resolution, resolution, resolution);  //每个0.03*0.03*0.03的格子里只存放一个点
    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;
}

  CMakeLists.txt内容为，

cmake_minimum_required(VERSION 2.10)

set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})

include_directories("/usr/include/eigen3")

find_package(PCL REQUIRED)
include_directories(${PCL_DIRECTORIES})

set(LIBS ${OpenCV_LIBRARIES} ${PCL_LIBRARIES})

add_executable(main main.cpp)
target_link_libraries(main ${LIBS})

  程序运行结果为，

正在将图像转换为点云...
转换图像1中...
转换图像2中...
转换图像3中...
转换图像4中...
转换图像5中...
点云中点的数目为1309800个！
经体素网络滤波之后，点云中点的数目为31876个！

生成map.pcd点云地图文件，利用pcl_viewer打开如下图，
在这里插入图片描述
演示视频见点云地图展示。

3 从点云重建网格

  利用2中已经建立好的点云地图map.pcd重建网格地图，先计算点云的法线，再从法线计算网格。
  cpp文件内容为，

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

#include <iostream>

using namespace std;

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)
{
    //创建一个搜索树
    pcl::search::KdTree<SurfelT>::Ptr tree(new pcl::search::KdTree<SurfelT>);
    tree->setInputCloud(surfels);

    //初始化对象
    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()
{
    //load the points
    PointCloudPtr cloud(new PointCloud);
    string path = "../map.pcd";
    if(pcl::io::loadPCDFile(path, *cloud))
    {
        cout << "未能加载点云！" << endl;
        return 1;
    }
    cout << "已加载点云，总共有" << cloud->points.size() << "个点！" << endl;

    //compute surface elements
    cout << "计算法线..." << 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();

    return 0;
}

  CMakeLists.txt文件内容为，

cmake_minimum_required(VERSION 2.10)

set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})

find_package(PCL REQUIRED)
include_directories(${PCL_DIRECTORIES})

add_executable(main main.cpp)
target_link_libraries(main ${OpenCV_LIBRARIES} ${PCL_LIBRARIES})

  程序运行结果为，

已加载点云，总共有31876个点！
计算法线...
computing mesh...
display mesh...

显示的网格图如下，
在这里插入图片描述
展示视频见网格地图。

4 八叉树地图

  给定5张彩色图和相应的深度图，同时知道它们的位姿真值，求其八叉树地图。
  cpp文件内容为，

#include <iostream>
#include <fstream>

using namespace std;

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

#include <octomap/octomap.h>

#include <Eigen/Geometry>
#include <boost/format.hpp>

int main()
{
    vector<cv::Mat> colorImgs, depthImgs;  //彩色图和深度图
    vector<Eigen::Isometry3d> poses;  //相机位姿

    ifstream fin("../data/pose.txt");
    if(!fin)
    {
        cerr << "不能找到位姿文件！" << 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(auto &d : data) fin >> d;
        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, cy = 239.5, fx = 481.2, 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;
}

  CMakeLists.txt文件内容为，

cmake_minimum_required(VERSION 2.10)

set(CMAKE_BUILD_TYPE Release)
set(CMAKE_CXX_FLAGS "-std=c++14 -O3")

include_directories("/usr/include/eigen3")

find_package(OpenCV REQUIRED)
include_directories(${OpenCV_DIRECTORIES})

find_package(PCL REQUIRED)
include_directories(${PCL_DIRECTORIES})

find_package(octomap REQUIRED)
include_directories(${OCTOMAP_DIRECTORIES})

set(LIBS ${OpenCV_LIBRARIES} ${PCL_LIBRARIES} ${OCTOMAP_LIBRARIES})

add_executable(main main.cpp)
target_link_libraries(main ${LIBS})

  程序执行结果为，

正在将图像转换为Octomap...
转换图像1中...
转换图像2中...
转换图像3中...
转换图像4中...
转换图像5中...
saving octomap...
Writing 1136665 nodes to output stream... done.

生成八叉树地图文件octomap.bt，利用octovis打开如下，
在这里插入图片描述
演示视频见八叉树地图展示。

YMWM_
关注
专栏目录
视觉SLAM十四讲学习笔记——第十二讲 建图
weixin_61294345的博客
11-27 4599
SLAM实际上是一种底层技术，往往用来给上层应用提供信息。在前文已实现的部分，我们最多得到的仅是一个稀疏的地图，在需要实现导航、避障、重建等应用时，稀疏地图很难提供足够的信息，需要考虑如何重建稠密地图，即建模所有看到过的部分。 根据使用相机的不同可以分为：“单目稠密重建”和“RGB-D稠密建图”（双目应该和RGBD类似，都可以主动测得或计算出图像深度）。其中RGB-D相机的稠密重建较为简单，最直观、简单的方法就是将RGB-D数据转换为点云地图，再结合相机位姿进行拼接，并对点...
视觉slam十四讲 pdf_视觉SLAM十四讲 第13讲 建图
weixin_39869197的博客
12-17 551
单幅图像中的像素，只能提供物体与相机程序屏幕的角度及物体采集到的亮度，而无法提供物体的距离（Range）。在稠密重建中，需要知道每一个像素点（或）大部分像素点的距离。大致解决方案：使用单目相机，通过移动相机之后进行三角化测量像素的距离。使用双目相机，利用左右目的视差计算像素的距离（多目原理相同）。使用RGB-D相机直接获得像素距离。前两种方式称为立体视觉（Stereo Vision），其中移动单目...
DenthDepth:深度估计(三维场景构建)——单目视觉挑战激光雷达
人工智能
12-03 1192
网上看到这个 MAXIEYE周圣砚：欲靠单目视觉挑战激光雷达 搜了一下，感觉应该就是这个工程，https://github.com/ialhashim/DenseDepth 宣传的检测效果 我网上搜一张图片，运行一下效果大致如下： 检测的准不准，自己判断了，我进行了一点点修改，只是把数据源从单张图片改为了从摄像头获取， 新建camera_test.py impor...
《视觉 SLAM 十四讲》V2 第 12 讲 建图
最新发布
weixin_46034116的博客
10-18 333
文章目录12.2 单目稠密 重建12.2.2 极线搜索 && 块匹配12.2.3 高斯分布的深度滤波器12.3 单目稠密重建 【Code】待改进12.3.4 图像间的变换12.4 RGB-D 稠密建图12.4.1 点云地图 【Code】查询OpenCV版本 opencv_version12.4.2 从点云 重建 网格 【Code】查看PCL 版本 aptitude show libpcl-dev12.4.3 八叉树地图(Octomap) 【灵活压缩、随时更新】1
SLAM高翔十四讲（十二）第十二讲 建图
shendaiyan的博客
07-27 424
SLAM被称为同时定位与建图，前面我们讨论的是定位问题，本讲我们主要介绍建图问题。关于地图的用途，大致可以归为以下几点：定位、导航、避障、重建、交互。稀疏地图主要用于定位，而稠密地图用于导航、避障、重建，我们本将重点讨论稠密地图。
SLAM第12讲 建图实践部分
weixin_46417419的博客
04-27 2215
1 实践：单目稠密重建 我们需要下载示例程序所需要的数据集http://rpg.ifi.uzh.ch/datasets/remode_test_data.zip。它提供了一架无人机采集到的单目俯视图像，共200张，同时提供了每张图像对应的位姿。下面我们通过这些数据估算第一帧图像每个像素对应的深度值，即进行单目稠密重建。 估计稠密深度的步骤： 假设所有像素的深度满足某个初始的高斯分布； 当新数据产生时，通过极线搜索和块匹配确定投影点位置； 根据几何关系计算三角化后的深度及不确定性； 将当前观测融合进上一次的
【slam十四讲第二版】【课本例题代码向】【第十二讲~建图】【实践:单目稠密重建】【RGBD-稠密建图】【从点云重建网格】【octomap的安装】【实践:八叉树地图】
qq_45954434的博客
08-04 1395
视觉SLAM十四讲 第二版》笔记及课后习题（第十二讲）本工程采用的数据集是使用REMODE的测试数据集。它提供了一架无人集采集的单目俯视图像，共有200张，同时提供了每张图像的真实位姿数据集自取:链接:提取码: rpow。.......................................
第十二讲-建图
weixin_43008154的博客
03-16 126
本章说   题头诗：   重走SLAM路，之前学的全忘光   今日重拾起，痛定思痛写总结   长征路漫漫，君当切实行   这一章的内容简单，但对各种建图手段的逻辑梳理比较重要，所以我绘制了一张图进行说明 讲内容 ...
SLAM14讲-ch12建图笔记
qq_37746927的博客
04-08 5185
SLAM14讲-ch12建图笔记
slam十四讲第二版ch12所需数据集
12-27
总之，通过《SLAM十四讲》第二版的第十二章，读者将深入了解SLAM算法如何利用数据集进行学习和验证。这个过程不仅涉及理论知识，还包括对真实世界数据的理解和处理，这对于任何希望在机器人导航、自动驾驶等领域工作...
SLAM十四讲(第二版)所有第三方库库.zip
03-31
《SLAM十四讲》是一本深入介绍SLAM理论与实践的书籍，第二版则包含了更多更新的内容和技术。这个压缩包文件提供了书中所涉及的一些第三方库，这些库在实现SLAM算法时扮演着至关重要的角色。 1. **Ceres Solver**：...
slam十四讲 ch13——remode dataset
10-20
《SLAM十四讲》是一本深入探讨这一主题的经典著作，书中第十三章主要讲解了单目稠密重建的技术。在这一章中，作者可能会介绍如何利用单个摄像头的数据来实现环境的三维重建，这是SLAM研究领域的一个重要挑战。 ...
SLAM十四讲【1-6章】PPT.rar
10-31
5. **滤波器理论**：第二到第四章可能会详细讲解不同的滤波器，如扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）和粒子滤波（PF），它们在SLAM中的应用以及各自的优缺点。 6. ** slam 算法实现**：早期的SLAM方法，...
第10讲 后端2
YMWM_的博客
05-29 1136
目录1 用g2o_viewer进行优化2 利用g2o默认的顶点和边进行优化3 利用自己定义的顶点和边进行优化 1 用g2o_viewer进行优化   打开终端，输入g2o_viewer sphere.g2o，得下图， 点击左下角Optimize按钮，执行优化，优化结果为， 2 利用g2o默认的顶点和边进行优化   cpp文件内容为， #include <iostream> #include <fstream> #include <string> #include &l
第9讲 后端1
YMWM_的博客
05-26 979
目录1 Ceres BA的书写1.1 源文件和头文件1.2 CMakeLists.txt文件1.3 执行结果 1 Ceres BA的书写 1.1 源文件和头文件 待更新 1.2 CMakeLists.txt文件   内容如下， find_package(Ceres REQUIRED) include_directories(${CERES_DIRECTORIES}) include_directories("/usr/include/eigen3") add_library(bal_common com
第5讲 相机与图像
YMWM_的博客
05-03 880
目录1 操作OpenCV图像 1 操作OpenCV图像   cpp文件内容如下， #include <iostream> #include <chrono> #include <ctime> using namespace std; #include <opencv2/core/core.hpp> #include <opencv2/highgui/highgui.hpp> using namespace cv; string path =
第3讲 三维空间刚体运动
YMWM_的博客
04-27 657
Eigen的和使用   cpp文件内容如下， 在这里插入代码片 更新中！！！
第6讲 非线性优化（偏实践）
YMWM_的博客
05-09 601
目录1 手写高斯牛顿法2 1 手写高斯牛顿法   对于如下测量方程， y=eax2+bx+c+w y=e^{ax^2+bx+c}+w y=eax2+bx+c+w 其中yyy表示对状态(a,b,c)T(a,b,c)^T(a,b,c)T的测量；www为噪声，它服从均值为0，方差为1的高斯分布。同时，有如下100个数据点， xdata[100]={0,0.01,0.02,…xi…，0.99} x_{data}[100] = \{0, 0.01, 0.02, …x_i…， 0.99 \} xdata​[100]=
视觉slam十四讲 第二版 pdf
06-23
《视觉SLAM十四讲》第二版PDF是一本关于视觉定位与地图构建技术的经典教材，由国内知名专家学者合作撰写而成。本书系统地介绍了基于视觉传感器的SLAM技术的各个方面，包括相机模型、图像特征提取、初始姿态估计、...
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

YMWM_

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值