41 #ifndef PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_SPHERE_H_
42 #define PCL_SAMPLE_CONSENSUS_IMPL_SAC_MODEL_NORMAL_SPHERE_H_
44 #include <pcl/sample_consensus/sac_model_normal_sphere.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::SampleConsensusModelNormalSphere::selectWithinDistance] No input dataset containing normals was given!\n");
59 if (!isModelValid (model_coefficients))
66 Eigen::Vector4f center = 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)
79 Eigen::Vector4f p ((*input_)[(*indices_)[i]].x,
80 (*input_)[(*indices_)[i]].y,
81 (*input_)[(*indices_)[i]].z,
84 Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0],
85 (*normals_)[(*indices_)[i]].normal[1],
86 (*normals_)[(*indices_)[i]].normal[2],
89 Eigen::Vector4f n_dir = p - center;
90 double d_euclid = std::abs (n_dir.norm () - model_coefficients[3]);
93 double d_normal = std::abs (
getAngle3D (n, n_dir));
94 d_normal = (std::min) (d_normal,
M_PI - d_normal);
96 double distance = std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid);
100 inliers.push_back ((*indices_)[i]);
101 error_sqr_dists_.push_back (
static_cast<double> (
distance));
107 template <
typename Po
intT,
typename Po
intNT> std::size_t
109 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
113 PCL_ERROR (
"[pcl::SampleConsensusModelNormalSphere::getDistancesToModel] No input dataset containing normals was given!\n");
118 if (!isModelValid (model_coefficients))
123 Eigen::Vector4f center = model_coefficients;
126 std::size_t nr_p = 0;
129 for (std::size_t i = 0; i < indices_->size (); ++i)
133 Eigen::Vector4f p ((*input_)[(*indices_)[i]].x,
134 (*input_)[(*indices_)[i]].y,
135 (*input_)[(*indices_)[i]].z,
138 Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0],
139 (*normals_)[(*indices_)[i]].normal[1],
140 (*normals_)[(*indices_)[i]].normal[2],
143 Eigen::Vector4f n_dir = (p-center);
144 double d_euclid = std::abs (n_dir.norm () - model_coefficients[3]);
147 double d_normal = std::abs (
getAngle3D (n, n_dir));
148 d_normal = (std::min) (d_normal,
M_PI - d_normal);
150 if (std::abs (normal_distance_weight_ * d_normal + (1.0 - normal_distance_weight_) * d_euclid) < threshold)
157 template <
typename Po
intT,
typename Po
intNT>
void
159 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
163 PCL_ERROR (
"[pcl::SampleConsensusModelNormalSphere::getDistancesToModel] No input dataset containing normals was given!\n");
168 if (!isModelValid (model_coefficients))
175 Eigen::Vector4f center = model_coefficients;
178 distances.resize (indices_->size ());
181 for (std::size_t i = 0; i < indices_->size (); ++i)
185 Eigen::Vector4f p ((*input_)[(*indices_)[i]].x,
186 (*input_)[(*indices_)[i]].y,
187 (*input_)[(*indices_)[i]].z,
190 Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0],
191 (*normals_)[(*indices_)[i]].normal[1],
192 (*normals_)[(*indices_)[i]].normal[2],
195 Eigen::Vector4f n_dir = (p-center);
196 double d_euclid = std::abs (n_dir.norm () - model_coefficients[3]);
199 double d_normal = std::abs (
getAngle3D (n, n_dir));
200 d_normal = (std::min) (d_normal,
M_PI - d_normal);
202 distances[i] = std::abs (normal_distance_weight_ * d_normal + (1 - normal_distance_weight_) * d_euclid);
206 #define PCL_INSTANTIATE_SampleConsensusModelNormalSphere(PointT, PointNT) template class PCL_EXPORTS pcl::SampleConsensusModelNormalSphere<PointT, PointNT>;
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.
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 getDistancesToModel(const Eigen::VectorXf &model_coefficients, std::vector< double > &distances) const override
Compute all distances from the cloud data to a given sphere model.
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.
float distance(const PointT &p1, const PointT &p2)
IndicesAllocator<> Indices
Type used for indices in PCL.