PCL点云获取+拼接(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|
    
    

    
    

            </div>

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

  
  

            </div>