41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PARALLEL_PLANE_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PARALLEL_PLANE_H_
44 #include <pcl/sample_consensus/sac_model_normal_parallel_plane.h>
47 template <
typename Po
intT,
typename Po
intNT>
void
49 const Eigen::VectorXf &model_coefficients,
const double threshold, std::vector<int> &inliers)
53 PCL_ERROR (
"[pcl::SampleConsensusModelNormalParallelPlane::selectWithinDistance] No input dataset containing normals was given!\n");
58 if (!isModelValid (model_coefficients))
65 Eigen::Vector4f coeff = model_coefficients;
68 inliers.resize (indices_->size ());
69 error_sqr_dists_.resize (indices_->size ());
72 for (
size_t i = 0; i < indices_->size (); ++i)
76 Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 1);
77 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
78 double d_euclid = fabs (coeff.dot (p));
82 d_normal = (std::min) (d_normal, fabs(M_PI - d_normal));
84 double distance = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
85 if (distance < threshold)
88 inliers[nr_p] = (*indices_)[i];
89 error_sqr_dists_[nr_p] = distance;
93 inliers.resize (nr_p);
94 error_sqr_dists_.resize (nr_p);
98 template <
typename Po
intT,
typename Po
intNT>
int
100 const Eigen::VectorXf &model_coefficients,
const double threshold)
104 PCL_ERROR (
"[pcl::SampleConsensusModelNormalParallelPlane::countWithinDistance] No input dataset containing normals was given!\n");
109 if (!isModelValid (model_coefficients))
113 Eigen::Vector4f coeff = model_coefficients;
118 for (
size_t i = 0; i < indices_->size (); ++i)
122 Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 1);
123 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
124 double d_euclid = fabs (coeff.dot (p));
127 double d_normal = fabs (
getAngle3D (n, coeff));
128 d_normal = (std::min) (d_normal, fabs(M_PI - d_normal));
130 if (fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid) < threshold)
138 template <
typename Po
intT,
typename Po
intNT>
void
140 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
144 PCL_ERROR (
"[pcl::SampleConsensusModelNormalParallelPlane::getDistancesToModel] No input dataset containing normals was given!\n");
149 if (!isModelValid (model_coefficients))
156 Eigen::Vector4f coeff = model_coefficients;
158 distances.resize (indices_->size ());
161 for (
size_t i = 0; i < indices_->size (); ++i)
165 Eigen::Vector4f p (input_->points[(*indices_)[i]].x, input_->points[(*indices_)[i]].y, input_->points[(*indices_)[i]].z, 1);
166 Eigen::Vector4f n (normals_->points[(*indices_)[i]].normal[0], normals_->points[(*indices_)[i]].normal[1], normals_->points[(*indices_)[i]].normal[2], 0);
167 double d_euclid = fabs (coeff.dot (p));
171 d_normal = (std::min) (d_normal, fabs (M_PI - d_normal));
173 distances[i] = fabs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
178 template <
typename Po
intT,
typename Po
intNT>
bool
182 if (model_coefficients.size () != 4)
184 PCL_ERROR (
"[pcl::SampleConsensusModelNormalParallelPlane::isModelValid] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ());
189 if (eps_angle_ > 0.0)
192 Eigen::Vector4f coeff = model_coefficients;
196 if (fabs (axis_.dot (coeff)) < cos_angle_)
202 if (fabs (-model_coefficients[3] - distance_from_origin_) > eps_dist_)
209 #define PCL_INSTANTIATE_SampleConsensusModelNormalParallelPlane(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalParallelPlane<PointT, PointNT>;
211 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PARALLEL_PLANE_H_