ROS, PCL 점 구름 필터

하드웨어: Kinect 2, UR5.
소프트웨어: Ubuntu 16.04, ROS kinetic.
처리 되 지 않 은 Kinect 점 구름 은 소음 을 포함 하고 시야 범위 가 너무 넓 기 때문에 처리 해 야 합 니 다.나 는 직통, 반경, 체 소 세 가지 방식 을 채택 했다.
아래 내용 은 순 전 히 개인 학습 입 니 다. 만약 잘못 이 있 으 면 지적 해 주 셔 서 감사합니다.!
1. PCL 필터 학습
공식 튜 토리 얼:http://pointclouds.org/documentation/tutorials/index.php#filtering-tutorial
참고 튜 토리 얼:https://blog.csdn.net/fandq1223/article/details/53185053
                  https://blog.csdn.net/u013019296/article/details/70052319
                  https://blog.csdn.net/qq_34719188/article/details/79179430
다음은 제 가 사용 하 는 세 가지 필터 입 니 다. 필터 의 주체 와 매개 변수 설정 부분 만 있 습 니 다 (ROS 에 서 는 이것 만 사 용 됩 니 다).
직통 여파
특정한 축선 의 특정한 범위 내의 점 을 보류 하거나 삭제 하여 시야 범 위 를 바 꿉 니 다.

  pcl::PassThrough<:pointxyz> pass;    //     
  pass.setInputCloud (cloud);              //      
  pass.setFilterFieldName ("z");           //       ，z 
  pass.setFilterLimits (0.0, 1.0);         //       
  //pass.setFilterLimitsNegative (true);   //             ，    
  pass.filter (*cloud_filtered);           //    

체 소 필터
체 소 망 격 을 사용 하여 샘플링 을 한다. 이렇게 하면 점 운 의 수량 과 데이터 (변 화 를 발견 하지 못 했다) 를 줄 이 고 점 운 표면의 형상 체 징 을 보존 하면 조준, 표면 재건 등 을 높 일 수 있다.

  pcl::VoxelGrid<:pclpointcloud2> sor;  //       
  sor.setInputCloud (cloud);                //      
  sor.setLeafSize (0.01f, 0.01f, 0.01f);    //         ，0.01m   
  sor.filter (*cloud_filtered);             //    

반경 필터
노 이 즈 를 줄 이 고 이 그림 에서 제약 조건 에 맞 는 점 을 삭제 합 니 다.

    pcl::RadiusOutlierRemoval<:pointxyz> outrem;  //      
    outrem.setInputCloud(cloud);                      //      
    outrem.setRadiusSearch(0.8);                      //      ，0.8m，      
    outrem.setMinNeighborsInRadius (2);               //          XX   
    outrem.filter (*cloud_filtered);                  //    

2. ROS 의 PCL
공식 튜 토리 얼:http://wiki.ros.org/pcl/Tutorials#pcl.2BAC8-Tutorials.2BAC8-hydro.Create_a_ROS_package
ROS 패키지 만 들 기

cd ~/catkin_ws/src
catkin_create_pkg my_pcl_tutorial pcl_conversions pcl_ros roscpp sensor_msgs

package. xml 를 열 고 텍스트 뒤에 추가 합 니 다.

  libpcl-all-dev
  libpcl-all

예제 프로그램 (초보 자 출발)
이것 은 원시 적 인 예시 프로그램 입 니 다. 알 아 볼 수 없 는 외출 우회전 (농담 입 니 다. 구체 적 인 절 차 는 예시 절차 후에 설명 이 있 습 니 다).

#include 
// PCL specific includes
#include 
#include 
#include 
#include 

ros::Publisher pub;

void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& input)
{
  // Create a container for the data.
  sensor_msgs::PointCloud2 output;

  // Do data processing here...
  output = *input;

  // Publish the data.
  pub.publish (output);
}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "my_pcl_tutorial");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe ("input", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<:pointcloud2> ("output", 1);

  // Spin
  ros::spin ();
}

CMakeLists. txt 에 새로 만 든 가방 을 추가 합 니 다. (새로 만 든 가방 마다 CMakeLists. txt 에 경 로 를 추가 합 니 다.)

add_executable(example src/example.cpp)
target_link_libraries(example ${catkin_LIBRARIES})

이 예제 프로그램 은 어떠한 필터 도 하지 않 았 다.
다음은 세 가지 ROS 필터.
프레임 워 크 는 ROS 의 example 를 참고 할 수 있 습 니 다.voxelgrid.cpp：
http://wiki.ros.org/pcl/Tutorials/hydro?action=AttachFile&do=view&target=example_voxelgrid.cpp
직통 여파

#include 
// PCL specific includes
#include 
#include 
#include 
#include 
//    
#include 
#include 

ros::Publisher pub;

void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
  // Container for original & filtered data
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; 
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  pcl::PCLPointCloud2 cloud_filtered;

  // Convert to PCL data type
  pcl_conversions::toPCL(*cloud_msg, *cloud);

  // Perform the actual filtering
  pcl::PassThrough<:pclpointcloud2> pass;
    // build the filter
  pass.setInputCloud (cloudPtr);
  pass.setFilterFieldName ("z");
  pass.setFilterLimits (0.0, 1.3);
    // apply filter
  pass.filter (cloud_filtered);

  // Convert to ROS data type
  sensor_msgs::PointCloud2 cloud_pt;
  pcl_conversions::moveFromPCL(cloud_filtered, cloud_pt);

  // Publish the data
  pub.publish (cloud_pt);
}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "PassThrough");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud   
  ros::Subscriber sub = nh.subscribe<:pointcloud2> ("/cloud_input", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud   
  pub = nh.advertise<:pointcloud2> ("/filtered_PassThrough", 1);

  // Spin
  ros::spin ();
}

체 소 필터

#include 
// PCL specific includes
#include 
#include 
#include 
#include 
//  
#include 
#include 

ros::Publisher pub;

void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
  // Container for original & filtered data
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; 
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  pcl::PCLPointCloud2 cloud_filtered;

  // Convert to PCL data type
  pcl_conversions::toPCL(*cloud_msg, *cloud);

  // Perform the actual filtering
    pcl::VoxelGrid<:pclpointcloud2> sor;
    // build the filter
    sor.setInputCloud (cloudPtr);
    sor.setLeafSize (0.005, 0.005, 0.005);
    // apply filter
    sor.filter (cloud_filtered);

  // Convert to ROS data type
  sensor_msgs::PointCloud2 cloud_vog;
  pcl_conversions::moveFromPCL(cloud_filtered, cloud_vog);

  // Publish the data
  pub.publish (cloud_vog);
}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "VoxelGrid");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe<:pointcloud2> ("/filtered_PassThrough", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<:pointcloud2> ("/filtered_VoxelGrid", 1);

  // Spin
  ros::spin ();
}

반경 필터

#include 
// PCL specific includes
#include 
#include 
#include 
#include 
//  
#include 
#include 
#include 


ros::Publisher pub;

void 
cloud_cb (const sensor_msgs::PointCloud2ConstPtr& cloud_msg)
{
  // Container for original & filtered data
  pcl::PCLPointCloud2* cloud = new pcl::PCLPointCloud2; 
  pcl::PCLPointCloud2ConstPtr cloudPtr(cloud);
  pcl::PCLPointCloud2 cloud_filtered;

  // Convert to PCL data type
  pcl_conversions::toPCL(*cloud_msg, *cloud);

  // Perform the actual filtering
    pcl::RadiusOutlierRemoval<:pclpointcloud2> outrem;
     // build the filter  
    outrem.setInputCloud(cloudPtr);
    outrem.setRadiusSearch(0.08);
    outrem.setMinNeighborsInRadius (60);
     // apply filter
    outrem.filter (cloud_filtered);

  // Convert to ROS data type
  sensor_msgs::PointCloud2 cloud_rad;
  pcl_conversions::moveFromPCL(cloud_filtered, cloud_rad);

  // Publish the data
  pub.publish (cloud_rad);
}

int
main (int argc, char** argv)
{
  // Initialize ROS
  ros::init (argc, argv, "RadiusOutlierRemoval");
  ros::NodeHandle nh;

  // Create a ROS subscriber for the input point cloud
  ros::Subscriber sub = nh.subscribe<:pointcloud2> ("/filtered_VoxelGrid", 1, cloud_cb);

  // Create a ROS publisher for the output point cloud
  pub = nh.advertise<:pointcloud2> ("/filtered_RadiusOutlierRemoval", 1);

  // Spin
  ros::spin ();
}

마지막 으로 CMakeLists. txt 에 세 가지 필터 방식 을 추가 합 니 다.

add_executable(passthrough src/passthrough.cpp)
target_link_libraries(passthrough ${catkin_LIBRARIES})

add_executable(radius_outlier_removal src/radius_outlier_removal.cpp)
target_link_libraries(radius_outlier_removal ${catkin_LIBRARIES})

add_executable(voxel_grid src/voxel_grid.cpp)
target_link_libraries(voxel_grid ${catkin_LIBRARIES})

주: (1) 자신의 필터 순서에 따라 필터 의 입력 과 출력 을 조정 합 니 다.
       (2) rviz 에 필터 그림 을 표시 하려 면 체 소 필 터 를 마지막 에 두 지 않 는 것 을 권장 합 니 다.
       (3) 개인 테스트: octomap 가 지원 하 는 체 소 필터 의 최소 0.001.
       (4) 체 소 필터 값 이 너무 작고 반지름 필터 의 값 이 너무 커서 표시 할 수 없습니다 (모두 걸 러 냈 습 니 다).
현재 세 가지 필터 의. cpp 를 다 썼 습 니 다. 다음은 ROS 에서 어떻게 호출 하 는 지 입 니 다.
3. 여 파 를 ROS 에 추가
여 기 는 주로 필터 후의 그림 을 rviz 에 표시 합 니 다.
octomap 수정
나의 또 다른 박문 을 참고 하 세 요.https://blog.csdn.net/zhang970187013/article/details/81098175
세 번 째 부분 대 sensorskinect. yaml 수정, 주요 수정 pointcloud_topic, octomap 호출 필터 후의 출력 (마지막 필터 의 output) 을 실현 합 니 다.

sensors:
  - sensor_plugin: occupancy_map_monitor/PointCloudOctomapUpdater
    point_cloud_topic: /filtered_RadiusOutlierRemoval
    max_range: 2
    point_subsample: 1
    padding_offset: 0.1
    padding_scale: 1.0
    filtered_cloud_topic: filtered_cloud

sensor 에서manager. launch. xml 파일 에서 매개 변수 제어 디 스 플레이 의 정밀도 수정:

시작 launch 파일 수정
이전 단 계 는 출력 을 호출 할 수 있 지만 필터 처 리 를 실행 하지 않 았 기 때문에 시작 파일 에 필터 처 리 를 추가 합 니 다.
이 부분 도 앞의 박문 을 참고 하여 launch 에 추가 합 니 다.

node 와 remap 를 시작 하 는 형식 은 참고 하 십시오:http://wiki.ros.org/roslaunch/XML/node화해시키다http://wiki.ros.org/roslaunch/XML/remap