本次博文介绍固定点拼接点云，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|
