💻
Tutorial
  • INTRO
  • Part 0 (개요)
    • README
    • 3D 영상처리
    • [별첨] PCL & PCD란 (100%)
    • chapter02 : PCL 설치 (100%)
    • chapter03 : ROS 실습 준비(100%)
  • Part 1 (초급)
    • README
    • PCL 기반 로봇 비젼
    • [별첨] 파일 생성 및 입출력 (70%)
      • PCL-Cpp (70%)
      • PCL-Python (70%)
      • Open3D-Python (70%)
      • ROS 실습 (90%)
    • Filter
    • [별첨] 샘플링 (70%)
      • 다운샘플링-PCL-Cpp (70%)
      • 다운샘플링-PCL-Python (50%)
      • 업샘플링-PCL-Cpp (70%)
      • ROS 실습 (90%)
    • [별첨] 관심 영역 설정 (70%)
      • PCL-Cpp (70%)
      • PCL-Python (70%)
      • ROS 실습 (90%)
    • [별첨] 노이즈 제거 (70%)
      • PCL-Cpp (70%)
      • PCL-Python (50%)
      • ROS 실습 (90%)
  • Part 2 (중급)
    • README
    • Kd-Tree/Octree Search
    • Chapter03 : Sample Consensus
    • [별첨] 바닥제거 (RANSAC) (70%)
      • PCL-Cpp (70%)
      • PCL-Python (70%)
      • ROS 실습 (90%)
    • 군집화 (70%)
      • Euclidean-PCL-Cpp (70%)
      • Euclidean-PCL-Python (0%)
      • Conditional-Euclidean-PCL-Cpp (50%)
      • DBSCAN-PCL-Python (0%)
      • Region-Growing-PCL-Cpp (50%)
      • Region-Growing-RGB-PCL-Cpp (50%)
      • Min-Cut-PCL-Cpp (50%)
      • Model-Outlier-Removal-PCL-Cpp (50%)
      • Progressive-Morphological-Filter-PCL-Cpp (50%)
    • 포인트 탐색과 배경제거 (60%)
      • Search-Octree-PCL-Cpp (70%)
      • Search-Octree-PCL-Python (70%)
      • Search-Kdtree-PCL-Cpp (70%)
      • Search-Kdtree-PCL-Python (70%)
      • Compression-PCL-Cpp (70%)
      • DetectChanges-PCL-Cpp (50%)
      • DetectChanges-PCL-Python (50%)
    • 특징 찾기 (50%)
      • PFH-PCL-Cpp
      • FPFH-PCL-Cpp
      • Normal-PCL-Cpp (70%)
      • Normal-PCL-Python (80%)
      • Tmp
    • 분류/인식 (30%)
      • 인식-GeometricConsistencyGrouping
      • SVM-RGBD-PCL-Python (70%)
      • SVM-LIDAR-PCL-Python (0%)
      • SVM-ROS (0%)
    • 정합 (70%)
      • ICP-PCL-Cpp (70%)
      • ICP-ROS 실습 (10%)
    • 재구성 (30%)
      • Smoothig-PCL-Cpp (70%)
      • Smoothig-PCL-Python (70%)
      • Triangulation-PCL-Cpp (70%)
  • Part 3 (고급)
    • README
    • 딥러닝 기반 학습 데이터 생성 (0%)
      • PointGAN (90%)
      • AutoEncoder (0%)
    • 딥러닝 기반 샘플링 기법 (0%)
      • DenseLidarNet (50%)
      • Point Cloud Upsampling Network
      • Pseudo-LiDAR
    • 딥러닝 기반 자율주행 탐지 기술 (0%)
    • 딥러닝 기반 자율주행 분류 기술 (0%)
      • Multi3D
      • PointNet
      • VoxelNet (50%)
      • YOLO3D
      • SqueezeSeg
      • butNet
  • Snippets
    • PCL-Snippets
    • PCL-Python-Helper (10%)
    • Lidar Data Augmentation
  • Appendix
    • 시각화Code
    • 시각화툴
    • Annotation툴
    • Point Cloud Libraries (0%)
    • 데이터셋
    • Cling_PCL
    • 참고 자료
    • 작성 계획_Tips
    • 용어집
Powered by GitBook
On this page

Was this helpful?

  1. Part 2 (중급)
  2. 포인트 탐색과 배경제거 (60%)

Search-Kdtree-PCL-Cpp (70%)

PreviousSearch-Octree-PCL-Python (70%)NextSearch-Kdtree-PCL-Python (70%)

Last updated 5 years ago

Was this helpful?

코드는 에서 다운로드 가능합니다. 샘플파일은 을 사용하였습니다.

#include <pcl/point_cloud.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/io/pcd_io.h>
#include <iostream>
#include <vector>
#include <ctime>

//How to use a KdTree to search
//http://pointclouds.org/documentation/tutorials/kdtree_search.php#kdtree-search
//Commnets : Hunjung, Lim (hunjung.lim@hotmail.com)

int
main (int argc, char** argv)
{

  // *.PCD 파일 읽기 (https://raw.githubusercontent.com/adioshun/gitBook_Tutorial_PCL/master/Intermediate/sample/cloud_cluster_0.pcd)
  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>);    
  pcl::io::loadPCDFile<pcl::PointXYZRGB>("cloud_cluster_0.pcd", *cloud);

  // 시각적 확인을 위해 색상 통일 (255,255,255)
  for (size_t i = 0; i < cloud->points.size(); ++i){
  cloud->points[i].r = 255;
  cloud->points[i].g = 255;
  cloud->points[i].b = 255;
  }

  //KdTree 오브젝트 생성 
  pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;
  kdtree.setInputCloud (cloud);    //입력 

     //기준점(searchPoint) 설정 방법 #1(x,y,z 좌표 지정)
     //pcl::PointXYZRGB searchPoint;
     //searchPoint.x = 0.026256f;
     //searchPoint.y = -1.464739f;
     //searchPoint.z = 0.929567f;
  //기준점(searchPoint) 설정 방법 #2(3000번째 포인트)
  pcl::PointXYZRGB searchPoint = cloud->points[3000]; 

  //기준점 좌표 출력 
  std::cout << "searchPoint :" << searchPoint.x << " " << searchPoint.y << " " << searchPoint.z  << std::endl;


  //기준점에서 가까운 순서중 K번째까지의 포인트 탐색 (K nearest neighbor search)
  int K = 10;   // 탐색할 포인트 수 설정 
  std::vector<int> pointIdxNKNSearch(K);
  std::vector<float> pointNKNSquaredDistance(K);

  if ( kdtree.nearestKSearch (searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance) > 0 )
  {
    //시각적 확인을 위하여 색상 변경 (0,255,0)
    for (size_t i = 0; i < pointIdxNKNSearch.size (); ++i)
    {
      cloud->points[pointIdxNKNSearch[i]].r = 0;
      cloud->points[pointIdxNKNSearch[i]].g = 255;
      cloud->points[pointIdxNKNSearch[i]].b = 0;
    }
  }

  // 탐색된 점의 수 출력 
  std::cout << "K = 10 :" << pointIdxNKNSearch.size() << std::endl;


  // 기준점에서 지정된 반경내 포인트 탐색 (Neighbor search within radius)
  float radius = 0.02; //탐색할 반경 설정(Set the search radius)
  std::vector<int> pointIdxRadiusSearch;
  std::vector<float> pointRadiusSquaredDistance;

  if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 )
  {
    //시각적 확인을 위하여 색상 변경 (0,0,255)
    for (size_t i = 0; i < pointIdxRadiusSearch.size (); ++i)
      for (size_t i = 0; i < pointIdxRadiusSearch.size(); ++i)
        {
        cloud->points[pointIdxRadiusSearch[i]].r = 0;
        cloud->points[pointIdxRadiusSearch[i]].g = 0;
        cloud->points[pointIdxRadiusSearch[i]].b = 255;
        }
  }

  // 탐색된 점의 수 출력 
  std::cout << "Radius 0.02 nearest neighbors: " << pointIdxRadiusSearch.size() << std::endl;

  // 생성된 포인트클라우드 저장 
  pcl::io::savePCDFile<pcl::PointXYZRGB>("Kdtree_AllinOne.pcd", *cloud);

  return 0;
}

결과

searchPoint :0.0346006 -1.46636 0.975463
K = 10 :10
Radius 0.02 nearest neighbors: 141

참고위치

결과

[이곳]
[cloud_cluster_0.pcd]