《视觉slam十四讲》ch5相机与图像学习笔记（3）——实践部分 RGB-D相机代码解释及相关函数介绍_poses.push_back

        在这篇博客中，主要介绍《视觉SLAM十四讲》第五讲的实践部分——RGB-D代码详解。关于imageBasics的代码可见我另一篇博客：

《视觉slam十四讲》ch5学习笔记（1）—— 实践部分imageBasics代码讲解_sticker_阮的博客-CSDN博客_视觉slam十四讲代码

        关于双目视觉的代码可见我另一篇博客：

《视觉slam十四讲》ch5 学习笔记（2）——实践部分双目视觉代码讲解_sticker_阮的博客-CSDN博客

1.代码详解

        代码框架：输入RGB-D图像——读取每个像素点的深度值——转化成真实世界下的深度——计算图像三维坐标——形成3D点云。

        代码流程图如下：

 源码加上详细的注释如下：

#include <iostream>
#include <fstream>
#include <opencv2/opencv.hpp>
#include <boost/format.hpp>  // for formating strings
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>
#include<chrono>
using namespace std;
typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;
typedef Eigen::Matrix<double, 6, 1> Vector6d;
 
// 在pangolin中画图，已写好，无需调整
void showPointCloud(
    const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud);
 
int main(int argc, char **argv) {
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    TrajectoryType poses;         // 相机位姿
 
    ifstream fin("/home/rxz/slambook2/ch5/rgbd/pose.txt"); //ifstream默认以输入方式打开文件，文件内容为5张图片的相机外参位姿，即平移向量加旋转四元数[x,y,z,qx,qy,qz,qw]
    if (!fin) {
        cerr << "请在有pose.txt的目录下运行此程序" << endl;
        return 1;
    }
 
    chrono::steady_clock::time_point t1=chrono::steady_clock::now();    
    for (int i = 0; i < 5; i++) {
            //创建格式化字符串对象
        boost::format fmt("../%s/%d.%s"); //图像文件格式
        colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str()));
        depthImgs.push_back(cv::imread((fmt % "depth" % (i + 1) % "pgm").str(), -1)); // 使用-1读取原始图像
 
        double data[7] = {0};
      
        for (auto &d:data)//基于范围的for循环，auto表示自动类型推导
            fin >> d; //之前是采用输入的方式获得pose.txt文件，在这里将之前获取到的pose文件中的相机位姿输入给d
        Sophus::SE3d pose(Eigen::Quaterniond(data[6], data[3], data[4], data[5]),  //四元数
                          Eigen::Vector3d(data[0], data[1], data[2]));             //平移向量 三维的
        poses.push_back(pose);
    }
 
    // 计算点云并拼接
    // 相机内参 
    double cx = 325.5;
    double cy = 253.5;
    double fx = 518.0;
    double fy = 519.0;
    double depthScale = 1000.0;
    vector<Vector6d, Eigen::aligned_allocator<Vector6d>> pointcloud;
    pointcloud.reserve(1000000); //重新调整pointcloud容器大小，使其能容纳1000000个元素
 
    for (int i = 0; i < 5; i++) {
        cout << "转换图像中: " << i + 1 << endl;
        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        Sophus::SE3d 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]; // 读取第v行第u列元素的深度值
                if (d == 0) continue; // 为0表示没有测量到，continue表示跳出本次循环，继续下次循环
                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;  //将相机位姿与像素点3D坐标相乘
 
                Vector6d p;//前三维表示点云的位置，后三维表示点云的颜色
                p.head<3>() = pointWorld;//head<n>()函数是对于Eigen库中的向量类型而言的，表示提取前n个元素
                //opencv中图像的data数组表示把其颜色信息按行优先的方式展成的一维数组！
                //color.step等价于color.cols
                //color.channels()表示图像的通道数
 
                p[5] = color.data[v * color.step + u * color.channels()];   // blue
                p[4] = color.data[v * color.step + u * color.channels() + 1]; // green
                p[3] = color.data[v * color.step + u * color.channels() + 2]; // red
                pointcloud.push_back(p);
            }
    }
 
    cout << "点云共有" << pointcloud.size() << "个点." << endl;
    showPointCloud(pointcloud);
    chrono::steady_clock::time_point t2=chrono::steady_clock::now();
    chrono::duration<double>time_used=chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout<<"used time by this project: "<<time_used.count()<<"second!"<<endl;
    return 0;
}
 
void showPointCloud(const vector<Vector6d, Eigen::aligned_allocator<Vector6d>> &pointcloud) {
 
    if (pointcloud.empty()) {
        cerr << "Point cloud is empty!" << endl;
        return;
    }
 
    pangolin::CreateWindowAndBind("Point Cloud Viewer", 1024, 768);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
 //ProjectionMatrix()中各参数依次为图像宽度=1024、图像高度=768、fx=500、fy=500、cx=512、cy=389、最近距离=0.1和最远距离=1000
   //ModelViewLookAt()中各参数为相机位置，被观察点位置和相机哪个轴朝上
   //比如，ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)表示相机在(0, -0.1, -1.8)位置处观看视点(0, 0, 0)，并设置相机XYZ轴正方向为（0，-1，0），即右上前
 
   //创建交互视图，显示上一帧图像内容
 
    pangolin::OpenGlRenderState s_cam(
        pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
        pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
    );
 
    pangolin::View &d_cam = pangolin::CreateDisplay()
        .SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
        .SetHandler(new pangolin::Handler3D(s_cam));
   //SetBounds()内的前4个参数分别表示交互视图的大小，均为相对值，范围在0.0至1.0之间
   //第1个参数表示bottom，即为视图最下面在整个窗口中的位置
   //第2个参数为top，即为视图最上面在整个窗口中的位置
   //第3个参数为left，即视图最左边在整个窗口中的位置
   //第4个参数为right，即为视图最右边在整个窗口中的位置
   //第5个参数为aspect，表示横纵比
 
    while (pangolin::ShouldQuit() == false) {//如果Pangolin窗口没关闭
        glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);//清空颜色和深度缓存，使得前后帧不会互相干扰
 
        d_cam.Activate(s_cam); //激活相机
        glClearColor(1.0f, 1.0f, 1.0f, 1.0f);//设置背景颜色为白色
 
        glPointSize(2);
        glBegin(GL_POINTS);//绘制点云
        for (auto &p: pointcloud) {
            glColor3d(p[3] / 255.0, p[4] / 255.0, p[5] / 255.0);//设置颜色信息
            glVertex3d(p[0], p[1], p[2]);//设置位置信息
        }
        glEnd();
        pangolin::FinishFrame();//按照上面的设置执行渲染
        usleep(5000);   // sleep 5 ms
    }
    return;
}

2.相关函数解释：

（1）boost::forma

        c++语言中传统的格式数是c语言的sprintf函数，缺点是不安全，在stl中引入了stringstream, 它虽然解决了安全性的问题，但是没有sprintf函数直观。boost::format是用来替代sprintf，格式化字符串的，既具有安全性，又具有直观性。

作用：批量存储文件路径

       在本代码中是以以下形式出现的：

boost::format fmt("../ %s / %d.%s"); //图像文件格式

colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str()));

depthImgs.push_back(cv::imread((fmt % "depth" % (i + 1) % "pgm").str(), -1));

        其中，先按照第一句代码规定图像的文件格式，其中最开始的 ../ 表示图像存储的相对路径，后面的 %s，%d表示类型。

        随后第二句的意思为：按照第一句规定的格式，读取图像，其中“color”为存储5张彩色图的文件夹（即路径），之后的i+1表示第几张图片，后面的"png"表示图片的后缀名。总的而言，就是把“color”文件中的5张彩色图依次存放在colorImgs这个容器内。

        第三句代码含义同第二句。

(2). return 0 return 1 区别

return 0 代表程序正常退出，return 1代表程序异常退出

(3).cerr指令

       cerr为流对象，为标准错误流，常用于错误语句的输出。具体含义见下表：

(4).auto

for (auto &d:data)  //其中data为一个容器，效果是利用d遍历并获得data容器中的每一个值

fin >> d;

        使用auto，不用声明变量类型，直接复制数组元素类型。
注：for(auto iter:vec)不改变迭代对象的值，for(auto &iter:vec)可以改变迭代对象的值
两者都可以获取到迭代容器中的值，但是使用auto iter时不会对容器对象造成改变，而使用auto &iter,对于iter的任何修改将直接修改容器内对应的值。
上述两句代码的意思：总而言之，就是从pose.txt文件中读取相机位姿，并输入存储在data[]容器中。

3.效果图：

参考链接;视觉SLAM十四讲CH5代码解析及课后习题详解_长沙有肥鱼的博客-CSDN博客_slam十四讲代码