点云拼接算法

1、粗配准拼接

#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;

//////////////////////////////////////////////////////////////////////////////////////////////////////////////////
void
    compute (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);
}

///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
int
    main (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);

     // Compute
    Eigen::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.精配准拼接

/* \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);
    }
}
/* ]--- */
时间： 2024-10-20 09:58:16

点云拼接算法的相关文章

点云拼接注册

原文链接 点云拼接,配准,注册有什么联系 点云拼接,配准,注册说的是同一个概念,就是把不同位置的点云通过重叠部分的信息,变换到同一个位置.下面我们就用注册这个名词来描述这个过程.注册一般分为三类:粗注册,精细注册和全局注册. 粗注册:一般用于注册两个位置相差很大的点云,比如两帧位于相机坐标系的点云.粗注册方法大致分为两类:有标记点粗注册和无标记点粗注册.标记点可以是用户手动标记的,也可以是物体扫描时贴的标记点.无标记点注册更加方便一些,但是对于一些比较对称或者重合部分很少的情况,可能会有一些失误

一种平面提取的点云简化算法

1.本算法使用了PCL点云库,因此运行此代码需要安装PCL (http://pointclouds.org/) 其中平面区域的简化效率时70%,其它区域的简化效率时30%. //downSample #include <pcl/ModelCoefficients.h> #include <pcl/point_types.h> #include <pcl/io/pcd_io.h> #include <pcl/filters/extract_indices.h>

高效真实的云渲染算法 【转】

原文链接 : http://www.cnblogs.com/effulgent/archive/2008/10/06/1305029.html 原文: Realistic and Fast Cloud Rendering NinianeWang MicrosoftCorporation(nowatGoogleInc.) [email protected] November11,2003 最近在网上看到很多云渲染的效果图,但很多人吝于向人分享经验知识,我不是很赞同这种想法,本来就是属于别人的知识,

云变换算法

function [cent1,band1,he1,peak1,i]=ybh(err) yzi=xlsread('sheet1.xls','B2:B261'); zd=max(yzi); zx=max(yzi); erro=err; i=1; n=0; table=tabulate(yzi); for m=1:size(table,1) if table(m,2)~=0 n=n+1; x(n)=table(m,1);%第一列所有的值 %y(n)=table(m,3)/100;%每一行数出现的频率

PCL—点云分割（最小割算法）

1.点云分割的精度 在之前的两个章节里介绍了基于采样一致的点云分割和基于临近搜索的点云分割算法.基于采样一致的点云分割算法显然是意识流的,它只能割出大概的点云(可能是杯子的一部分,但杯把儿肯定没分割出来).基于欧式算法的点云分割面对有牵连的点云就无力了(比如风筝和人,在不用三维形态学去掉中间的线之前,是无法分割风筝和人的).基于法线等信息的区域生长算法则对平面更有效,没法靠它来分割桌上的碗和杯子.也就是说,上述算法更关注能不能分割,除此之外,我们还需要一个方法来解决分割的"好不好"这个

算法的学习就像打副本

想连续几周每天都研究算法是不怎么现实的一件事情. 算法的学习就像打副本.网游大都是这样子的,团本每周都有固定的CD,每个团本有几个BOSS,相应的BOSS掉落固定的装备和材料等.首先,团本有周CD,若没有CD,可以天天打,不出2个月就打吐了,算法的学习也差不多,每周花个3到8小时来研究一下,刷刷题,也是比较好的一件事,若超过20小时在算法上,大脑也会抗拒的,因此不宜过多,掌握好节奏很重要.然后,每个BOSS都有相应的机制需要熟悉,算法也是这样,每道算法题都是有一点的技巧在里面的,你需要牢固掌握才

点云处理软件开发进度

一.点云显示模块 根据PCL中国官方论坛上田博士的四篇文章http://www.pclcn.org/bbs/forum.php?mod=viewthread&tid=223&page=1&extra=#pid750,在MFC环境中搭建了基本的点云显示模块.这是后续所有操作的基础. 1.需要解决的问题有: ( 1)由于田博士在帖子里说,PCL-1.6.0-AllInOne-msvc2010-win32中提供的VTK5.8缺少关键文件vtkMFCWindow.h和vktMFC.lib,

在hexo静态博客中利用d3-cloud来展现标签云

效果: http://lucyhao.com/tags/ hexo自带的tag cloud的标签展现不太美观,想能够展现出“云”效果的标签.在网上找到了d3-cloud这个项目,github地址:https://github.com/jasondavies/d3-cloud demo地址:https://www.jasondavies.com/wordcloud/ hexo生成的是静态博客,所以最后在网上看到的都是静态的内容,也就是说,我们的看到的标签云也是静态的已经生成好的内容,并不会随着刷新

从IaaS到AI，马云为何让阿里云去扛人工智能大旗？

免费开通大数据服务:https://www.aliyun.com/product/odps 绝大多数人对阿里云的定位仍是国内市场最大的IaaS提供商.不过,随着国内人工智能市场在2016年迎来爆发,阿里开始在人工智能领域发力,阿里云的这一角色正在悄然转变. 布局AI领域,阿里云扛起阿里人工智能大旗 虽然阿里不是BAT三座山头中在人工智能领域的声势最旺的那个(百度躺枪),但事实上阿里从2015年也已经开始了人工智能领域的布局. 2015年6月,阿里巴巴联合富士康以145亿日元的价码战略投资软银旗下