RGB-D稠密地图

#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; // 相机位姿
、、读取.txt文件的位姿到poses, 彩色图和深度图到colorImgs, depthImgs
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);存放5张图片的点云信息
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; //利用T转换到世界坐标系
、、
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§;
}
// 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);要进行滤波的点云对象是current
statistical_filter.filter(*tmp);滤波的结果放入tmp中
(*pointCloud) += *tmp;将tmp结果转移到pointCloud中
}
、、
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);要进行滤波的点云对象是pointCloud
voxel_filter.filter(*tmp);滤波的结果放入tmp中
tmp->swap(*pointCloud);将tmp结果转移到pointCloud中
//
cout << “滤波之后，点云共有” << pointCloud->size() << “个点.” << endl;
//保存为map.pcd点云文件，可以使用pcl_viewer工具打开
pcl::io::savePCDFileBinary(“map.pcd”, *pointCloud);
return 0;
}