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_hash_table.cpp
38 *
39 * Created on: Jun 20, 2012
40 * Author: papazov
41 *
42 * Visualizes the hash table cell entries belonging to a model. Since the hash table is a 3D cube structure, each cell
43 * is a cube in 3D. It is visualized by a cube which is scaled according to the number of entries in the cell -> the more
44 * entries the bigger the cube.
45 */
46
47 #include <pcl/recognition/ransac_based/obj_rec_ransac.h>
48 #include <pcl/visualization/pcl_visualizer.h>
49 #include <pcl/console/print.h>
50 #include <pcl/point_cloud.h>
51 #include <vtkPolyDataReader.h>
52 #include <vtkDoubleArray.h>
53 #include <vtkDataArray.h>
54 #include <vtkPointData.h>
55 #include <vtkGlyph3D.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 inline double
my_sqr(double a)67 my_sqr (double a){ return a*a;}
68
69 bool vtk_to_pointcloud (const char* file_name, PointCloud<PointXYZ>& points_in, PointCloud<Normal>& normals_in, double bounds[6]);
70 void visualize (const ModelLibrary::HashTable& hash_table);
71
72 //===========================================================================================================================================
73
74 int
main(int argc,char ** argv)75 main (int argc, char** argv)
76 {
77 // Make sure that we have the right number of arguments
78 if (argc != 2)
79 {
80 print_info ("\nVisualizes the hash table after adding the provided mesh to it.\n"
81 "usage:\n"
82 "./obj_rec_ransac_hash_table <mesh.vtk>\n");
83 return (-1);
84 }
85
86 ObjRecRANSAC::PointCloudIn points_in;
87 ObjRecRANSAC::PointCloudN normals_in;
88 double b[6];
89
90 if ( !vtk_to_pointcloud (argv[1], points_in, normals_in, b) )
91 return (-1);
92
93 // Compute the bounding box diagonal
94 float diag = static_cast<float> (sqrt (my_sqr (b[1]-b[0]) + my_sqr (b[3]-b[2]) + my_sqr (b[5]-b[4])));
95
96 // Create the recognition object (we need it only for its hash table)
97 ObjRecRANSAC objrec (diag/8.0f, diag/60.0f);
98 objrec.addModel (points_in, normals_in, "test_model");
99
100 // Start visualization (and the main VTK loop)
101 visualize (objrec.getHashTable ());
102
103 return (0);
104 }
105
106 //===========================================================================================================================================
107
vtk_to_pointcloud(const char * file_name,PointCloud<PointXYZ> & points_in,PointCloud<Normal> & normals_in,double b[6])108 bool vtk_to_pointcloud (const char* file_name, PointCloud<PointXYZ>& points_in, PointCloud<Normal>& normals_in, double b[6])
109 {
110 std::size_t len = strlen (file_name);
111 if ( file_name[len-3] != 'v' || file_name[len-2] != 't' || file_name[len-1] != 'k' )
112 {
113 fprintf (stderr, "ERROR: we need a .vtk object!\n");
114 return false;
115 }
116
117 // Load the model
118 vtkSmartPointer<vtkPolyDataReader> reader = vtkSmartPointer<vtkPolyDataReader>::New ();
119 reader->SetFileName (file_name);
120 reader->Update ();
121
122 // Get the points
123 vtkPolyData *vtk_poly = reader->GetOutput ();
124 vtkPoints *vtk_points = vtk_poly->GetPoints ();
125 vtkIdType num_points = vtk_points->GetNumberOfPoints ();
126 double p[3];
127
128 vtk_poly->ComputeBounds ();
129 vtk_poly->GetBounds (b);
130 points_in.resize (num_points);
131
132 // Copy the points
133 for ( vtkIdType i = 0 ; i < num_points ; ++i )
134 {
135 vtk_points->GetPoint (i, p);
136 points_in[i].x = static_cast<float> (p[0]);
137 points_in[i].y = static_cast<float> (p[1]);
138 points_in[i].z = static_cast<float> (p[2]);
139 }
140
141 // Check if we have normals
142 vtkDataArray *vtk_normals = vtk_poly->GetPointData ()->GetNormals ();
143 if ( vtk_normals )
144 {
145 normals_in.resize (num_points);
146 // Copy the normals
147 for ( vtkIdType i = 0 ; i < num_points ; ++i )
148 {
149 vtk_normals->GetTuple (i, p);
150 normals_in[i].normal_x = static_cast<float> (p[0]);
151 normals_in[i].normal_y = static_cast<float> (p[1]);
152 normals_in[i].normal_z = static_cast<float> (p[2]);
153 }
154 }
155
156 return true;
157 }
158
159 //===========================================================================================================================================
160
161 void
visualize(const ModelLibrary::HashTable & hash_table)162 visualize (const ModelLibrary::HashTable& hash_table)
163 {
164 PCLVisualizer vis;
165 vis.setBackgroundColor (0.1, 0.1, 0.1);
166
167 const ModelLibrary::HashTableCell* cells = hash_table.getVoxels ();
168 std::size_t max_num_entries = 0;
169 int id3[3], num_cells = hash_table.getNumberOfVoxels ();
170 float half_side, b[6], cell_center[3], spacing = hash_table.getVoxelSpacing ()[0];
171 char cube_id[128];
172
173 // Just get the maximal number of entries in the cells
174 for ( int i = 0 ; i < num_cells ; ++i, ++cells )
175 {
176 if (!cells->empty ()) // That's the number of models in the cell (it's maximum one, since we loaded only one model)
177 {
178 std::size_t num_entries = (*cells->begin ()).second.size(); // That's the number of entries in the current cell for the model we loaded
179 // Get the max number of entries
180 if ( num_entries > max_num_entries )
181 max_num_entries = num_entries;
182 }
183 }
184
185 // Now, that we have the max. number of entries, we can compute the
186 // right scale factor for the spheres
187 float s = (0.5f*spacing)/static_cast<float> (max_num_entries);
188
189 std::cout << "s = " << s << ", max_num_entries = " << max_num_entries << std::endl;
190
191 // Now, render a sphere with the right radius at the right place
192 cells = hash_table.getVoxels ();
193 for ( int i = 0; i < num_cells ; ++i, ++cells )
194 {
195 // Does the cell have any entries?
196 if (!cells->empty ())
197 {
198 hash_table.compute3dId (i, id3);
199 hash_table.computeVoxelCenter (id3, cell_center);
200
201 // That's half of the cube's side length
202 half_side = s*static_cast<float> ((*cells->begin ()).second.size ());
203
204 // Adjust the bounds of the cube
205 b[0] = cell_center[0] - half_side; b[1] = cell_center[0] + half_side;
206 b[2] = cell_center[1] - half_side; b[3] = cell_center[1] + half_side;
207 b[4] = cell_center[2] - half_side; b[5] = cell_center[2] + half_side;
208
209 // Set the id
210 sprintf (cube_id, "cube %i", i);
211
212 // Add to the visualizer
213 vis.addCube (b[0], b[1], b[2], b[3], b[4], b[5], 1.0, 1.0, 0.0, cube_id);
214 }
215 }
216
217 vis.addCoordinateSystem(1.5, "global");
218 vis.resetCamera ();
219
220 // Enter the main loop
221 while (!vis.wasStopped ())
222 {
223 vis.spinOnce (100);
224 std::this_thread::sleep_for(100ms);
225 }
226 }
227
228