1 /*
2  * Software License Agreement (BSD License)
3  *
4  *  Copyright (c) 2011, Willow Garage, Inc.
5  *  All rights reserved.
6  *
7  *  Redistribution and use in source and binary forms, with or without
8  *  modification, are permitted provided that the following conditions
9  *  are met:
10  *
11  *   * Redistributions of source code must retain the above copyright
12  *     notice, this list of conditions and the following disclaimer.
13  *   * Redistributions in binary form must reproduce the above
14  *     copyright notice, this list of conditions and the following
15  *     disclaimer in the documentation and/or other materials provided
16  *     with the distribution.
17  *   * Neither the name of Willow Garage, Inc. nor the names of its
18  *     contributors may be used to endorse or promote products derived
19  *     from this software without specific prior written permission.
20  *
21  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  *  POSSIBILITY OF SUCH DAMAGE.
33  *
34  * @author: Koen Buys
35  */
36 
37 #include <pcl/gpu/people/organized_plane_detector.h>
38 
39 #include <pcl/console/print.h>
40 
41 #include <pcl/filters/voxel_grid.h>
42 
43 #include <pcl/features/integral_image_normal.h>
44 
45 #include <pcl/common/transforms.h>
46 
47 #include <pcl/segmentation/organized_multi_plane_segmentation.h>
48 #include <pcl/segmentation/planar_polygon_fusion.h>
49 #include <pcl/segmentation/plane_coefficient_comparator.h>
50 #include <pcl/segmentation/euclidean_plane_coefficient_comparator.h>
51 #include <pcl/segmentation/rgb_plane_coefficient_comparator.h>
52 #include <pcl/segmentation/edge_aware_plane_comparator.h>
53 #include <pcl/segmentation/euclidean_cluster_comparator.h>
54 #include <pcl/segmentation/organized_connected_component_segmentation.h>
55 
56 
OrganizedPlaneDetector(int rows,int cols)57 pcl::gpu::people::OrganizedPlaneDetector::OrganizedPlaneDetector(int rows, int cols)
58 {
59   PCL_DEBUG("[pcl::gpu::people::OrganizedPlaneDetector::OrganizedPlaneDetector] : (D) : Constructor called\n");
60 
61   // Set NE defaults
62   ne_.setNormalEstimationMethod (ne_.COVARIANCE_MATRIX);
63   ne_.setMaxDepthChangeFactor (0.02f);
64   ne_.setNormalSmoothingSize (20.0f);
65 
66   // Set MPS defaults
67   mps_MinInliers_ = 10000;
68   mps_AngularThreshold_ = pcl::deg2rad (3.0);   //3 degrees
69   mps_DistanceThreshold_ = 0.02;                //2cm
70   mps_use_planar_refinement_ = true;
71 
72   mps_.setMinInliers (mps_MinInliers_);
73   mps_.setAngularThreshold (mps_AngularThreshold_);
74   mps_.setDistanceThreshold (mps_DistanceThreshold_);
75 
76   allocate_buffers(rows, cols);
77 }
78 
79 void
process(const PointCloud<PointTC>::ConstPtr & cloud)80 pcl::gpu::people::OrganizedPlaneDetector::process(const PointCloud<PointTC>::ConstPtr &cloud)
81 {
82   PCL_DEBUG("[pcl::gpu::people::OrganizedPlaneDetector::process] : (D) : Called\n");
83 
84   // Estimate Normals
85   pcl::PointCloud<pcl::Normal>::Ptr normal_cloud (new pcl::PointCloud<pcl::Normal>);
86   ne_.setInputCloud (cloud);
87   ne_.compute (*normal_cloud);
88 
89   // Segment Planes
90   std::vector<pcl::PlanarRegion<PointTC>, Eigen::aligned_allocator<pcl::PlanarRegion<PointTC> > > regions;
91   std::vector<pcl::ModelCoefficients> model_coefficients;
92   std::vector<pcl::PointIndices> inlier_indices;
93   pcl::PointCloud<pcl::Label>::Ptr labels (new pcl::PointCloud<pcl::Label>);
94 
95   std::vector<pcl::PointIndices> label_indices;
96   std::vector<pcl::PointIndices> boundary_indices;
97 
98   mps_.setInputNormals (normal_cloud);
99   mps_.setInputCloud (cloud);
100   if (mps_use_planar_refinement_)
101   {
102     mps_.segmentAndRefine (regions, model_coefficients, inlier_indices, labels, label_indices, boundary_indices);
103   }
104   else
105   {
106     //mps_.segment (regions);
107     mps_.segment (model_coefficients, inlier_indices);
108   }
109 
110   // Fill in the probabilities
111   for(const auto &inlier_index : inlier_indices)                           // iterate over all found planes
112   {
113     for(const auto &index : inlier_index.indices)                           // iterate over all the indices in that plane
114     {
115       P_l_host_[index].probs[pcl::gpu::people::Background] = 1.f;   // set background at max
116     }
117   }
118 }
119 
120 
121 void
allocate_buffers(int rows,int cols)122 pcl::gpu::people::OrganizedPlaneDetector::allocate_buffers(int rows, int cols)
123 {
124   PCL_DEBUG("[pcl::gpu::people::OrganizedPlaneDetector::allocate_buffers] : (D) : Called\n");
125 
126   // Create histogram on host
127   P_l_host_.resize(rows*cols);
128   P_l_host_.width = cols;
129   P_l_host_.height = rows;
130 
131   P_l_host_prev_.resize(rows*cols);
132   P_l_host_prev_.width = cols;
133   P_l_host_prev_.height = rows;
134 
135   // Create all the label probabilities on device
136   P_l_dev_.create(rows,cols);
137   P_l_dev_prev_.create(rows,cols);
138 }
139 
140 void
emptyHostLabelProbability(HostLabelProbability & histogram)141 pcl::gpu::people::OrganizedPlaneDetector::emptyHostLabelProbability(HostLabelProbability& histogram)
142 {
143   for(auto &point : histogram.points)
144   {
145     for(int label = 0; label < pcl::gpu::people::NUM_LABELS; label++)
146     {
147       point.probs[label] = 0.f;
148     }
149   }
150 }
151 
152 int
copyHostLabelProbability(HostLabelProbability & src,HostLabelProbability & dst)153 pcl::gpu::people::OrganizedPlaneDetector::copyHostLabelProbability(HostLabelProbability& src,
154                                                                    HostLabelProbability& dst)
155 {
156   if(src.size() != dst.size())
157   {
158     PCL_ERROR("[pcl::gpu::people::OrganizedPlaneDetector::copyHostLabelProbability] : (E) : Sizes don't match\n");
159     return -1;
160   }
161   for(std::size_t hist = 0; hist < src.size(); hist++)
162   {
163     for(int label = 0; label < pcl::gpu::people::NUM_LABELS; label++)
164     {
165       dst[hist].probs[label] = src[hist].probs[label];
166     }
167   }
168   return 1;
169 }
170 
171 int
copyAndClearHostLabelProbability(HostLabelProbability & src,HostLabelProbability & dst)172 pcl::gpu::people::OrganizedPlaneDetector::copyAndClearHostLabelProbability(HostLabelProbability& src,
173                                                                            HostLabelProbability& dst)
174 {
175   if(src.size() != dst.size())
176   {
177     PCL_ERROR("[pcl::gpu::people::OrganizedPlaneDetector::copyHostLabelProbability] : (E) : Sizes don't match\n");
178     return -1;
179   }
180   for(std::size_t hist = 0; hist < src.size(); hist++)
181   {
182     for(int label = 0; label < pcl::gpu::people::NUM_LABELS; label++)
183     {
184       dst[hist].probs[label] = src[hist].probs[label];
185       src[hist].probs[label] = 0.f;
186     }
187   }
188   return 1;
189 }
190