51 Eigen::Vector4f model_coefficients;
52 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
53 Eigen::Vector4f xyz_centroid;
58 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
59 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
60 eigen33 (covariance_matrix, eigen_value, eigen_vector);
62 model_coefficients[0] = eigen_vector [0];
63 model_coefficients[1] = eigen_vector [1];
64 model_coefficients[2] = eigen_vector [2];
65 model_coefficients[3] = 0;
68 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
70 float distance_to_plane = model_coefficients[0] * point.x +
71 model_coefficients[1] * point.y +
72 model_coefficients[2] * point.z +
73 model_coefficients[3];
76 ppoint.x = point.x - distance_to_plane * model_coefficients[0];
77 ppoint.y = point.y - distance_to_plane * model_coefficients[1];
78 ppoint.z = point.z - distance_to_plane * model_coefficients[2];
83 k0 = (std::abs (model_coefficients[0] ) > std::abs (model_coefficients[1])) ? 0 : 1;
84 k0 = (std::abs (model_coefficients[k0]) > std::abs (model_coefficients[2])) ? k0 : 2;
90 for (std::size_t i = 0; i < polygon.
size (); ++i)
92 Eigen::Vector4f pt (polygon[i].x, polygon[i].y, polygon[i].z, 0);
93 xy_polygon[i].x = pt[k1];
94 xy_polygon[i].y = pt[k2];
99 Eigen::Vector4f pt (ppoint.x, ppoint.y, ppoint.z, 0);
151 output.
header = input_->header;
159 if (
static_cast<int> (planar_hull_->size ()) < min_pts_hull_)
161 PCL_ERROR(
"[pcl::%s::segment] Not enough points (%zu) in the hull!\n",
162 getClassName().c_str(),
163 static_cast<std::size_t
>(planar_hull_->size()));
169 Eigen::Vector4f model_coefficients;
170 EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix;
171 Eigen::Vector4f xyz_centroid;
176 EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;
177 EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;
178 eigen33 (covariance_matrix, eigen_value, eigen_vector);
180 model_coefficients[0] = eigen_vector [0];
181 model_coefficients[1] = eigen_vector [1];
182 model_coefficients[2] = eigen_vector [2];
183 model_coefficients[3] = 0;
186 model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid);
189 Eigen::Vector4f vp (vpx_, vpy_, vpz_, 0);
191 vp -= (*planar_hull_)[0].getVector4fMap ();
194 float cos_theta = vp.dot (model_coefficients);
198 model_coefficients *= -1;
199 model_coefficients[3] = 0;
201 model_coefficients[3] = -1 * (model_coefficients.dot ((*planar_hull_)[0].getVector4fMap ()));
207 sacmodel.
projectPoints (*indices_, model_coefficients, projected_points,
false);
212 k0 = (std::abs (model_coefficients[0] ) > std::abs (model_coefficients[1])) ? 0 : 1;
213 k0 = (std::abs (model_coefficients[k0]) > std::abs (model_coefficients[2])) ? k0 : 2;
218 polygon.
resize (planar_hull_->size ());
219 for (std::size_t i = 0; i < planar_hull_->size (); ++i)
221 Eigen::Vector4f pt ((*planar_hull_)[i].x, (*planar_hull_)[i].y, (*planar_hull_)[i].z, 0);
222 polygon[i].x = pt[k1];
223 polygon[i].y = pt[k2];
230 std::vector<pcl::PointCloud<PointT>> polygons(polygons_.size());
231 if (polygons_.empty()) {
232 polygons.push_back(polygon);
235 for (
size_t i = 0; i < polygons_.size(); i++) {
236 const auto& polygon_i = polygons_[i];
237 polygons[i].reserve(polygon_i.vertices.size());
238 for (
const auto& pointIdx : polygon_i.vertices) {
239 polygons[i].points.push_back(polygon[pointIdx]);
244 output.
indices.resize (indices_->size ());
246 for (std::size_t i = 0; i < projected_points.size (); ++i)
250 if (distance < height_limit_min_ || distance > height_limit_max_)
254 Eigen::Vector4f pt (projected_points[i].x,
255 projected_points[i].y,
256 projected_points[i].z, 0);
260 bool in_poly =
false;
261 for (
const auto& poly : polygons) {
269 output.
indices[l++] = (*indices_)[i];
void projectPoints(const Indices &inliers, const Eigen::VectorXf &model_coefficients, PointCloud &projected_points, bool copy_data_fields=true) const override
Create a new point cloud with inliers projected onto the plane model.
unsigned int computeMeanAndCovarianceMatrix(const pcl::PointCloud< PointT > &cloud, Eigen::Matrix< Scalar, 3, 3 > &covariance_matrix, Eigen::Matrix< Scalar, 4, 1 > ¢roid)
Compute the normalized 3x3 covariance matrix and the centroid of a given set of points in a single lo...
void eigen33(const Matrix &mat, typename Matrix::Scalar &eigenvalue, Vector &eigenvector)
determines the eigenvector and eigenvalue of the smallest eigenvalue of the symmetric positive semi d...
double pointToPlaneDistanceSigned(const Point &p, double a, double b, double c, double d)
Get the distance from a point to a plane (signed) defined by ax+by+cz+d=0.