【PCL】（三十） ModelOutlierRemove滤波

（三十） ModelOutlierRemove滤波

以下代码实现滤除点云中不属于球模型的异常点。

model_outlier_removal.cpp

#include 
#include 
#include 

int main ()
{
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);
      pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_sphere_filtered (new pcl::PointCloud<pcl::PointXYZ>);

      // 1. Generate cloud data
      std::size_t noise_size = 5;
      std::size_t sphere_data_size = 10;
      cloud->width = noise_size + sphere_data_size;
      cloud->height = 1;
      cloud->points.resize (cloud->width * cloud->height);
      // 1.1 Add noise
      for (std::size_t i = 0; i < noise_size; ++i)
      {
            (*cloud)[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
            (*cloud)[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
            (*cloud)[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
      }
      // 1.2 Add sphere:
      double rand_x1 = 1;
      double rand_x2 = 1;
      for (std::size_t i = noise_size; i < (noise_size + sphere_data_size); ++i)
      {
            // See: http://mathworld.wolfram.com/SpherePointPicking.html
            while (pow (rand_x1, 2) + pow (rand_x2, 2) >= 1)
            {
                  rand_x1 = (rand () % 100) / (50.0f) - 1;
                  rand_x2 = (rand () % 100) / (50.0f) - 1;
            }
            double pre_calc = sqrt (1 - pow (rand_x1, 2) - pow (rand_x2, 2));
            (*cloud)[i].x = 2 * rand_x1 * pre_calc;
            (*cloud)[i].y = 2 * rand_x2 * pre_calc;
            (*cloud)[i].z = 1 - 2 * (pow (rand_x1, 2) + pow (rand_x2, 2));
            rand_x1 = 1;
            rand_x2 = 1;
      }

      std::cerr << "Cloud before filtering: " << std::endl;
      for (const auto& point: *cloud)
      std::cout << "    " << point.x << " " << point.y << " " << point.z << std::endl;

      // 2. filter sphere:
      // 2.1 generate model:
      // modelparameter for this sphere:
      // position.x: 0, position.y: 0, position.z:0, radius: 1
      pcl::ModelCoefficients sphere_coeff;
      sphere_coeff.values.resize (4);
      sphere_coeff.values[0] = 0;
      sphere_coeff.values[1] = 0;
      sphere_coeff.values[2] = 0;
      sphere_coeff.values[3] = 1;

      pcl::ModelOutlierRemoval<pcl::PointXYZ> sphere_filter;
      sphere_filter.setModelCoefficients (sphere_coeff);
      sphere_filter.setThreshold (0.05);
      sphere_filter.setModelType (pcl::SACMODEL_SPHERE);
      sphere_filter.setInputCloud (cloud);
      sphere_filter.filter (*cloud_sphere_filtered);

      std::cerr << "Sphere after filtering: " << std::endl;
      for (const auto& point: *cloud_sphere_filtered)
            std::cout << "    " << point.x << " " << point.y << " " << point.z << std::endl;

      return (0);
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69

cmake_minimum_required(VERSION 3.5 FATAL_ERROR)

project(model_outlier_removal)

find_package(PCL 1.7 REQUIRED)

include_directories(${PCL_INCLUDE_DIRS})
link_directories(${PCL_LIBRARY_DIRS})
add_definitions(${PCL_DEFINITIONS})

add_executable (model_outlier_removal model_outlier_removal.cpp)
target_link_libraries (model_outlier_removal ${PCL_LIBRARIES})
1
2
3
4
5
6
7
8
9
10
11
12

$ ./model_outlier_removal
1

Cloud before filtering: 
    0.352222 -0.151883 -0.106395
    -0.397406 -0.473106 0.292602
    -0.731898 0.667105 0.441304
    -0.734766 0.854581 -0.0361733
    -0.4607 -0.277468 -0.916762
    -0.82 -0.341666 0.4592
    -0.728589 0.667873 0.152
    -0.3134 -0.873043 -0.3736
    0.62553 0.590779 0.5096
    -0.54048 0.823588 -0.172
    -0.707627 0.424576 0.5648
    -0.83153 0.523556 0.1856
    -0.513903 -0.719464 0.4672
    0.291534 0.692393 0.66
    0.258758 0.654505 -0.7104
Sphere after filtering: 
    -0.82 -0.341666 0.4592
    -0.728589 0.667873 0.152
    -0.3134 -0.873043 -0.3736
    0.62553 0.590779 0.5096
    -0.54048 0.823588 -0.172
    -0.707627 0.424576 0.5648
    -0.83153 0.523556 0.1856
    -0.513903 -0.719464 0.4672
    0.291534 0.692393 0.66
    0.258758 0.654505 -0.7104
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27