41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_
44 #include <pcl/sample_consensus/sac_model_normal_plane.h>
47 template <
typename Po
intT,
typename Po
intNT>
void
49 const Eigen::VectorXf &model_coefficients,
const double threshold,
Indices &inliers)
53 PCL_ERROR (
"[pcl::SampleConsensusModelNormalPlane::selectWithinDistance] No input dataset containing normals was given!\n");
59 if (!isModelValid (model_coefficients))
66 Eigen::Vector4f coeff = model_coefficients;
70 error_sqr_dists_.clear ();
71 inliers.reserve (indices_->size ());
72 error_sqr_dists_.reserve (indices_->size ());
75 for (std::size_t i = 0; i < indices_->size (); ++i)
77 const PointT &pt = input_->points[(*indices_)[i]];
78 const PointNT &nt = normals_->points[(*indices_)[i]];
81 Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
82 Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
83 double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
86 double d_normal = std::abs (
getAngle3D (n, coeff));
87 d_normal = (std::min) (d_normal,
M_PI - d_normal);
90 double weight = normal_distance_weight_ * (1.0 - nt.curvature);
92 double distance = std::abs (weight * d_normal + (1.0 - weight) * d_euclid);
96 inliers.push_back ((*indices_)[i]);
97 error_sqr_dists_.push_back (
distance);
103 template <
typename Po
intT,
typename Po
intNT> std::size_t
105 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
109 PCL_ERROR (
"[pcl::SampleConsensusModelNormalPlane::countWithinDistance] No input dataset containing normals was given!\n");
114 if (!isModelValid (model_coefficients))
118 Eigen::Vector4f coeff = model_coefficients;
121 std::size_t nr_p = 0;
124 for (std::size_t i = 0; i < indices_->size (); ++i)
126 const PointT &pt = input_->points[(*indices_)[i]];
127 const PointNT &nt = normals_->points[(*indices_)[i]];
130 Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
131 Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
132 double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
135 double d_normal = std::abs (
getAngle3D (n, coeff));
136 d_normal = (std::min) (d_normal,
M_PI - d_normal);
139 double weight = normal_distance_weight_ * (1.0 - nt.curvature);
141 if (std::abs (weight * d_normal + (1.0 - weight) * d_euclid) < threshold)
148 template <
typename Po
intT,
typename Po
intNT>
void
150 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
154 PCL_ERROR (
"[pcl::SampleConsensusModelNormalPlane::getDistancesToModel] No input dataset containing normals was given!\n");
159 if (!isModelValid (model_coefficients))
166 Eigen::Vector4f coeff = model_coefficients;
169 distances.resize (indices_->size ());
172 for (std::size_t i = 0; i < indices_->size (); ++i)
174 const PointT &pt = input_->points[(*indices_)[i]];
175 const PointNT &nt = normals_->points[(*indices_)[i]];
178 Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
179 Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
180 double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
183 double d_normal = std::abs (
getAngle3D (n, coeff));
184 d_normal = (std::min) (d_normal,
M_PI - d_normal);
187 double weight = normal_distance_weight_ * (1.0 - nt.curvature);
189 distances[i] = std::abs (weight * d_normal + (1.0 - weight) * d_euclid);
193 #define PCL_INSTANTIATE_SampleConsensusModelNormalPlane(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalPlane<PointT, PointNT>;
195 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_