integration.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Point Cloud Library (PCL) - www.pointclouds.org
00005  * Copyright (c) 2009-2012, Willow Garage, Inc.
00006  * Copyright (c) 2012-, Open Perception, Inc.
00007  *
00008  * All rights reserved.
00009  *
00010  * Redistribution and use in source and binary forms, with or without
00011  * modification, are permitted provided that the following conditions
00012  * are met:
00013  *
00014  *  * Redistributions of source code must retain the above copyright
00015  *    notice, this list of conditions and the following disclaimer.
00016  *  * Redistributions in binary form must reproduce the above
00017  *    copyright notice, this list of conditions and the following
00018  *    disclaimer in the documentation and/or other materials provided
00019  *    with the distribution.
00020  *  * Neither the name of the copyright holder(s) nor the names of its
00021  *    contributors may be used to endorse or promote products derived
00022  *    from this software without specific prior written permission.
00023  *
00024  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  * POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  * $Id$
00038  *
00039  */
00040 
00041 #include <pcl/apps/in_hand_scanner/integration.h>
00042 
00043 #include <iostream>
00044 #include <vector>
00045 #include <limits>
00046 
00047 #include <pcl/kdtree/kdtree_flann.h>
00048 #include <pcl/apps/in_hand_scanner/boost.h>
00049 #include <pcl/apps/in_hand_scanner/visibility_confidence.h>
00050 #include <pcl/apps/in_hand_scanner/utils.h>
00051 
00053 
00054 pcl::ihs::Integration::Integration ()
00055   : kd_tree_              (new pcl::KdTreeFLANN <PointXYZ> ()),
00056     max_squared_distance_ (0.04f), // 0.2cm
00057     max_angle_            (45.f),
00058     min_weight_           (.3f),
00059     max_age_              (999),
00060     min_directions_       (1)
00061 {
00062 }
00063 
00065 
00066 bool
00067 pcl::ihs::Integration::reconstructMesh (const CloudXYZRGBNormalConstPtr& cloud_data,
00068                                         MeshPtr&                         mesh_model) const
00069 {
00070   if (!cloud_data)
00071   {
00072     std::cerr << "ERROR in integration.cpp: Cloud pointer is invalid\n";
00073     return (false);
00074   }
00075   if (!cloud_data->isOrganized ())
00076   {
00077     std::cerr << "ERROR in integration.cpp: Cloud is not organized\n";
00078     return (false);
00079   }
00080   const int width  = static_cast <int> (cloud_data->width);
00081   const int height = static_cast <int> (cloud_data->height);
00082 
00083   if (!mesh_model) mesh_model = MeshPtr (new Mesh ());
00084 
00085   mesh_model->clear ();
00086   mesh_model->reserveVertices (cloud_data->size ());
00087   mesh_model->reserveEdges ((width-1) * height + width * (height-1) + (width-1) * (height-1));
00088   mesh_model->reserveFaces (2 * (width-1) * (height-1));
00089 
00090   // Store which vertex is set at which position (initialized with invalid indices)
00091   VertexIndices vertex_indices (cloud_data->size (), VertexIndex ());
00092 
00093   // 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.
00094   // NOTE: The default constructor of PointIHS has to initialize with NaNs!
00095   CloudIHSPtr cloud_model (new CloudIHS ());
00096   cloud_model->resize (cloud_data->size ());
00097 
00098   // Set the model points not reached by the main loop
00099   for (int c=0; c<width; ++c)
00100   {
00101     const PointXYZRGBNormal& pt_d = cloud_data->operator [] (c);
00102     const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1])
00103 
00104     if (!boost::math::isnan (pt_d.x) && weight > min_weight_)
00105     {
00106       cloud_model->operator [] (c) = PointIHS (pt_d, weight);
00107     }
00108   }
00109   for (int r=1; r<height; ++r)
00110   {
00111     for (int c=0; c<2; ++c)
00112     {
00113       const PointXYZRGBNormal& pt_d = cloud_data->operator [] (r*width + c);
00114       const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1])
00115 
00116       if (!boost::math::isnan (pt_d.x) && weight > min_weight_)
00117       {
00118         cloud_model->operator [] (r*width + c) = PointIHS (pt_d, weight);
00119       }
00120     }
00121   }
00122 
00123   // 4   2 - 1  //
00124   //     |   |  //
00125   // *   3 - 0  //
00126   //            //
00127   // 4 - 2   1  //
00128   //   \   \    //
00129   // *   3 - 0  //
00130   const int offset_1 = -width;
00131   const int offset_2 = -width - 1;
00132   const int offset_3 =        - 1;
00133   const int offset_4 = -width - 2;
00134 
00135   for (int r=1; r<height; ++r)
00136   {
00137     for (int c=2; c<width; ++c)
00138     {
00139       const int ind_0 = r*width + c;
00140       const int ind_1 = ind_0 + offset_1;
00141       const int ind_2 = ind_0 + offset_2;
00142       const int ind_3 = ind_0 + offset_3;
00143       const int ind_4 = ind_0 + offset_4;
00144 
00145       assert (ind_0 >= 0 && ind_0 < static_cast <int> (cloud_data->size ()));
00146       assert (ind_1 >= 0 && ind_1 < static_cast <int> (cloud_data->size ()));
00147       assert (ind_2 >= 0 && ind_2 < static_cast <int> (cloud_data->size ()));
00148       assert (ind_3 >= 0 && ind_3 < static_cast <int> (cloud_data->size ()));
00149       assert (ind_4 >= 0 && ind_4 < static_cast <int> (cloud_data->size ()));
00150 
00151       const PointXYZRGBNormal& pt_d_0 = cloud_data->operator  [] (ind_0);
00152       PointIHS&                pt_m_0 = cloud_model->operator [] (ind_0);
00153       const PointIHS&          pt_m_1 = cloud_model->operator [] (ind_1);
00154       const PointIHS&          pt_m_2 = cloud_model->operator [] (ind_2);
00155       const PointIHS&          pt_m_3 = cloud_model->operator [] (ind_3);
00156       const PointIHS&          pt_m_4 = cloud_model->operator [] (ind_4);
00157 
00158       VertexIndex& vi_0 = vertex_indices [ind_0];
00159       VertexIndex& vi_1 = vertex_indices [ind_1];
00160       VertexIndex& vi_2 = vertex_indices [ind_2];
00161       VertexIndex& vi_3 = vertex_indices [ind_3];
00162       VertexIndex& vi_4 = vertex_indices [ind_4];
00163 
00164       const float weight = -pt_d_0.normal_z; // weight = -dot (normal, [0; 0; 1])
00165 
00166       if (!boost::math::isnan (pt_d_0.x) && weight > min_weight_)
00167       {
00168         pt_m_0 = PointIHS (pt_d_0, weight);
00169       }
00170 
00171       this->addToMesh (pt_m_0,pt_m_1,pt_m_2,pt_m_3, vi_0,vi_1,vi_2,vi_3, mesh_model);
00172       if (Mesh::IsManifold::value) // Only needed for the manifold mesh
00173       {
00174         this->addToMesh (pt_m_0,pt_m_2,pt_m_4,pt_m_3, vi_0,vi_2,vi_4,vi_3, mesh_model);
00175       }
00176     }
00177   }
00178 
00179   return (true);
00180 }
00181 
00183 
00184 bool
00185 pcl::ihs::Integration::merge (const CloudXYZRGBNormalConstPtr& cloud_data,
00186                               MeshPtr&                         mesh_model,
00187                               const Eigen::Matrix4f&           T) const
00188 {
00189   if (!cloud_data)
00190   {
00191     std::cerr << "ERROR in integration.cpp: Cloud pointer is invalid\n";
00192     return (false);
00193   }
00194   if (!cloud_data->isOrganized ())
00195   {
00196     std::cerr << "ERROR in integration.cpp: Data cloud is not organized\n";
00197     return (false);
00198   }
00199   if (!mesh_model)
00200   {
00201     std::cerr << "ERROR in integration.cpp: Mesh pointer is invalid\n";
00202     return (false);
00203   }
00204   if (!mesh_model->sizeVertices ())
00205   {
00206     std::cerr << "ERROR in integration.cpp: Model mesh is empty\n";
00207     return (false);
00208   }
00209 
00210   const int width  = static_cast <int> (cloud_data->width);
00211   const int height = static_cast <int> (cloud_data->height);
00212 
00213   // Nearest neighbor search
00214   CloudXYZPtr xyz_model (new CloudXYZ ());
00215   xyz_model->reserve (mesh_model->sizeVertices ());
00216   for (unsigned int i=0; i<mesh_model->sizeVertices (); ++i)
00217   {
00218     const PointIHS& pt = mesh_model->getVertexDataCloud () [i];
00219     xyz_model->push_back (PointXYZ (pt.x, pt.y, pt.z));
00220   }
00221   kd_tree_->setInputCloud (xyz_model);
00222   std::vector <int>   index (1);
00223   std::vector <float> squared_distance (1);
00224 
00225   mesh_model->reserveVertices (mesh_model->sizeVertices () + cloud_data->size ());
00226   mesh_model->reserveEdges    (mesh_model->sizeEdges    () + (width-1) * height + width * (height-1) + (width-1) * (height-1));
00227   mesh_model->reserveFaces    (mesh_model->sizeFaces    () + 2 * (width-1) * (height-1));
00228 
00229   // Data cloud in model coordinates (this does not change the connectivity information) and weights
00230   CloudIHSPtr cloud_data_transformed (new CloudIHS ());
00231   cloud_data_transformed->resize (cloud_data->size ());
00232 
00233   // Sensor position in model coordinates
00234   const Eigen::Vector4f& sensor_eye = T * Eigen::Vector4f (0.f, 0.f, 0.f, 1.f);
00235 
00236   // Store which vertex is set at which position (initialized with invalid indices)
00237   VertexIndices vertex_indices (cloud_data->size (), VertexIndex ());
00238 
00239   // Set the transformed points not reached by the main loop
00240   for (int c=0; c<width; ++c)
00241   {
00242     const PointXYZRGBNormal& pt_d = cloud_data->operator [] (c);
00243     const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1])
00244 
00245     if (!boost::math::isnan (pt_d.x) && weight > min_weight_)
00246     {
00247       PointIHS& pt_d_t = cloud_data_transformed->operator [] (c);
00248       pt_d_t = PointIHS (pt_d, weight);
00249       pt_d_t.getVector4fMap ()       = T * pt_d_t.getVector4fMap ();
00250       pt_d_t.getNormalVector4fMap () = T * pt_d_t.getNormalVector4fMap ();
00251     }
00252   }
00253   for (int r=1; r<height; ++r)
00254   {
00255     for (int c=0; c<2; ++c)
00256     {
00257       const PointXYZRGBNormal& pt_d = cloud_data->operator [] (r*width + c);
00258       const float weight = -pt_d.normal_z; // weight = -dot (normal, [0; 0; 1])
00259 
00260       if (!boost::math::isnan (pt_d.x) && weight > min_weight_)
00261       {
00262         PointIHS& pt_d_t = cloud_data_transformed->operator [] (r*width + c);
00263         pt_d_t = PointIHS (pt_d, weight);
00264         pt_d_t.getVector4fMap ()       = T * pt_d_t.getVector4fMap ();
00265         pt_d_t.getNormalVector4fMap () = T * pt_d_t.getNormalVector4fMap ();
00266       }
00267     }
00268   }
00269 
00270   // 4   2 - 1  //
00271   //     |   |  //
00272   // *   3 - 0  //
00273   //            //
00274   // 4 - 2   1  //
00275   //   \   \    //
00276   // *   3 - 0  //
00277   const int offset_1 = -width;
00278   const int offset_2 = -width - 1;
00279   const int offset_3 =        - 1;
00280   const int offset_4 = -width - 2;
00281 
00282   const float dot_min = std::cos (max_angle_ * 17.45329252e-3); // deg to rad
00283 
00284   for (int r=1; r<height; ++r)
00285   {
00286     for (int c=2; c<width; ++c)
00287     {
00288       const int ind_0 = r*width + c;
00289       const int ind_1 = ind_0 + offset_1;
00290       const int ind_2 = ind_0 + offset_2;
00291       const int ind_3 = ind_0 + offset_3;
00292       const int ind_4 = ind_0 + offset_4;
00293 
00294       assert (ind_0 >= 0 && ind_0 < static_cast <int> (cloud_data->size ()));
00295       assert (ind_1 >= 0 && ind_1 < static_cast <int> (cloud_data->size ()));
00296       assert (ind_2 >= 0 && ind_2 < static_cast <int> (cloud_data->size ()));
00297       assert (ind_3 >= 0 && ind_3 < static_cast <int> (cloud_data->size ()));
00298       assert (ind_4 >= 0 && ind_4 < static_cast <int> (cloud_data->size ()));
00299 
00300       const PointXYZRGBNormal& pt_d_0   = cloud_data->operator             [] (ind_0);
00301       PointIHS&                pt_d_t_0 = cloud_data_transformed->operator [] (ind_0);
00302       const PointIHS&          pt_d_t_1 = cloud_data_transformed->operator [] (ind_1);
00303       const PointIHS&          pt_d_t_2 = cloud_data_transformed->operator [] (ind_2);
00304       const PointIHS&          pt_d_t_3 = cloud_data_transformed->operator [] (ind_3);
00305       const PointIHS&          pt_d_t_4 = cloud_data_transformed->operator [] (ind_4);
00306 
00307       VertexIndex& vi_0 = vertex_indices [ind_0];
00308       VertexIndex& vi_1 = vertex_indices [ind_1];
00309       VertexIndex& vi_2 = vertex_indices [ind_2];
00310       VertexIndex& vi_3 = vertex_indices [ind_3];
00311       VertexIndex& vi_4 = vertex_indices [ind_4];
00312 
00313       const float weight = -pt_d_0.normal_z; // weight = -dot (normal, [0; 0; 1])
00314 
00315       if (!boost::math::isnan (pt_d_0.x) && weight > min_weight_)
00316       {
00317         pt_d_t_0 = PointIHS (pt_d_0, weight);
00318         pt_d_t_0.getVector4fMap ()       = T * pt_d_t_0.getVector4fMap ();
00319         pt_d_t_0.getNormalVector4fMap () = T * pt_d_t_0.getNormalVector4fMap ();
00320 
00321         pcl::PointXYZ tmp; tmp.getVector4fMap () = pt_d_t_0.getVector4fMap ();
00322 
00323         // NN search
00324         if (!kd_tree_->nearestKSearch (tmp, 1, index, squared_distance))
00325         {
00326           std::cerr << "ERROR in integration.cpp: nearestKSearch failed!\n";
00327           return (false);
00328         }
00329 
00330         // Average out corresponding points
00331         if (squared_distance [0] <= max_squared_distance_)
00332         {
00333           PointIHS& pt_m = mesh_model->getVertexDataCloud () [index [0]]; // Non-const reference!
00334 
00335           if (pt_m.getNormalVector4fMap ().dot (pt_d_t_0.getNormalVector4fMap ()) >= dot_min)
00336           {
00337             vi_0 = VertexIndex (index [0]);
00338 
00339             const float W   = pt_m.weight;         // Old accumulated weight
00340             const float w   = pt_d_t_0.weight;     // Weight of new point
00341             const float WW  = pt_m.weight = W + w; // New accumulated weight
00342 
00343             const float r_m = static_cast <float> (pt_m.r);
00344             const float g_m = static_cast <float> (pt_m.g);
00345             const float b_m = static_cast <float> (pt_m.b);
00346 
00347             const float r_d = static_cast <float> (pt_d_t_0.r);
00348             const float g_d = static_cast <float> (pt_d_t_0.g);
00349             const float b_d = static_cast <float> (pt_d_t_0.b);
00350 
00351             pt_m.getVector4fMap ()       = ( W*pt_m.getVector4fMap ()       + w*pt_d_t_0.getVector4fMap ())       / WW;
00352             pt_m.getNormalVector4fMap () = ((W*pt_m.getNormalVector4fMap () + w*pt_d_t_0.getNormalVector4fMap ()) / WW).normalized ();
00353             pt_m.r                       = this->trimRGB ((W*r_m + w*r_d) / WW);
00354             pt_m.g                       = this->trimRGB ((W*g_m + w*g_d) / WW);
00355             pt_m.b                       = this->trimRGB ((W*b_m + w*b_d) / WW);
00356 
00357             // Point has been observed again -> give it some extra time to live
00358             pt_m.age = 0;
00359 
00360             // Add a direction
00361             pcl::ihs::addDirection (pt_m.getNormalVector4fMap (), sensor_eye-pt_m.getVector4fMap (), pt_m.directions);
00362 
00363           } // dot normals
00364         } // squared distance
00365       } // !isnan && min weight
00366 
00367       // Connect
00368       // 4   2 - 1  //
00369       //     |   |  //
00370       // *   3 - 0  //
00371       //            //
00372       // 4 - 2   1  //
00373       //   \   \    //
00374       // *   3 - 0  //
00375       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);
00376       if (Mesh::IsManifold::value) // Only needed for the manifold mesh
00377       {
00378         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);
00379       }
00380     }
00381   }
00382 
00383   return (true);
00384 }
00385 
00387 
00388 void
00389 pcl::ihs::Integration::age (const MeshPtr& mesh, const bool cleanup) const
00390 {
00391   for (unsigned int i=0; i<mesh->sizeVertices (); ++i)
00392   {
00393     PointIHS& pt = mesh->getVertexDataCloud () [i];
00394     if (pt.age < max_age_)
00395     {
00396       // Point survives
00397       ++(pt.age);
00398     }
00399     else if (pt.age == max_age_) // Judgement Day
00400     {
00401       if (pcl::ihs::countDirections (pt.directions) < min_directions_)
00402       {
00403         // Point dies (no need to transform it)
00404         mesh->deleteVertex (VertexIndex (i));
00405       }
00406       else
00407       {
00408         // Point becomes immortal
00409         pt.age = std::numeric_limits <unsigned int>::max ();
00410       }
00411     }
00412   }
00413 
00414   if (cleanup)
00415   {
00416     mesh->cleanUp ();
00417   }
00418 }
00419 
00421 
00422 void
00423 pcl::ihs::Integration::removeUnfitVertices (const MeshPtr& mesh, const bool cleanup) const
00424 {
00425   for (unsigned int i=0; i<mesh->sizeVertices (); ++i)
00426   {
00427     if (pcl::ihs::countDirections (mesh->getVertexDataCloud () [i].directions) < min_directions_)
00428     {
00429       // Point dies (no need to transform it)
00430       mesh->deleteVertex (VertexIndex (i));
00431     }
00432   }
00433 
00434   if (cleanup)
00435   {
00436     mesh->cleanUp ();
00437   }
00438 }
00439 
00441 
00442 void
00443 pcl::ihs::Integration::setMaxSquaredDistance (const float squared_distance)
00444 {
00445   if (squared_distance > 0) max_squared_distance_ = squared_distance;
00446 }
00447 
00448 float
00449 pcl::ihs::Integration::getMaxSquaredDistance () const
00450 {
00451   return (max_squared_distance_);
00452 }
00453 
00455 
00456 void
00457 pcl::ihs::Integration::setMaxAngle (const float angle)
00458 {
00459   max_angle_ = pcl::ihs::clamp (angle, 0.f, 180.f);
00460 }
00461 
00462 float
00463 pcl::ihs::Integration::getMaxAngle () const
00464 {
00465   return (max_angle_);
00466 }
00467 
00469 
00470 void
00471 pcl::ihs::Integration::setMaxAge (const unsigned int age)
00472 {
00473   max_age_ = age < 1 ? 1 : age;
00474 }
00475 
00476 unsigned int
00477 pcl::ihs::Integration::getMaxAge () const
00478 {
00479   return (max_age_);
00480 }
00481 
00483 
00484 void
00485 pcl::ihs::Integration::setMinDirections (const unsigned int directions)
00486 {
00487   min_directions_ = directions < 1 ? 1 : directions;
00488 }
00489 
00490 unsigned int
00491 pcl::ihs::Integration::getMinDirections () const
00492 {
00493   return (min_directions_);
00494 }
00495 
00497 
00498 uint8_t
00499 pcl::ihs::Integration::trimRGB (const float val) const
00500 {
00501   return (static_cast <uint8_t> (val > 255.f ? 255 : val));
00502 }
00503 
00505 
00506 void
00507 pcl::ihs::Integration::addToMesh (const PointIHS& pt_0,
00508                                   const PointIHS& pt_1,
00509                                   const PointIHS& pt_2,
00510                                   const PointIHS& pt_3,
00511                                   VertexIndex&    vi_0,
00512                                   VertexIndex&    vi_1,
00513                                   VertexIndex&    vi_2,
00514                                   VertexIndex&    vi_3,
00515                                   const MeshPtr&  mesh) const
00516 {
00517   // Treated bitwise
00518   // 2 - 1
00519   // |   |
00520   // 3 - 0
00521   const unsigned char is_finite = static_cast <unsigned char> (
00522                                     (1 * !boost::math::isnan (pt_0.x)) |
00523                                     (2 * !boost::math::isnan (pt_1.x)) |
00524                                     (4 * !boost::math::isnan (pt_2.x)) |
00525                                     (8 * !boost::math::isnan (pt_3.x)));
00526 
00527   switch (is_finite)
00528   {
00529     case  7: this->addToMesh (pt_0, pt_1, pt_2, vi_0, vi_1, vi_2, mesh); break; // 0-1-2
00530     case 11: this->addToMesh (pt_0, pt_1, pt_3, vi_0, vi_1, vi_3, mesh); break; // 0-1-3
00531     case 13: this->addToMesh (pt_0, pt_2, pt_3, vi_0, vi_2, vi_3, mesh); break; // 0-2-3
00532     case 14: this->addToMesh (pt_1, pt_2, pt_3, vi_1, vi_2, vi_3, mesh); break; // 1-2-3
00533     case 15: // 0-1-2-3
00534     {
00535       if (!distanceThreshold (pt_0, pt_1, pt_2, pt_3)) break;
00536       if (!vi_0.isValid ()) vi_0 = mesh->addVertex (pt_0);
00537       if (!vi_1.isValid ()) vi_1 = mesh->addVertex (pt_1);
00538       if (!vi_2.isValid ()) vi_2 = mesh->addVertex (pt_2);
00539       if (!vi_3.isValid ()) vi_3 = mesh->addVertex (pt_3);
00540       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)
00541       {
00542         return;
00543       }
00544       mesh->addTrianglePair (vi_0, vi_1, vi_2, vi_3);
00545 
00546       break;
00547     }
00548   }
00549 }
00550 
00552 
00553 void
00554 pcl::ihs::Integration::addToMesh (const PointIHS& pt_0,
00555                                   const PointIHS& pt_1,
00556                                   const PointIHS& pt_2,
00557                                   VertexIndex&    vi_0,
00558                                   VertexIndex&    vi_1,
00559                                   VertexIndex&    vi_2,
00560                                   const MeshPtr&  mesh) const
00561 {
00562   if (!distanceThreshold (pt_0, pt_1, pt_2)) return;
00563 
00564   if (!vi_0.isValid ()) vi_0 = mesh->addVertex (pt_0);
00565   if (!vi_1.isValid ()) vi_1 = mesh->addVertex (pt_1);
00566   if (!vi_2.isValid ()) vi_2 = mesh->addVertex (pt_2);
00567   if (vi_0==vi_1 || vi_0==vi_2 || vi_1==vi_2)
00568   {
00569     return;
00570   }
00571   mesh->addFace (vi_0, vi_1, vi_2);
00572 }
00573 
00575 
00576 bool
00577 pcl::ihs::Integration::distanceThreshold (const PointIHS& pt_0,
00578                                           const PointIHS& pt_1,
00579                                           const PointIHS& pt_2) const
00580 {
00581   if ((pt_0.getVector3fMap () - pt_1.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false);
00582   if ((pt_1.getVector3fMap () - pt_2.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false);
00583   if ((pt_2.getVector3fMap () - pt_0.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false);
00584   return (true);
00585 }
00586 
00588 
00589 bool
00590 pcl::ihs::Integration::distanceThreshold (const PointIHS& pt_0,
00591                                           const PointIHS& pt_1,
00592                                           const PointIHS& pt_2,
00593                                           const PointIHS& pt_3) const
00594 {
00595   if ((pt_0.getVector3fMap () - pt_1.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false);
00596   if ((pt_1.getVector3fMap () - pt_2.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false);
00597   if ((pt_2.getVector3fMap () - pt_3.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false);
00598   if ((pt_3.getVector3fMap () - pt_0.getVector3fMap ()).squaredNorm () > max_squared_distance_) return (false);
00599   return (true);
00600 }
00601 


ros_in_hand_scanner
Author(s):
autogenerated on Thu Jun 6 2019 20:39:38