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_model_opps.cpp
38 *
39 * Created on: Jan 29, 2013
40 * Author: papazov
41 *
42 * Adds a model to the model library and visualizes the oriented point pairs (opps) sampled from the model.
43 */
44
45 #include <pcl/recognition/ransac_based/obj_rec_ransac.h>
46 #include <pcl/visualization/pcl_visualizer.h>
47 #include <pcl/console/print.h>
48 #include <pcl/point_cloud.h>
49 #include <vtkVersion.h>
50 #include <vtkPolyDataReader.h>
51 #include <vtkDoubleArray.h>
52 #include <vtkDataArray.h>
53 #include <vtkPointData.h>
54 #include <vtkHedgeHog.h>
55 #include <cstdio>
56 #include <thread>
57 #include <vector>
58
59 using namespace std::chrono_literals;
60 using namespace pcl;
61 using namespace io;
62 using namespace console;
63 using namespace recognition;
64 using namespace visualization;
65
66 #define _SHOW_MODEL_OCTREE_POINTS_
67 //#define _SHOW_MODEL_OCTREE_NORMALS_
68
69 void run (float pair_width, float voxel_size, float max_coplanarity_angle);
70 void showModelOpps (PCLVisualizer& viz, const ModelLibrary::HashTable& hash_table, const ModelLibrary::Model* model, float pair_width);
71 bool vtk_to_pointcloud (const char* file_name, PointCloud<PointXYZ>& pcl_points, PointCloud<Normal>& pcl_normals);
72
73 //===========================================================================================================================================
74
75 int
main(int argc,char ** argv)76 main (int argc, char** argv)
77 {
78 printf ("\nUsage: ./pcl_obj_rec_ransac_model_opps <pair_width> <voxel_size> <max_coplanarity_angle>\n\n");
79
80 const int num_params = 3;
81 float parameters[num_params] = {10.0f/*pair width*/, 5.0f/*voxel size*/, 5.0f/*max co-planarity angle*/};
82 std::string parameter_names[num_params] = {"pair_width", "voxel_size", "max_coplanarity_angle"};
83
84 // Read the user input if any
85 for ( int i = 0 ; i < argc-1 && i < num_params ; ++i )
86 {
87 parameters[i] = static_cast<float> (atof (argv[i+1]));
88 if ( parameters[i] <= 0.0f )
89 {
90 fprintf(stderr, "ERROR: the %i-th parameter has to be positive and not %f\n", i+1, parameters[i]);
91 return (-1);
92 }
93 }
94
95 printf ("The following parameter values will be used:\n");
96 for ( int i = 0 ; i < num_params ; ++i )
97 std::cout << " " << parameter_names[i] << " = " << parameters[i] << std::endl;
98 std::cout << std::endl;
99
100 run (parameters[0], parameters[1], parameters[2]);
101
102 return (0);
103 }
104
105 //===============================================================================================================================
106
run(float pair_width,float voxel_size,float max_coplanarity_angle)107 void run (float pair_width, float voxel_size, float max_coplanarity_angle)
108 {
109 PointCloud<PointXYZ>::Ptr model_points (new PointCloud<PointXYZ> ());
110 PointCloud<Normal>::Ptr model_normals (new PointCloud<Normal> ());
111
112 char model_name[] = "../../test/tum_amicelli_box.vtk";
113
114 // Get the points and normals from the input vtk file
115 if ( !vtk_to_pointcloud (model_name, *model_points, *model_normals) )
116 return;
117
118 // The recognition object
119 ObjRecRANSAC objrec (pair_width, voxel_size);
120 objrec.setMaxCoplanarityAngleDegrees (max_coplanarity_angle);
121 // Add the model
122 objrec.addModel (*model_points, *model_normals, "amicelli");
123
124 const ModelLibrary::Model* model = objrec.getModel ("amicelli");
125 if ( !model )
126 return;
127
128 // The visualizer
129 PCLVisualizer viz;
130
131 // Run the recognition and update the viewer
132 showModelOpps (viz, objrec.getHashTable (), model, pair_width);
133
134 // Visualize a sphere with the radius 'pair_width'
135 pcl::PointXYZ sphere_center;
136 sphere_center.x = model->getOctree ().getFullLeaves ()[0]->getData ()->getPoint ()[0];
137 sphere_center.y = model->getOctree ().getFullLeaves ()[0]->getData ()->getPoint ()[1];
138 sphere_center.z = model->getOctree ().getFullLeaves ()[0]->getData ()->getPoint ()[2];
139 viz.addSphere (sphere_center, pair_width, 0.0, 0.2, 1.0);
140
141 #ifdef _SHOW_MODEL_OCTREE_POINTS_
142 PointCloud<PointXYZ>::Ptr octree_points (new PointCloud<PointXYZ> ());
143
144 model->getOctree ().getFullLeavesPoints (*octree_points);
145 viz.addPointCloud (octree_points, "octree points");
146 viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "octree points");
147 viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "octree points");
148 #endif
149
150 #if defined _SHOW_MODEL_OCTREE_NORMALS_ && defined _SHOW_MODEL_OCTREE_POINTS_
151 PointCloud<Normal>::Ptr octree_normals (new PointCloud<Normal> ());
152
153 model->getOctree ().getNormalsOfFullLeaves (*octree_normals);
154 viz.addPointCloudNormals<PointXYZ,Normal> (octree_points, octree_normals, 1, 6.0f, "octree normals");
155 #endif
156
157 // Enter the main loop
158 while (!viz.wasStopped ())
159 {
160 //main loop of the visualizer
161 viz.spinOnce (100);
162 std::this_thread::sleep_for(100ms);
163 }
164 }
165
166 //===============================================================================================================================
167
showModelOpps(PCLVisualizer & viz,const ModelLibrary::HashTable & hash_table,const ModelLibrary::Model * model,float pair_width)168 void showModelOpps (PCLVisualizer& viz, const ModelLibrary::HashTable& hash_table, const ModelLibrary::Model* model, float pair_width)
169 {
170 printf ("Visualizing ... "); fflush (stdout);
171
172 const ModelLibrary::HashTableCell* cells = hash_table.getVoxels ();
173
174 // The opps points and lines
175 vtkSmartPointer<vtkPolyData> vtk_opps = vtkSmartPointer<vtkPolyData>::New ();
176 vtkSmartPointer<vtkPoints> vtk_opps_points = vtkSmartPointer<vtkPoints>::New ();
177 vtkSmartPointer<vtkCellArray> vtk_opps_lines = vtkSmartPointer<vtkCellArray>::New ();
178 #ifndef _SHOW_MODEL_OCTREE_NORMALS_
179 vtkSmartPointer<vtkHedgeHog> vtk_hedge_hog = vtkSmartPointer<vtkHedgeHog>::New ();
180 vtkSmartPointer<vtkDoubleArray> vtk_normals = vtkSmartPointer<vtkDoubleArray>::New ();
181 vtk_normals->SetNumberOfComponents (3);
182 #endif
183 vtkIdType ids[2] = {0, 1};
184
185 // Check cell by cell
186 const int num_cells = hash_table.getNumberOfVoxels ();
187 for (int i = 0 ; i < num_cells ; ++i )
188 {
189 // Make sure that we get only point pairs belonging to 'model'
190 ModelLibrary::HashTableCell::const_iterator res = cells[i].find (model);
191 if ( res == cells[i].end () )
192 continue;
193
194 // Get the opps in the current cell
195 const ModelLibrary::node_data_pair_list& data_pairs = res->second;
196
197 for (const auto &data_pair : data_pairs)
198 {
199 vtk_opps_points->InsertNextPoint (data_pair.first->getPoint ());
200 vtk_opps_points->InsertNextPoint (data_pair.second->getPoint ());
201 vtk_opps_lines->InsertNextCell (2, ids);
202 ids[0] += 2;
203 ids[1] += 2;
204 #ifndef _SHOW_MODEL_OCTREE_NORMALS_
205 vtk_normals->InsertNextTuple3 (data_pair.first->getNormal ()[0], data_pair.first->getNormal ()[1], data_pair.first->getNormal ()[2]);
206 vtk_normals->InsertNextTuple3 (data_pair.second->getNormal ()[0], data_pair.second->getNormal ()[1], data_pair.second->getNormal ()[2]);
207 #endif
208 }
209 }
210
211 // Save points and connecting lines
212 vtk_opps->SetPoints (vtk_opps_points);
213 vtk_opps->SetLines (vtk_opps_lines);
214 #ifndef _SHOW_MODEL_OCTREE_NORMALS_
215 // Save the normals
216 vtk_opps->GetPointData ()->SetNormals (vtk_normals);
217 // Setup the hedge hog object
218 vtk_hedge_hog->SetInputData (vtk_opps);
219 vtk_hedge_hog->SetVectorModeToUseNormal ();
220 vtk_hedge_hog->SetScaleFactor (0.5f*pair_width);
221 vtk_hedge_hog->Update ();
222 // Show the opps' normals
223 viz.addModelFromPolyData (vtk_hedge_hog->GetOutput (), "opps' normals");
224 #endif
225
226 viz.addModelFromPolyData (vtk_opps, "opps");
227 viz.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 1.0, 0.0, "opps");
228
229 printf ("done.\n");
230 }
231
232 //===============================================================================================================================
233
vtk_to_pointcloud(const char * file_name,PointCloud<PointXYZ> & pcl_points,PointCloud<Normal> & pcl_normals)234 bool vtk_to_pointcloud (const char* file_name, PointCloud<PointXYZ>& pcl_points, PointCloud<Normal>& pcl_normals)
235 {
236 std::size_t len = strlen (file_name);
237 if ( file_name[len-3] != 'v' || file_name[len-2] != 't' || file_name[len-1] != 'k' )
238 {
239 fprintf (stderr, "ERROR: we need a .vtk object!\n");
240 return false;
241 }
242
243 // Load the model
244 vtkSmartPointer<vtkPolyDataReader> reader = vtkSmartPointer<vtkPolyDataReader>::New ();
245 reader->SetFileName (file_name);
246 reader->Update ();
247
248 // Get the points
249 vtkPolyData *vtk_poly = reader->GetOutput ();
250 vtkPoints *vtk_points = vtk_poly->GetPoints ();
251 vtkIdType num_points = vtk_points->GetNumberOfPoints ();
252 double p[3];
253
254 pcl_points.resize (num_points);
255
256 // Copy the points
257 for ( vtkIdType i = 0 ; i < num_points ; ++i )
258 {
259 vtk_points->GetPoint (i, p);
260 pcl_points[i].x = static_cast<float> (p[0]);
261 pcl_points[i].y = static_cast<float> (p[1]);
262 pcl_points[i].z = static_cast<float> (p[2]);
263 }
264
265 // Check if we have normals
266 vtkDataArray *vtk_normals = vtk_poly->GetPointData ()->GetNormals ();
267 if ( !vtk_normals )
268 return false;
269
270 pcl_normals.resize (num_points);
271 // Copy the normals
272 for ( vtkIdType i = 0 ; i < num_points ; ++i )
273 {
274 vtk_normals->GetTuple (i, p);
275 pcl_normals[i].normal_x = static_cast<float> (p[0]);
276 pcl_normals[i].normal_y = static_cast<float> (p[1]);
277 pcl_normals[i].normal_z = static_cast<float> (p[2]);
278 }
279
280 return true;
281 }
282
283 //===============================================================================================================================
284