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 the copyright holder(s) 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 */
35
36 /*
37 * obj_rec_ransac_result.cpp
38 *
39 * Created on: Jan 23, 2013
40 * Author: papazov
41 *
42 * Visualizes the result of the ObjRecRANSAC class.
43 */
44
45 #include <pcl/segmentation/sac_segmentation.h>
46 #include <pcl/recognition/ransac_based/obj_rec_ransac.h>
47 #include <pcl/visualization/pcl_visualizer.h>
48 #include <pcl/console/print.h>
49 #include <pcl/point_cloud.h>
50 #include <vtkVersion.h>
51 #include <vtkPolyDataReader.h>
52 #include <vtkDoubleArray.h>
53 #include <vtkDataArray.h>
54 #include <vtkPointData.h>
55 #include <vtkHedgeHog.h>
56 #include <vtkTransformPolyDataFilter.h>
57 #include <vtkRenderer.h>
58 #include <vtkRenderWindow.h>
59 #include <vtkTransform.h>
60 #include <cstdio>
61 #include <vector>
62 #include <list>
63 #include <thread>
64
65 using namespace std::chrono_literals;
66 using namespace pcl;
67 using namespace io;
68 using namespace console;
69 using namespace recognition;
70 using namespace visualization;
71
72 class CallbackParameters;
73
74 void keyboardCB (const pcl::visualization::KeyboardEvent &event, void* params_void);
75 void update (CallbackParameters* params);
76 bool vtk2PointCloud (const char* file_name, PointCloud<PointXYZ>& pcl_points, PointCloud<Normal>& pcl_normals, vtkPolyData* vtk_data);
77 void run (float pair_width, float voxel_size, float max_coplanarity_angle);
78 bool loadScene (const char* file_name, PointCloud<PointXYZ>& non_plane_points, PointCloud<Normal>& non_plane_normals,
79 PointCloud<PointXYZ>& plane_points);
80
81 //#define _SHOW_SCENE_POINTS_
82 #define _SHOW_OCTREE_POINTS_
83 //#define _SHOW_OCTREE_NORMALS_
84 //#define _SHOW_OCTREE_
85
86 class CallbackParameters
87 {
88 public:
CallbackParameters(ObjRecRANSAC & objrec,PCLVisualizer & viz,PointCloud<PointXYZ> & scene_points,PointCloud<Normal> & scene_normals)89 CallbackParameters (ObjRecRANSAC& objrec, PCLVisualizer& viz, PointCloud<PointXYZ>& scene_points, PointCloud<Normal>& scene_normals)
90 : objrec_ (objrec),
91 viz_ (viz),
92 scene_points_ (scene_points),
93 scene_normals_ (scene_normals)
94 {}
95
96 ObjRecRANSAC& objrec_;
97 PCLVisualizer& viz_;
98 PointCloud<PointXYZ>& scene_points_;
99 PointCloud<Normal>& scene_normals_;
100 std::list<vtkActor*> actors_;
101 };
102
103 //===========================================================================================================================================
104
105 int
main(int argc,char ** argv)106 main (int argc, char** argv)
107 {
108 printf ("\nUsage: ./pcl_obj_rec_ransac_scene_opps <pair_width> <voxel_size> <max_coplanarity_angle>\n\n");
109
110 const int num_params = 3;
111 float parameters[num_params] = {40.0f/*pair width*/, 5.0f/*voxel size*/, 15.0f/*max co-planarity angle*/};
112 std::string parameter_names[num_params] = {"pair_width", "voxel_size", "max_coplanarity_angle"};
113
114 // Read the user input if any
115 for ( int i = 0 ; i < argc-1 && i < num_params ; ++i )
116 {
117 parameters[i] = static_cast<float> (atof (argv[i+1]));
118 if ( parameters[i] <= 0.0f )
119 {
120 fprintf(stderr, "ERROR: the %i-th parameter has to be positive and not %f\n", i+1, parameters[i]);
121 return (-1);
122 }
123 }
124
125 printf ("The following parameter values will be used:\n");
126 for ( int i = 0 ; i < num_params ; ++i )
127 std::cout << " " << parameter_names[i] << " = " << parameters[i] << std::endl;
128 std::cout << std::endl;
129
130 run (parameters[0], parameters[1], parameters[2]);
131 }
132
133 //===========================================================================================================================================
134
135 void
run(float pair_width,float voxel_size,float max_coplanarity_angle)136 run (float pair_width, float voxel_size, float max_coplanarity_angle)
137 {
138 // The object recognizer
139 ObjRecRANSAC objrec (pair_width, voxel_size);
140 objrec.setMaxCoplanarityAngleDegrees (max_coplanarity_angle);
141
142 // The models to be loaded
143 std::list<std::string> model_names;
144 model_names.emplace_back("tum_amicelli_box");
145 model_names.emplace_back("tum_rusk_box");
146 model_names.emplace_back("tum_soda_bottle");
147
148 std::list<PointCloud<PointXYZ>::Ptr> model_points_list;
149 std::list<PointCloud<Normal>::Ptr> model_normals_list;
150 std::list<vtkSmartPointer<vtkPolyData> > vtk_models_list;
151
152 // Load the models and add them to the recognizer
153 for (const auto &model_name : model_names)
154 {
155 PointCloud<PointXYZ>::Ptr model_points (new PointCloud<PointXYZ> ());
156 model_points_list.push_back (model_points);
157
158 PointCloud<Normal>::Ptr model_normals (new PointCloud<Normal> ());
159 model_normals_list.push_back (model_normals);
160
161 vtkSmartPointer<vtkPolyData> vtk_model = vtkSmartPointer<vtkPolyData>::New ();
162 vtk_models_list.push_back (vtk_model);
163
164 // Compose the file
165 std::string file_name = std::string("../../test/") + model_name + std::string (".vtk");
166
167 // Get the points and normals from the input model
168 if ( !vtk2PointCloud (file_name.c_str (), *model_points, *model_normals, vtk_model) )
169 continue;
170
171 // Add the model
172 objrec.addModel (*model_points, *model_normals, model_name, vtk_model);
173 }
174
175 // The scene in which the models are supposed to be recognized
176 PointCloud<PointXYZ>::Ptr non_plane_points (new PointCloud<PointXYZ> ()), plane_points (new PointCloud<PointXYZ> ());
177 PointCloud<Normal>::Ptr non_plane_normals (new PointCloud<Normal> ());
178
179 // Detect the largest plane in the dataset
180 if ( !loadScene ("../../test/tum_table_scene.vtk", *non_plane_points, *non_plane_normals, *plane_points) )
181 return;
182
183 // The parameters for the callback function and the visualizer
184 PCLVisualizer viz;
185 CallbackParameters params(objrec, viz, *non_plane_points, *non_plane_normals);
186 viz.registerKeyboardCallback (keyboardCB, static_cast<void*> (¶ms));
187
188 // Run the recognition and update the viewer. Have a look at this method, to see how to start the recognition and use the result!
189 update (¶ms);
190
191 // From this line on: visualization stuff only!
192 #ifdef _SHOW_OCTREE_
193 show_octree(objrec.getSceneOctree (), viz);
194 #endif
195
196 #ifdef _SHOW_SCENE_POINTS_
197 viz.addPointCloud (scene_points, "scene points");
198 viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "scene points");
199 #endif
200
201 #ifdef _SHOW_OCTREE_POINTS_
202 PointCloud<PointXYZ>::Ptr octree_points (new PointCloud<PointXYZ> ());
203 objrec.getSceneOctree ().getFullLeavesPoints (*octree_points);
204 viz.addPointCloud (octree_points, "octree points");
205 // viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "octree points");
206 viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "octree points");
207
208 viz.addPointCloud (plane_points, "plane points");
209 viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 0.9, 0.9, 0.9, "plane points");
210 #endif
211
212 #if defined _SHOW_OCTREE_NORMALS_ && defined _SHOW_OCTREE_POINTS_
213 PointCloud<Normal>::Ptr normals_octree (new PointCloud<Normal> ());
214 objrec.getSceneOctree ().getNormalsOfFullLeaves (*normals_octree);
215 viz.addPointCloudNormals<PointXYZ,Normal> (points_octree, normals_octree, 1, 6.0f, "normals out");
216 #endif
217
218 // Enter the main loop
219 while (!viz.wasStopped ())
220 {
221 //main loop of the visualizer
222 viz.spinOnce (100);
223 std::this_thread::sleep_for(100ms);
224 }
225 }
226
227 //===============================================================================================================================
228
229 void
keyboardCB(const pcl::visualization::KeyboardEvent & event,void * params_void)230 keyboardCB (const pcl::visualization::KeyboardEvent &event, void* params_void)
231 {
232 if (event.getKeyCode () == 13 /*enter*/ && event.keyUp ())
233 update (static_cast<CallbackParameters*> (params_void));
234 }
235
236 //===============================================================================================================================
237
238 void
update(CallbackParameters * params)239 update (CallbackParameters* params)
240 {
241 // Clear the visualizer from old object instances
242 vtkRenderer *renderer = params->viz_.getRenderWindow ()->GetRenderers ()->GetFirstRenderer ();
243 for (const auto &actor : params->actors_)
244 renderer->RemoveActor (actor);
245 params->actors_.clear ();
246
247 // This will be the output of the recognition
248 std::list<ObjRecRANSAC::Output> rec_output;
249
250 // For convenience
251 ObjRecRANSAC& objrec = params->objrec_;
252
253 // Run the recognition method
254 objrec.recognize (params->scene_points_, params->scene_normals_, rec_output);
255 int i = 0;
256
257 // Show the hypotheses
258 for ( std::list<ObjRecRANSAC::Output>::iterator it = rec_output.begin () ; it != rec_output.end () ; ++it, ++i )
259 {
260 std::cout << it->object_name_ << " has a confidence value of " << it->match_confidence_ << std::endl;
261
262 // Make a copy of the VTK model
263 vtkSmartPointer<vtkPolyData> vtk_model = vtkSmartPointer<vtkPolyData>::New ();
264 vtk_model->DeepCopy (static_cast<vtkPolyData*> (it->user_data_));
265
266 // Setup the matrix
267 vtkSmartPointer<vtkMatrix4x4> vtk_mat = vtkSmartPointer<vtkMatrix4x4>::New ();
268 vtk_mat->Identity ();
269 const float *t = it->rigid_transform_;
270 // Setup the rotation
271 vtk_mat->SetElement (0, 0, t[0]); vtk_mat->SetElement (0, 1, t[1]); vtk_mat->SetElement (0, 2, t[2]);
272 vtk_mat->SetElement (1, 0, t[3]); vtk_mat->SetElement (1, 1, t[4]); vtk_mat->SetElement (1, 2, t[5]);
273 vtk_mat->SetElement (2, 0, t[6]); vtk_mat->SetElement (2, 1, t[7]); vtk_mat->SetElement (2, 2, t[8]);
274 // Setup the translation
275 vtk_mat->SetElement (0, 3, t[9]); vtk_mat->SetElement (1, 3, t[10]); vtk_mat->SetElement (2, 3, t[11]);
276
277 // Setup the transform based on the matrix
278 vtkSmartPointer<vtkTransform> vtk_transform = vtkSmartPointer<vtkTransform>::New ();
279 vtk_transform->SetMatrix (vtk_mat);
280
281 // Setup the transformator
282 vtkSmartPointer<vtkTransformPolyDataFilter> vtk_transformator = vtkSmartPointer<vtkTransformPolyDataFilter>::New ();
283 vtk_transformator->SetTransform (vtk_transform);
284 vtk_transformator->SetInputData (vtk_model);
285 vtk_transformator->Update ();
286
287 // Visualize
288 vtkSmartPointer<vtkActor> vtk_actor = vtkSmartPointer<vtkActor>::New();
289 vtkSmartPointer<vtkPolyDataMapper> vtk_mapper = vtkSmartPointer<vtkPolyDataMapper>::New ();
290 vtk_mapper->SetInputData (vtk_transformator->GetOutput ());
291 vtk_actor->SetMapper(vtk_mapper);
292 // Set the appearance & add to the renderer
293 vtk_actor->GetProperty ()->SetColor (0.6, 0.7, 0.9);
294 renderer->AddActor(vtk_actor);
295 params->actors_.push_back (vtk_actor);
296
297 #ifdef _SHOW_TRANSFORM_SPACE_
298 if ( transform_space.getPositionCellBounds ((*acc_hypo)->pos_id_, cb) )
299 {
300 sprintf (pos_cell_name, "cell [%i, %i, %i]\n", (*acc_hypo)->pos_id_[0], (*acc_hypo)->pos_id_[1], (*acc_hypo)->pos_id_[2]);
301 params->viz_.addCube (cb[0], cb[1], cb[2], cb[3], cb[4], cb[5], 1.0, 1.0, 1.0, pos_cell_name);
302 }
303 else
304 printf ("WARNING: no cell at position [%i, %i, %i]\n", (*acc_hypo)->pos_id_[0], (*acc_hypo)->pos_id_[1], (*acc_hypo)->pos_id_[2]);
305 #endif
306 }
307 }
308
309 //===============================================================================================================================
310
311 bool
loadScene(const char * file_name,PointCloud<PointXYZ> & non_plane_points,PointCloud<Normal> & non_plane_normals,PointCloud<PointXYZ> & plane_points)312 loadScene (const char* file_name, PointCloud<PointXYZ>& non_plane_points, PointCloud<Normal>& non_plane_normals,
313 PointCloud<PointXYZ>& plane_points)
314 {
315 PointCloud<PointXYZ>::Ptr all_points (new PointCloud<PointXYZ> ());
316 PointCloud<Normal>::Ptr all_normals (new PointCloud<Normal> ());
317
318 // Get the points and normals from the input scene
319 if ( !vtk2PointCloud (file_name, *all_points, *all_normals, nullptr) )
320 return false;
321
322 // Detect the largest plane and remove it from the sets
323 pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
324 pcl::PointIndices::Ptr inliers (new pcl::PointIndices ());
325 // Create the segmentation object
326 pcl::SACSegmentation<pcl::PointXYZ> seg;
327 // Optional
328 seg.setOptimizeCoefficients (true);
329 // Mandatory
330 seg.setModelType (pcl::SACMODEL_PLANE);
331 seg.setMethodType (pcl::SAC_RANSAC);
332 seg.setDistanceThreshold (10.0);
333
334 seg.setInputCloud (all_points);
335 seg.segment (*inliers, *coefficients);
336
337 if (inliers->indices.empty ())
338 {
339 PCL_ERROR ("Could not estimate a planar model for the given dataset.");
340 return false;
341 }
342
343 // Copy the non-planar points
344 non_plane_points.resize (all_points->size () - inliers->indices.size ());
345 non_plane_normals.resize (all_points->size () - inliers->indices.size ());
346 plane_points.resize (inliers->indices.size ());
347
348 // Make sure that the ids are sorted
349 sort (inliers->indices.begin (), inliers->indices.end ());
350 pcl::uindex_t j = 0, i = 0;
351 for ( pcl::index_t id = 0 ; i < inliers->indices.size () ; ++id)
352 {
353 if ( id == inliers->indices[i] )
354 {
355 plane_points[i] = (*all_points)[id];
356 ++i;
357 }
358 else
359 {
360 non_plane_points[j] = (*all_points)[id];
361 non_plane_normals[j] = (*all_normals)[id];
362 ++j;
363 }
364 }
365
366 // Just copy the rest of the non-plane points
367 for ( std::size_t id = inliers->indices.size (); id < all_points->size () ; ++id, ++j )
368 {
369 non_plane_points[j] = (*all_points)[id];
370 non_plane_normals[j] = (*all_normals)[id];
371 }
372
373 return true;
374 }
375
376 //===============================================================================================================================
377
378 bool
vtk2PointCloud(const char * file_name,PointCloud<PointXYZ> & pcl_points,PointCloud<Normal> & pcl_normals,vtkPolyData * vtk_data)379 vtk2PointCloud (const char* file_name, PointCloud<PointXYZ>& pcl_points, PointCloud<Normal>& pcl_normals, vtkPolyData* vtk_data)
380 {
381 std::size_t len = strlen (file_name);
382 if ( file_name[len-3] != 'v' || file_name[len-2] != 't' || file_name[len-1] != 'k' )
383 {
384 fprintf (stderr, "ERROR: we need a .vtk object!\n");
385 return false;
386 }
387
388 // Load the model
389 vtkSmartPointer<vtkPolyDataReader> reader = vtkSmartPointer<vtkPolyDataReader>::New ();
390 reader->SetFileName (file_name);
391 reader->Update ();
392
393 // Get the points
394 vtkPolyData *vtk_poly = reader->GetOutput ();
395 vtkPoints *vtk_points = vtk_poly->GetPoints ();
396 vtkIdType num_points = vtk_points->GetNumberOfPoints ();
397 double p[3];
398
399 // Shall we copy the vtk object
400 if ( vtk_data )
401 vtk_data->DeepCopy (vtk_poly);
402
403 pcl_points.resize (num_points);
404
405 // Copy the points
406 for ( vtkIdType i = 0 ; i < num_points ; ++i )
407 {
408 vtk_points->GetPoint (i, p);
409 pcl_points[i].x = static_cast<float> (p[0]);
410 pcl_points[i].y = static_cast<float> (p[1]);
411 pcl_points[i].z = static_cast<float> (p[2]);
412 }
413
414 // Check if we have normals
415 vtkDataArray *vtk_normals = vtk_poly->GetPointData ()->GetNormals ();
416 if ( !vtk_normals )
417 return false;
418
419 pcl_normals.resize (num_points);
420 // Copy the normals
421 for ( vtkIdType i = 0 ; i < num_points ; ++i )
422 {
423 vtk_normals->GetTuple (i, p);
424 pcl_normals[i].normal_x = static_cast<float> (p[0]);
425 pcl_normals[i].normal_y = static_cast<float> (p[1]);
426 pcl_normals[i].normal_z = static_cast<float> (p[2]);
427 }
428
429 return true;
430 }
431
432 //===============================================================================================================================
433