计算机视觉
图像处理

PCL室内三维重建2

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

使用方法:

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

2.ICP算法,精确配准

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

正视图

俯视图

代码:

#include 
#include 
#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 PointCloud;
    typedef pcl::PointCloud SurfaceNormals;
    typedef pcl::PointCloud LocalFeatures;
    typedef pcl::search::KdTree 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 vox_grid;
		vox_grid.setInputCloud (xyz_);
		vox_grid.setLeafSize (voxel_grid_size, voxel_grid_size, voxel_grid_size);
		vox_grid.filter (*gridsample);

		pcl::StatisticalOutlierRemoval sor;
		sor.setInputCloud(gridsample);
		sor.setMeanK(50);
		sor.setStddevMulThresh(1.0);
		sor.filter(*tempCloud);
		cout<<"cloud size before filtering:"<size()<<endl;
		cout<<"cloud size after filtering:"<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 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::ConstPtr source_cloud)
    {
		icp.setInputCloud(source_cloud);
    }

    void setTargetCloud (PointCloud::ConstPtr target_cloud)
    {
		icp.setInputTarget(target_cloud);
    }
    
	// Align the given template cloud to the target specified by setTargetCloud ()
    void align (PointCloud &temp)
    {
      
      pcl::PointCloud 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 object_templates;
	std::stringstream ss;
    TemplateAlignment my_alignment;
	MyICP my_icp;
	Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform, pairTransform2;
	PointCloud::Ptr result (new PointCloud);
	PointCloud::Ptr my_cloud (new PointCloud);
	PointCloud::Ptr Color_in (new PointCloud);
    PointCloud::Ptr Color_out (new PointCloud);
	PointCloud Final_Color;
	PointCloud::Ptr temp (new PointCloud);
	PointCloud temp2;
	
	ss.str("");
	ss<<"color_"<<begin<<".pcd";
	if(io::loadPCDFile(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(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|

转载注明来源:CV视觉网 » PCL室内三维重建2

分享到:更多 ()
扫描二维码,给作者 打赏
pay_weixinpay_weixin

请选择你看完该文章的感受:

0不错 0超赞 0无聊 0扯淡 1不解 0路过

评论 3

评论前必须登录!