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_scene_opps.cpp
38 *
39 * Created on: Jan 17, 2013
40 * Author: papazov
41 *
42 * Calls recognize() of the ObjRecRANSAC class and visualizes the oriented point pairs (opp) sampled from the scene.
43 * Does NOT perform full recognition.
44 */
45
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 <cstdio>
57 #include <thread>
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 class CallbackParameters;
67
68 void run (float pair_width, float voxel_size, float max_coplanarity_angle);
69 bool vtk_to_pointcloud (const char* file_name, PointCloud<PointXYZ>& pcl_points, PointCloud<Normal>& pcl_normals);
70 void update (CallbackParameters* params);
71
72 //#define _SHOW_SCENE_POINTS_
73 #define _SHOW_OCTREE_POINTS_
74 //#define _SHOW_OCTREE_NORMALS_
75
76 class CallbackParameters
77 {
78 public:
CallbackParameters(ObjRecRANSAC & objrec,PCLVisualizer & viz,PointCloud<PointXYZ> & points,PointCloud<Normal> & normals)79 CallbackParameters (ObjRecRANSAC& objrec, PCLVisualizer& viz, PointCloud<PointXYZ>& points, PointCloud<Normal>& normals)
80 : objrec_ (objrec),
81 viz_ (viz),
82 points_ (points),
83 normals_ (normals)
84 { }
85
86 ObjRecRANSAC& objrec_;
87 PCLVisualizer& viz_;
88 PointCloud<PointXYZ>& points_;
89 PointCloud<Normal>& normals_;
90 };
91
92 //===========================================================================================================================================
93
94 int
main(int argc,char ** argv)95 main (int argc, char** argv)
96 {
97 printf ("\nUsage: ./pcl_obj_rec_ransac_scene_opps <pair_width> <voxel_size> <max_coplanarity_angle>\n\n");
98
99 const int num_params = 3;
100 float parameters[num_params] = {40.0f/*pair width*/, 5.0f/*voxel size*/, 15.0f/*max co-planarity angle*/};
101 std::string parameter_names[num_params] = {"pair_width", "voxel_size", "max_coplanarity_angle"};
102
103 // Read the user input if any
104 for ( int i = 0 ; i < argc-1 && i < num_params ; ++i )
105 {
106 parameters[i] = static_cast<float> (atof (argv[i+1]));
107 if ( parameters[i] <= 0.0f )
108 {
109 fprintf(stderr, "ERROR: the %i-th parameter has to be positive and not %f\n", i+1, parameters[i]);
110 return (-1);
111 }
112 }
113
114 printf ("The following parameter values will be used:\n");
115 for ( int i = 0 ; i < num_params ; ++i )
116 std::cout << " " << parameter_names[i] << " = " << parameters[i] << std::endl;
117 std::cout << std::endl;
118
119 run (parameters[0], parameters[1], parameters[2]);
120
121 return (0);
122 }
123
124 //===============================================================================================================================
125
keyboardCB(const pcl::visualization::KeyboardEvent & event,void * params_void)126 void keyboardCB (const pcl::visualization::KeyboardEvent &event, void* params_void)
127 {
128 if (event.getKeyCode () == 13 /*enter*/ && event.keyUp ())
129 update (static_cast<CallbackParameters*> (params_void));
130 }
131
132 //===============================================================================================================================
133
update(CallbackParameters * params)134 void update (CallbackParameters* params)
135 {
136 std::list<ObjRecRANSAC::Output> dummy_output;
137
138 // Run the recognition method
139 params->objrec_.recognize (params->points_, params->normals_, dummy_output);
140
141 // Build the vtk objects visualizing the lines between the opps
142 const std::list<ObjRecRANSAC::OrientedPointPair>& opps = params->objrec_.getSampledOrientedPointPairs ();
143 std::cout << "There is (are) " << opps.size () << " oriented point pair(s).\n";
144 // The opps points
145 vtkSmartPointer<vtkPolyData> vtk_opps = vtkSmartPointer<vtkPolyData>::New ();
146 vtkSmartPointer<vtkPoints> vtk_opps_points = vtkSmartPointer<vtkPoints>::New ();
147 vtk_opps_points->SetNumberOfPoints (2*static_cast<vtkIdType> (opps.size ()));
148 vtkSmartPointer<vtkCellArray> vtk_opps_lines = vtkSmartPointer<vtkCellArray>::New ();
149 // The opps normals
150 vtkSmartPointer<vtkDoubleArray> vtk_normals = vtkSmartPointer<vtkDoubleArray>::New ();
151 vtk_normals->SetNumberOfComponents (3);
152 vtk_normals->SetNumberOfTuples (2*static_cast<vtkIdType> (opps.size ()));
153 vtkIdType ids[2] = {0, 1};
154
155 // Insert the points and compute the lines
156 for (const auto &opp : opps)
157 {
158 vtk_opps_points->SetPoint (ids[0], opp.p1_[0], opp.p1_[1], opp.p1_[2]);
159 vtk_opps_points->SetPoint (ids[1], opp.p2_[0], opp.p2_[1], opp.p2_[2]);
160 vtk_opps_lines->InsertNextCell (2, ids);
161
162 vtk_normals->SetTuple3 (ids[0], opp.n1_[0], opp.n1_[1], opp.n1_[2]);
163 vtk_normals->SetTuple3 (ids[1], opp.n2_[0], opp.n2_[1], opp.n2_[2]);
164
165 ids[0] += 2;
166 ids[1] += 2;
167 }
168 vtk_opps->SetPoints (vtk_opps_points);
169 vtk_opps->GetPointData ()->SetNormals (vtk_normals);
170 vtk_opps->SetLines (vtk_opps_lines);
171
172 vtkSmartPointer<vtkHedgeHog> vtk_hh = vtkSmartPointer<vtkHedgeHog>::New ();
173 vtk_hh->SetVectorModeToUseNormal ();
174 vtk_hh->SetScaleFactor (0.5f*params->objrec_.getPairWidth ());
175 vtk_hh->SetInputData (vtk_opps);
176 vtk_hh->Update ();
177
178 // The lines
179 std::string lines_str_id = "opps";
180 params->viz_.removeShape(lines_str_id);
181 params->viz_.addModelFromPolyData (vtk_opps, lines_str_id);
182 params->viz_.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0.0, 0.0, 1.0, lines_str_id);
183 // The normals
184 std::string normals_str_id = "opps normals";
185 params->viz_.removeShape(normals_str_id);
186 params->viz_.addModelFromPolyData (vtk_hh->GetOutput (), normals_str_id);
187 params->viz_.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 1.0, 0.0, normals_str_id);
188
189 }
190
191 //===============================================================================================================================
192
run(float pair_width,float voxel_size,float max_coplanarity_angle)193 void run (float pair_width, float voxel_size, float max_coplanarity_angle)
194 {
195 PointCloud<PointXYZ>::Ptr scene_points (new PointCloud<PointXYZ> ());
196 PointCloud<Normal>::Ptr scene_normals (new PointCloud<Normal> ());
197
198 // Get the points and normals from the input vtk file
199 if ( !vtk_to_pointcloud ("../../test/tum_table_scene.vtk", *scene_points, *scene_normals) )
200 return;
201
202 // The recognition object
203 ObjRecRANSAC objrec (pair_width, voxel_size);
204 objrec.setMaxCoplanarityAngleDegrees (max_coplanarity_angle);
205 // Switch to the test mode in which only oriented point pairs from the scene are sampled
206 objrec.enterTestModeSampleOPP ();
207
208 // The visualizer
209 PCLVisualizer viz;
210
211 CallbackParameters params(objrec, viz, *scene_points, *scene_normals);
212 viz.registerKeyboardCallback (keyboardCB, static_cast<void*> (¶ms));
213
214 // Run the recognition and update the viewer
215 update (¶ms);
216
217 #ifdef _SHOW_SCENE_POINTS_
218 viz.addPointCloud (scene_points, "cloud in");
219 viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud in");
220 #endif
221
222 #ifdef _SHOW_OCTREE_POINTS_
223 PointCloud<PointXYZ>::Ptr octree_points (new PointCloud<PointXYZ> ());
224 objrec.getSceneOctree ().getFullLeavesPoints (*octree_points);
225 viz.addPointCloud (octree_points, "octree points");
226 viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "octree points");
227 viz.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_COLOR, 1.0, 0.0, 0.0, "octree points");
228 #endif
229
230 #if defined _SHOW_OCTREE_NORMALS_ && defined _SHOW_OCTREE_POINTS_
231 PointCloud<Normal>::Ptr octree_normals (new PointCloud<Normal> ());
232 objrec.getSceneOctree ().getNormalsOfFullLeaves (*octree_normals);
233 viz.addPointCloudNormals<PointXYZ,Normal> (octree_points, octree_normals, 1, 6.0f, "normals out");
234 #endif
235
236 // Enter the main loop
237 while (!viz.wasStopped ())
238 {
239 //main loop of the visualizer
240 viz.spinOnce (100);
241 std::this_thread::sleep_for(100ms);
242 }
243 }
244
245 //===============================================================================================================================
246
vtk_to_pointcloud(const char * file_name,PointCloud<PointXYZ> & pcl_points,PointCloud<Normal> & pcl_normals)247 bool vtk_to_pointcloud (const char* file_name, PointCloud<PointXYZ>& pcl_points, PointCloud<Normal>& pcl_normals)
248 {
249 std::size_t len = strlen (file_name);
250 if ( file_name[len-3] != 'v' || file_name[len-2] != 't' || file_name[len-1] != 'k' )
251 {
252 fprintf (stderr, "ERROR: we need a .vtk object!\n");
253 return false;
254 }
255
256 // Load the model
257 vtkSmartPointer<vtkPolyDataReader> reader = vtkSmartPointer<vtkPolyDataReader>::New ();
258 reader->SetFileName (file_name);
259 reader->Update ();
260
261 // Get the points
262 vtkPolyData *vtk_poly = reader->GetOutput ();
263 vtkPoints *vtk_points = vtk_poly->GetPoints ();
264 vtkIdType num_points = vtk_points->GetNumberOfPoints ();
265 double p[3];
266
267 pcl_points.resize (num_points);
268
269 // Copy the points
270 for ( vtkIdType i = 0 ; i < num_points ; ++i )
271 {
272 vtk_points->GetPoint (i, p);
273 pcl_points[i].x = static_cast<float> (p[0]);
274 pcl_points[i].y = static_cast<float> (p[1]);
275 pcl_points[i].z = static_cast<float> (p[2]);
276 }
277
278 // Check if we have normals
279 vtkDataArray *vtk_normals = vtk_poly->GetPointData ()->GetNormals ();
280 if ( !vtk_normals )
281 return false;
282
283 pcl_normals.resize (num_points);
284 // Copy the normals
285 for ( vtkIdType i = 0 ; i < num_points ; ++i )
286 {
287 vtk_normals->GetTuple (i, p);
288 pcl_normals[i].normal_x = static_cast<float> (p[0]);
289 pcl_normals[i].normal_y = static_cast<float> (p[1]);
290 pcl_normals[i].normal_z = static_cast<float> (p[2]);
291 }
292
293 return true;
294 }
295
296 //===============================================================================================================================
297