input_data_processing.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/input_data_processing.h>
00042 
00043 #include <pcl/common/point_tests.h>
00044 #include <pcl/features/integral_image_normal.h>
00045 #include <pcl/apps/in_hand_scanner/boost.h>
00046 
00048 
00049 pcl::ihs::InputDataProcessing::InputDataProcessing ()
00050   : normal_estimation_ (new NormalEstimation ()),
00051 
00052     x_min_ (-30.f),
00053     x_max_ ( 30.f),
00054     y_min_ (-30.f),
00055     y_max_ ( 20.f),
00056     z_min_ ( 25.f),
00057     z_max_ ( 75.f),
00058 
00059     h_min_ (210.f),
00060     h_max_ (270.f),
00061     s_min_ (  0.2f),
00062     s_max_ (  1.f),
00063     v_min_ (  0.2f),
00064     v_max_ (  1.f),
00065 
00066     hsv_inverted_ (false),
00067     hsv_enabled_ (false),
00068 
00069     size_dilate_ (3),
00070     size_erode_  (3)
00071 {
00072   // Normal estimation
00073   normal_estimation_->setNormalEstimationMethod (NormalEstimation::AVERAGE_3D_GRADIENT);
00074   normal_estimation_->setMaxDepthChangeFactor (0.02f); // in meters
00075   normal_estimation_->setNormalSmoothingSize (10.0f);
00076 }
00077 
00079 
00080 bool
00081 pcl::ihs::InputDataProcessing::segment (const CloudXYZRGBAConstPtr& cloud_in,
00082                                         CloudXYZRGBNormalPtr&       cloud_out,
00083                                         CloudXYZRGBNormalPtr&       cloud_discarded) const
00084 {
00085   if (!cloud_in)
00086   {
00087     std::cerr << "ERROR in input_data_processing.cpp: Input cloud is invalid.\n";
00088     return (false);
00089   }
00090   if (!cloud_in->isOrganized ())
00091   {
00092     std::cerr << "ERROR in input_data_processing.cpp: Input cloud must be organized.\n";
00093     return (false);
00094   }
00095 
00096   if (!cloud_out)       cloud_out       = CloudXYZRGBNormalPtr (new CloudXYZRGBNormal ());
00097   if (!cloud_discarded) cloud_discarded = CloudXYZRGBNormalPtr (new CloudXYZRGBNormal ());
00098 
00099   const unsigned int width  = cloud_in->width;
00100   const unsigned int height = cloud_in->height;
00101 
00102   // Calculate the normals
00103   CloudNormalsPtr cloud_normals (new CloudNormals ());
00104   normal_estimation_->setInputCloud (cloud_in);
00105   normal_estimation_->compute (*cloud_normals);
00106 
00107   // Get the XYZ and HSV masks.
00108   MatrixXb xyz_mask (height, width);
00109   MatrixXb hsv_mask (height, width);
00110 
00111   // cm -> m for the comparison
00112   const float x_min = .01f * x_min_;
00113   const float x_max = .01f * x_max_;
00114   const float y_min = .01f * y_min_;
00115   const float y_max = .01f * y_max_;
00116   const float z_min = .01f * z_min_;
00117   const float z_max = .01f * z_max_;
00118 
00119   float h, s, v;
00120   for (MatrixXb::Index r=0; r<xyz_mask.rows (); ++r)
00121   {
00122     for (MatrixXb::Index c=0; c<xyz_mask.cols (); ++c)
00123     {
00124       const PointXYZRGBA& xyzrgb = (*cloud_in)      [r*width + c];
00125       const Normal&       normal = (*cloud_normals) [r*width + c];
00126 
00127       xyz_mask (r, c) = hsv_mask (r, c) = false;
00128 
00129       if (!boost::math::isnan (xyzrgb.x) && !boost::math::isnan (normal.normal_x) &&
00130           xyzrgb.x  >= x_min             && xyzrgb.x  <= x_max                    &&
00131           xyzrgb.y  >= y_min             && xyzrgb.y  <= y_max                    &&
00132           xyzrgb.z  >= z_min             && xyzrgb.z  <= z_max)
00133       {
00134         xyz_mask (r, c) = true;
00135 
00136         this->RGBToHSV (xyzrgb.r, xyzrgb.g, xyzrgb.b, h, s, v);
00137         if (h >= h_min_ && h <= h_max_ && s >= s_min_ && s <= s_max_ && v >= v_min_ && v <= v_max_)
00138         {
00139           if (!hsv_inverted_) hsv_mask (r, c) = true;
00140         }
00141         else
00142         {
00143           if (hsv_inverted_) hsv_mask (r, c) = true;
00144         }
00145       }
00146     }
00147   }
00148 
00149   this->erode  (xyz_mask, size_erode_);
00150   if (hsv_enabled_) this->dilate (hsv_mask, size_dilate_);
00151   else              hsv_mask.setZero ();
00152 
00153   // Copy the normals into the clouds.
00154   cloud_out->reserve (cloud_in->size ());
00155   cloud_discarded->reserve (cloud_in->size ());
00156 
00157   pcl::PointXYZRGBNormal pt_out, pt_discarded;
00158   pt_discarded.r = 50;
00159   pt_discarded.g = 50;
00160   pt_discarded.b = 230;
00161 
00162   PointXYZRGBA xyzrgb;
00163   Normal       normal;
00164 
00165   for (MatrixXb::Index r=0; r<xyz_mask.rows (); ++r)
00166   {
00167     for (MatrixXb::Index c=0; c<xyz_mask.cols (); ++c)
00168     {
00169       if (xyz_mask (r, c))
00170       {
00171         xyzrgb = (*cloud_in)      [r*width + c];
00172         normal = (*cloud_normals) [r*width + c];
00173 
00174         // m -> cm
00175         xyzrgb.getVector3fMap () = 100.f * xyzrgb.getVector3fMap ();
00176 
00177         if (hsv_mask (r, c))
00178         {
00179           pt_discarded.getVector4fMap ()       = xyzrgb.getVector4fMap ();
00180           pt_discarded.getNormalVector4fMap () = normal.getNormalVector4fMap ();
00181 
00182           pt_out.x = std::numeric_limits <float>::quiet_NaN ();
00183         }
00184         else
00185         {
00186           pt_out.getVector4fMap ()       = xyzrgb.getVector4fMap ();
00187           pt_out.getNormalVector4fMap () = normal.getNormalVector4fMap ();
00188           pt_out.rgba                    = xyzrgb.rgba;
00189 
00190           pt_discarded.x = std::numeric_limits <float>::quiet_NaN ();
00191         }
00192       }
00193       else
00194       {
00195         pt_out.x       = std::numeric_limits <float>::quiet_NaN ();
00196         pt_discarded.x = std::numeric_limits <float>::quiet_NaN ();
00197       }
00198 
00199       cloud_out->push_back       (pt_out);
00200       cloud_discarded->push_back (pt_discarded);
00201     }
00202   }
00203 
00204   cloud_out->width    = cloud_discarded->width    = width;
00205   cloud_out->height   = cloud_discarded->height   = height;
00206   cloud_out->is_dense = cloud_discarded->is_dense = false;
00207 
00208   return (true);
00209 }
00210 
00212 
00213 bool
00214 pcl::ihs::InputDataProcessing::calculateNormals (const CloudXYZRGBAConstPtr& cloud_in,
00215                                                  CloudXYZRGBNormalPtr&       cloud_out) const
00216 {
00217   if (!cloud_in)
00218   {
00219     std::cerr << "ERROR in input_data_processing.cpp: Input cloud is invalid.\n";
00220     return (false);
00221   }
00222 
00223   if (!cloud_out)
00224     cloud_out = CloudXYZRGBNormalPtr (new CloudXYZRGBNormal ());
00225 
00226   // Calculate the normals
00227   CloudNormalsPtr cloud_normals (new CloudNormals ());
00228   normal_estimation_->setInputCloud (cloud_in);
00229   normal_estimation_->compute (*cloud_normals);
00230 
00231   // Copy the input cloud and normals into the output cloud.
00232   cloud_out->resize (cloud_in->size ());
00233   cloud_out->width    = cloud_in->width;
00234   cloud_out->height   = cloud_in->height;
00235   cloud_out->is_dense = false;
00236 
00237   CloudXYZRGBA::const_iterator it_in  = cloud_in->begin ();
00238   CloudNormals::const_iterator it_n   = cloud_normals->begin ();
00239   CloudXYZRGBNormal::iterator  it_out = cloud_out->begin ();
00240 
00241   PointXYZRGBNormal invalid_pt;
00242   invalid_pt.x        = invalid_pt.y        = invalid_pt.z        = std::numeric_limits <float>::quiet_NaN ();
00243   invalid_pt.normal_x = invalid_pt.normal_y = invalid_pt.normal_z = std::numeric_limits <float>::quiet_NaN ();
00244   invalid_pt.data   [3] = 1.f;
00245   invalid_pt.data_n [3] = 0.f;
00246 
00247   for (; it_in!=cloud_in->end (); ++it_in, ++it_n, ++it_out)
00248   {
00249     if (!boost::math::isnan (it_n->getNormalVector4fMap ()))
00250     {
00251       // m -> cm
00252       it_out->getVector4fMap ()       = 100.f * it_in->getVector4fMap ();
00253       it_out->data [3]                = 1.f;
00254       it_out->rgba                    = it_in->rgba;
00255       it_out->getNormalVector4fMap () = it_n->getNormalVector4fMap ();
00256     }
00257     else
00258     {
00259       *it_out = invalid_pt;
00260     }
00261   }
00262 
00263   return (true);
00264 }
00265 
00267 
00268 void
00269 pcl::ihs::InputDataProcessing::erode (MatrixXb& mask, const int k) const
00270 {
00271   mask = manhattan (mask, false).array () > k;
00272 }
00273 
00275 
00276 void
00277 pcl::ihs::InputDataProcessing::dilate (MatrixXb& mask, const int k) const
00278 {
00279   mask = manhattan (mask, true).array () <= k;
00280 }
00281 
00283 
00284 pcl::ihs::InputDataProcessing::MatrixXi
00285 pcl::ihs::InputDataProcessing::manhattan (const MatrixXb& mat, const bool comp) const
00286 {
00287   MatrixXi dist (mat.rows (), mat.cols ());
00288   const int d_max = dist.rows () + dist.cols ();
00289 
00290   // Forward
00291   for (MatrixXi::Index r = 0; r < dist.rows (); ++r)
00292   {
00293     for (MatrixXi::Index c = 0; c < dist.cols (); ++c)
00294     {
00295       if (mat (r, c) == comp)
00296       {
00297         dist (r, c) = 0;
00298       }
00299       else
00300       {
00301         dist (r, c) = d_max;
00302         if (r > 0) dist (r, c) = std::min (dist (r, c), dist (r-1, c  ) + 1);
00303         if (c > 0) dist (r, c) = std::min (dist (r, c), dist (r  , c-1) + 1);
00304       }
00305     }
00306   }
00307 
00308   // Backward
00309   for (int r = dist.rows () - 1; r >= 0; --r)
00310   {
00311     for (int c = dist.cols () - 1; c >= 0; --c)
00312     {
00313       if (r < dist.rows ()-1) dist (r, c) = std::min (dist (r, c), dist (r+1, c  ) + 1);
00314       if (c < dist.cols ()-1) dist (r, c) = std::min (dist (r, c), dist (r  , c+1) + 1);
00315     }
00316   }
00317 
00318   return (dist);
00319 }
00320 
00322 
00323 void
00324 pcl::ihs::InputDataProcessing::RGBToHSV (const unsigned char r,
00325                                          const unsigned char g,
00326                                          const unsigned char b,
00327                                          float&              h,
00328                                          float&              s,
00329                                          float&              v) const
00330 {
00331   const unsigned char max = std::max (r, std::max (g, b));
00332   const unsigned char min = std::min (r, std::min (g, b));
00333 
00334   v = static_cast <float> (max) / 255.f;
00335 
00336   if (max == 0) // division by zero
00337   {
00338     s = 0.f;
00339     h = 0.f; // h = -1.f;
00340     return;
00341   }
00342 
00343   const float diff = static_cast <float> (max - min);
00344   s = diff / static_cast <float> (max);
00345 
00346   if (min == max) // diff == 0 -> division by zero
00347   {
00348     h = 0;
00349     return;
00350   }
00351 
00352   if      (max == r) h = 60.f * (      static_cast <float> (g - b) / diff);
00353   else if (max == g) h = 60.f * (2.f + static_cast <float> (b - r) / diff);
00354   else               h = 60.f * (4.f + static_cast <float> (r - g) / diff); // max == b
00355 
00356   if (h < 0.f) h += 360.f;
00357 }
00358 


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