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