您的位置：首页 > 其它

PCL 室内三维重建

2015-10-06 00:00 288 查看

手头有三个prime sensor摄像头，分别固定在不同角度，打算根据RGBD信息，将三个摄像头的点云数据拼接起来。
设备限制+能力不足，一直没有把point cloud library 1.8环境搭建起来，因此无法实时读取点云信息。此外，笔记本电脑USB芯片总线中断协议限制，亦无法同时使用三个摄像头。
在如此坑爹的境地，分享下我是怎么搞三维重建的。。。。

本文环境win7+vs2012+opencv2.4.9+openni2.0+pcl1.7.2

一、点云数据获取
1.采用openni2.0 采集depth和rgb数据
2.opencv实时显示(否则你不知道采集的是啥)，借助IplImage接口拷贝RGB数据给cloudpoint(直接cloudpoint之间赋值结果不对)
3.利用PCL的IO接口，将数据倒腾到PCD文件（PCD采用binary形式保存，ASCII性能实在太差）
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
// 标准库头文件
#include <iostream>
#include <string>
#include <vector>
// OpenCV头文件
#include <opencv2\core\core.hpp>
#include <opencv2\highgui\highgui.hpp>
#include <opencv2\imgproc\imgproc.hpp>
// OpenNI头文件
#include <OpenNI.h>
typedef unsigned char uint8_t;
// namespace
using namespace std;
using namespace openni;
using namespace cv;
using namespace pcl;

void CheckOpenNIError( Status result, string status )
{
if( result != STATUS_OK )
cerr << status << " Error: " << OpenNI::getExtendedError() << endl;
}

int main( int argc, char **argv )
{
Status result = STATUS_OK;
int i,j;
float x=0.0,y=0.0,z=0.0,xx=0.0;
//IplImage *test,*test2;
IplImage *test2;

//point cloud
PointCloud<PointXYZRGB> cloud;

//opencv image
Mat cvBGRImg;
Mat cvDepthImg;

//OpenNI2 image
VideoFrameRef oniDepthImg;
VideoFrameRef oniColorImg;

namedWindow("depth");
namedWindow("image");

char key=0;

// 初始化OpenNI
result = OpenNI::initialize();
CheckOpenNIError( result, "initialize context" );

// open device
Device device;
result = device.open( openni::ANY_DEVICE );
CheckOpenNIError( result, "open device" );

// create depth stream
VideoStream oniDepthStream;
result = oniDepthStream.create( device, openni::SENSOR_DEPTH );
CheckOpenNIError( result, "create depth stream" );

// set depth video mode
VideoMode modeDepth;
modeDepth.setResolution( 640, 480 );
modeDepth.setFps( 30 );
modeDepth.setPixelFormat( PIXEL_FORMAT_DEPTH_1_MM );
oniDepthStream.setVideoMode(modeDepth);
// start depth stream
result = oniDepthStream.start();
CheckOpenNIError( result, "start depth stream" );

// create color stream
VideoStream oniColorStream;
result = oniColorStream.create( device, openni::SENSOR_COLOR );
CheckOpenNIError( result, "create color stream" );
// set color video mode
VideoMode modeColor;
modeColor.setResolution( 640, 480 );
modeColor.setFps( 30 );
modeColor.setPixelFormat( PIXEL_FORMAT_RGB888 );
oniColorStream.setVideoMode( modeColor);
// start color stream
result = oniColorStream.start();
CheckOpenNIError( result, "start color stream" );

while(true)
{
// read frame
if( oniColorStream.readFrame( &oniColorImg ) == STATUS_OK )
{
// convert data into OpenCV type
Mat cvRGBImg( oniColorImg.getHeight(), oniColorImg.getWidth(), CV_8UC3, (void*)oniColorImg.getData() );
cvtColor( cvRGBImg, cvBGRImg, CV_RGB2BGR );
imshow( "image", cvBGRImg );
}

if( oniDepthStream.readFrame( &oniDepthImg ) == STATUS_OK )
{
Mat cvRawImg16U( oniDepthImg.getHeight(), oniDepthImg.getWidth(), CV_16UC1, (void*)oniDepthImg.getData() );
cvRawImg16U.convertTo( cvDepthImg, CV_8U, 255.0/(oniDepthStream.getMaxPixelValue()));
imshow( "depth", cvDepthImg );
}
// quit
if( cv::waitKey( 1 ) == 'q' )
break;
// capture depth and color data
if( cv::waitKey( 1 ) == 'c' )
{
//get data
DepthPixel *pDepth = (DepthPixel*)oniDepthImg.getData();
//create point cloud
cloud.width = oniDepthImg.getWidth();
cloud.height = oniDepthImg.getHeight();
cloud.is_dense = false;
cloud.points.resize(cloud.width * cloud.height);
//test = cvCreateImage(cvSize(cloud.width,cloud.height),IPL_DEPTH_8U,3);
test2 = &IplImage(cvBGRImg);

for(i=0;i<oniDepthImg.getHeight();i++)
{
for(j=0;j<oniDepthImg.getWidth();j++)
{
float k = i;
float m = j;
xx = pDepth[i*oniDepthImg.getWidth()+j];
CoordinateConverter::convertDepthToWorld (oniDepthStream,m,k,xx,&x,&y,&z);
cloud[i*cloud.width+j].x = x;
cloud[i*cloud.width+j].y = y;
cloud[i*cloud.width+j].z = xx;
cloud[i*cloud.width+j].b = (uint8_t)test2->imageData[i*test2->widthStep+j*3+0];
cloud[i*cloud.width+j].g = (uint8_t)test2->imageData[i*test2->widthStep+j*3+1];
cloud[i*cloud.width+j].r = (uint8_t)test2->imageData[i*test2->widthStep+j*3+2];
/* test->imageData[i*test->widthStep+j*3+0] = test2->imageData[i*test2->widthStep+j*3+0];
test->imageData[i*test->widthStep+j*3+1] = test2->imageData[i*test2->widthStep+j*3+1];
test->imageData[i*test->widthStep+j*3+2] = test2->imageData[i*test2->widthStep+j*3+2];*/
}
}

//cvSaveImage("test.jpg",test);
pcl::io::savePCDFileBinaryCompressed("test_pcdc.pcd",cloud);
cerr<<"Saved "<<cloud.points.size()<<" data points to test_pcd.pcd."<<endl;
imwrite("c_color.jpg",cvBGRImg);
imwrite("c_depth.jpg",cvDepthImg);
/*for(size_t i=0;i<cloud.points.size();++i)
cerr<<" "<<cloud.points[i].x<<" "<<cloud.points[i].y<<" "<<cloud.points[i].z<<endl;*/
}
}
}
按C键，获取点云信息，按P键退出
我们得到下面三组数据：

二、点云展示
这个没啥东西，直接读取PCD，调用show即可
#include <pcl/visualization/cloud_viewer.h>
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>

int main ()
{
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);

if(pcl::io::loadPCDFile<pcl::PointXYZRGB>("test_pcda.pcd",*cloud)==-1)//*打开点云文件
{
PCL_ERROR("Couldn't read file test_pcd.pcd\n");
return(-1);
}
std::cout<<"Loaded "<<cloud->width*cloud->height<<" data points from test_pcd.pcd with the following fields: "<<std::endl;

pcl::visualization::CloudViewer viewer("My First Cloud Viewer");
viewer.showCloud(cloud);
while(!viewer.wasStopped())
{

}
}
三组数据对应结果：

三、点云拼接
我直接拿原始数据让ICP处理，所以速度非常慢！关于ICP解释，摘录自《点云库PCL学习教程》
Iterative Closest Point，迭代最近点算法，对待拼接的2片点云，首先根据一定的准则确立对应点集P与Q，其中对应点对的个数为N。然后通过最小二乘法迭代计算最优的坐标变换，即旋转矩阵R和平移矢量t，使得误差函数最小。ICP算法计算简便直观且可使拼接具有较好的精度，但是算法的运行速度以及向全局最优的收敛性却在很大程度上依赖于给定的初始变换估计以及在迭代过程中对应关系的确立。各种粗拼接技术可为ICP算法提供较好的初始位置，所以迭代过程中确立正确对应点集以避免迭代陷入局部极值成为各种改进算法的关键，决定了算法的收敛速度与最终的拼接精度。
ICP处理流程：
1.对原始点云数据进行采样
2.确定初始对应点集
3.去除错误对应点对
4.坐标变换的求解
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <time.h>

int main (int argc, char** argv)
{
clock_t start,finish;
double totaltime;

pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_out2 (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr my_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
pcl::PointCloud<pcl::PointXYZRGB>::Ptr my_cloud2 (new pcl::PointCloud<pcl::PointXYZRGB>);

start=clock();
if(pcl::io::loadPCDFile<pcl::PointXYZRGB>("test_pcda.pcd",*cloud_in)==-1)//*打开点云文件
{
PCL_ERROR("Couldn't read file test_pcd.pcd\n");
return(-1);
}
finish=clock();
totaltime = (double)(finish-start)/CLOCKS_PER_SEC;
cout<<"\n load test_pcda.pcd data : "<<totaltime<<"seconds!"<<endl;

start=clock();
if(pcl::io::loadPCDFile<pcl::PointXYZRGB>("test_pcdb.pcd",*cloud_out)==-1)//*打开点云文件
{
PCL_ERROR("Couldn't read file test_pcd.pcd\n");
return(-1);
}
finish=clock();
totaltime = (double)(finish-start)/CLOCKS_PER_SEC;
cout<<"\n load test_pcdb.pcd data : "<<totaltime<<"seconds!"<<endl;

start=clock();
if(pcl::io::loadPCDFile<pcl::PointXYZRGB>("test_pcdc.pcd",*cloud_out2)==-1)//*打开点云文件
{
PCL_ERROR("Couldn't read file test_pcd.pcd\n");
return(-1);
}
finish=clock();
totaltime = (double)(finish-start)/CLOCKS_PER_SEC;
cout<<"\n load test_pcdc.pcd data : "<<totaltime<<"seconds!"<<endl;

//call icp api
start=clock();
pcl::IterativeClosestPoint<pcl::PointXYZRGB, pcl::PointXYZRGB> icp;
icp.setInputCloud(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud<pcl::PointXYZRGB> Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;

finish=clock();
totaltime = (double)(finish-start)/CLOCKS_PER_SEC;
cout<<"\n first time call icp process : "<<totaltime<<"seconds!"<<endl;

//构造拼接临时的点云
for(int i=0;i< Final.points.size();i++)
{
pcl::PointXYZRGB basic_point;
basic_point.x = Final.points[i].x;
basic_point.y = Final.points[i].y;
basic_point.z = Final.points[i].z;
basic_point.r = Final.points[i].r;
basic_point.g = Final.points[i].g;
basic_point.b = Final.points[i].b;
my_cloud->points.push_back(basic_point);
}

//call icp api another time
start=clock();
icp.setInputCloud(cloud_out2);
icp.setInputTarget(my_cloud);
pcl::PointCloud<pcl::PointXYZRGB> Final2;
icp.align(Final2);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
finish=clock();
totaltime = (double)(finish-start)/CLOCKS_PER_SEC;
cout<<"\n second time call icp process : "<<totaltime<<"seconds!"<<endl;

//my_cloud.reset();
//构造拼接最终的点云
for(int i=0;i< Final2.points.size();i++)
{
pcl::PointXYZRGB basic_point;
basic_point.x = Final2.points[i].x;
basic_point.y = Final2.points[i].y;
basic_point.z = Final2.points[i].z;
basic_point.r = Final2.points[i].r;
basic_point.g = Final2.points[i].g;
basic_point.b = Final2.points[i].b;
my_cloud2->points.push_back(basic_point);
}

pcl::visualization::CloudViewer viewer("My First Cloud Viewer");
viewer.showCloud(my_cloud2);
while(!viewer.wasStopped())
{

}
return (0);
}

运行结果好像不大对头，收敛失败了（三幅点云图像之间的交集过小所致。。）