PCL 室内三维重建

简介: 手头有三个prime sensor摄像头,分别固定在不同角度,打算根据RGBD信息,将三个摄像头的点云数据拼接起来。设备限制+能力不足,一直没有把point cloud library 1.8环境搭建起来,因此无法实时读取点云信息。

手头有三个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);
}



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



换一个顺序也不对:




四、编译错误解决

代码写的是很简单,但是各种开源的东西凑到一起就蛋疼了,这不遇到了opencv中的flann模块与pcl中的flann模块的冲突!

给大家讲讲我是怎么发现的。。。

问题现象:


有个坑爹结构体,跑出来了!!咋办?

source insight导入point cloud library 所有源码,再把flann第三方所有头文件导入,关联后会找到这个坑爹结构体


然后看看有谁调用它,Flann.hpp


它位于pcl第三方目录下


到现在位置我觉得没啥问题,由于《学习教程》中没有引用flann.hpp头文件,我想尝试引用一下会不会解决问题呢,后来vs2012的智能关联功能,帮了我大忙!


由于本次代码没有用到opencv的第三方库,果断干掉opencv2这块头文件



万事大吉!



目录
相关文章
|
算法
PCL 室内三维重建II
本次博文介绍固定点拼接点云,kinect拍摄两幅画面,二者之间旋转10度,运行环境vs2012 pcl1.7.2 使用方法: 1.采样一致性初始配准算法SAC-IA,粗略估计初始变换矩阵 2.
1688 0
|
6月前
|
存储 编解码 安全
带三维重建和还原的PACS源码 医学影像PACS系统源码
带三维重建和还原的PACS源码 医学影像PACS系统源码 PACS及影像存取与传输系统”( Picture Archiving and Communication System),为以实现医学影像数字化存储、诊断为核心任务,从医学影像设备(如CT、CR、DR、MR、DSA、RF等)获取影像,集中存储、综合管理医学影像及病人相关信息,建立数字化工作流程。系统可实现检查预约、病人信息登记、计算机阅片、电子报告书写、胶片打印、数据备份等一系列满足影像科室日常工作的功能,并且由于影像数字化存储,用户可利用影像处理与测量技术辅助诊断、方便快捷地查找资料或利用网络将资料传输至临床科室,还可与医院HIS、L
88 0
|
6月前
|
存储 数据采集 固态存储
带三维重建和还原功能的医学影像管理系统(pacs)源码
带三维重建和还原功能的医学影像管理系统(pacs)源码
105 0
|
6月前
|
存储 数据可视化 vr&ar
突破传统 重新定义:3D医学影像PACS系统源码(包含RIS放射信息) 实现三维重建与还原
突破传统,重新定义PACS/RIS服务,洞察用户需求,关注应用场景,新一代PACS/RIS系统,系统顶层设计采用集中+分布式架构,满足医院影像全流程业务运行,同时各模块均可独立部署,满足医院未来影像信息化扩展新需求、感受新时代影像服务便捷性、易用性!系统基于平台化设计,与第三方服务自然接入无压力,从功能多样化到调阅速度快;覆盖(放射、超声、内镜、病理、核医学、心血管、临床科室等,是以影像采集、传输、存储、诊断、报告书写和科室管理)为核心应用的模块化PACS/RIS系统,实现了全院级影像信息的合理共享与应用。
120 0
突破传统 重新定义:3D医学影像PACS系统源码(包含RIS放射信息) 实现三维重建与还原
|
6月前
|
存储 数据采集 编解码
【PACS】医学影像管理系统源码带三维重建后处理技术
【PACS】医学影像管理系统源码带三维重建后处理技术
116 0
|
6月前
|
C++
【C++医学影像PACS】CT检查中的三维重建是什么检查?
【C++医学影像PACS】CT检查中的三维重建是什么检查?
163 0
|
存储 数据库 数据安全/隐私保护
基于C++开发,支持三维重建,多平面重建技术的医学影像PACS系统源码
支持非DICOM标准的影像设备的图像采集和处理。 3)支持各种扫描仪、数码相机等影像输入设备。 4)支持各大主流厂商的CT、MR、DSA、ECT、US、数字胃肠、内镜等影像设备; 5)支持所有的DICOM相机,支持各大厂家的激光相机。 6)系统完全支持HL7接口和ICD—10编码,可与HIS系统无缝连接。 7)提供全院级、科室级工作站以及远程会诊工作站,三维重建,多平面重建。
162 0
基于C++开发,支持三维重建,多平面重建技术的医学影像PACS系统源码
|
6月前
|
数据采集 存储 数据可视化
医院影像PACS系统三维重建技术(获取数据、预处理、重建)
开放式体系结构,完全符合DICOM3.0标准,提供HL7标准接口,可实现与提供相应标准接口的HIS系统以及其他医学信息系统间的数据通信。
231 3
|
6月前
|
存储 编解码 监控
【C++】医学影像PACS三维重建后处理系统源码
系统完全符合国际标准的DICOM3.0标准
76 2
|
6月前
|
存储
医院PACS系统全套源码 强大的三维重建功能
对非DICOM影像,如超声、病理、心电图等进行了集成,做到了可以同时处理DICOM标准图像和非DICOM图像。
57 1

热门文章

最新文章