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> 48 template <
typename Po
intT,
typename Po
intNT>
void 50 const Eigen::VectorXf &model_coefficients,
const double threshold,
Indices &inliers)
54 PCL_ERROR (
"[pcl::SampleConsensusModelNormalPlane::selectWithinDistance] No input dataset containing normals was given!\n");
60 if (!isModelValid (model_coefficients))
67 Eigen::Vector4f coeff = model_coefficients;
71 error_sqr_dists_.clear ();
72 inliers.reserve (indices_->size ());
73 error_sqr_dists_.reserve (indices_->size ());
76 for (std::size_t i = 0; i < indices_->size (); ++i)
78 const PointT &pt = (*input_)[(*indices_)[i]];
79 const PointNT &nt = (*normals_)[(*indices_)[i]];
82 Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
83 Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
84 double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
87 double d_normal = std::abs (
getAngle3D (n, coeff));
88 d_normal = (std::min) (d_normal,
M_PI - d_normal);
91 double weight = normal_distance_weight_ * (1.0 - nt.curvature);
93 double distance = std::abs (weight * d_normal + (1.0 - weight) * d_euclid);
97 inliers.push_back ((*indices_)[i]);
98 error_sqr_dists_.push_back (
distance);
104 template <
typename Po
intT,
typename Po
intNT> std::size_t
106 const Eigen::VectorXf &model_coefficients,
const double threshold)
const 110 PCL_ERROR (
"[pcl::SampleConsensusModelNormalPlane::countWithinDistance] No input dataset containing normals was given!\n");
115 if (!isModelValid (model_coefficients))
119 Eigen::Vector4f coeff = model_coefficients;
122 std::size_t nr_p = 0;
125 for (std::size_t i = 0; i < indices_->size (); ++i)
127 const PointT &pt = (*input_)[(*indices_)[i]];
128 const PointNT &nt = (*normals_)[(*indices_)[i]];
131 Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
132 Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
133 double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
136 double d_normal = std::abs (
getAngle3D (n, coeff));
137 d_normal = (std::min) (d_normal,
M_PI - d_normal);
140 double weight = normal_distance_weight_ * (1.0 - nt.curvature);
142 if (std::abs (weight * d_normal + (1.0 - weight) * d_euclid) < threshold)
149 template <
typename Po
intT,
typename Po
intNT>
void 151 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const 155 PCL_ERROR (
"[pcl::SampleConsensusModelNormalPlane::getDistancesToModel] No input dataset containing normals was given!\n");
160 if (!isModelValid (model_coefficients))
167 Eigen::Vector4f coeff = model_coefficients;
170 distances.resize (indices_->size ());
173 for (std::size_t i = 0; i < indices_->size (); ++i)
175 const PointT &pt = (*input_)[(*indices_)[i]];
176 const PointNT &nt = (*normals_)[(*indices_)[i]];
179 Eigen::Vector4f p (pt.x, pt.y, pt.z, 0.0f);
180 Eigen::Vector4f n (nt.normal_x, nt.normal_y, nt.normal_z, 0.0f);
181 double d_euclid = std::abs (coeff.dot (p) + model_coefficients[3]);
184 double d_normal = std::abs (
getAngle3D (n, coeff));
185 d_normal = (std::min) (d_normal,
M_PI - d_normal);
188 double weight = normal_distance_weight_ * (1.0 - nt.curvature);
190 distances[i] = std::abs (weight * d_normal + (1.0 - weight) * d_euclid);
194 #define PCL_INSTANTIATE_SampleConsensusModelNormalPlane(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalPlane<PointT, PointNT>; 196 #endif // PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_PLANE_H_ double getAngle3D(const Eigen::Vector4f &v1, const Eigen::Vector4f &v2, const bool in_degree=false)
Compute the smallest angle between two 3D vectors in radians (default) or degree. ...
Define standard C methods and C++ classes that are common to all methods.
std::size_t countWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold) const override
Count all the points which respect the given model coefficients as inliers.
void selectWithinDistance(const Eigen::VectorXf &model_coefficients, const double threshold, Indices &inliers) override
Select all the points which respect the given model coefficients as inliers.
float distance(const PointT &p1, const PointT &p2)
IndicesAllocator<> Indices
Type used for indices in PCL.
void getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given plane model.
A point structure representing Euclidean xyz coordinates, and the RGB color.