template <typename PointNT> void pcl::GridProjection<PointNT>::getProjectionWithPlaneFit (const Eigen::Vector4f &p, std::vector<int> (&pt_union_indices), Eigen::Vector4f &projection) { // Compute the plane coefficients Eigen::Vector4f model_coefficients; /// @remark iterative weighted least squares or sac might give better results Eigen::Matrix3f covariance_matrix; Eigen::Vector4f xyz_centroid; computeMeanAndCovarianceMatrix (*data_, pt_union_indices, covariance_matrix, xyz_centroid); // Get the plane normal EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); // The normalization is not necessary, since the eigenvectors from libeigen are already normalized model_coefficients[0] = eigen_vector [0]; model_coefficients[1] = eigen_vector [1]; model_coefficients[2] = eigen_vector [2]; model_coefficients[3] = 0; // Hessian form (D = nc . p_plane (centroid here) + p) model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid); // Projected point Eigen::Vector3f point (p.x (), p.y (), p.z ()); //= Eigen::Vector3f::MapAligned (&output.points[cp].x, 3); float distance = point.dot (model_coefficients.head <3> ()) + model_coefficients[3]; point -= distance * model_coefficients.head < 3 > (); projection = Eigen::Vector4f (point[0], point[1], point[2], 0); }
template <typename PointT, typename Scalar> inline unsigned int pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, const pcl::PointIndices &indices, Eigen::Matrix<Scalar, 3, 3> &covariance_matrix, Eigen::Matrix<Scalar, 4, 1> ¢roid) { return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid)); }
template <typename PointT> inline unsigned int pcl::computeMeanAndCovarianceMatrix (const pcl::PointCloud<PointT> &cloud, const pcl::PointIndices &indices, Eigen::Matrix3d &covariance_matrix, Eigen::Vector4d ¢roid) { return (computeMeanAndCovarianceMatrix (cloud, indices.indices, covariance_matrix, centroid)); }
template <typename PointT> bool pcl::isPointIn2DPolygon (const PointT &point, const pcl::PointCloud<PointT> &polygon) { // Compute the plane coefficients Eigen::Vector4f model_coefficients; EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; Eigen::Vector4f xyz_centroid; computeMeanAndCovarianceMatrix (polygon, covariance_matrix, xyz_centroid); // Compute the model coefficients EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; eigen33 (covariance_matrix, eigen_value, eigen_vector); model_coefficients[0] = eigen_vector [0]; model_coefficients[1] = eigen_vector [1]; model_coefficients[2] = eigen_vector [2]; model_coefficients[3] = 0; // Hessian form (D = nc . p_plane (centroid here) + p) model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid); float distance_to_plane = model_coefficients[0] * point.x + model_coefficients[1] * point.y + model_coefficients[2] * point.z + model_coefficients[3]; PointT ppoint; // Calculate the projection of the point on the plane ppoint.x = point.x - distance_to_plane * model_coefficients[0]; ppoint.y = point.y - distance_to_plane * model_coefficients[1]; ppoint.z = point.z - distance_to_plane * model_coefficients[2]; // Create a X-Y projected representation for within bounds polygonal checking int k0, k1, k2; // Determine the best plane to project points onto k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1; k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2; k1 = (k0 + 1) % 3; k2 = (k0 + 2) % 3; // Project the convex hull pcl::PointCloud<PointT> xy_polygon; xy_polygon.points.resize (polygon.points.size ()); for (size_t i = 0; i < polygon.points.size (); ++i) { Eigen::Vector4f pt (polygon.points[i].x, polygon.points[i].y, polygon.points[i].z, 0); xy_polygon.points[i].x = pt[k1]; xy_polygon.points[i].y = pt[k2]; xy_polygon.points[i].z = 0; } PointT xy_point; xy_point.z = 0; Eigen::Vector4f pt (ppoint.x, ppoint.y, ppoint.z, 0); xy_point.x = pt[k1]; xy_point.y = pt[k2]; return (pcl::isXYPointIn2DXYPolygon (xy_point, xy_polygon)); }
void UniformSamplingExtractor<PointT>::filterPlanar (const PointInTPtr & input, std::vector<int> &kp_idx) { //create a search object typename pcl::search::Search<PointT>::Ptr tree; if (input->isOrganized () && !force_unorganized_) tree.reset (new pcl::search::OrganizedNeighbor<PointT> ()); else tree.reset (new pcl::search::KdTree<PointT> (false)); tree->setInputCloud (input); size_t kept = 0; for (size_t i = 0; i < kp_idx.size (); i++) { std::vector<int> neighborhood_indices; std::vector<float> neighborhood_dist; if (tree->radiusSearch (kp_idx[i], radius_, neighborhood_indices, neighborhood_dist)) { EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; Eigen::Vector4f xyz_centroid; EIGEN_ALIGN16 Eigen::Vector3f eigenValues; EIGEN_ALIGN16 Eigen::Matrix3f eigenVectors; //compute planarity of the region computeMeanAndCovarianceMatrix (*input, neighborhood_indices, covariance_matrix, xyz_centroid); pcl::eigen33 (covariance_matrix, eigenVectors, eigenValues); float eigsum = eigenValues.sum (); if (!pcl_isfinite(eigsum)) PCL_ERROR("Eigen sum is not finite\n"); float t_planar = threshold_planar_; if(z_adaptative_) t_planar *= (1 + (std::max(input->points[kp_idx[i]].z,1.f) - 1.f)); //if ((fabs (eigenValues[0] - eigenValues[1]) < 1.5e-4) || (eigsum != 0 && fabs (eigenValues[0] / eigsum) > 1.e-2)) if ((fabs (eigenValues[0] - eigenValues[1]) < 1.5e-4) || (eigsum != 0 && fabs (eigenValues[0] / eigsum) > t_planar)) { //region is not planar, add to filtered keypoint kp_idx[kept] = kp_idx[i]; kept++; } } } kp_idx.resize (kept); }
template <typename PointInT> void pcl::ConvexHull<PointInT>::calculateInputDimension () { PCL_DEBUG ("[pcl::%s::calculateInputDimension] WARNING: Input dimension not specified. Automatically determining input dimension.\n", getClassName ().c_str ()); EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; Eigen::Vector4f xyz_centroid; computeMeanAndCovarianceMatrix (*input_, *indices_, covariance_matrix, xyz_centroid); EIGEN_ALIGN16 Eigen::Vector3f eigen_values; pcl::eigen33 (covariance_matrix, eigen_values); if (eigen_values[0] / eigen_values[2] < 1.0e-3) dimension_ = 2; else dimension_ = 3; }
template <typename PointT> void pcl::SampleConsensusModelStick<PointT>::optimizeModelCoefficients ( const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) { // Needs a valid set of model coefficients if (!isModelValid (model_coefficients)) { optimized_coefficients = model_coefficients; return; } // Need at least 2 points to estimate a line if (inliers.size () <= 2) { PCL_ERROR ("[pcl::SampleConsensusModelStick::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ()); optimized_coefficients = model_coefficients; return; } optimized_coefficients.resize (7); // Compute the 3x3 covariance matrix Eigen::Vector4f centroid; Eigen::Matrix3f covariance_matrix; computeMeanAndCovarianceMatrix (*input_, inliers, covariance_matrix, centroid); optimized_coefficients[0] = centroid[0]; optimized_coefficients[1] = centroid[1]; optimized_coefficients[2] = centroid[2]; // Extract the eigenvalues and eigenvectors Eigen::Vector3f eigen_values; Eigen::Vector3f eigen_vector; pcl::eigen33 (covariance_matrix, eigen_values); pcl::computeCorrespondingEigenVector (covariance_matrix, eigen_values [2], eigen_vector); //optimized_coefficients.template segment<3> (3) = eigen_vector; optimized_coefficients[0] = eigen_vector[0]; optimized_coefficients[1] = eigen_vector[1]; optimized_coefficients[2] = eigen_vector[2]; }
template <typename PointT> void pcl::SampleConsensusModelPlane<PointT>::optimizeModelCoefficients ( const std::vector<int> &inliers, const Eigen::VectorXf &model_coefficients, Eigen::VectorXf &optimized_coefficients) { // Needs a valid set of model coefficients if (model_coefficients.size () != 4) { PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Invalid number of model coefficients given (%zu)!\n", model_coefficients.size ()); optimized_coefficients = model_coefficients; return; } // Need at least 3 points to estimate a plane if (inliers.size () < 4) { PCL_ERROR ("[pcl::SampleConsensusModelPlane::optimizeModelCoefficients] Not enough inliers found to support a model (%zu)! Returning the same coefficients.\n", inliers.size ()); optimized_coefficients = model_coefficients; return; } Eigen::Vector4f plane_parameters; // Use Least-Squares to fit the plane through all the given sample points and find out its coefficients EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; Eigen::Vector4f xyz_centroid; computeMeanAndCovarianceMatrix (*input_, inliers, covariance_matrix, xyz_centroid); // Compute the model coefficients EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); // Hessian form (D = nc . p_plane (centroid here) + p) optimized_coefficients.resize (4); optimized_coefficients[0] = eigen_vector [0]; optimized_coefficients[1] = eigen_vector [1]; optimized_coefficients[2] = eigen_vector [2]; optimized_coefficients[3] = 0; optimized_coefficients[3] = -1 * optimized_coefficients.dot (xyz_centroid); }
template <typename PointT> void pcl::ExtractPolygonalPrismData<PointT>::segment (pcl::PointIndices &output) { output.header = input_->header; if (!initCompute ()) { output.indices.clear (); return; } if (static_cast<int> (planar_hull_->points.size ()) < min_pts_hull_) { PCL_ERROR ("[pcl::%s::segment] Not enough points (%zu) in the hull!\n", getClassName ().c_str (), planar_hull_->points.size ()); output.indices.clear (); return; } // Compute the plane coefficients Eigen::Vector4f model_coefficients; EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix; Eigen::Vector4f xyz_centroid; computeMeanAndCovarianceMatrix (*planar_hull_, covariance_matrix, xyz_centroid); // Compute the model coefficients EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; eigen33 (covariance_matrix, eigen_value, eigen_vector); model_coefficients[0] = eigen_vector [0]; model_coefficients[1] = eigen_vector [1]; model_coefficients[2] = eigen_vector [2]; model_coefficients[3] = 0; // Hessian form (D = nc . p_plane (centroid here) + p) model_coefficients[3] = -1 * model_coefficients.dot (xyz_centroid); // Need to flip the plane normal towards the viewpoint Eigen::Vector4f vp (vpx_, vpy_, vpz_, 0); // See if we need to flip any plane normals vp -= planar_hull_->points[0].getVector4fMap (); vp[3] = 0; // Dot product between the (viewpoint - point) and the plane normal float cos_theta = vp.dot (model_coefficients); // Flip the plane normal if (cos_theta < 0) { model_coefficients *= -1; model_coefficients[3] = 0; // Hessian form (D = nc . p_plane (centroid here) + p) model_coefficients[3] = -1 * (model_coefficients.dot (planar_hull_->points[0].getVector4fMap ())); } // Project all points PointCloud projected_points; SampleConsensusModelPlane<PointT> sacmodel (input_); sacmodel.projectPoints (*indices_, model_coefficients, projected_points, false); // Create a X-Y projected representation for within bounds polygonal checking int k0, k1, k2; // Determine the best plane to project points onto k0 = (fabs (model_coefficients[0] ) > fabs (model_coefficients[1])) ? 0 : 1; k0 = (fabs (model_coefficients[k0]) > fabs (model_coefficients[2])) ? k0 : 2; k1 = (k0 + 1) % 3; k2 = (k0 + 2) % 3; // Project the convex hull pcl::PointCloud<PointT> polygon; polygon.points.resize (planar_hull_->points.size ()); for (size_t i = 0; i < planar_hull_->points.size (); ++i) { Eigen::Vector4f pt (planar_hull_->points[i].x, planar_hull_->points[i].y, planar_hull_->points[i].z, 0); polygon.points[i].x = pt[k1]; polygon.points[i].y = pt[k2]; polygon.points[i].z = 0; } PointT pt_xy; pt_xy.z = 0; output.indices.resize (indices_->size ()); int l = 0; for (size_t i = 0; i < projected_points.points.size (); ++i) { // Check the distance to the user imposed limits from the table planar model double distance = pointToPlaneDistanceSigned (input_->points[(*indices_)[i]], model_coefficients); if (distance < height_limit_min_ || distance > height_limit_max_) continue; // Check what points are inside the hull Eigen::Vector4f pt (projected_points.points[i].x, projected_points.points[i].y, projected_points.points[i].z, 0); pt_xy.x = pt[k1]; pt_xy.y = pt[k2]; if (!pcl::isXYPointIn2DXYPolygon (pt_xy, polygon)) continue; output.indices[l++] = (*indices_)[i]; } output.indices.resize (l); deinitCompute (); }