PCL 室内三维重建II

简介: 本次博文介绍固定点拼接点云,kinect拍摄两幅画面,二者之间旋转10度,运行环境vs2012 pcl1.7.2使用方法:1.采样一致性初始配准算法SAC-IA,粗略估计初始变换矩阵2.

本次博文介绍固定点拼接点云,kinect拍摄两幅画面,二者之间旋转10度,运行环境vs2012 pcl1.7.2

使用方法:

1.采样一致性初始配准算法SAC-IA,粗略估计初始变换矩阵

2.ICP算法,精确配准

原始点云(拼接前,隔着10度)

正视图

俯视图



代码:

#include <iostream>
#include <vector>
#include <Eigen/Core>
#include <pcl/registration/icp.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/features/fpfh.h>
#include <pcl/registration/ia_ransac.h>
#include <pcl/visualization/cloud_viewer.h>

using namespace std;
using namespace pcl;

class FeatureCloud
{
  public:
    // A bit of shorthand
    typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
    typedef pcl::PointCloud<pcl::Normal> SurfaceNormals;
    typedef pcl::PointCloud<pcl::FPFHSignature33> LocalFeatures;
    typedef pcl::search::KdTree<pcl::PointXYZ> SearchMethod;

    FeatureCloud () :
      search_method_xyz_ (new SearchMethod),
      normal_radius_ (0.5f),
      feature_radius_ (0.5f),
      voxel_grid_size (0.07f)
    {
    	
    }

    ~FeatureCloud () {}

    // Process the given cloud
    void setInputCloud (PointCloud::Ptr xyz)
    {
      xyz_ = xyz;
      processInput ();
    }

    // Load and process the cloud in the given PCD file
    void loadInputCloud (const std::string &pcd_file)
    {
      xyz_ = PointCloud::Ptr (new PointCloud);
      pcl::io::loadPCDFile (pcd_file, *xyz_);
      processInput ();
    }

    // Get a pointer to the cloud 3D points
    PointCloud::Ptr getPointCloud () const
    {
      return (tempCloud);
    }

    // Get a pointer to the cloud of 3D surface normals
    SurfaceNormals::Ptr getSurfaceNormals () const
    {
      return (normals_);
    }

    // Get a pointer to the cloud of feature descriptors
    LocalFeatures::Ptr getLocalFeatures () const
    {
      return (features_);
    }

  protected:
    // Compute the surface normals and local features
    void processInput ()
    {
      mysample();
      computeSurfaceNormals ();
      computeLocalFeatures ();
    }

	void mysample()
	{
		gridsample = PointCloud::Ptr (new PointCloud);
		tempCloud = PointCloud::Ptr (new PointCloud);
		pcl::VoxelGrid<pcl::PointXYZ> vox_grid;
		vox_grid.setInputCloud (xyz_);
		vox_grid.setLeafSize (voxel_grid_size, voxel_grid_size, voxel_grid_size);
		vox_grid.filter (*gridsample);

		pcl::StatisticalOutlierRemoval<pcl::PointXYZ> sor;
		sor.setInputCloud(gridsample);
		sor.setMeanK(50);
		sor.setStddevMulThresh(1.0);
		sor.filter(*tempCloud);
		cout<<"cloud size before filtering:"<<xyz_->size()<<endl;
		cout<<"cloud size after filtering:"<<tempCloud->size()<<endl;
	}

    // Compute the surface normals
    void computeSurfaceNormals ()
    {
      normals_ = SurfaceNormals::Ptr (new SurfaceNormals);

      pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> norm_est;
      norm_est.setInputCloud (tempCloud);
      norm_est.setSearchMethod (search_method_xyz_);
      norm_est.setRadiusSearch (normal_radius_);
      norm_est.compute (*normals_);
    }

    // Compute the local feature descriptors
    void computeLocalFeatures ()
    {
      features_ = LocalFeatures::Ptr (new LocalFeatures);

      pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est;
      fpfh_est.setInputCloud (tempCloud);
      fpfh_est.setInputNormals (normals_);
      fpfh_est.setSearchMethod (search_method_xyz_);
      fpfh_est.setRadiusSearch (feature_radius_);
      fpfh_est.compute (*features_);
    }

  private:
    // Point cloud data
    PointCloud::Ptr xyz_;
	PointCloud::Ptr gridsample;
	PointCloud::Ptr tempCloud;
    SurfaceNormals::Ptr normals_;
    LocalFeatures::Ptr features_;
    SearchMethod::Ptr search_method_xyz_;

    // Parameters
    float normal_radius_;
    float feature_radius_;
	float voxel_grid_size;
};


class TemplateAlignment
{
  public:

    TemplateAlignment () :
      min_sample_distance_ (0.01f),
      max_correspondence_distance_ (0.01f*0.01f),
      nr_iterations_ (300)
    {
      // Intialize the parameters in the Sample Consensus Intial Alignment (SAC-IA) algorithm
      sac_ia_.setMinSampleDistance (min_sample_distance_);
      sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);
      sac_ia_.setMaximumIterations (nr_iterations_);
    }

    ~TemplateAlignment () {}

	void setSourceCloud(FeatureCloud &source_cloud)
    {
		sac_ia_.setInputCloud (source_cloud.getPointCloud ());
        sac_ia_.setSourceFeatures (source_cloud.getLocalFeatures ());
    }

    void setTargetCloud (FeatureCloud &target_cloud)
    {
      sac_ia_.setInputTarget (target_cloud.getPointCloud ());
      sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());
    }

    // Align the given template cloud to the target specified by setTargetCloud ()
    void align ()
    {
      
      pcl::PointCloud<pcl::PointXYZ> registration_output;
      sac_ia_.align (registration_output);

      fitness_score = (float) sac_ia_.getFitnessScore (max_correspondence_distance_);
      final_transformation = sac_ia_.getFinalTransformation ();
    }

	Eigen::Matrix4f getMatrix()
	{
		return final_transformation;
	}

	float getScore()
	{
		return fitness_score;
	}

  private:
    // The Sample Consensus Initial Alignment (SAC-IA) registration routine and its parameters
    pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia_;
    float min_sample_distance_;
	float fitness_score;
    float max_correspondence_distance_;
	Eigen::Matrix4f final_transformation;
    int nr_iterations_;
};

class MyICP
{
	public:

    MyICP ()
    {
       // Intialize the parameters in the ICP algorithm
       icp.setMaxCorrespondenceDistance(0.01);
       icp.setTransformationEpsilon(1e-7);
       icp.setEuclideanFitnessEpsilon(1);
       icp.setMaximumIterations(100);
    }

    ~MyICP () {}

	void setSourceCloud(PointCloud<PointXYZ>::ConstPtr source_cloud)
    {
		icp.setInputCloud(source_cloud);
    }

    void setTargetCloud (PointCloud<PointXYZ>::ConstPtr target_cloud)
    {
		icp.setInputTarget(target_cloud);
    }
    
	// Align the given template cloud to the target specified by setTargetCloud ()
    void align (PointCloud<PointXYZ> &temp)
    {
      
      pcl::PointCloud<pcl::PointXYZ> registration_output;
      icp.align (temp);

      fitness_score =  icp.getFitnessScore();
	  final_transformation = icp.getFinalTransformation ();
    }

	float getScore()
	{
		return fitness_score;
	}

	Eigen::Matrix4f getMatrix()
	{
		return final_transformation;
	}
  private:
    IterativeClosestPoint<PointXYZ, PointXYZ> icp;
	Eigen::Matrix4f final_transformation;
	float fitness_score;
};

int main (int argc, char **argv)
{
	int begin = 0;
	int end = 2;
    std::vector<FeatureCloud> object_templates;
	std::stringstream ss;
    TemplateAlignment my_alignment;
	MyICP my_icp;
	Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform, pairTransform2;
	PointCloud<PointXYZRGB>::Ptr result (new PointCloud<PointXYZRGB>);
	PointCloud<PointXYZRGB>::Ptr my_cloud (new PointCloud<PointXYZRGB>);
	PointCloud<PointXYZRGB>::Ptr Color_in (new PointCloud<PointXYZRGB>);
    PointCloud<PointXYZRGB>::Ptr Color_out (new PointCloud<PointXYZRGB>);
	PointCloud<PointXYZRGB> Final_Color;
	PointCloud<PointXYZ>::Ptr temp (new PointCloud<PointXYZ>);
	PointCloud<PointXYZ> temp2;
	
	ss.str("");
	ss<<"color_"<<begin<<".pcd";
	if(io::loadPCDFile<PointXYZRGB>(ss.str(),*Color_in)==-1)//*打开点云文件
    {
	   PCL_ERROR("Couldn't read file test_pcd.pcd\n");
	   return(-1);
    }

    //load data
	for(int j = begin;j < end;j++)
	{
	    std::stringstream ss;
	    ss << j << ".pcd";
	    FeatureCloud template_cloud;
		template_cloud.loadInputCloud (ss.str());
    	object_templates.push_back (template_cloud);
	}

	Final_Color = *Color_in;
	
	for (size_t i = begin + 1; i < begin + object_templates.size (); ++i)
  	{
		cout<<i<<endl;
		//cout<<"first size:"<<object_templates[i-1].getPointCloud()->size()<<", second size:"<<object_templates[i].getPointCloud()->size()<<endl;
    	my_alignment.setTargetCloud(object_templates[i-1-begin]);
		my_alignment.setSourceCloud(object_templates[i-begin]);
		my_alignment.align();
		cout<<"sac_ia fitness score:"<<my_alignment.getScore()<<endl;
		
		//update the global transform
		pairTransform = my_alignment.getMatrix();
		//print matrix
		printf ("\n");
		printf ("    | %6.3f %6.3f %6.3f %6.3f| \n", pairTransform (0,0), pairTransform (0,1), pairTransform (0,2), pairTransform (0,3));
		printf ("R = | %6.3f %6.3f %6.3f %6.3f| \n", pairTransform (1,0), pairTransform (1,1), pairTransform (1,2), pairTransform (1,3));
		printf ("    | %6.3f %6.3f %6.3f %6.3f| \n", pairTransform (2,0), pairTransform (2,1), pairTransform (2,2), pairTransform (2,3));
		printf ("    | %6.3f %6.3f %6.3f %6.3f| \n", pairTransform (3,0), pairTransform (3,1), pairTransform (3,2), pairTransform (3,3));
		GlobalTransform = GlobalTransform * pairTransform;
		//GlobalTransform =  pairTransform;
		
		//transform current pair into the global transform
		pcl::transformPointCloud (*object_templates[i-begin].getPointCloud(), *temp, GlobalTransform);

		my_icp.setSourceCloud(temp);
		my_icp.setTargetCloud(object_templates[i-1-begin].getPointCloud());
		my_icp.align(temp2);
		cout<<"icp fitness score:"<<my_icp.getScore()<<endl;
		pairTransform2 = my_icp.getMatrix();
		printf ("\n");
		printf ("    | %6.3f %6.3f %6.3f %6.3f| \n", pairTransform2 (0,0), pairTransform2 (0,1), pairTransform2 (0,2), pairTransform2 (0,3));
		printf ("R = | %6.3f %6.3f %6.3f %6.3f| \n", pairTransform2 (1,0), pairTransform2 (1,1), pairTransform2 (1,2), pairTransform2 (1,3));
		printf ("    | %6.3f %6.3f %6.3f %6.3f| \n", pairTransform2 (2,0), pairTransform2 (2,1), pairTransform2 (2,2), pairTransform2 (2,3));
		printf ("    | %6.3f %6.3f %6.3f %6.3f| \n", pairTransform2 (3,0), pairTransform2 (3,1), pairTransform2 (3,2), pairTransform2 (3,3));
		GlobalTransform = GlobalTransform * pairTransform2;

		ss.str("");
	    ss<<"color_"<<i<<".pcd";
	    if(pcl::io::loadPCDFile<pcl::PointXYZRGB>(ss.str(),*Color_out)==-1)//*打开点云彩色文件
	    {
		   PCL_ERROR("Couldn't read file test_pcd.pcd\n");
		   return(-1);
	    }
		//transform current pair into the global transform
		pcl::transformPointCloud (*Color_out, *result, GlobalTransform);
		Final_Color = Final_Color + *result;
    }

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

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

    }
    return (0);
}

结果如下,角度不见了~~





别高兴太早,这套算法如果这么牛逼,我也不用这么蛋疼了。如果用他拼接360度,必定失败,如果有用这个方法能搞定连续多幅图片拼接的朋友,请私信我。


下面是我用NDT方法,连续拼接90度的结果,只能这样了。。。

Filtered cloud contains 540
ndt fitness score:0.0227071

    |  0.985  0.007 -0.174  0.003|
R = | -0.007  1.000  0.002  0.000|
    |  0.174 -0.000  0.985 -0.006|
    |  0.000  0.000  0.000  1.000|
Filtered cloud contains 420
ndt fitness score:0.0343324

    |  0.989  0.040 -0.146  0.005|
R = | -0.037  0.999  0.021 -0.005|
    |  0.146 -0.015  0.989 -0.005|
    |  0.000  0.000  0.000  1.000|
Filtered cloud contains 552
ndt fitness score:0.0802134

    |  0.968 -0.016 -0.249  0.152|
R = |  0.021  1.000  0.016 -0.014|
    |  0.248 -0.020  0.969 -0.012|
    |  0.000  0.000  0.000  1.000|
Filtered cloud contains 926
ndt fitness score:0.0198928

    |  0.978 -0.015 -0.210  0.148|
R = |  0.019  1.000  0.017 -0.024|
    |  0.209 -0.020  0.978  0.016|
    |  0.000  0.000  0.000  1.000|
Filtered cloud contains 575
ndt fitness score:0.0492542

    |  0.962 -0.007 -0.273  0.085|
R = |  0.006  1.000 -0.001 -0.002|
    |  0.273 -0.000  0.962 -0.009|
    |  0.000  0.000  0.000  1.000|
Filtered cloud contains 412
ndt fitness score:0.00171811

    |  0.992 -0.024 -0.127  0.001|
R = |  0.023  1.000 -0.007 -0.000|
    |  0.127  0.004  0.992  0.003|
    |  0.000  0.000  0.000  1.000|
Filtered cloud contains 295
ndt fitness score:0.00152303

    |  0.983 -0.001 -0.182  0.086|
R = |  0.003  1.000  0.010  0.038|
    |  0.182 -0.011  0.983  0.090|
    |  0.000  0.000  0.000  1.000|
Filtered cloud contains 191
ndt fitness score:0.023204

    |  0.975 -0.080 -0.208  0.121|
R = |  0.092  0.995  0.047 -0.142|
    |  0.203 -0.065  0.977  0.103|
    |  0.000  0.000  0.000  1.000|
Filtered cloud contains 133
ndt fitness score:0.00556793

    |  0.983  0.003 -0.184  0.008|
R = | -0.004  1.000 -0.002  0.000|
    |  0.184  0.003  0.983  0.011|
    |  0.000  0.000  0.000  1.000|



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

热门文章

最新文章