目录
本文的目的是通过TUM数据集构建一个实时变化的动态点云。
构建点云要用到PCL库
本文的TUM数据集下载的是fr1xyz
pcl::PointCloud()函数，
pcl::PointXYZRGB类型(pcl::PointXYZRGB定义的量由六个参数组成，前三个是世界坐标，后三个是rgb)
pcl::visualization::CloudViewer viewer ("Viewer")函数，可视化
实现难点
1. 稠密重建的原理是什么
2. 怎么实现rgb图、深度图、位姿图的对齐
3. 相机的内参是什么
4. 怎么实现实时图片和点云图的显示
1. 稠密重建的原理是什么
目的：得到世界坐标并染色。
A. 首先遍历深度图的所有像素，将2维点装换到3维的空间坐标。再通过u=fx*X/Z+cx的变形。这里的Z是已知的，因此，通过这个可以得到相机坐标，再通过位姿的R、t变化得到世界坐标。
注意：v在数组里是纵坐标，u是横坐标。rgb三个对应。
pcl::PointXYZRGB定义的量由六个参数组成，前三个是世界坐标，后三个是rgb。因此实现了染色
B.本程序还用了三个滤波器来优化地图：
1. 统计滤波器去除孤立点
2. 深度滤波器去除深度无效的点
PointCloud::Ptr tmp(new PointCloud);
pcl::StatisticalOutlierRemoval<PointT> statistical_filter;
//设置参数
statistical_filter.setMeanK(50);
statistical_filter.setStddevMulThersh(1.0);
//设置输入点云
statistical_filter.setInputClould(current);
//开始滤波
statistical_filter.filter(*tmp);
//点云拼合
(*pointCloud)+=*tmp3. 体素滤波器去除一个小空间内重复的点。
pcl::VoxelGrid<PointT> voxel_filter;
double resolution = 0.03;
// resolution:分辨率
voxel_filter.setLeafSize(resolution, resolution, resolution);
PointCloud::Ptr tmp(new PointCloud);
//输入要滤波的点云
voxel_filter.setInputCloud(pointCloud);
//tmp为滤波输出
voxel_filter.filter(*tmp);
//复制到pointCloud
tmp->swap(*pointCloud);注意这里的resolution是分辨率，单位是米，可以改的。数值越大最后得到的点越少。
resolution=0.003,最清晰
resolution=0.03
resolution=0.3几乎看不见
2. 怎么实现rgb图、深度图、位姿图的对齐
需要用到一个python脚本。一次能够对两个.txt文件进行对齐，因此进行两次操作。
python associate.py rgb.txt depth.txt > associate.txtpython associate.py associate.txt groundtruth.txt > associations.txt得到的associations.txt就是我们要的文件
3. 相机的内参是什么
(图来自高翔博士的blog)
4. 怎么实现实时图片和点云图的显示
A.实现图片的显示
这个很简单，在读取循环图片时，加入
cv::imshow("img",colorImgs[i]);
cv::waitKey(30);//30s表示持续时间B.实现点云实时的显示
这个要用到pcl库的viewer，在读取循环图片时，加入
viewer.showCloud(pointCloud);可以在循环之后加入下面，使窗口不会直接关闭
while (!viewer.wasStopped ())
{
}总代码
本文使用了tum数据集的79帧。
也可以到笔者的GitHub上面下载dense4
#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>
#include <pcl/visualization/cloud_viewer.h>
int main(int argc, char **argv) {
int N=79;
vector<cv::Mat> colorImgs, depthImgs; // 彩色图和深度图
vector<Eigen::Isometry3d> poses; // 相机位姿
ifstream fin("../associations.txt");
if (!fin) {
cerr << "cannot find pose file" << endl;
return 1;
}
for (int i = 0; i < N; i++) {
string data[12] ;
for (int i = 0; i < 12; i++) {
fin >> data[i];
}
colorImgs.push_back(cv::imread("/home/wood/data_for_orbslam2/rgbd_dataset_freiburg1_xyz/"+data[1]));
depthImgs.push_back(cv::imread("/home/wood/data_for_orbslam2/rgbd_dataset_freiburg1_xyz/"+data[3], -1)); // 使用-1读取原始图像
double double_data[7];
for(int j=5;j<12;j++)
double_data[j-5]=atof(data[j].c_str());
Eigen::Quaterniond q(double_data[6], double_data[3], double_data[4], double_data[5]);
Eigen::Isometry3d T(q);
T.pretranslate(Eigen::Vector3d(double_data[0], double_data[1], double_data[2]));
poses.push_back(T);//位姿
}
// 计算点云并拼接
// 相机内参
double cx = 318.6;
double cy = 255.3;
double fx = 517.3;
double fy = 516.5;
double depthScale = 5000.0;
cout << "正在将图像转换为点云..." << endl;
// 定义点云使用的格式：这里用的是XYZRGB
typedef pcl::PointXYZRGB PointT;
typedef pcl::PointCloud<PointT> PointCloud;
// 新建一个点云
PointCloud::Ptr pointCloud(new PointCloud);
//viewer
pcl::visualization::CloudViewer viewer ("Viewer");
for (int i = 0; i < N; i++) {
PointCloud::Ptr current(new PointCloud);
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;//世界坐标
PointT p;//the fisrt 3 dimensions of p is directions and the next 3 is rgb
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);
}
// 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);
statistical_filter.filter(*tmp);
(*pointCloud) += *current;
cv::imshow("img",colorImgs[i]);
cv::waitKey(30);
viewer.showCloud(pointCloud);
}
while (!viewer.wasStopped ())
{
}
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);
voxel_filter.filter(*tmp);
tmp->swap(*pointCloud);
cout << "滤波之后，点云共有" << pointCloud->size() << "个点." << endl;
pcl::io::savePCDFileBinary("map.pcd", *pointCloud);
return 0;
}
效果图
4万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
