#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; // 相机位姿
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" ); // 图像文件格式./data/color(%S)/1(%d).png(%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}; // 一共7个,0-6
for ( int i=0; i<7; i++ )
{
fin>>data[i];
}
Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );// 四元数【6】实部
Eigen::Isometry3d T(q);// 四元数(4*1)到旋转矩阵(4*4)
T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
poses.push_back( T );
}
// 计算点云并拼接
// 相机内参
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;
double depthScale = 1000.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 );
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表示没有测量到
if ( d >= 7000 ) continue; // 深度太大时不稳定,去掉
Eigen::Vector3d point;
point[2] = double(d)/depthScale; //z 相机坐标系
point[0] = (u-cx)*point[2]/fx;//x
point[1] = (v-cy)*point[2]/fy; //y
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 );
}
// 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) += *tmp;
}
pointCloud->is_dense = false;
cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
// voxel filter
pcl::VoxelGrid<PointT> voxel_filter; // 降采样滤波器
voxel_filter.setLeafSize( 0.01, 0.01, 0.01 ); // resolution // 设置滤波时创建的体素大小为 1cm 立方体
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;
}
#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; // 相机位姿
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" ); // 图像文件格式./data/color(%S)/1(%d).png(%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}; // 一共7个,0-6
for ( int i=0; i<7; i++ )
{
fin>>data[i];
}
Eigen::Quaterniond q( data[6], data[3], data[4], data[5] );// 四元数【6】实部
Eigen::Isometry3d T(q);// 四元数(4*1)到旋转矩阵(4*4)
T.pretranslate( Eigen::Vector3d( data[0], data[1], data[2] ));
poses.push_back( T );
}
// 计算点云并拼接
// 相机内参
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;
double depthScale = 1000.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 );
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表示没有测量到
if ( d >= 7000 ) continue; // 深度太大时不稳定,去掉
Eigen::Vector3d point;
point[2] = double(d)/depthScale; //z 相机坐标系
point[0] = (u-cx)*point[2]/fx;//x
point[1] = (v-cy)*point[2]/fy; //y
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 );
}
// 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) += *tmp;
}
pointCloud->is_dense = false;
cout<<"点云共有"<<pointCloud->size()<<"个点."<<endl;
// voxel filter
pcl::VoxelGrid<PointT> voxel_filter; // 降采样滤波器
voxel_filter.setLeafSize( 0.01, 0.01, 0.01 ); // resolution // 设置滤波时创建的体素大小为 1cm 立方体
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;
}