50 const Eigen::VectorXf &model_coefficients,
const double threshold,
Indices &inliers)
54 PCL_ERROR (
"[pcl::SampleConsensusModelNormalSphere::selectWithinDistance] No input dataset containing normals was given!\n");
60 if (!isModelValid (model_coefficients))
67 Eigen::Vector4f center = 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)
80 Eigen::Vector4f p ((*input_)[(*indices_)[i]].x,
81 (*input_)[(*indices_)[i]].y,
82 (*input_)[(*indices_)[i]].z,
85 Eigen::Vector4f n_dir = p - center;
86 const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (n_dir.norm () - model_coefficients[3]);
87 if (weighted_euclid_dist > threshold)
91 Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0],
92 (*normals_)[(*indices_)[i]].normal[1],
93 (*normals_)[(*indices_)[i]].normal[2],
95 double d_normal = std::abs (
getAngle3D (n, n_dir));
96 d_normal = (std::min) (d_normal,
M_PI - d_normal);
98 double distance = std::abs (normal_distance_weight_ * d_normal + weighted_euclid_dist);
99 if (distance < threshold)
102 inliers.push_back ((*indices_)[i]);
103 error_sqr_dists_.push_back (
static_cast<double> (distance));
111 const Eigen::VectorXf &model_coefficients,
const double threshold)
const
115 PCL_ERROR (
"[pcl::SampleConsensusModelNormalSphere::getDistancesToModel] No input dataset containing normals was given!\n");
120 if (!isModelValid (model_coefficients))
125 Eigen::Vector4f center = model_coefficients;
128 std::size_t nr_p = 0;
131 for (std::size_t i = 0; i < indices_->size (); ++i)
135 Eigen::Vector4f p ((*input_)[(*indices_)[i]].x,
136 (*input_)[(*indices_)[i]].y,
137 (*input_)[(*indices_)[i]].z,
140 Eigen::Vector4f n_dir = (p-center);
141 const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (n_dir.norm () - model_coefficients[3]);
142 if (weighted_euclid_dist > threshold)
146 Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0],
147 (*normals_)[(*indices_)[i]].normal[1],
148 (*normals_)[(*indices_)[i]].normal[2],
150 double d_normal = std::abs (
getAngle3D (n, n_dir));
151 d_normal = (std::min) (d_normal,
M_PI - d_normal);
153 if (std::abs (normal_distance_weight_ * d_normal + weighted_euclid_dist) < threshold)
162 const Eigen::VectorXf &model_coefficients, std::vector<double> &distances)
const
166 PCL_ERROR (
"[pcl::SampleConsensusModelNormalSphere::getDistancesToModel] No input dataset containing normals was given!\n");
171 if (!isModelValid (model_coefficients))
178 Eigen::Vector4f center = model_coefficients;
181 distances.resize (indices_->size ());
184 for (std::size_t i = 0; i < indices_->size (); ++i)
188 Eigen::Vector4f p ((*input_)[(*indices_)[i]].x,
189 (*input_)[(*indices_)[i]].y,
190 (*input_)[(*indices_)[i]].z,
193 Eigen::Vector4f n_dir = (p-center);
194 const double weighted_euclid_dist = (1.0 - normal_distance_weight_) * std::abs (n_dir.norm () - model_coefficients[3]);
197 Eigen::Vector4f n ((*normals_)[(*indices_)[i]].normal[0],
198 (*normals_)[(*indices_)[i]].normal[1],
199 (*normals_)[(*indices_)[i]].normal[2],
201 double d_normal = std::abs (
getAngle3D (n, n_dir));
202 d_normal = (std::min) (d_normal,
M_PI - d_normal);
204 distances[i] = std::abs (normal_distance_weight_ * d_normal + weighted_euclid_dist);
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.