1 /*
2 * Software License Agreement (BSD License)
3 *
4 * Point Cloud Library (PCL) - www.pointclouds.org
5 * Copyright (c) 2009-2012, Willow Garage, Inc.
6 * Copyright (c) 2012-, Open Perception, Inc.
7 *
8 * All rights reserved.
9 *
10 * Redistribution and use in source and binary forms, with or without
11 * modification, are permitted provided that the following conditions
12 * are met:
13 *
14 * * Redistributions of source code must retain the above copyright
15 * notice, this list of conditions and the following disclaimer.
16 * * Redistributions in binary form must reproduce the above
17 * copyright notice, this list of conditions and the following
18 * disclaimer in the documentation and/or other materials provided
19 * with the distribution.
20 * * Neither the name of the copyright holder(s) nor the names of its
21 * contributors may be used to endorse or promote products derived
22 * from this software without specific prior written permission.
23 *
24 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
25 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
26 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
27 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
28 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
29 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
30 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
31 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
32 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
33 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
34 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
35 * POSSIBILITY OF SUCH DAMAGE.
36 *
37 * $Id$
38 *
39 */
40
41 #include <pcl/apps/in_hand_scanner/integration.h>
42
43 #include <iostream>
44 #include <vector>
45 #include <limits>
46
47 #include <pcl/kdtree/kdtree_flann.h>
48 #include <pcl/apps/in_hand_scanner/visibility_confidence.h>
49 #include <pcl/apps/in_hand_scanner/utils.h>
50
51 ////////////////////////////////////////////////////////////////////////////////
52
Integration()53 pcl::ihs::Integration::Integration ()
54 : kd_tree_ (new pcl::KdTreeFLANN <PointXYZ> ()),
55 max_squared_distance_ (0.04f), // 0.2cm
56 max_angle_ (45.f),
57 min_weight_ (.3f),
58 max_age_ (30),
59 min_directions_ (5)
60 {
61 }
62
63 ////////////////////////////////////////////////////////////////////////////////
64
65 bool
reconstructMesh(const CloudXYZRGBNormalConstPtr & cloud_data,MeshPtr & mesh_model) const66 pcl::ihs::Integration::reconstructMesh (const CloudXYZRGBNormalConstPtr& cloud_data,
67 MeshPtr& mesh_model) const
68 {
69 if (!cloud_data)
70 {
71 std::cerr << "ERROR in integration.cpp: Cloud pointer is invalid\n";
72 return (false);
73 }
74 if (!cloud_data->isOrganized ())
75 {
76 std::cerr << "ERROR in integration.cpp: Cloud is not organized\n";
77 return (false);
78 }
79 const int width = static_cast <int> (cloud_data->width);
80 const int height = static_cast <int> (cloud_data->height);
81
82 if (!mesh_model) mesh_model = MeshPtr (new Mesh ());
83
84 mesh_model->clear ();
85 mesh_model->reserveVertices (cloud_data->size ());
86 mesh_model->reserveEdges ((width-1) * height + width * (height-1) + (width-1) * (height-1));
87 mesh_model->reserveFaces (2 * (width-1) * (height-1));
88
89 // Store which vertex is set at which position (initialized with invalid indices)
90 VertexIndices vertex_indices (cloud_data->size (), VertexIndex ());
91
92 // Convert to the model cloud type. This is actually not needed but avoids code duplication (see merge). And reconstructMesh is called only the first reconstruction step anyway.
93 // NOTE: The default constructor of PointIHS has to initialize with NaNs!
94 CloudIHSPtr cloud_model (new CloudIHS ());
95 cloud_model->resize (cloud_data->size ());
96
97 // Set the model points not reached by the main loop
98 for (int c=0; c<width; ++c)
99 {
100 const PointXYZRGBNormal& pt_d = cloud_data->operator [] (c);
101 const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1])
102
103 if (!std::isnan (pt_d.x) && weight > min_weight_)
104 {
105 cloud_model->operator [] (c) = PointIHS (pt_d, weight);
106 }
107 }
108 for (int r=1; r<height; ++r)
109 {
110 for (int c=0; c<2; ++c)
111 {
112 const PointXYZRGBNormal& pt_d = cloud_data->operator [] (r*width + c);
113 const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1])
114
115 if (!std::isnan (pt_d.x) && weight > min_weight_)
116 {
117 cloud_model->operator [] (r*width + c) = PointIHS (pt_d, weight);
118 }
119 }
120 }
121
122 // 4 2 - 1 //
123 // | | //
124 // * 3 - 0 //
125 // //
126 // 4 - 2 1 //
127 // \ \ //
128 // * 3 - 0 //
129 const int offset_1 = -width;
130 const int offset_2 = -width - 1;
131 const int offset_3 = - 1;
132 const int offset_4 = -width - 2;
133
134 for (int r=1; r<height; ++r)
135 {
136 for (int c=2; c<width; ++c)
137 {
138 const int ind_0 = r*width + c;
139 const int ind_1 = ind_0 + offset_1;
140 const int ind_2 = ind_0 + offset_2;
141 const int ind_3 = ind_0 + offset_3;
142 const int ind_4 = ind_0 + offset_4;
143
144 assert (ind_0 >= 0 && ind_0 < static_cast <int> (cloud_data->size ()));
145 assert (ind_1 >= 0 && ind_1 < static_cast <int> (cloud_data->size ()));
146 assert (ind_2 >= 0 && ind_2 < static_cast <int> (cloud_data->size ()));
147 assert (ind_3 >= 0 && ind_3 < static_cast <int> (cloud_data->size ()));
148 assert (ind_4 >= 0 && ind_4 < static_cast <int> (cloud_data->size ()));
149
150 const PointXYZRGBNormal& pt_d_0 = cloud_data->operator [] (ind_0);
151 PointIHS& pt_m_0 = cloud_model->operator [] (ind_0);
152 const PointIHS& pt_m_1 = cloud_model->operator [] (ind_1);
153 const PointIHS& pt_m_2 = cloud_model->operator [] (ind_2);
154 const PointIHS& pt_m_3 = cloud_model->operator [] (ind_3);
155 const PointIHS& pt_m_4 = cloud_model->operator [] (ind_4);
156
157 VertexIndex& vi_0 = vertex_indices [ind_0];
158 VertexIndex& vi_1 = vertex_indices [ind_1];
159 VertexIndex& vi_2 = vertex_indices [ind_2];
160 VertexIndex& vi_3 = vertex_indices [ind_3];
161 VertexIndex& vi_4 = vertex_indices [ind_4];
162
163 const float weight = -pt_d_0.normal_z; // weight = -dot (normal, [0; 0; 1])
164
165 if (!std::isnan (pt_d_0.x) && weight > min_weight_)
166 {
167 pt_m_0 = PointIHS (pt_d_0, weight);
168 }
169
170 this->addToMesh (pt_m_0,pt_m_1,pt_m_2,pt_m_3, vi_0,vi_1,vi_2,vi_3, mesh_model);
171 if (Mesh::IsManifold::value) // Only needed for the manifold mesh
172 {
173 this->addToMesh (pt_m_0,pt_m_2,pt_m_4,pt_m_3, vi_0,vi_2,vi_4,vi_3, mesh_model);
174 }
175 }
176 }
177
178 return (true);
179 }
180
181 ////////////////////////////////////////////////////////////////////////////////
182
183 bool
merge(const CloudXYZRGBNormalConstPtr & cloud_data,MeshPtr & mesh_model,const Eigen::Matrix4f & T) const184 pcl::ihs::Integration::merge (const CloudXYZRGBNormalConstPtr& cloud_data,
185 MeshPtr& mesh_model,
186 const Eigen::Matrix4f& T) const
187 {
188 if (!cloud_data)
189 {
190 std::cerr << "ERROR in integration.cpp: Cloud pointer is invalid\n";
191 return (false);
192 }
193 if (!cloud_data->isOrganized ())
194 {
195 std::cerr << "ERROR in integration.cpp: Data cloud is not organized\n";
196 return (false);
197 }
198 if (!mesh_model)
199 {
200 std::cerr << "ERROR in integration.cpp: Mesh pointer is invalid\n";
201 return (false);
202 }
203 if (!mesh_model->sizeVertices ())
204 {
205 std::cerr << "ERROR in integration.cpp: Model mesh is empty\n";
206 return (false);
207 }
208
209 const int width = static_cast <int> (cloud_data->width);
210 const int height = static_cast <int> (cloud_data->height);
211
212 // Nearest neighbor search
213 CloudXYZPtr xyz_model (new CloudXYZ ());
214 xyz_model->reserve (mesh_model->sizeVertices ());
215 for (std::size_t i=0; i<mesh_model->sizeVertices (); ++i)
216 {
217 const PointIHS& pt = mesh_model->getVertexDataCloud () [i];
218 xyz_model->push_back (PointXYZ (pt.x, pt.y, pt.z));
219 }
220 kd_tree_->setInputCloud (xyz_model);
221 pcl::Indices index (1);
222 std::vector <float> squared_distance (1);
223
224 mesh_model->reserveVertices (mesh_model->sizeVertices () + cloud_data->size ());
225 mesh_model->reserveEdges (mesh_model->sizeEdges () + (width-1) * height + width * (height-1) + (width-1) * (height-1));
226 mesh_model->reserveFaces (mesh_model->sizeFaces () + 2 * (width-1) * (height-1));
227
228 // Data cloud in model coordinates (this does not change the connectivity information) and weights
229 CloudIHSPtr cloud_data_transformed (new CloudIHS ());
230 cloud_data_transformed->resize (cloud_data->size ());
231
232 // Sensor position in model coordinates
233 const Eigen::Vector4f& sensor_eye = T * Eigen::Vector4f (0.f, 0.f, 0.f, 1.f);
234
235 // Store which vertex is set at which position (initialized with invalid indices)
236 VertexIndices vertex_indices (cloud_data->size (), VertexIndex ());
237
238 // Set the transformed points not reached by the main loop
239 for (int c=0; c<width; ++c)
240 {
241 const PointXYZRGBNormal& pt_d = cloud_data->operator [] (c);
242 const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1])
243
244 if (!std::isnan (pt_d.x) && weight > min_weight_)
245 {
246 PointIHS& pt_d_t = cloud_data_transformed->operator [] (c);
247 pt_d_t = PointIHS (pt_d, weight);
248 pt_d_t.getVector4fMap () = T * pt_d_t.getVector4fMap ();
249 pt_d_t.getNormalVector4fMap () = T * pt_d_t.getNormalVector4fMap ();
250 }
251 }
252 for (int r=1; r<height; ++r)
253 {
254 for (int c=0; c<2; ++c)
255 {
256 const PointXYZRGBNormal& pt_d = cloud_data->operator [] (r*width + c);
257 const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1])
258
259 if (!std::isnan (pt_d.x) && weight > min_weight_)
260 {
261 PointIHS& pt_d_t = cloud_data_transformed->operator [] (r*width + c);
262 pt_d_t = PointIHS (pt_d, weight);
263 pt_d_t.getVector4fMap () = T * pt_d_t.getVector4fMap ();
264 pt_d_t.getNormalVector4fMap () = T * pt_d_t.getNormalVector4fMap ();
265 }
266 }
267 }
268
269 // 4 2 - 1 //
270 // | | //
271 // * 3 - 0 //
272 // //
273 // 4 - 2 1 //
274 // \ \ //
275 // * 3 - 0 //
276 const int offset_1 = -width;
277 const int offset_2 = -width - 1;
278 const int offset_3 = - 1;
279 const int offset_4 = -width - 2;
280
281 const float dot_min = std::cos (max_angle_ * 17.45329252e-3); // deg to rad
282
283 for (int r=1; r<height; ++r)
284 {
285 for (int c=2; c<width; ++c)
286 {
287 const int ind_0 = r*width + c;
288 const int ind_1 = ind_0 + offset_1;
289 const int ind_2 = ind_0 + offset_2;
290 const int ind_3 = ind_0 + offset_3;
291 const int ind_4 = ind_0 + offset_4;
292
293 assert (ind_0 >= 0 && ind_0 < static_cast <int> (cloud_data->size ()));
294 assert (ind_1 >= 0 && ind_1 < static_cast <int> (cloud_data->size ()));
295 assert (ind_2 >= 0 && ind_2 < static_cast <int> (cloud_data->size ()));
296 assert (ind_3 >= 0 && ind_3 < static_cast <int> (cloud_data->size ()));
297 assert (ind_4 >= 0 && ind_4 < static_cast <int> (cloud_data->size ()));
298
299 const PointXYZRGBNormal& pt_d_0 = cloud_data->operator [] (ind_0);
300 PointIHS& pt_d_t_0 = cloud_data_transformed->operator [] (ind_0);
301 const PointIHS& pt_d_t_1 = cloud_data_transformed->operator [] (ind_1);
302 const PointIHS& pt_d_t_2 = cloud_data_transformed->operator [] (ind_2);
303 const PointIHS& pt_d_t_3 = cloud_data_transformed->operator [] (ind_3);
304 const PointIHS& pt_d_t_4 = cloud_data_transformed->operator [] (ind_4);
305
306 VertexIndex& vi_0 = vertex_indices [ind_0];
307
308 const float weight = -pt_d_0.normal_z; // weight = -dot (normal, [0; 0; 1])
309
310 if (!std::isnan (pt_d_0.x) && weight > min_weight_)
311 {
312 pt_d_t_0 = PointIHS (pt_d_0, weight);
313 pt_d_t_0.getVector4fMap () = T * pt_d_t_0.getVector4fMap ();
314 pt_d_t_0.getNormalVector4fMap () = T * pt_d_t_0.getNormalVector4fMap ();
315
316 pcl::PointXYZ tmp; tmp.getVector4fMap () = pt_d_t_0.getVector4fMap ();
317
318 // NN search
319 if (!kd_tree_->nearestKSearch (tmp, 1, index, squared_distance))
320 {
321 std::cerr << "ERROR in integration.cpp: nearestKSearch failed!\n";
322 return (false);
323 }
324
325 // Average out corresponding points
326 if (squared_distance [0] <= max_squared_distance_)
327 {
328 PointIHS& pt_m = mesh_model->getVertexDataCloud () [index [0]]; // Non-const reference!
329
330 if (pt_m.getNormalVector4fMap ().dot (pt_d_t_0.getNormalVector4fMap ()) >= dot_min)
331 {
332 vi_0 = VertexIndex (index [0]);
333
334 const float W = pt_m.weight; // Old accumulated weight
335 const float w = pt_d_t_0.weight; // Weight of new point
336 const float WW = pt_m.weight = W + w; // New accumulated weight
337
338 const float r_m = static_cast <float> (pt_m.r);
339 const float g_m = static_cast <float> (pt_m.g);
340 const float b_m = static_cast <float> (pt_m.b);
341
342 const float r_d = static_cast <float> (pt_d_t_0.r);
343 const float g_d = static_cast <float> (pt_d_t_0.g);
344 const float b_d = static_cast <float> (pt_d_t_0.b);
345
346 pt_m.getVector4fMap () = ( W*pt_m.getVector4fMap () + w*pt_d_t_0.getVector4fMap ()) / WW;
347 pt_m.getNormalVector4fMap () = ((W*pt_m.getNormalVector4fMap () + w*pt_d_t_0.getNormalVector4fMap ()) / WW).normalized ();
348 pt_m.r = this->trimRGB ((W*r_m + w*r_d) / WW);
349 pt_m.g = this->trimRGB ((W*g_m + w*g_d) / WW);
350 pt_m.b = this->trimRGB ((W*b_m + w*b_d) / WW);
351
352 // Point has been observed again -> give it some extra time to live
353 pt_m.age = 0;
354
355 // Add a direction
356 pcl::ihs::addDirection (pt_m.getNormalVector4fMap (), sensor_eye-pt_m.getVector4fMap (), pt_m.directions);
357
358 } // dot normals
359 } // squared distance
360 } // !isnan && min weight
361
362 // Connect
363 // 4 2 - 1 //
364 // | | //
365 // * 3 - 0 //
366 // //
367 // 4 - 2 1 //
368 // \ \ //
369 // * 3 - 0 //
370
371 VertexIndex& vi_1 = vertex_indices [ind_1];
372 VertexIndex& vi_2 = vertex_indices [ind_2];
373 VertexIndex& vi_3 = vertex_indices [ind_3];
374 VertexIndex& vi_4 = vertex_indices [ind_4];
375
376 this->addToMesh (pt_d_t_0,pt_d_t_1,pt_d_t_2,pt_d_t_3, vi_0,vi_1,vi_2,vi_3, mesh_model);
377 if (Mesh::IsManifold::value) // Only needed for the manifold mesh
378 {
379 this->addToMesh (pt_d_t_0,pt_d_t_2,pt_d_t_4,pt_d_t_3, vi_0,vi_2,vi_4,vi_3, mesh_model);
380 }
381 }
382 }
383
384 return (true);
385 }
386
387 ////////////////////////////////////////////////////////////////////////////////
388
389 void
age(const MeshPtr & mesh,const bool cleanup) const390 pcl::ihs::Integration::age (const MeshPtr& mesh, const bool cleanup) const
391 {
392 for (std::size_t i=0; i<mesh->sizeVertices (); ++i)
393 {
394 PointIHS& pt = mesh->getVertexDataCloud () [i];
395 if (pt.age < max_age_)
396 {
397 // Point survives
398 ++(pt.age);
399 }
400 else if (pt.age == max_age_) // Judgement Day
401 {
402 if (pcl::ihs::countDirections (pt.directions) < min_directions_)
403 {
404 // Point dies (no need to transform it)
405 mesh->deleteVertex (VertexIndex (i));
406 }
407 else
408 {
409 // Point becomes immortal
410 pt.age = std::numeric_limits <unsigned int>::max ();
411 }
412 }
413 }
414
415 if (cleanup)
416 {
417 mesh->cleanUp ();
418 }
419 }
420
421 ////////////////////////////////////////////////////////////////////////////////
422
423 void
removeUnfitVertices(const MeshPtr & mesh,const bool cleanup) const424 pcl::ihs::Integration::removeUnfitVertices (const MeshPtr& mesh, const bool cleanup) const
425 {
426 for (std::size_t i=0; i<mesh->sizeVertices (); ++i)
427 {
428 if (pcl::ihs::countDirections (mesh->getVertexDataCloud () [i].directions) < min_directions_)
429 {
430 // Point dies (no need to transform it)
431 mesh->deleteVertex (VertexIndex (i));
432 }
433 }
434
435 if (cleanup)
436 {
437 mesh->cleanUp ();
438 }
439 }
440
441 ////////////////////////////////////////////////////////////////////////////////
442
443 void
setMaxSquaredDistance(const float squared_distance)444 pcl::ihs::Integration::setMaxSquaredDistance (const float squared_distance)
445 {
446 if (squared_distance > 0) max_squared_distance_ = squared_distance;
447 }
448
449 float
getMaxSquaredDistance() const450 pcl::ihs::Integration::getMaxSquaredDistance () const
451 {
452 return (max_squared_distance_);
453 }
454
455 ////////////////////////////////////////////////////////////////////////////////
456
457 void
setMaxAngle(const float angle)458 pcl::ihs::Integration::setMaxAngle (const float angle)
459 {
460 max_angle_ = pcl::ihs::clamp (angle, 0.f, 180.f);
461 }
462
463 float
getMaxAngle() const464 pcl::ihs::Integration::getMaxAngle () const
465 {
466 return (max_angle_);
467 }
468
469 ////////////////////////////////////////////////////////////////////////////////
470
471 void
setMaxAge(const unsigned int age)472 pcl::ihs::Integration::setMaxAge (const unsigned int age)
473 {
474 max_age_ = age < 1 ? 1 : age;
475 }
476
477 unsigned int
getMaxAge() const478 pcl::ihs::Integration::getMaxAge () const
479 {
480 return (max_age_);
481 }
482
483 ////////////////////////////////////////////////////////////////////////////////
484
485 void
setMinDirections(const unsigned int directions)486 pcl::ihs::Integration::setMinDirections (const unsigned int directions)
487 {
488 min_directions_ = directions < 1 ? 1 : directions;
489 }
490
491 unsigned int
getMinDirections() const492 pcl::ihs::Integration::getMinDirections () const
493 {
494 return (min_directions_);
495 }
496
497 ////////////////////////////////////////////////////////////////////////////////
498
499 std::uint8_t
trimRGB(const float val) const500 pcl::ihs::Integration::trimRGB (const float val) const
501 {
502 return (static_cast <std::uint8_t> (val > 255.f ? 255 : val));
503 }
504
505 ////////////////////////////////////////////////////////////////////////////////
506
507 void
addToMesh(const PointIHS & pt_0,const PointIHS & pt_1,const PointIHS & pt_2,const PointIHS & pt_3,VertexIndex & vi_0,VertexIndex & vi_1,VertexIndex & vi_2,VertexIndex & vi_3,const MeshPtr & mesh) const508 pcl::ihs::Integration::addToMesh (const PointIHS& pt_0,
509 const PointIHS& pt_1,
510 const PointIHS& pt_2,
511 const PointIHS& pt_3,
512 VertexIndex& vi_0,
513 VertexIndex& vi_1,
514 VertexIndex& vi_2,
515 VertexIndex& vi_3,
516 const MeshPtr& mesh) const
517 {
518 // Treated bitwise
519 // 2 - 1
520 // | |
521 // 3 - 0
522 const unsigned char is_finite = static_cast <unsigned char> (
523 (1 * !std::isnan (pt_0.x)) |
524 (2 * !std::isnan (pt_1.x)) |
525 (4 * !std::isnan (pt_2.x)) |
526 (8 * !std::isnan (pt_3.x)));
527
528 switch (is_finite)
529 {
530 case 7: this->addToMesh (pt_0, pt_1, pt_2, vi_0, vi_1, vi_2, mesh); break; // 0-1-2
531 case 11: this->addToMesh (pt_0, pt_1, pt_3, vi_0, vi_1, vi_3, mesh); break; // 0-1-3
532 case 13: this->addToMesh (pt_0, pt_2, pt_3, vi_0, vi_2, vi_3, mesh); break; // 0-2-3
533 case 14: this->addToMesh (pt_1, pt_2, pt_3, vi_1, vi_2, vi_3, mesh); break; // 1-2-3
534 case 15: // 0-1-2-3
535 {
536 if (!distanceThreshold (pt_0, pt_1, pt_2, pt_3)) break;
537 if (!vi_0.isValid ()) vi_0 = mesh->addVertex (pt_0);
538 if (!vi_1.isValid ()) vi_1 = mesh->addVertex (pt_1);
539 if (!vi_2.isValid ()) vi_2 = mesh->addVertex (pt_2);
540 if (!vi_3.isValid ()) vi_3 = mesh->addVertex (pt_3);
541 if (vi_0==vi_1 || vi_0==vi_2 || vi_0==vi_3 || vi_1==vi_2 || vi_1==vi_3 || vi_2==vi_3)
542 {
543 return;
544 }
545 mesh->addTrianglePair (vi_0, vi_1, vi_2, vi_3);
546
547 break;
548 }
549 }
550 }
551
552 ////////////////////////////////////////////////////////////////////////////////
553
554 void
addToMesh(const PointIHS & pt_0,const PointIHS & pt_1,const PointIHS & pt_2,VertexIndex & vi_0,VertexIndex & vi_1,VertexIndex & vi_2,const MeshPtr & mesh) const555 pcl::ihs::Integration::addToMesh (const PointIHS& pt_0,
556 const PointIHS& pt_1,
557 const PointIHS& pt_2,
558 VertexIndex& vi_0,
559 VertexIndex& vi_1,
560 VertexIndex& vi_2,
561 const MeshPtr& mesh) const
562 {
563 if (!distanceThreshold (pt_0, pt_1, pt_2)) return;
564
565 if (!vi_0.isValid ()) vi_0 = mesh->addVertex (pt_0);
566 if (!vi_1.isValid ()) vi_1 = mesh->addVertex (pt_1);
567 if (!vi_2.isValid ()) vi_2 = mesh->addVertex (pt_2);
568 if (vi_0==vi_1 || vi_0==vi_2 || vi_1==vi_2)
569 {
570 return;
571 }
572 mesh->addFace (vi_0, vi_1, vi_2);
573 }
574
575 ////////////////////////////////////////////////////////////////////////////////
576
577 bool
distanceThreshold(const PointIHS & pt_0,const PointIHS & pt_1,const PointIHS & pt_2) const578 pcl::ihs::Integration::distanceThreshold (const PointIHS& pt_0,
579 const PointIHS& pt_1,
580 const PointIHS& pt_2) const
581 {
582 if ((pt_0.getVector3fMap () - pt_1.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false);
583 if ((pt_1.getVector3fMap () - pt_2.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false);
584 if ((pt_2.getVector3fMap () - pt_0.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false);
585 return (true);
586 }
587
588 ////////////////////////////////////////////////////////////////////////////////
589
590 bool
distanceThreshold(const PointIHS & pt_0,const PointIHS & pt_1,const PointIHS & pt_2,const PointIHS & pt_3) const591 pcl::ihs::Integration::distanceThreshold (const PointIHS& pt_0,
592 const PointIHS& pt_1,
593 const PointIHS& pt_2,
594 const PointIHS& pt_3) const
595 {
596 if ((pt_0.getVector3fMap () - pt_1.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false);
597 if ((pt_1.getVector3fMap () - pt_2.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false);
598 if ((pt_2.getVector3fMap () - pt_3.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false);
599 if ((pt_3.getVector3fMap () - pt_0.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false);
600 return (true);
601 }
602
603 ////////////////////////////////////////////////////////////////////////////////
604