点云删除,找到该点后,从源点云中删除

代码目的:
假设有两片点云cloudA、cloudB,若在cloudB中找到cloudA的数据点,则从cloudB中删除该点。

#include <pcl/point_cloud.h>        //点类型定义头文件
#include <pcl/kdtree/kdtree_flann.h> //kdtree类定义头文件

#include <iostream>
#include <vector>
#include <ctime>
const double eps = 1.0e-6;

int
main(int argc, char** argv)
{
    //srand(time(NULL));   //用系统时间初始化随机种子
    //创建一个PointCloud<pcl::PointXYZ>
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudA(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloudB(new pcl::PointCloud<pcl::PointXYZ>);
    // 随机点云生成
    cloudA->width = 20;             //此处点云数量
    cloudA->height = 1;                //表示点云为无序点云
    cloudA->points.resize(cloudA->width * cloudA->height);

    for (size_t i = 0; i < cloudA->points.size(); ++i)   //循环填充点云数据
    {
        cloudA->points[i].x = i + 1;
        cloudA->points[i].y = i + 1;
        cloudA->points[i].z = i + 1;
    }

    cloudB->width = 20;             //此处点云数量
    cloudB->height = 1;                //表示点云为无序点云
    cloudB->points.resize(cloudB->width * cloudB->height);
    for (size_t i = 0; i < cloudB->points.size(); ++i)   //循环填充点云数据
    {
        cloudB->points[i].x = i + 8;
        cloudB->points[i].y = i + 8;
        cloudB->points[i].z = i + 8;
    }
    //cloudBackup->points = cloud->points;
    //创建KdTreeFLANN对象,并把创建的点云设置为输入,创建一个searchPoint变量作为查询点
    pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;

    pcl::PointXYZ searchPoint;
    int K = 1;
    std::vector<int> pointIdxNKNSearch(K);      //存储查询点近邻索引
    std::vector<float> pointNKNSquaredDistance(K); //存储近邻点对应距离平方
    std::vector<pcl::PointXYZ> DeleteData;
    int num = 0;
    for (auto iterA = cloudA->begin(); iterA != cloudA->end(); iterA++)
    {
        searchPoint.x = iterA->x;
        searchPoint.y = iterA->y;
        searchPoint.z = iterA->z;
        kdtree.setInputCloud(cloudB);    //在cloudB中找到对应点后,在cloudB中直接删除该点
        num = kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance);
        if (num > 0)
        {
            if (sqrt(pointNKNSquaredDistance[0])<eps)
            {
                auto iterB = cloudB->begin() + pointIdxNKNSearch[0];
                cloudB->erase(iterB);
                DeleteData.push_back(searchPoint);
                if (cloudB->size()==0)
                {
                    break;
                }
                searchPoint.x = 0;
                searchPoint.y = 0;
                searchPoint.z = 0;
                num = 0;
                pointIdxNKNSearch.clear();
                pointNKNSquaredDistance.clear();
            }
        }
        
    }
    
    for (auto iter = DeleteData.begin(); iter != DeleteData.end(); iter++)
    {
        std::cout << iter->x << " " << iter->y << " " << iter->z << std::endl;
    }

    system("pause");
    return 0;
}


最后编辑于
©著作权归作者所有,转载或内容合作请联系作者
  • 序言:七十年代末,一起剥皮案震惊了整个滨河市,随后出现的几起案子,更是在滨河造成了极大的恐慌,老刑警刘岩,带你破解...
    沈念sama阅读 215,133评论 6 497
  • 序言:滨河连续发生了三起死亡事件,死亡现场离奇诡异,居然都是意外死亡,警方通过查阅死者的电脑和手机,发现死者居然都...
    沈念sama阅读 91,682评论 3 390
  • 文/潘晓璐 我一进店门,熙熙楼的掌柜王于贵愁眉苦脸地迎上来,“玉大人,你说我怎么就摊上这事。” “怎么了?”我有些...
    开封第一讲书人阅读 160,784评论 0 350
  • 文/不坏的土叔 我叫张陵,是天一观的道长。 经常有香客问我,道长,这世上最难降的妖魔是什么? 我笑而不...
    开封第一讲书人阅读 57,508评论 1 288
  • 正文 为了忘掉前任,我火速办了婚礼,结果婚礼上,老公的妹妹穿的比我还像新娘。我一直安慰自己,他们只是感情好,可当我...
    茶点故事阅读 66,603评论 6 386
  • 文/花漫 我一把揭开白布。 她就那样静静地躺着,像睡着了一般。 火红的嫁衣衬着肌肤如雪。 梳的纹丝不乱的头发上,一...
    开封第一讲书人阅读 50,607评论 1 293
  • 那天,我揣着相机与录音,去河边找鬼。 笑死,一个胖子当着我的面吹牛,可吹牛的内容都是我干的。 我是一名探鬼主播,决...
    沈念sama阅读 39,604评论 3 415
  • 文/苍兰香墨 我猛地睁开眼,长吁一口气:“原来是场噩梦啊……” “哼!你这毒妇竟也来了?” 一声冷哼从身侧响起,我...
    开封第一讲书人阅读 38,359评论 0 270
  • 序言:老挝万荣一对情侣失踪,失踪者是张志新(化名)和其女友刘颖,没想到半个月后,有当地人在树林里发现了一具尸体,经...
    沈念sama阅读 44,805评论 1 307
  • 正文 独居荒郊野岭守林人离奇死亡,尸身上长有42处带血的脓包…… 初始之章·张勋 以下内容为张勋视角 年9月15日...
    茶点故事阅读 37,121评论 2 330
  • 正文 我和宋清朗相恋三年,在试婚纱的时候发现自己被绿了。 大学时的朋友给我发了我未婚夫和他白月光在一起吃饭的照片。...
    茶点故事阅读 39,280评论 1 344
  • 序言:一个原本活蹦乱跳的男人离奇死亡,死状恐怖,灵堂内的尸体忽然破棺而出,到底是诈尸还是另有隐情,我是刑警宁泽,带...
    沈念sama阅读 34,959评论 5 339
  • 正文 年R本政府宣布,位于F岛的核电站,受9级特大地震影响,放射性物质发生泄漏。R本人自食恶果不足惜,却给世界环境...
    茶点故事阅读 40,588评论 3 322
  • 文/蒙蒙 一、第九天 我趴在偏房一处隐蔽的房顶上张望。 院中可真热闹,春花似锦、人声如沸。这庄子的主人今日做“春日...
    开封第一讲书人阅读 31,206评论 0 21
  • 文/苍兰香墨 我抬头看了看天上的太阳。三九已至,却和暖如春,着一层夹袄步出监牢的瞬间,已是汗流浃背。 一阵脚步声响...
    开封第一讲书人阅读 32,442评论 1 268
  • 我被黑心中介骗来泰国打工, 没想到刚下飞机就差点儿被人妖公主榨干…… 1. 我叫王不留,地道东北人。 一个月前我还...
    沈念sama阅读 47,193评论 2 367
  • 正文 我出身青楼,却偏偏与公主长得像,于是被迫代替她去往敌国和亲。 传闻我的和亲对象是个残疾皇子,可洞房花烛夜当晚...
    茶点故事阅读 44,144评论 2 352