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-2010, 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 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_ 00042 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_ 00043 00044 #include <pcl/sample_consensus/sac_model_normal_plane.h> 00045 00046 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00047 template <typename PointT, typename PointNT> void 00048 pcl::SampleConsensusModelNormalPlane<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::SampleConsensusModelNormalPlane::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 plane normal 00066 Eigen::Vector4f coeff = model_coefficients; 00067 coeff[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 const PointT &pt = input_->points[(*indices_)[i]]; 00077 const PointNT &nt = normals_->points[(*indices_)[i]]; 00078 // Calculate the distance from the point to the plane normal as the dot product 00079 // D = (P-A).N/|N| 00080 Eigen::Vector4f p (pt.x, pt.y, pt.z, 0); 00081 Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0); 00082 double d_euclid = fabs (coeff.dot (p) + model_coefficients[3]); 00083 00084 // Calculate the angular distance between the point normal and the plane normal 00085 double d_normal = fabs (getAngle3D (n, coeff)); 00086 d_normal = (std::min) (d_normal, M_PI - d_normal); 00087 00088 // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence 00089 double weight = normal_distance_weight_ * (1.0 - nt.curvature); 00090 00091 double distance = fabs (weight * d_normal + (1.0 - weight) * d_euclid); 00092 if (distance < threshold) 00093 { 00094 // Returns the indices of the points whose distances are smaller than the threshold 00095 inliers[nr_p] = (*indices_)[i]; 00096 error_sqr_dists_[nr_p] = distance; 00097 ++nr_p; 00098 } 00099 } 00100 inliers.resize (nr_p); 00101 error_sqr_dists_.resize (nr_p); 00102 } 00103 00104 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00105 template <typename PointT, typename PointNT> int 00106 pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::countWithinDistance ( 00107 const Eigen::VectorXf &model_coefficients, const double threshold) 00108 { 00109 if (!normals_) 00110 { 00111 PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::countWithinDistance] No input dataset containing normals was given!\n"); 00112 return (0); 00113 } 00114 00115 // Check if the model is valid given the user constraints 00116 if (!isModelValid (model_coefficients)) 00117 return (0); 00118 00119 // Obtain the plane normal 00120 Eigen::Vector4f coeff = model_coefficients; 00121 coeff[3] = 0; 00122 00123 int nr_p = 0; 00124 00125 // Iterate through the 3d points and calculate the distances from them to the plane 00126 for (size_t i = 0; i < indices_->size (); ++i) 00127 { 00128 const PointT &pt = input_->points[(*indices_)[i]]; 00129 const PointNT &nt = normals_->points[(*indices_)[i]]; 00130 // Calculate the distance from the point to the plane normal as the dot product 00131 // D = (P-A).N/|N| 00132 Eigen::Vector4f p (pt.x, pt.y, pt.z, 0); 00133 Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0); 00134 double d_euclid = fabs (coeff.dot (p) + model_coefficients[3]); 00135 00136 // Calculate the angular distance between the point normal and the plane normal 00137 double d_normal = fabs (getAngle3D (n, coeff)); 00138 d_normal = (std::min) (d_normal, M_PI - d_normal); 00139 00140 // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence 00141 double weight = normal_distance_weight_ * (1.0 - nt.curvature); 00142 00143 if (fabs (weight * d_normal + (1.0 - weight) * d_euclid) < threshold) 00144 nr_p++; 00145 } 00146 return (nr_p); 00147 } 00148 00149 ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// 00150 template <typename PointT, typename PointNT> void 00151 pcl::SampleConsensusModelNormalPlane<PointT, PointNT>::getDistancesToModel ( 00152 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances) 00153 { 00154 if (!normals_) 00155 { 00156 PCL_ERROR ("[pcl::SampleConsensusModelNormalPlane::getDistancesToModel] No input dataset containing normals was given!\n"); 00157 return; 00158 } 00159 00160 // Check if the model is valid given the user constraints 00161 if (!isModelValid (model_coefficients)) 00162 { 00163 distances.clear (); 00164 return; 00165 } 00166 00167 // Obtain the plane normal 00168 Eigen::Vector4f coeff = model_coefficients; 00169 coeff[3] = 0; 00170 00171 distances.resize (indices_->size ()); 00172 00173 // Iterate through the 3d points and calculate the distances from them to the plane 00174 for (size_t i = 0; i < indices_->size (); ++i) 00175 { 00176 const PointT &pt = input_->points[(*indices_)[i]]; 00177 const PointNT &nt = normals_->points[(*indices_)[i]]; 00178 // Calculate the distance from the point to the plane normal as the dot product 00179 // D = (P-A).N/|N| 00180 Eigen::Vector4f p (pt.x, pt.y, pt.z, 0); 00181 Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0); 00182 double d_euclid = fabs (coeff.dot (p) + model_coefficients[3]); 00183 00184 // Calculate the angular distance between the point normal and the plane normal 00185 double d_normal = fabs (getAngle3D (n, coeff)); 00186 d_normal = (std::min) (d_normal, M_PI - d_normal); 00187 00188 // Weight with the point curvature. On flat surfaces, curvature -> 0, which means the normal will have a higher influence 00189 double weight = normal_distance_weight_ * (1.0 - nt.curvature); 00190 00191 distances[i] = fabs (weight * d_normal + (1.0 - weight) * d_euclid); 00192 } 00193 } 00194 00195 #define PCL_INSTANTIATE_SampleConsensusModelNormalPlane(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalPlane<PointT, PointNT>; 00196 00197 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_ 00198