project_inliers.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) 2010-2012, Willow Garage, Inc.
00006  *
00007  *  All rights reserved.
00008  *
00009  *  Redistribution and use in source and binary forms, with or without
00010  *  modification, are permitted provided that the following conditions
00011  *  are met:
00012  *
00013  *   * Redistributions of source code must retain the above copyright
00014  *     notice, this list of conditions and the following disclaimer.
00015  *   * Redistributions in binary form must reproduce the above
00016  *     copyright notice, this list of conditions and the following
00017  *     disclaimer in the documentation and/or other materials provided
00018  *     with the distribution.
00019  *   * Neither the name of Willow Garage, Inc. nor the names of its
00020  *     contributors may be used to endorse or promote products derived
00021  *     from this software without specific prior written permission.
00022  *
00023  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00024  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00025  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00026  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00027  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00028  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00029  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00030  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00031  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00032  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00033  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00034  *  POSSIBILITY OF SUCH DAMAGE.
00035  *
00036  * $Id: project_inliers.cpp 5026 2012-03-12 02:51:44Z rusu $
00037  *
00038  */
00039 
00040 #include <pcl/impl/instantiate.hpp>
00041 #include <pcl/point_types.h>
00042 #include <pcl/filters/project_inliers.h>
00043 #include <pcl/filters/impl/project_inliers.hpp>
00044 
00046 void
00047 pcl::ProjectInliers<sensor_msgs::PointCloud2>::applyFilter (PointCloud2 &output)
00048 {
00049   if (indices_->empty ())
00050   {
00051     PCL_WARN ("[pcl::%s::applyFilter] No indices given or empty indices!\n", getClassName ().c_str ());
00052     output.width = output.height = 0;
00053     output.data.clear ();
00054     return;
00055   }
00056 
00057   //Eigen::Map<Eigen::VectorXf, Eigen::Aligned> model_coefficients (&model_->values[0], model_->values.size ());
00058   // More expensive than a map but safer (32bit architectures seem to complain)
00059   Eigen::VectorXf model_coefficients (model_->values.size ());
00060   for (size_t i = 0; i < model_->values.size (); ++i)
00061     model_coefficients[i] = model_->values[i];
00062 
00063   // Construct the model and project
00064   if (!initSACModel (model_type_))
00065   {
00066     PCL_ERROR ("[pcl::%s::segment] Error initializing the SAC model!\n", getClassName ().c_str ());
00067     output.width = output.height = 0;
00068     output.data.clear ();
00069     return;
00070   }
00071   pcl::PointCloud<pcl::PointXYZ> cloud_out;
00072 
00073   if (!copy_all_fields_)
00074     sacmodel_->projectPoints (*indices_, model_coefficients, cloud_out, false);
00075   else
00076     sacmodel_->projectPoints (*indices_, model_coefficients, cloud_out, true);
00077 
00078   if (copy_all_data_)
00079   {
00080     output.height       = input_->height;
00081     output.width        = input_->width;
00082     output.is_bigendian = input_->is_bigendian;
00083     output.point_step   = input_->point_step;
00084     output.row_step     = input_->row_step;
00085     output.data         = input_->data;
00086     output.is_dense     = input_->is_dense;
00087 
00088     // Get the distance field index
00089     int x_idx = -1, y_idx = -1, z_idx = -1;
00090     for (int d = 0; d < static_cast<int> (output.fields.size ()); ++d)
00091     {
00092       if (output.fields[d].name == "x") x_idx = d;
00093       if (output.fields[d].name == "y") y_idx = d;
00094       if (output.fields[d].name == "z") z_idx = d;
00095     }
00096     if (x_idx == -1 || y_idx == -1 || z_idx == -1)
00097     {
00098       PCL_ERROR ("[pcl::%s::segment] X (%d) Y (%d) Z (%d) field dimensions not found!\n", getClassName ().c_str (), x_idx, y_idx, z_idx);
00099       output.width = output.height = 0;
00100       output.data.clear ();
00101       return;
00102     }
00103 
00104     // Copy the projected points
00105     for (size_t i = 0; i < indices_->size (); ++i)
00106     {
00107       memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[x_idx].offset], &cloud_out.points[(*indices_)[i]].x, sizeof (float));
00108       memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[y_idx].offset], &cloud_out.points[(*indices_)[i]].y, sizeof (float));
00109       memcpy (&output.data[(*indices_)[i] * output.point_step + output.fields[z_idx].offset], &cloud_out.points[(*indices_)[i]].z, sizeof (float));
00110     }
00111   }
00112   else
00113   {
00114     if (!copy_all_fields_)
00115     {
00116       pcl::toROSMsg<pcl::PointXYZ> (cloud_out, output);
00117     }
00118     else
00119     {
00120       // Copy everything
00121       output.height       = 1;
00122       output.width        = static_cast<uint32_t> (indices_->size ());
00123       output.point_step   = input_->point_step;
00124       output.data.resize (output.width * output.point_step);
00125       output.is_bigendian = input_->is_bigendian;
00126       output.row_step     = output.point_step * output.width;
00127       // All projections should return valid data, so is_dense = true
00128       output.is_dense     = true;
00129 
00130       // Get the distance field index
00131       int x_idx = -1, y_idx = -1, z_idx = -1;
00132       for (int d = 0; d < static_cast<int> (output.fields.size ()); ++d)
00133       {
00134         if (output.fields[d].name == "x") x_idx = d;
00135         if (output.fields[d].name == "y") y_idx = d;
00136         if (output.fields[d].name == "z") z_idx = d;
00137       }
00138 
00139       if (x_idx == -1 || y_idx == -1 || z_idx == -1)
00140       {
00141         PCL_ERROR ("[pcl::%s::segment] X (%d) Y (%d) Z (%d) field dimensions not found!\n", getClassName ().c_str (), x_idx, y_idx, z_idx);
00142         output.width = output.height = 0;
00143         output.data.clear ();
00144         return;
00145       }
00146 
00147       // Copy the projected points
00148       for (size_t i = 0; i < indices_->size (); ++i)
00149       {
00150         memcpy (&output.data[i * output.point_step], &input_->data[(*indices_)[i] * input_->point_step], output.point_step);
00151         memcpy (&output.data[i * output.point_step + output.fields[x_idx].offset], &cloud_out.points[(*indices_)[i]].x, sizeof (float));
00152         memcpy (&output.data[i * output.point_step + output.fields[y_idx].offset], &cloud_out.points[(*indices_)[i]].y, sizeof (float));
00153         memcpy (&output.data[i * output.point_step + output.fields[z_idx].offset], &cloud_out.points[(*indices_)[i]].z, sizeof (float));
00154       }
00155     }
00156   }
00157 }
00158 
00160 bool
00161 pcl::ProjectInliers<sensor_msgs::PointCloud2>::initSACModel (int model_type)
00162 {
00163   // Convert the input data
00164   PointCloud<PointXYZ> cloud;
00165   fromROSMsg (*input_, cloud);
00166   PointCloud<PointXYZ>::Ptr cloud_ptr = cloud.makeShared ();
00167 
00168   // Build the model
00169   switch (model_type)
00170   {
00171     case SACMODEL_PLANE:
00172     {
00173       //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PLANE\n", getClassName ().c_str ());
00174       sacmodel_.reset (new SampleConsensusModelPlane<pcl::PointXYZ> (cloud_ptr));
00175       break;
00176     }
00177     case SACMODEL_LINE:
00178     {
00179       //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_LINE\n", getClassName ().c_str ());
00180       sacmodel_.reset (new SampleConsensusModelLine<pcl::PointXYZ> (cloud_ptr));
00181       break;
00182     }
00183     case SACMODEL_CIRCLE2D:
00184     {
00185       //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_CIRCLE2D\n", getClassName ().c_str ());
00186       sacmodel_.reset (new SampleConsensusModelCircle2D<pcl::PointXYZ> (cloud_ptr));
00187       break;
00188     }
00189     case SACMODEL_SPHERE:
00190     {
00191       //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_SPHERE\n", getClassName ().c_str ());
00192       sacmodel_.reset (new SampleConsensusModelSphere<pcl::PointXYZ> (cloud_ptr));
00193       break;
00194     }
00195     case SACMODEL_PARALLEL_LINE:
00196     {
00197       //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PARALLEL_LINE\n", getClassName ().c_str ());
00198       sacmodel_.reset (new SampleConsensusModelParallelLine<pcl::PointXYZ> (cloud_ptr));
00199       break;
00200     }
00201     case SACMODEL_PERPENDICULAR_PLANE:
00202     {
00203       //PCL_DEBUG ("[pcl::%s::initSACModel] Using a model of type: SACMODEL_PERPENDICULAR_PLANE\n", getClassName ().c_str ());
00204       sacmodel_.reset (new SampleConsensusModelPerpendicularPlane<pcl::PointXYZ> (cloud_ptr));
00205       break;
00206     }
00207     case SACMODEL_CYLINDER:
00208     {
00209       //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_CYLINDER\n", getClassName ().c_str ());
00210       sacmodel_.reset (new SampleConsensusModelCylinder<pcl::PointXYZ, Normal> (cloud_ptr));
00211       break;
00212     }
00213     case SACMODEL_NORMAL_PLANE:
00214     {
00215       //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_NORMAL_PLANE\n", getClassName ().c_str ());
00216       sacmodel_.reset (new SampleConsensusModelNormalPlane<pcl::PointXYZ, Normal> (cloud_ptr));
00217       break;
00218     }
00219     case SACMODEL_CONE:
00220     {
00221       //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_CONE\n", getClassName ().c_str ());
00222       sacmodel_.reset (new SampleConsensusModelCone<pcl::PointXYZ, Normal> (cloud_ptr));
00223       break;
00224     }
00225     case SACMODEL_NORMAL_SPHERE:
00226     {
00227       //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_NORMAL_SPHERE\n", getClassName ().c_str ());
00228       sacmodel_.reset (new SampleConsensusModelNormalSphere<pcl::PointXYZ, Normal> (cloud_ptr));
00229       break;
00230     }
00231     case SACMODEL_NORMAL_PARALLEL_PLANE:
00232     {
00233       //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_NORMAL_PARALLEL_PLANE\n", getClassName ().c_str ());
00234       sacmodel_.reset (new SampleConsensusModelNormalParallelPlane<pcl::PointXYZ, Normal> (cloud_ptr));
00235       break;
00236     }
00237     case SACMODEL_PARALLEL_PLANE:
00238     {
00239       //PCL_DEBUG ("[pcl::%s::segment] Using a model of type: SACMODEL_PARALLEL_PLANE\n", getClassName ().c_str ());
00240       sacmodel_.reset (new SampleConsensusModelParallelPlane<pcl::PointXYZ> (cloud_ptr));
00241       break;
00242     }
00243     default:
00244     {
00245       PCL_ERROR ("[pcl::%s::initSACModel] No valid model given!\n", getClassName ().c_str ());
00246       return (false);
00247     }
00248   }
00249   return (true);
00250 }
00251 
00252 // Instantiations of specific point types
00253 #ifdef PCL_ONLY_CORE_POINT_TYPES
00254   PCL_INSTANTIATE(ProjectInliers, (pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGB)(pcl::PointXYZRGBA))
00255 #else
00256   PCL_INSTANTIATE(ProjectInliers, PCL_XYZ_POINT_TYPES)
00257 #endif


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:17:25