1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2012-, Open Perception, Inc.
6 *
7 * All rights reserved.
8 *
9 * Redistribution and use in source and binary forms, with or without
10 * modification, are permitted provided that the following conditions
11 * are met:
12 *
13 * * Redistributions of source code must retain the above copyright
14 * notice, this list of conditions and the following disclaimer.
15 * * Redistributions in binary form must reproduce the above
16 * copyright notice, this list of conditions and the following
17 * disclaimer in the documentation and/or other materials provided
18 * with the distribution.
19 * * Neither the name of the copyright holder(s) nor the names of its
20 * contributors may be used to endorse or promote products derived
21 * from this software without specific prior written permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *
36 * $Id: test_surface.cpp 6579 2012-07-27 18:57:32Z rusu $
37 *
38 */
39
40 #include <pcl/test/gtest.h>
41
42 #include <pcl/point_types.h>
43 #include <pcl/io/pcd_io.h>
44 #include <pcl/io/vtk_io.h>
45 #include <pcl/features/normal_3d.h>
46 #include <pcl/surface/ear_clipping.h>
47 #include <pcl/common/common.h>
48
49 using namespace pcl;
50 using namespace pcl::io;
51
52 PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>);
53 PointCloud<PointNormal>::Ptr cloud_with_normals (new PointCloud<PointNormal>);
54 search::KdTree<PointXYZ>::Ptr tree;
55 search::KdTree<PointNormal>::Ptr tree2;
56
57 // add by ktran to test update functions
58 PointCloud<PointXYZ>::Ptr cloud1 (new PointCloud<PointXYZ>);
59 PointCloud<PointNormal>::Ptr cloud_with_normals1 (new PointCloud<PointNormal>);
60 search::KdTree<PointXYZ>::Ptr tree3;
61 search::KdTree<PointNormal>::Ptr tree4;
62
63 //////////////////////////////////////////////////////////////////////////////////////////////////////////////////
TEST(PCL,EarClipping)64 TEST (PCL, EarClipping)
65 {
66 PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>());
67 cloud->height = 1;
68 cloud->points.emplace_back( 0.f, 0.f, 0.5f);
69 cloud->points.emplace_back( 5.f, 0.f, 0.6f);
70 cloud->points.emplace_back( 9.f, 4.f, 0.5f);
71 cloud->points.emplace_back( 4.f, 7.f, 0.5f);
72 cloud->points.emplace_back( 2.f, 5.f, 0.5f);
73 cloud->points.emplace_back(-1.f, 8.f, 0.5f);
74 cloud->width = cloud->size ();
75
76 Vertices vertices;
77 vertices.vertices.resize (cloud->size ());
78 for (int i = 0; i < static_cast<int> (vertices.vertices.size ()); ++i)
79 vertices.vertices[i] = i;
80
81 PolygonMesh::Ptr mesh (new PolygonMesh);
82 toPCLPointCloud2 (*cloud, mesh->cloud);
83 mesh->polygons.push_back (vertices);
84
85 EarClipping clipper;
86 PolygonMesh::ConstPtr mesh_aux (mesh);
87 clipper.setInputMesh (mesh_aux);
88
89 PolygonMesh triangulated_mesh;
90 clipper.process (triangulated_mesh);
91
92 EXPECT_EQ (triangulated_mesh.polygons.size (), 4);
93 for (const auto &polygon : triangulated_mesh.polygons)
94 EXPECT_EQ (polygon.vertices.size (), 3);
95
96 const int truth[][3] = { {5, 0, 1},
97 {2, 3, 4},
98 {4, 5, 1},
99 {1, 2, 4} };
100
101 for (int pi = 0; pi < static_cast<int> (triangulated_mesh.polygons.size ()); ++pi)
102 for (int vi = 0; vi < 3; ++vi)
103 {
104 EXPECT_EQ (triangulated_mesh.polygons[pi].vertices[vi], truth[pi][vi]);
105 }
106 }
107
TEST(PCL,EarClippingCubeTest)108 TEST (PCL, EarClippingCubeTest)
109 {
110 PointCloud<PointXYZ>::Ptr cloud (new PointCloud<PointXYZ>());
111 cloud->height = 1;
112 //bottom of cube (z=0)
113 cloud->points.emplace_back( 0.f, 0.f, 0.f);
114 cloud->points.emplace_back( 1.f, 0.f, 0.f);
115 cloud->points.emplace_back( 1.f, 1.f, 0.f);
116 cloud->points.emplace_back( 0.f, 1.f, 0.f);
117 //top of cube (z=1.0)
118 cloud->points.emplace_back( 0.f, 0.f, 1.f);
119 cloud->points.emplace_back( 1.f, 0.f, 1.f);
120 cloud->points.emplace_back( 1.f, 1.f, 1.f);
121 cloud->points.emplace_back( 0.f, 1.f, 1.f);
122 cloud->width = cloud->size ();
123
124 Vertices vertices;
125 vertices.vertices.resize(4);
126
127 const int squares[][4] = { {1, 5, 6, 2},
128 {2, 6, 7, 3},
129 {3, 7, 4, 0},
130 {0, 4, 5, 1},
131 {4, 7, 6, 5},
132 {0, 1, 2, 3} };
133
134 const int truth[][3] = { {2, 1, 5},
135 {6, 2, 5},
136 {3, 2, 6},
137 {7, 3, 6},
138 {0, 3, 7},
139 {4, 0, 7},
140 {1, 0, 4},
141 {5, 1, 4},
142 {5, 4, 7},
143 {6, 5, 7},
144 {3, 0, 1},
145 {2, 3, 1} };
146
147 PolygonMesh::Ptr mesh (new PolygonMesh);
148 toPCLPointCloud2 (*cloud, mesh->cloud);
149
150 for (const auto &square : squares)
151 {
152 vertices.vertices[0] = square[0];
153 vertices.vertices[1] = square[1];
154 vertices.vertices[2] = square[2];
155 vertices.vertices[3] = square[3];
156 mesh->polygons.push_back (vertices);
157 }
158
159 EarClipping clipper;
160 PolygonMesh::ConstPtr mesh_aux (mesh);
161 clipper.setInputMesh (mesh_aux);
162
163 PolygonMesh triangulated_mesh;
164 clipper.process (triangulated_mesh);
165
166 EXPECT_EQ (triangulated_mesh.polygons.size (), 12);
167 for (const auto &polygon : triangulated_mesh.polygons)
168 EXPECT_EQ (polygon.vertices.size (), 3);
169
170
171
172 for (int pi = 0; pi < static_cast<int> (triangulated_mesh.polygons.size ()); ++pi)
173 {
174 for (int vi = 0; vi < 3; ++vi)
175 {
176 EXPECT_EQ (triangulated_mesh.polygons[pi].vertices[vi], truth[pi][vi]);
177 }
178 }
179 }
180
181 /* ---[ */
182 int
main(int argc,char ** argv)183 main (int argc, char** argv)
184 {
185 if (argc < 2)
186 {
187 std::cerr << "No test file given. Please download `bun0.pcd` and pass its path to the test." << std::endl;
188 return (-1);
189 }
190
191 // Load file
192 pcl::PCLPointCloud2 cloud_blob;
193 loadPCDFile (argv[1], cloud_blob);
194 fromPCLPointCloud2 (cloud_blob, *cloud);
195
196 // Create search tree
197 tree.reset (new search::KdTree<PointXYZ> (false));
198 tree->setInputCloud (cloud);
199
200 // Normal estimation
201 NormalEstimation<PointXYZ, Normal> n;
202 PointCloud<Normal>::Ptr normals (new PointCloud<Normal> ());
203 n.setInputCloud (cloud);
204 //n.setIndices (indices[B);
205 n.setSearchMethod (tree);
206 n.setKSearch (20);
207 n.compute (*normals);
208
209 // Concatenate XYZ and normal information
210 pcl::concatenateFields (*cloud, *normals, *cloud_with_normals);
211
212 // Create search tree
213 tree2.reset (new search::KdTree<PointNormal>);
214 tree2->setInputCloud (cloud_with_normals);
215
216 // Process for update cloud
217 if(argc == 3){
218 pcl::PCLPointCloud2 cloud_blob1;
219 loadPCDFile (argv[2], cloud_blob1);
220 fromPCLPointCloud2 (cloud_blob1, *cloud1);
221 // Create search tree
222 tree3.reset (new search::KdTree<PointXYZ> (false));
223 tree3->setInputCloud (cloud1);
224
225 // Normal estimation
226 NormalEstimation<PointXYZ, Normal> n1;
227 PointCloud<Normal>::Ptr normals1 (new PointCloud<Normal> ());
228 n1.setInputCloud (cloud1);
229
230 n1.setSearchMethod (tree3);
231 n1.setKSearch (20);
232 n1.compute (*normals1);
233
234 // Concatenate XYZ and normal information
235 pcl::concatenateFields (*cloud1, *normals1, *cloud_with_normals1);
236 // Create search tree
237 tree4.reset (new search::KdTree<PointNormal>);
238 tree4->setInputCloud (cloud_with_normals1);
239 }
240
241 // Testing
242 testing::InitGoogleTest (&argc, argv);
243 return (RUN_ALL_TESTS ());
244 }
245 /* ]--- */
246