1 /*
2  * Software License Agreement (BSD License)
3  *
4  *  Point Cloud Library (PCL) - www.pointclouds.org
5  *  Copyright (c) 2010-2011, Willow Garage, Inc.
6  *
7  *  All rights reserved.
8  *
9  *  Redistribution and use in source and binary forms, with or without
10  *  modification, are permitted provided that the following conditions
11  *  are met:
12  *
13  *   * Redistributions of source code must retain the above copyright
14  *     notice, this list of conditions and the following disclaimer.
15  *   * Redistributions in binary form must reproduce the above
16  *     copyright notice, this list of conditions and the following
17  *     disclaimer in the documentation and/or other materials provided
18  *     with the distribution.
19  *   * Neither the name of Willow Garage, Inc. nor the names of its
20  *     contributors may be used to endorse or promote products derived
21  *     from this software without specific prior written permission.
22  *
23  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34  *  POSSIBILITY OF SUCH DAMAGE.
35  *
36  *
37  *
38  */
39 
40 #include <pcl/console/parse.h>
41 #include <pcl/console/print.h>
42 #include <pcl/console/time.h>
43 #include <pcl/features/integral_image_normal.h>
44 #include <pcl/features/organized_edge_detection.h>
45 #include <pcl/io/pcd_io.h>
46 #include <pcl/visualization/cloud_viewer.h>
47 #include <pcl/PCLPointCloud2.h>
48 #include <pcl/point_cloud.h>
49 #include <pcl/point_types.h>
50 
51 #include <thread>
52 
53 using namespace pcl;
54 using namespace pcl::io;
55 using namespace pcl::console;
56 using namespace std::chrono_literals;
57 
58 float default_th_dd = 0.02f;
59 int default_max_search = 50;
60 
61 using Cloud = pcl::PointCloud<pcl::PointXYZRGBA>;
62 using CloudPtr = Cloud::Ptr;
63 using CloudConstPtr = Cloud::ConstPtr;
64 
65 pcl::visualization::PCLVisualizer viewer("3D Edge Viewer");
66 
67 void
printHelp(int,char ** argv)68 printHelp(int, char** argv)
69 {
70   // clang-format off
71   print_error ("Syntax is: %s input.pcd output.pcd <options>\n", argv[0]);
72   print_info ("  where options are:\n");
73   print_info ("                     -th_dd X       = the tolerance in meters for difference in depth values between neighboring points (The value is set for 1 meter and is adapted with respect to depth value linearly. (e.g. 2.0*th_dd in 2 meter depth)) (default: ");
74   print_value ("%f", default_th_dd); print_info (")\n");
75   print_info ("                     -max_search X  = the max search distance for deciding occluding and occluded edges (default: ");
76   print_value ("%d", default_max_search); print_info (")\n");
77   // clang-format on
78 }
79 
80 bool
loadCloud(const std::string & filename,pcl::PCLPointCloud2 & cloud)81 loadCloud(const std::string& filename, pcl::PCLPointCloud2& cloud)
82 {
83   TicToc tt;
84   print_highlight("Loading ");
85   print_value("%s ", filename.c_str());
86 
87   tt.tic();
88   if (loadPCDFile(filename, cloud) < 0)
89     return false;
90   print_info("[done, ");
91   print_value("%g", tt.toc());
92   print_info(" ms : ");
93   print_value("%d", cloud.width * cloud.height);
94   print_info(" points]\n");
95   print_info("Available dimensions: ");
96   print_value("%s\n", pcl::getFieldsList(cloud).c_str());
97 
98   return true;
99 }
100 
101 void
saveCloud(const std::string & filename,const pcl::PCLPointCloud2 & output)102 saveCloud(const std::string& filename, const pcl::PCLPointCloud2& output)
103 {
104   TicToc tt;
105   tt.tic();
106 
107   print_highlight("Saving ");
108   print_value("%s ", filename.c_str());
109 
110   pcl::io::savePCDFile(filename,
111                        output,
112                        Eigen::Vector4f::Zero(),
113                        Eigen::Quaternionf::Identity(),
114                        true); // Save as binary
115 
116   print_info("[done, ");
117   print_value("%g", tt.toc());
118   print_info(" ms : ");
119   print_value("%d", output.width * output.height);
120   print_info(" points]\n");
121 }
122 
123 void
keyboard_callback(const pcl::visualization::KeyboardEvent & event,void *)124 keyboard_callback(const pcl::visualization::KeyboardEvent& event, void*)
125 {
126   if (event.keyUp()) {
127     double opacity;
128     switch (event.getKeyCode()) {
129     case '1':
130       viewer.getPointCloudRenderingProperties(
131           pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "nan boundary edges");
132       viewer.setPointCloudRenderingProperties(
133           pcl::visualization::PCL_VISUALIZER_OPACITY,
134           1.0 - opacity,
135           "nan boundary edges");
136       break;
137     case '2':
138       viewer.getPointCloudRenderingProperties(
139           pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluding edges");
140       viewer.setPointCloudRenderingProperties(
141           pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0 - opacity, "occluding edges");
142       break;
143     case '3':
144       viewer.getPointCloudRenderingProperties(
145           pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "occluded edges");
146       viewer.setPointCloudRenderingProperties(
147           pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0 - opacity, "occluded edges");
148       break;
149     case '4':
150       viewer.getPointCloudRenderingProperties(
151           pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "high curvature edges");
152       viewer.setPointCloudRenderingProperties(
153           pcl::visualization::PCL_VISUALIZER_OPACITY,
154           1.0 - opacity,
155           "high curvature edges");
156       break;
157     case '5':
158       viewer.getPointCloudRenderingProperties(
159           pcl::visualization::PCL_VISUALIZER_OPACITY, opacity, "rgb edges");
160       viewer.setPointCloudRenderingProperties(
161           pcl::visualization::PCL_VISUALIZER_OPACITY, 1.0 - opacity, "rgb edges");
162       break;
163     }
164   }
165 }
166 
167 void
compute(const pcl::PCLPointCloud2::ConstPtr & input,pcl::PCLPointCloud2 & output,float th_dd,int max_search)168 compute(const pcl::PCLPointCloud2::ConstPtr& input,
169         pcl::PCLPointCloud2& output,
170         float th_dd,
171         int max_search)
172 {
173   CloudPtr cloud(new Cloud);
174   fromPCLPointCloud2(*input, *cloud);
175 
176   pcl::PointCloud<pcl::Normal>::Ptr normal(new pcl::PointCloud<pcl::Normal>);
177   pcl::IntegralImageNormalEstimation<PointXYZRGBA, pcl::Normal> ne;
178   ne.setNormalEstimationMethod(ne.COVARIANCE_MATRIX);
179   ne.setNormalSmoothingSize(10.0f);
180   ne.setBorderPolicy(ne.BORDER_POLICY_MIRROR);
181   ne.setInputCloud(cloud);
182   ne.compute(*normal);
183 
184   TicToc tt;
185   tt.tic();
186 
187   // OrganizedEdgeBase<PointXYZRGBA, Label> oed;
188   // OrganizedEdgeFromRGB<PointXYZRGBA, Label> oed;
189   // OrganizedEdgeFromNormals<PointXYZRGBA, Normal, Label> oed;
190   OrganizedEdgeFromRGBNormals<PointXYZRGBA, Normal, Label> oed;
191   oed.setInputNormals(normal);
192   oed.setInputCloud(cloud);
193   oed.setDepthDisconThreshold(th_dd);
194   oed.setMaxSearchNeighbors(max_search);
195   oed.setEdgeType(oed.EDGELABEL_NAN_BOUNDARY | oed.EDGELABEL_OCCLUDING |
196                   oed.EDGELABEL_OCCLUDED | oed.EDGELABEL_HIGH_CURVATURE |
197                   oed.EDGELABEL_RGB_CANNY);
198   PointCloud<Label> labels;
199   std::vector<PointIndices> label_indices;
200   oed.compute(labels, label_indices);
201   print_info("Detecting all edges... [done, ");
202   print_value("%g", tt.toc());
203   print_info(" ms]\n");
204 
205   // Make gray point clouds
206   for (auto& point : cloud->points) {
207     std::uint8_t gray = std::uint8_t((point.r + point.g + point.b) / 3);
208     point.r = point.g = point.b = gray;
209   }
210 
211   // Display edges in PCLVisualizer
212   viewer.setSize(640, 480);
213   viewer.addCoordinateSystem(0.2f, "global");
214   viewer.addPointCloud(cloud, "original point cloud");
215   viewer.registerKeyboardCallback(&keyboard_callback);
216 
217   pcl::PointCloud<pcl::PointXYZRGBA>::Ptr occluding_edges(
218       new pcl::PointCloud<pcl::PointXYZRGBA>),
219       occluded_edges(new pcl::PointCloud<pcl::PointXYZRGBA>),
220       nan_boundary_edges(new pcl::PointCloud<pcl::PointXYZRGBA>),
221       high_curvature_edges(new pcl::PointCloud<pcl::PointXYZRGBA>),
222       rgb_edges(new pcl::PointCloud<pcl::PointXYZRGBA>);
223 
224   pcl::copyPointCloud(*cloud, label_indices[0].indices, *nan_boundary_edges);
225   pcl::copyPointCloud(*cloud, label_indices[1].indices, *occluding_edges);
226   pcl::copyPointCloud(*cloud, label_indices[2].indices, *occluded_edges);
227   pcl::copyPointCloud(*cloud, label_indices[3].indices, *high_curvature_edges);
228   pcl::copyPointCloud(*cloud, label_indices[4].indices, *rgb_edges);
229 
230   const int point_size = 2;
231   viewer.addPointCloud<pcl::PointXYZRGBA>(nan_boundary_edges, "nan boundary edges");
232   viewer.setPointCloudRenderingProperties(
233       pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "nan boundary edges");
234   viewer.setPointCloudRenderingProperties(
235       pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 0.0f, 1.0f, "nan boundary edges");
236 
237   viewer.addPointCloud<pcl::PointXYZRGBA>(occluding_edges, "occluding edges");
238   viewer.setPointCloudRenderingProperties(
239       pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluding edges");
240   viewer.setPointCloudRenderingProperties(
241       pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 0.0f, "occluding edges");
242 
243   viewer.addPointCloud<pcl::PointXYZRGBA>(occluded_edges, "occluded edges");
244   viewer.setPointCloudRenderingProperties(
245       pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "occluded edges");
246   viewer.setPointCloudRenderingProperties(
247       pcl::visualization::PCL_VISUALIZER_COLOR, 1.0f, 0.0f, 0.0f, "occluded edges");
248 
249   viewer.addPointCloud<pcl::PointXYZRGBA>(high_curvature_edges, "high curvature edges");
250   viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,
251                                           point_size,
252                                           "high curvature edges");
253   viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,
254                                           1.0f,
255                                           1.0f,
256                                           0.0f,
257                                           "high curvature edges");
258 
259   viewer.addPointCloud<pcl::PointXYZRGBA>(rgb_edges, "rgb edges");
260   viewer.setPointCloudRenderingProperties(
261       pcl::visualization::PCL_VISUALIZER_POINT_SIZE, point_size, "rgb edges");
262   viewer.setPointCloudRenderingProperties(
263       pcl::visualization::PCL_VISUALIZER_COLOR, 0.0f, 1.0f, 1.0f, "rgb edges");
264 
265   while (!viewer.wasStopped()) {
266     viewer.spinOnce();
267     std::this_thread::sleep_for(100us);
268   }
269 
270   // Combine point clouds and edge labels
271   pcl::PCLPointCloud2 output_edges;
272   toPCLPointCloud2(labels, output_edges);
273   concatenateFields(*input, output_edges, output);
274 }
275 
276 int
main(int argc,char ** argv)277 main(int argc, char** argv)
278 {
279   print_info("Detect 3D edges from organized point cloud data. For more information, "
280              "use: %s -h\n",
281              argv[0]);
282   bool help = false;
283   parse_argument(argc, argv, "-h", help);
284   if (argc < 3 || help) {
285     printHelp(argc, argv);
286     return -1;
287   }
288 
289   // Parse the command line arguments for .pcd files
290   std::vector<int> p_file_indices;
291   p_file_indices = parse_file_extension_argument(argc, argv, ".pcd");
292   if (p_file_indices.size() != 2) {
293     print_error("Need one input PCD file and one output PCD file to continue.\n");
294     return -1;
295   }
296 
297   // Command line parsing
298   float th_dd = default_th_dd;
299   int max_search = default_max_search;
300 
301   parse_argument(argc, argv, "-th_dd", th_dd);
302   parse_argument(argc, argv, "-max_search", max_search);
303 
304   print_info("th_dd: ");
305   print_value("%f\n", th_dd);
306   print_info("max_search: ");
307   print_value("%d\n", max_search);
308 
309   // Load the first file
310   pcl::PCLPointCloud2::Ptr cloud(new pcl::PCLPointCloud2);
311   if (!loadCloud(argv[p_file_indices[0]], *cloud))
312     return -1;
313 
314   // Perform the feature estimation
315   pcl::PCLPointCloud2 output;
316 
317   compute(cloud, output, th_dd, max_search);
318 
319   // Save into the second file
320   saveCloud(argv[p_file_indices[1]], output);
321 }
322