Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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
00056 if (!isModelValid (model_coefficients))
00057 {
00058 inliers.clear ();
00059 return;
00060 }
00061
00062
00063 Eigen::Vector4f coeff = model_coefficients;
00064 coeff[3] = 0;
00065
00066 int nr_p = 0;
00067 inliers.resize (indices_->size ());
00068
00069 for (size_t i = 0; i < indices_->size (); ++i)
00070 {
00071
00072
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
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
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
00103 if (!isModelValid (model_coefficients))
00104 return (0);
00105
00106
00107 Eigen::Vector4f coeff = model_coefficients;
00108 coeff[3] = 0;
00109
00110 int nr_p = 0;
00111
00112
00113 for (size_t i = 0; i < indices_->size (); ++i)
00114 {
00115
00116
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
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
00143 if (!isModelValid (model_coefficients))
00144 {
00145 distances.clear ();
00146 return;
00147 }
00148
00149
00150 Eigen::Vector4f coeff = model_coefficients;
00151 coeff[3] = 0;
00152
00153 distances.resize (indices_->size ());
00154
00155
00156 for (size_t i = 0; i < indices_->size (); ++i)
00157 {
00158
00159
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
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
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