Point Cloud Library (PCL)
1.7.0
|
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: sac_model_normal_sphere.hpp schrandt $ 00038 * 00039 */ 00040 00041 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_SPHERE_H_ 00042 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_SPHERE_H_ 00043 00044 #include <pcl/sample_consensus/sac_model_normal_sphere.h> 00045 00046 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00047 template <typename PointT, typename PointNT> void 00048 pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::selectWithinDistance ( 00049 const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers) 00050 { 00051 if (!normals_) 00052 { 00053 PCL_ERROR ("[pcl::SampleConsensusModelNormalSphere::selectWithinDistance] No input dataset containing normals was given!\n"); 00054 inliers.clear (); 00055 return; 00056 } 00057 00058 // Check if the model is valid given the user constraints 00059 if (!isModelValid (model_coefficients)) 00060 { 00061 inliers.clear (); 00062 return; 00063 } 00064 00065 // Obtain the sphere center 00066 Eigen::Vector4f center = model_coefficients; 00067 center[3] = 0; 00068 00069 int nr_p = 0; 00070 inliers.resize (indices_->size ()); 00071 error_sqr_dists_.resize (indices_->size ()); 00072 00073 // Iterate through the 3d points and calculate the distances from them to the plane 00074 for (size_t i = 0; i < indices_->size (); ++i) 00075 { 00076 // Calculate the distance from the point to the sphere center as the difference between 00077 // dist(point,sphere_origin) and sphere_radius 00078 Eigen::Vector4f p (input_->points[(*indices_)[i]].x, 00079 input_->points[(*indices_)[i]].y, 00080 input_->points[(*indices_)[i]].z, 00081 0); 00082 00083 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], 00084 normals_->points[(*indices_)[i]].normal[1], 00085 normals_->points[(*indices_)[i]].normal[2], 00086 0); 00087 00088 Eigen::Vector4f n_dir = p - center; 00089 double d_euclid = fabs (n_dir.norm () - model_coefficients[3]); 00090 00091 // Calculate the angular distance between the point normal and the plane normal 00092 double d_normal = fabs (getAngle3D (n, n_dir)); 00093 d_normal = (std::min) (d_normal, M_PI - d_normal); 00094 00095 double distance = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); 00096 if (distance < threshold) 00097 { 00098 // Returns the indices of the points whose distances are smaller than the threshold 00099 inliers[nr_p] = (*indices_)[i]; 00100 error_sqr_dists_[nr_p] = static_cast<double> (distance); 00101 ++nr_p; 00102 } 00103 } 00104 inliers.resize (nr_p); 00105 error_sqr_dists_.resize (nr_p); 00106 } 00107 00108 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00109 template <typename PointT, typename PointNT> int 00110 pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::countWithinDistance ( 00111 const Eigen::VectorXf &model_coefficients, const double threshold) 00112 { 00113 if (!normals_) 00114 { 00115 PCL_ERROR ("[pcl::SampleConsensusModelNormalSphere::getDistancesToModel] No input dataset containing normals was given!\n"); 00116 return (0); 00117 } 00118 00119 // Check if the model is valid given the user constraints 00120 if (!isModelValid (model_coefficients)) 00121 return(0); 00122 00123 00124 // Obtain the shpere centroid 00125 Eigen::Vector4f center = model_coefficients; 00126 center[3] = 0; 00127 00128 int nr_p = 0; 00129 00130 // Iterate through the 3d points and calculate the distances from them to the plane 00131 for (size_t i = 0; i < indices_->size (); ++i) 00132 { 00133 // Calculate the distance from the point to the sphere centroid as the difference between 00134 // dist(point,sphere_origin) and sphere_radius 00135 Eigen::Vector4f p (input_->points[(*indices_)[i]].x, 00136 input_->points[(*indices_)[i]].y, 00137 input_->points[(*indices_)[i]].z, 00138 0); 00139 00140 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], 00141 normals_->points[(*indices_)[i]].normal[1], 00142 normals_->points[(*indices_)[i]].normal[2], 00143 0); 00144 00145 Eigen::Vector4f n_dir = (p-center); 00146 double d_euclid = fabs (n_dir.norm () - model_coefficients[3]); 00147 // 00148 // Calculate the angular distance between the point normal and the plane normal 00149 double d_normal = fabs (getAngle3D (n, n_dir)); 00150 d_normal = (std::min) (d_normal, M_PI - d_normal); 00151 00152 if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold) 00153 nr_p++; 00154 } 00155 return (nr_p); 00156 } 00157 00158 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00159 template <typename PointT, typename PointNT> void 00160 pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::getDistancesToModel ( 00161 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) 00162 { 00163 if (!normals_) 00164 { 00165 PCL_ERROR ("[pcl::SampleConsensusModelNormalSphere::getDistancesToModel] No input dataset containing normals was given!\n"); 00166 return; 00167 } 00168 00169 // Check if the model is valid given the user constraints 00170 if (!isModelValid (model_coefficients)) 00171 { 00172 distances.clear (); 00173 return; 00174 } 00175 00176 // Obtain the sphere centroid 00177 Eigen::Vector4f center = model_coefficients; 00178 center[3] = 0; 00179 00180 distances.resize (indices_->size ()); 00181 00182 // Iterate through the 3d points and calculate the distances from them to the plane 00183 for (size_t i = 0; i < indices_->size (); ++i) 00184 { 00185 // Calculate the distance from the point to the sphere as the difference between 00186 // dist(point,sphere_origin) and sphere_radius 00187 Eigen::Vector4f p (input_->points[(*indices_)[i]].x, 00188 input_->points[(*indices_)[i]].y, 00189 input_->points[(*indices_)[i]].z, 00190 0); 00191 00192 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], 00193 normals_->points[(*indices_)[i]].normal[1], 00194 normals_->points[(*indices_)[i]].normal[2], 00195 0); 00196 00197 Eigen::Vector4f n_dir = (p-center); 00198 double d_euclid = fabs (n_dir.norm () - model_coefficients[3]); 00199 // 00200 // Calculate the angular distance between the point normal and the plane normal 00201 double d_normal = fabs (getAngle3D (n, n_dir)); 00202 d_normal = (std::min) (d_normal, M_PI - d_normal); 00203 00204 distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid); 00205 } 00206 } 00207 00208 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00209 template <typename PointT, typename PointNT> bool 00210 pcl::SampleConsensusModelNormalSphere<PointT, PointNT>::isModelValid (const Eigen::VectorXf &model_coefficients) 00211 { 00212 // Needs a valid model coefficients 00213 if (model_coefficients.size () != 4) 00214 { 00215 PCL_ERROR ("[pcl::SampleConsensusModelNormalSphere::selectWithinDistance] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); 00216 return (false); 00217 } 00218 00219 if (radius_min_ != -std::numeric_limits<double>::max() && model_coefficients[3] < radius_min_) 00220 return (false); 00221 if (radius_max_ != std::numeric_limits<double>::max() && model_coefficients[3] > radius_max_) 00222 return (false); 00223 00224 return (true); 00225 } 00226 00227 #define PCL_INSTANTIATE_SampleConsensusModelNormalSphere(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalSphere<PointT, PointNT>; 00228 00229 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_SPHERE_H_ 00230