多帧点云数据拼接合并_点云拼接算法
/*\author Radu Bogdan Rusu
* adaptation Raphael
Favier*/#include#include#include#include#include#include#include#include#include#include#include#include
usingpcl::visualization::PointCloudColorHandlerGenericField;usingpcl::visualization::PointCloudColorHandlerCustom;//简单类型定义
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloudPointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloudPointCloudWithNormals;//这是⼀个辅助教程，因此我们可以负担全局变量//创建可视化⼯具
pcl::visualization::PCLVisualizer *p;//定义左右视点
intvp_1, vp_2;//处理点云的⽅便的结构定义
structPCD
{
PointCloud::Ptr cloud;
std::stringf_name;
PCD() : cloud (newPointCloud) {};
};structPCDComparator
{bool operator () (const PCD& p1, const PCD&p2)
{return (p1.f_name
}
};//以< x, y, z, curvature >形式定义⼀个新的点
class MyPointReprentation : public pcl::PointReprentation {using pcl::PointReprentation::nr_dimensions_;public:
MyPointReprentation ()
{//定义尺⼨值
nr_dimensions_ = 4;
}//覆盖copyToFloatArray⽅法来定义我们的特征⽮量
virtual void copyToFloatArray (const PointNormalT &p, float * out) const{//< x, y, z, curvature >
out[0] =p.x;out[1] =p.y;out[2] =p.z;out[3] =p.curvature;
}
};
/** 在可视化窗⼝的第⼀视点显⽰源点云和⽬标点云
**/
void showCloudsLeft(const PointCloud::Ptr cloud_target, constPointCloud::Ptr cloud_source)
{
p->removePointCloud ("vp1_target");
p->removePointCloud ("vp1_source");
PointCloudColorHandlerCustom tgt_h (cloud_target, 0, 255, 0);
PointCloudColorHandlerCustom src_h (cloud_source, 255, 0, 0);
p->addPointCloud (cloud_target, tgt_h, "vp1_target", vp_1);
p->addPointCloud (cloud_source, src_h, "vp1_source", vp_1);
PCL_INFO ("Press q to begin the registration.\n");
p->spin();
}
/
**在可视化窗⼝的第⼆视点显⽰源点云和⽬标点云
**/
void showCloudsRight(const PointCloudWithNormals::Ptr cloud_target, constPointCloudWithNormals::Ptr cloud_source) {
p->removePointCloud ("source");
p->removePointCloud ("target");
PointCloudColorHandlerGenericField tgt_color_handler (cloud_target, "curvature");if (!tgt_color_handler.isCapable ()) PCL_WARN ("Cannot create curvature color handler!");
PointCloudColorHandlerGenericField src_color_handler (cloud_source, "curvature");if (!src_color_handler.isCapable ()) PCL_WARN ("Cannot create curvature color handler!");
p->addPointCloud (cloud_target, tgt_color_handler, "target", vp_2);
p->addPointCloud (cloud_source, src_color_handler, "source", vp_2);
p->spinOnce();
}
/**加载⼀组我们想要匹配在⼀起的PCD⽂件
* 参数argc是参数的数量 (pass from main ())
*参数 argv 实际的命令⾏参数 (pass from main ())
*参数models点云数据集的合成⽮量*/
void loadData (int argc, char **argv, std::vector > &models)
{
std::string extension (".pcd");//假定第⼀个参数是实际测试模型
for (int i = 1; i < argc; i++)
{
std::string fname = std::string(argv[i]);//⾄少需要5个字符长(因为.plot就有 5个字符)
if (fname.size () <=extension.size ())continue;
std::transform (fname.begin (), d (), fname.begin (), (int(*)(int))tolower);//检查参数是⼀个pcd⽂件
if (pare (fname.size () - extension.size (), extension.size (), extension) == 0)
{//加载点云并保存在总体的模型列表中
PCD m;
m.f_name=argv[i];
pcl::io::loadPCDFile (argv[i],*m.cloud);//从点云中移除NAN点
std::vectorindices;
pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices);
models.push_back (m);
}
霎时间}
}
/**匹配⼀对点云数据集并且返还结果
*参数 cloud_src 是源点云
*参数 cloud_src 是⽬标点云
蜜汁南瓜
*参数output输出的配准结果的源点云
*参数final_transform是在来源和⽬标之间的转换*/
void pairAlign (const PointCloud::Ptr cloud_src, const PointCloud::Ptr cloud_tgt, PointCloud::Ptr output, Eigen::Matrix4f &final_transform, bool downsample = fal)
{//
//为了⼀致性和⾼速的下采样//注意：为了⼤数据集需要允许这项
PointCloud::Ptr src (newPointCloud);
PointCloud::Ptr tgt (newPointCloud);
pcl::VoxelGridgrid;if(downsample)
腾讯客服网站{
grid.tLeafSize (0.05, 0.05, 0.05);
grid.tInputCloud (cloud_src);
grid.filter (*src);
grid.tInputCloud (cloud_tgt);保卫延安小说
grid.filter (*tgt);
}el{
src=cloud_src;
采购合同模板
tgt=cloud_tgt;
}//计算曲⾯法线和曲率
PointCloudWithNormals::Ptr points_with_normals_src (newPointCloudWithNormals);
PointCloudWithNormals::Ptr points_with_normals_tgt (newPointCloudWithNormals);
pcl::NormalEstimationnorm_est;
pcl::arch::KdTree<:pointxyz>::Ptr tree (new pcl::arch::KdTree<:pointxyz>());
norm_est.tSearchMethod (tree);
norm_est.tKSearch (30);
norm_est.tInputCloud (src);
pute (*points_with_normals_src);
pcl::copyPointCloud (*src, *points_with_normals_src);
norm_est.tInputCloud (tgt);
pute (*points_with_normals_tgt);
pcl::copyPointCloud (*tgt, *points_with_normals_tgt);//
//举例说明我们⾃定义点的表⽰(以上定义)
MyPointReprentation point_reprentation;//调整'curvature'尺⼨权重以便使它和x, y, z平衡
float alpha[4] = {1.0, 1.0, 1.0, 1.0};
point_reprentation.tRescaleValues (alpha);//
//配准
pcl::IterativeClostPointNonLinearreg;
reg.tTransformationEpsilon (1e-6);//将两个对应关系之间的(srctgt)最⼤距离设置为10厘⽶//注意：根据你的数据集⼤⼩来调整reg.tMaxCorrespondenceDistance (0.1);//设置点表⽰
reg.tPointReprentation (boost::make_shared(point_reprentation));
reg.tInputCloud (points_with_normals_src);
lend和borrow的区别
reg.tInputTarget (points_with_normals_tgt);//
//在⼀个循环中运⾏相同的最优化并且使结果可视化
Eigen::Matrix4f Ti =Eigen::Matrix4f::Identity (), prev, targetToSource;
PointCloudWithNormals::Ptr reg_result=points_with_normals_src;
reg.tMaximumIterations (2);for (int i = 0; i < 30; ++i)
{
PCL_INFO ("Iteration Nr. %d.\n", i);//为了可视化的⽬的保存点云
points_with_normals_src =reg_result;//估计
reg.tInputCloud (points_with_normals_src);
reg.align (*reg_result);//在每⼀个迭代之间累积转换
Ti = FinalTransformation () *Ti;//如果这次转换和之前转换之间的差异⼩于阈值//则通过减⼩最⼤对应距离来改善程序
if (fabs ((LastIncrementalTransformation () - prev).sum ())
reg.tMaxCorrespondenceDistance (MaxCorrespondenceDistance ()- 0.001); LastIncrementalTransformation ();//可视化当前状态
showCloudsRight(points_with_normals_tgt, points_with_normals_src);
}//
//得到⽬标点云到源点云的变换
targetToSource =Ti.inver();//补肾精的食物
//把⽬标点云转换回源框架
pcl::transformPointCloud (*cloud_tgt, *output, targetToSource);
p->removePointCloud ("source");
p->removePointCloud ("target");
PointCloudColorHandlerCustom cloud_tgt_h (output, 0, 255, 0); PointCloudColorHandlerCustom cloud_src_h (cloud_src, 255, 0, 0);
p->addPointCloud (output, cloud_tgt_h, "target", vp_2);
p->addPointCloud (cloud_src, cloud_src_h, "source", vp_2);
PCL_INFO ("Press q to continue the registration.\n");
p->spin ();
p->removePointCloud ("source");
p->removePointCloud ("target");//添加源点云到转换⽬标
*output += *cloud_src;
final_transform=targetToSource;
}/*---[*/
int main (int argc, char**argv)
{
pcl::PointCloud<:pointxyz>::Ptr target1 (new pcl::PointCloud<:pointxyz>);
pcl::PointCloud<:pointxyz>output;
pcl::io::loadPCDFile (argv[1], *target1);
pcl::io::loadPCDFile (argv[2], output);//加载数据
std::vector >data;
PCD m;
布迪厄std::vectorindices;
m.cloud=target1;//从点云中移除NAN点
pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices);
data.push_back (m);
