sac_model_normal_plane.hpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2009-2010, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage, Inc. nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *
00034  * $Id: sac_model_normal_plane.hpp 5026 2012-03-12 02:51:44Z rusu $
00035  *
00036  */
00037 
00038 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_
00039 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_
00040 
00041 #include <pcl/sample_consensus/sac_model_normal_plane.h>
00042 
00044 template <typename PointT, typename PointNT> void
00045 pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::selectWithinDistance (
00046       const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
00047 {
00048   if (!normals_)
00049   {
00050     PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::selectWithinDistance] No input dataset containing normals was given!\n");
00051     inliers.clear ();
00052     return;
00053   }
00054 
00055   // Check if the model is valid given the user constraints
00056   if (!isModelValid (model_coefficients))
00057   {
00058     inliers.clear ();
00059     return;
00060   }
00061 
00062   // Obtain the plane normal
00063   Eigen::Vector4f coeff = model_coefficients;
00064   coeff[3] = 0;
00065 
00066   int nr_p = 0;
00067   inliers.resize (indices_->size ());
00068   // Iterate through the 3d points and calculate the distances from them to the plane
00069   for (size_t i = 0; i < indices_->size (); ++i)
00070   {
00071     // Calculate the distance from the point to the plane normal as the dot product
00072     // D = (P-A).N/|N|
00073     Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
00074     Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
00075     double d_euclid = fabs (coeff.dot (p) + model_coefficients[3]);
00076 
00077     // Calculate the angular distance between the point normal and the plane normal
00078     double d_normal = fabs (getAngle3D (n, coeff));
00079     d_normal = (std::min) (d_normal, M_PI - d_normal);
00080 
00081     if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
00082     {
00083       // Returns the indices of the points whose distances are smaller than the threshold
00084       inliers[nr_p] = (*indices_)[i];
00085       nr_p++;
00086     }
00087   }
00088   inliers.resize (nr_p);
00089 }
00090 
00092 template <typename PointT, typename PointNT> int
00093 pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::countWithinDistance (
00094       const Eigen::VectorXf &model_coefficients, const double threshold)
00095 {
00096   if (!normals_)
00097   {
00098     PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::countWithinDistance] No input dataset containing normals was given!\n");
00099     return (0);
00100   }
00101 
00102   // Check if the model is valid given the user constraints
00103   if (!isModelValid (model_coefficients))
00104     return (0);
00105 
00106   // Obtain the plane normal
00107   Eigen::Vector4f coeff = model_coefficients;
00108   coeff[3] = 0;
00109 
00110   int nr_p = 0;
00111 
00112   // Iterate through the 3d points and calculate the distances from them to the plane
00113   for (size_t i = 0; i < indices_->size (); ++i)
00114   {
00115     // Calculate the distance from the point to the plane normal as the dot product
00116     // D = (P-A).N/|N|
00117     Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
00118     Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
00119     double d_euclid = fabs (coeff.dot (p) + model_coefficients[3]);
00120 
00121     // Calculate the angular distance between the point normal and the plane normal
00122     double d_normal = fabs (getAngle3D (n, coeff));
00123     d_normal = (std::min) (d_normal, M_PI - d_normal);
00124 
00125     if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
00126       nr_p++;
00127   }
00128   return (nr_p);
00129 }
00130 
00132 template <typename PointT, typename PointNT> void
00133 pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::getDistancesToModel (
00134       const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
00135 {
00136   if (!normals_)
00137   {
00138     PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::getDistancesToModel] No input dataset containing normals was given!\n");
00139     return;
00140   }
00141 
00142   // Check if the model is valid given the user constraints
00143   if (!isModelValid (model_coefficients))
00144   {
00145     distances.clear ();
00146     return;
00147   }
00148 
00149   // Obtain the plane normal
00150   Eigen::Vector4f coeff = model_coefficients;
00151   coeff[3] = 0;
00152 
00153   distances.resize (indices_->size ());
00154 
00155   // Iterate through the 3d points and calculate the distances from them to the plane
00156   for (size_t i = 0; i < indices_->size (); ++i)
00157   {
00158     // Calculate the distance from the point to the plane normal as the dot product
00159     // D = (P-A).N/|N|
00160     Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 0);
00161     Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
00162     double d_euclid = fabs (coeff.dot (p) + model_coefficients[3]);
00163 
00164     // Calculate the angular distance between the point normal and the plane normal
00165     double d_normal = fabs (getAngle3D (n, coeff));
00166     d_normal = (std::min) (d_normal, M_PI - d_normal);
00167 
00168     distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
00169   }
00170 }
00171 
00173 template <typename PointT, typename PointNT> bool 
00174 pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients)
00175 {
00176   // Needs a valid model coefficients
00177   if (model_coefficients.size () != 4)
00178   {
00179     PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::selectWithinDistance] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
00180     return (false);
00181   }
00182 
00183   return (true);
00184 }
00185 
00186 #define PCL_INSTANTIATE_SampleConsensusModelNormalPlane(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalPlane<PointT, PointNT>;
00187 
00188 #endif    // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_
00189 


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