AI 助理
备案控制台
开发者社区 人工智能 文章 正文

PCL 室内三维重建II

2015-11-02 1693
版权
版权声明：
本文内容由阿里云实名注册用户自发贡献，版权归原作者所有，阿里云开发者社区不拥有其著作权，亦不承担相应法律责任。具体规则请查看《 阿里云开发者社区用户服务协议》和 《阿里云开发者社区知识产权保护指引》。如果您发现本社区中有涉嫌抄袭的内容，填写 侵权投诉表单进行举报，一经查实，本社区将立刻删除涉嫌侵权内容。
简介： 本次博文介绍固定点拼接点云，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|



文章标签：
算法
关键词：
室内全息空间
taotao1233
目录
相关文章
taotao1233
|
算法 计算机视觉
PCL 室内三维重建
手头有三个prime sensor摄像头，分别固定在不同角度，打算根据RGBD信息，将三个摄像头的点云数据拼接起来。 设备限制+能力不足，一直没有把point cloud library 1.8环境搭建起来，因此无法实时读取点云信息。
taotao1233
2141 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）源码
源码星辰
107 0
源码世界
|
6月前
|
存储 数据可视化 vr&ar
突破传统 重新定义：3D医学影像PACS系统源码（包含RIS放射信息） 实现三维重建与还原
突破传统，重新定义PACS/RIS服务,洞察用户需求，关注应用场景，新一代PACS/RIS系统，系统顶层设计采用集中+分布式架构，满足医院影像全流程业务运行,同时各模块均可独立部署，满足医院未来影像信息化扩展新需求、感受新时代影像服务便捷性、易用性！系统基于平台化设计，与第三方服务自然接入无压力，从功能多样化到调阅速度快；覆盖（放射、超声、内镜、病理、核医学、心血管、临床科室等，是以影像采集、传输、存储、诊断、报告书写和科室管理）为核心应用的模块化PACS/RIS系统，实现了全院级影像信息的合理共享与应用。
源码世界
121 0
突破传统 重新定义：3D医学影像PACS系统源码（包含RIS放射信息） 实现三维重建与还原
源码星辰
|
6月前
|
存储 数据采集 编解码
【PACS】医学影像管理系统源码带三维重建后处理技术
【PACS】医学影像管理系统源码带三维重建后处理技术
源码星辰
117 0
源码星辰
|
6月前
|
C++
【C++医学影像PACS】CT检查中的三维重建是什么检查？
【C++医学影像PACS】CT检查中的三维重建是什么检查？
源码星辰
167 0
高端源码库
|
存储 数据库 数据安全/隐私保护
基于C++开发，支持三维重建，多平面重建技术的医学影像PACS系统源码
支持非DICOM标准的影像设备的图像采集和处理。 3）支持各种扫描仪、数码相机等影像输入设备。 4）支持各大主流厂商的CT、MR、DSA、ECT、US、数字胃肠、内镜等影像设备； 5）支持所有的DICOM相机，支持各大厂家的激光相机。 6）系统完全支持HL7接口和ICD—10编码，可与HIS系统无缝连接。 7）提供全院级、科室级工作站以及远程会诊工作站，三维重建，多平面重建。
高端源码库
164 0
基于C++开发，支持三维重建，多平面重建技术的医学影像PACS系统源码
源码星辰
|
6月前
|
数据采集 存储 数据可视化
医院影像PACS系统三维重建技术（获取数据、预处理、重建）
开放式体系结构，完全符合DICOM3.0标准，提供HL7标准接口，可实现与提供相应标准接口的HIS系统以及其他医学信息系统间的数据通信。
源码星辰
234 3
源码星辰
|
6月前
|
存储 编解码 监控
【C++】医学影像PACS三维重建后处理系统源码
系统完全符合国际标准的DICOM3.0标准
源码星辰
79 2
源码星辰
|
6月前
|
存储
医院PACS系统全套源码 强大的三维重建功能
对非DICOM影像，如超声、病理、心电图等进行了集成，做到了可以同时处理DICOM标准图像和非DICOM图像。
源码星辰
57 1

热门文章

最新文章

  • 1
    钻石价格预测的ML全流程！从模型构建调优道部署应用！⛵
  • 2
    分别用 VTK 体绘制和面绘制来实现医学图像三维重建
  • 3
    三维重建技术概述
  • 4
    国科大本科生连续在CVPR，AAAI发文，系统提出三维模型库变形分析方法
  • 5
    原型开发、模型构建和设计反馈在线工具
  • 6
    全面了解三维重建在建筑领域应用：多种技术思路、落地案例全都有
  • 7
    医院影像中心/放射科PACS系统源码 三维重建和还原
  • 8
    影像医学PACS系统源码带高级影像后处理功能（三维重建+还原）
  • 9
    英伟达开发最快 NeRf 技术：数秒内将 2D 照片合成为 3D 场景
  • 10
    Lesson 4.1 逻辑回归模型构建与多分类学习方法-1
  • 1
    医院影像PACS系统三维重建技术（获取数据、预处理、重建）
    234
  • 2
    【C++】医学影像PACS三维重建后处理系统源码
    79
  • 3
    【三维重建技术】C++医学影像处理系统源码
    94
  • 4
    基于Pix4Dmapper的三维模型重建应用——空间分析选址
    131
  • 5
    基于Pix4Dmapper的运动结构恢复无人机影像三维模型重建
    128
  • 6
    基于EinScan-S的编码结构光方法空间三维模型重建
    59
  • 7
    基于3DSOM的侧影轮廓方法空间三维模型重建
    79
  • 8
    医院PACS系统全套源码 强大的三维重建功能
    57
  • 9
    【C++】PACS医学图像存储和传输系统源码带三维重建
    93
  • 10
    医学影像系统源码（MRI、CT三维重建）
    93

    • 相关电子书

    更多
  • Redis&MongoDB的同步、迁移以及混合云场景构建
  • 低代码开发师（初级）实战教程
  • 阿里巴巴DevOps 最佳实践手册

    • 相关实验场景

    更多
  • Gazebo环境下基于ROS和OpenCV的阿克曼小车巡线
  • Gazebo环境下基于ROS和OpenCV的阿克曼小车识别路牌停车
    • 下一篇
    无影云桌面