1.点云粗配准拼接-Ransac

#include <pcl/common/transforms.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_registration.h>using namespace pcl;//
voidcompute (const PointCloud<PointXYZ>::Ptr &input, const PointCloud<PointXYZ>::Ptr &target,Eigen::Matrix4f &transformation,const double thresh)
{SampleConsensusModelRegistration<PointXYZ>::Ptr model (new SampleConsensusModelRegistration<PointXYZ> (input));model->setInputTarget (target);RandomSampleConsensus<PointXYZ> sac (model, thresh);sac.setMaxIterations (100000);if (!sac.computeModel (2)){PCL_ERROR ("Could not compute a valid transformation!\n");return;}Eigen::VectorXf coeff;sac.getModelCoefficients (coeff);transformation.row (0) = coeff.segment<4>(0);transformation.row (1) = coeff.segment<4>(4);transformation.row (2) = coeff.segment<4>(8);transformation.row (3) = coeff.segment<4>(12);
}///
intmain (int argc, char** argv)
{PointCloud<PointXYZ>::Ptr source (new PointCloud<PointXYZ>);PointCloud<PointXYZ>::Ptr target (new PointCloud<PointXYZ>);io::loadPCDFile ("1.pcd", *source);io::loadPCDFile ("2.pcd", *target);// ComputeEigen::Matrix4f transform;double thresh = 0.002;compute (source, target, transform, thresh);PointCloud<PointXYZ> output;transformPointCloud (*source, output, transform);output = output+*target;io::savePCDFileASCII ("result.pcd", output);return (0);
}

2.点云精配准

使用移除NAN点/ 体素过滤/ 计算曲面法线和曲率/ 非线性ICP/

/* \author Radu Bogdan Rusu
* adaptation Raphael Favier*/
#include <boost/make_shared.hpp>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/point_representation.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/filter.h>
#include <pcl/features/normal_3d.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/transforms.h>
#include <pcl/visualization/pcl_visualizer.h>
using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
//简单类型定义
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
typedef pcl::PointNormal PointNormalT;
typedef pcl::PointCloud<PointNormalT> PointCloudWithNormals;
//这是一个辅助教程，因此我们可以负担全局变量
//创建可视化工具
pcl::visualization::PCLVisualizer *p;
//定义左右视点
int vp_1, vp_2;
//处理点云的方便的结构定义
struct PCD
{PointCloud::Ptr cloud;std::string f_name;PCD() : cloud (new PointCloud) {};
};
struct PCDComparator
{bool operator () (const PCD& p1, const PCD& p2){return (p1.f_name < p2.f_name);}
};
//以< x, y, z, curvature >形式定义一个新的点
class MyPointRepresentation : public pcl::PointRepresentation <PointNormalT>
{using pcl::PointRepresentation<PointNormalT>::nr_dimensions_;
public:MyPointRepresentation (){//定义尺寸值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, const PointCloud::Ptr cloud_source)
{p->removePointCloud ("vp1_target");p->removePointCloud ("vp1_source");PointCloudColorHandlerCustom<PointT> tgt_h (cloud_target, 0, 255, 0);PointCloudColorHandlerCustom<PointT> 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, const PointCloudWithNormals::Ptr cloud_source)
{p->removePointCloud ("source");p->removePointCloud ("target");PointCloudColorHandlerGenericField<PointNormalT> tgt_color_handler (cloud_target, "curvature");if (!tgt_color_handler.isCapable ())PCL_WARN ("Cannot create curvature color handler!");PointCloudColorHandlerGenericField<PointNormalT> 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<PCD, Eigen::aligned_allocator<PCD> > &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 (), fname.end (), fname.begin (), (int(*)(int))tolower);//检查参数是一个pcd文件if (fname.compare (fname.size () - extension.size (), extension.size (), extension) == 0){//加载点云并保存在总体的模型列表中PCD m;m.f_name = argv[i];pcl::io::loadPCDFile (argv[i], *m.cloud);//从点云中移除NAN点std::vector<int> indices;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 = false)
{////为了一致性和高速的下采样//注意：为了大数据集需要允许这项PointCloud::Ptr src (new PointCloud);PointCloud::Ptr tgt (new PointCloud);pcl::VoxelGrid<PointT> grid;if (downsample){grid.setLeafSize (0.05, 0.05, 0.05);grid.setInputCloud (cloud_src);grid.filter (*src);grid.setInputCloud (cloud_tgt);grid.filter (*tgt);}else{src = cloud_src;tgt = cloud_tgt;}//计算曲面法线和曲率PointCloudWithNormals::Ptr points_with_normals_src (new PointCloudWithNormals);PointCloudWithNormals::Ptr points_with_normals_tgt (new PointCloudWithNormals);pcl::NormalEstimation<PointT, PointNormalT> norm_est;pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());norm_est.setSearchMethod (tree);norm_est.setKSearch (30);norm_est.setInputCloud (src);norm_est.compute (*points_with_normals_src);pcl::copyPointCloud (*src, *points_with_normals_src);norm_est.setInputCloud (tgt);norm_est.compute (*points_with_normals_tgt);pcl::copyPointCloud (*tgt, *points_with_normals_tgt);////举例说明我们自定义点的表示（以上定义）MyPointRepresentation point_representation;//调整'curvature'尺寸权重以便使它和x, y, z平衡float alpha[4] = {1.0, 1.0, 1.0, 1.0};point_representation.setRescaleValues (alpha);//// 配准pcl::IterativeClosestPointNonLinear<PointNormalT, PointNormalT> reg;reg.setTransformationEpsilon (1e-6);//将两个对应关系之间的(src<->tgt)最大距离设置为10厘米//注意：根据你的数据集大小来调整reg.setMaxCorrespondenceDistance (0.1);  //设置点表示reg.setPointRepresentation (boost::make_shared<const MyPointRepresentation> (point_representation));reg.setInputCloud (points_with_normals_src);reg.setInputTarget (points_with_normals_tgt);////在一个循环中运行相同的最优化并且使结果可视化Eigen::Matrix4f Ti = Eigen::Matrix4f::Identity (), prev, targetToSource;PointCloudWithNormals::Ptr reg_result = points_with_normals_src;reg.setMaximumIterations (2);for (int i = 0; i < 30; ++i){PCL_INFO ("Iteration Nr. %d.\n", i);//为了可视化的目的保存点云points_with_normals_src = reg_result;//估计reg.setInputCloud (points_with_normals_src);reg.align (*reg_result);//在每一个迭代之间累积转换Ti = reg.getFinalTransformation () * Ti;//如果这次转换和之前转换之间的差异小于阈值//则通过减小最大对应距离来改善程序if (fabs ((reg.getLastIncrementalTransformation () - prev).sum ()) < reg.getTransformationEpsilon ())reg.setMaxCorrespondenceDistance (reg.getMaxCorrespondenceDistance () - 0.001);prev = reg.getLastIncrementalTransformation ();//可视化当前状态showCloudsRight(points_with_normals_tgt, points_with_normals_src);}//// 得到目标点云到源点云的变换targetToSource = Ti.inverse();////把目标点云转换回源框架pcl::transformPointCloud (*cloud_tgt, *output, targetToSource);p->removePointCloud ("source");p->removePointCloud ("target");PointCloudColorHandlerCustom<PointT> cloud_tgt_h (output, 0, 255, 0);PointCloudColorHandlerCustom<PointT> 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<pcl::PointXYZ>::Ptr target1 (new pcl::PointCloud<pcl::PointXYZ>);pcl::PointCloud<pcl::PointXYZ> output;pcl::io::loadPCDFile (argv[1], *target1);pcl::io::loadPCDFile (argv[2], output);// 加载数据std::vector<PCD, Eigen::aligned_allocator<PCD> > data;PCD m;std::vector<int> indices;m.cloud = target1;//从点云中移除NAN点pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices);data.push_back (m);m.cloud = output.makeShared();pcl::removeNaNFromPointCloud(*m.cloud,*m.cloud, indices);data.push_back (m);//检查用户输入if (data.empty ()){return (-1);}PCL_INFO ("Loaded %d datasets.", (int)data.size ());PointCloud::Ptr result (new PointCloud), source, target;Eigen::Matrix4f GlobalTransform = Eigen::Matrix4f::Identity (), pairTransform;for (size_t i = 1; i < data.size (); ++i){source = data[i-1].cloud;target = data[i].cloud;PointCloud::Ptr temp (new PointCloud);PCL_INFO ("Aligning %s (%d) with %s (%d).\n", data[i-1].f_name.c_str (), source->points.size (), data[i].f_name.c_str (), target->points.size ());pairAlign (source, target, temp, pairTransform, true);//把当前的两两配对转换到全局变换pcl::transformPointCloud (*temp, *result, GlobalTransform);//update the global transform更新全局变换GlobalTransform = pairTransform * GlobalTransform;//保存配准对，转换到第一个点云框架中std::stringstream ss;ss << i << ".pcd";pcl::io::savePCDFile (ss.str (), *result, true);}
}
/* ]--- */

参考：

PCL三维点云拼接融合技术

PCL_三维点云拼接融合/点云粗配准/点云精配准相关推荐

PCL三维点云拼接融合技术
转自:https://blog.csdn.net/dcba2014/article/details/71859375?locationNum=2&fps=1 本例使用pcd格式点云文件进行配准 ...
python点云拼接
点云拼接主要是把不同的点云拼接到一起.通常,为了获得一个完整物体的三维点云,我们可能会在不同的视点进行数据采集,然后把采集的点云数据拼接到一起. 由于视点的不同,所采集到的多个点云的坐标系也会不一致. ...
【人工智能】云边融合的AI Cloud 不是简单的“云+边”
3月30日,"智涌钱塘"2018 AI Cloud生态国际峰会在杭州国际博览中心隆重举行.会上,海康威视宣布全面开放AI Cloud架构,与各方共建AI产业生态体系. 作为行业龙头 ...
点云的粗配准和精配准
1.前言点云配准是点云处理的重要技术之一,可以用来估计物体位姿,拼接多个视角下的点云. 分别用基于采样一致性的粗配准,以及粗配准与ICP精配准结合的方法进行配准实验. 粗配准流程图如下,主要为精配准 ...
深度相机Kinect2.0三维点云拼接实验（三）
文章目录概要刚体空间旋转的描述旋转矩阵欧拉角四元数刚体空间平移的描述总结概要 Kinect2.0是微软推出的一款RGB-D相机,它即支持普通相机的拍摄,也支持脉冲测量深度信息.本系 ...
深度相机Kinect2.0三维点云拼接实验（二）
文章目录摘要 Linux系统下的环境搭建 Ubuntu系统安装 ROS-Melodic安装 Kinect2.0驱动安装 PCL库安装 Open-CV库安装注意事项 Windows10系统下的环境搭 ...
三维点云拼接标记点拼接 SVD分解法
在三维重建的过程中每次只能测量有限的区域,那么拼接的操作就再所难免了,最终拼接的效果往往觉得了你做的产品是否真的有价值.很多市面上的产品在比较的时候首先看的是整体的重建效果,而整体的效果就是拼接决定的 ...
图像的点云拼接-原理讲解与代码实现
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达理解好图像的点云拼接,需要从相机的模型说起.理解相机的成像原理之后,便可更为深刻的理解图像的点云如何拼 ...
多帧点云数据拼接合并_点云拼接注册
点云拼接,配准,注册有什么联系点云拼接,配准,注册说的是同一个概念,就是把不同位置的点云通过重叠部分的信息,变换到同一个位置.下面我们就用注册这个名词来描述这个过程.注册一般分为三类:粗注册,精细注 ...
如何将一个向量投影到一个平面上_自动驾驶视觉融合相机校准与激光点云投影...
点云PCL免费知识星球,点云论文速读. 标题:自动驾驶视觉融合-相机校准与激光点云投影作者:williamhyin 来源:https://zhuanlan.zhihu.com/p/136263753 ...

PCL_三维点云拼接融合/点云粗配准/点云精配准

1.点云粗配准拼接-Ransac

2.点云精配准

参考：

PCL_三维点云拼接融合/点云粗配准/点云精配准相关推荐

最新文章

热门文章