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