In this blog, I mainly introduce the practical part of the fifth lecture of "Visual SLAM Fourteen Lectures"-the detailed explanation of RGB-D code. The code about imageBasics can be seen in my other blog:
The code about binocular vision can be seen in my other blog:
1. Detailed code explanation
Code framework : input RGB-D image - read the depth value of each pixel - convert it into the depth in the real world - calculate the three-dimensional coordinates of the image - form a 3D point cloud.
The code flow chart is as follows:
The source code plus detailed comments are as follows:
#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. Explanation of related functions:
（1）boost::forma
The traditional format number in c++ language is the sprintf function of c language. The disadvantage is that it is not safe. Stringstream is introduced in stl. Although it solves the problem of security, it is not as intuitive as the sprintf function. boost::format is used to replace sprintf and format strings, which is both safe and intuitive.
Function: Batch storage file path
In this code, it appears in the following form:
boost::format fmt("../ %s / %d.%s"); //image file format
colorImgs.push_back(cv::imread((fmt % "color" % (i + 1) % "png").str()));
depthImgs.push_back(cv::imread((fmt % "depth" % (i + 1) % "pgm").str(), -1));
Among them, first specify the file format of the image according to the first sentence of code, where the first ../ indicates the relative path of the image storage, and the following %s and %d indicate the type.
The meaning of the second sentence that follows is: read the image according to the format specified in the first sentence, where "color" is the folder (ie path) that stores 5 color images, and the subsequent i+1 indicates the number of images, and the following The "png" indicates the suffix name of the picture. In general, the five color images in the "color" file are stored in the colorImgs container in sequence.
The meaning of the third sentence code is the same as that of the second sentence.
(2). return 0 return 1 difference
return 0 means the program exits normally, return 1 means the program exits abnormally
(3). cerr command
cerr is a stream object, which is a standard error stream, and is often used to output error statements. See the table below for specific meanings:
| global stream object | name | cache |
|---|---|---|
cout |
standard output stream | with cache |
cin |
standard input stream | with cache |
clog |
standard log stream | with cache |
cerr |
standard error stream | no cache |
(4).auto
for (auto &d:data) // where data is a container, the effect is to use d to traverse and obtain each value in the data container
fin >> d;
Use auto, without declaring the variable type, directly copy the array element type.
Note : for (auto iter: vec) does not change the value of the iterable object, for (auto &iter: vec) can change the value of the iterative object, both can
get the value in the iterative container, but when using auto iter, the container will not be changed The object causes changes, and using auto &iter, any modification to iter will directly modify the corresponding value in the container.
The meaning of the above two codes : In a word, it is to read the camera pose from the pose.txt file and store it in the data[] container.
3. Effect picture:
