02-SAC

1. 모델 매칭을 점군내 모형 탐지 : SAC 모듈

공간에 존재 하는 물체들은 특정한 형태를 가지고 있습니다. 사람과 같이 복잡한 형태도 있지만 바닥(평면), 전봇대(원통), 간판(사각형), 전선(선) 등 간단한 형태도 있습니다. 본문에서는 점군속에 포함된 포형을 탐지 하는 법을 살펴 보겠습니다. 탐지 결과는 후에 배우는 세그멘테이션과 연계 하여 불필요한 부분을 제거하거나, 필요한 부분만 추출 할 때 사용 됩니다.

1. sample_consensus

sample_consensus는 포인트 클라우드 샘플(Sample)이 사전에 정의된 모형(=모델)과 일치(Consensus)하는지를 판악하여 모델의 파라미터를 추정하는 알고리즘 입니다. 간단한 형태의 도형들은 수학적으로 모델링이 가능합니다. 예를 들어 선의 경우 y=ax+b로 표현 가능합니다. 원의 경우 xxxx로 표현 가능합니다. 수학적 모델을 통해서 점군상에 존재 하는 모형을 찾는데는 허프만 변환과 sample_consensus 방법이 적용 가능합니다. 허프만 변환은 알고리즘이 간단하여 속도가 빠른 장점이 있지만, 잡음이 섞여 있는 데이터에는 적합 하지 않아 sample_consensus 방법을 많이 사용합니다.

PCL에서 구현되어 있는 sample consensus estimators 은 아래와 같습니다.

• SAC_RANSAC - RANdom SAmple Consensus
• SAC_LMEDS - Least Median of Squares
• SAC_MSAC - M-Estimator SAmple Consensus
• SAC_RRANSAC - Randomized RANSAC
• SAC_RMSAC - Randomized MSAC
• SAC_MLESAC - Maximum LikeLihood Estimation SAmple Consensus
• SAC_PROSAC - PROgressive SAmple Consensus

PCL에서 제공하는 모형을 표로 정리 하면 아래와 같습니다.

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>

// How to use Random Sample Consensus model
// http://pointclouds.org/documentation/tutorials/random_sample_consensus.php#random-sample-consensus


int
main(int argc, char** argv)
{
  // initialize PointClouds
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::PointCloud<pcl::PointXYZ>::Ptr final (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::io::loadPCDFile<pcl::PointXYZ>("sample_consenus_input.pcd", *cloud);
  //https://github.com/adioshun/gitBook_Tutorial_PCL/blob/master/Beginner/sample/sample_consenus_input.pcd

  std::vector<int> inliers;

  // created RandomSampleConsensus object and compute the appropriated model
  pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (cloud));  

  // model_p
  int debug_verbosity_level = 10;
  pcl::RandomSampleConsensus<pcl::PointXYZ> ransac (model_p);

  ransac.setDistanceThreshold (.01);
  ransac.computeModel(debug_verbosity_level);  //Compute the actual model and find the inliers. 
  ransac.getInliers(inliers);

  pcl::copyPointCloud (*cloud, inliers, *final);
  pcl::io::savePCDFile<pcl::PointXYZ>("sample_consenus_final_model_p.pcd", *final);

  return 0;

  // 다른 방법들 
  //https://github.com/PointCloudLibrary/pcl/blob/master/test/sample_consensus/test_sample_consensus_plane_models.cpp
 }

추출 하기 ModelOutlierRemoval

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/filters/model_outlier_removal.h>

// Filtering a PointCloud using ModelOutlierRemoval
// http://pointclouds.org/documentation/tutorials/model_outlier_removal.php#model-outlier-removal

int
main ()
{
  // *.PCD 파일 읽기 
  // https://github.com/adioshun/gitBook_Tutorial_PCL/blob/master/Intermediate/sample/sphere_pointcloud_with_noise.pcd
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
  pcl::io::loadPCDFile <pcl::PointXYZ> ("tabletop_passthrough.pcd", *cloud);

  // 포인트수 출력
  std::cout << "Loaded :" << cloud->width * cloud->height  << std::endl;

  std::cerr << "Cloud before filtering: " << std::endl;
  for (std::size_t i = 0; i < cloud->points.size (); ++i)
    std::cout << "    " << cloud->points[i].x << " " << cloud->points[i].y << " " << cloud->points[i].z << std::endl;

  // 2. filter plane:
  // 2.1 generate model:
  // modelparameter for this plane: ax + by + cz + d = 0
  pcl::ModelCoefficients plane_coeff;
  plane_coeff.values.resize (4);
  plane_coeff.values[0] = 3.88368e-05;
  plane_coeff.values[1] = 0.000606678;
  plane_coeff.values[2] = 1;
  plane_coeff.values[3] = -0.773654;

  pcl::ModelOutlierRemoval<pcl::PointXYZ> plane_filter;
  plane_filter.setModelCoefficients (plane_coeff);
  plane_filter.setThreshold (0.05);
  plane_filter.setModelType (pcl::SACMODEL_PLANE);
  plane_filter.setInputCloud (cloud);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane_filtered (new pcl::PointCloud<pcl::PointXYZ>);
  plane_filter.filter (*cloud_plane_filtered);

  pcl::io::savePCDFile<pcl::PointXYZ>("cloud_plane_filtered.pcd", *cloud_plane_filtered);


  return (0);
}

추출 하기 : indices

#include <iostream>
#include <pcl/ModelCoefficients.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/extract_indices.h>

//Extracting indices from a PointCloud
//http://pointclouds.org/documentation/tutorials/extract_indices.php

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

  pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGB>), 
                                        cloud_p (new pcl::PointCloud<pcl::PointXYZRGB>), 
                                        cloud_f (new pcl::PointCloud<pcl::PointXYZRGB>);

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

  // 포인트수 출력
  std::cout << "Loaded :" << cloud->width * cloud->height  << std::endl;

  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());


  // 오프젝트 생성 
  pcl::SACSegmentation<pcl::PointXYZRGB> seg;
  seg.setOptimizeCoefficients (true);       //(옵션)
  seg.setInputCloud (cloud);                //입력 
  seg.setModelType (pcl::SACMODEL_PLANE);   //적용 모델 
  seg.setMethodType (pcl::SAC_RANSAC);      //적용 방법 
  seg.setMaxIterations (1000);              //최대 실행 수
  seg.setDistanceThreshold (0.01);          //inlier로 처리할 거리 정보 
  seg.segment (*inliers, *coefficients);

  // Create the filtering object
  pcl::ExtractIndices<pcl::PointXYZRGB> extract;
  // Extract the inliers
 extract.setInputCloud (cloud);
 extract.setIndices (inliers);
 extract.setNegative (false);//false
 extract.filter (*cloud_p);
 std::cerr << "Filtered : " << cloud_p->width * cloud_p->height << " data points." << std::endl;

 std::stringstream ss;
 ss << "RANSAC_plane_false.pcd"; //RANSAC_plane_false.pcd
 pcl::PCDWriter writer2;
 writer2.write<pcl::PointXYZRGB> (ss.str (), *cloud_p, false);



  return (0);
}