求点云中近似落在平面上的点
PCL内部已经实现了，直接调用方便啊，前面自己写了一个。今天看到了，就记录下。
pcl::SampleConsensusModelPlane<PointT>::selectWithinDistance (
144 const Eigen::VectorXf &model_coefficients, const double threshold, std::vector<int> &inliers)
145 {
146 // Needs a valid set of model coefficients
147 if (model_coefficients.size () != 4)
148 {
149 PCL_ERROR (" Invalid number of model coefficients given (%lu)!\n", model_coefficients.size ());
150 return;
151 }
152
153 int nr_p = 0;
154 inliers.resize (indices_->size ());
155 error_sqr_dists_.resize (indices_->size ());
156
157 // Iterate through the 3d points and calculate the distances from them to the plane
158 for (size_t i = 0; i < indices_->size (); ++i)
159 {
160 // Calculate the distance from the point to the plane normal as the dot product
161 // D = (P-A).N/|N|
162 Eigen::Vector4f pt (input_->points[(*indices_)].x,
163 input_->points[(*indices_)].y,
164 input_->points[(*indices_)].z,
165 1);
166
167 float distance = fabsf (model_coefficients.dot (pt));
168
169 if (distance < threshold)
170 {
171 // Returns the indices of the points whose distances are smaller than the threshold
172 inliers = (*indices_);
173 error_sqr_dists_ = static_cast<double> (distance);
174 ++nr_p;
175 }
176 }
177 inliers.resize (nr_p);
178 error_sqr_dists_.resize (nr_p);
179 }
pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr dit (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (source_cloud));
Eigen::Vector4f coefficients = Eigen::Vector4f(a, b, c, d);
std::vector<int> inliers;
dit -> selectWithinDistance (coefficients, Threshold, inliers);
pcl::copyPointCloud<pcl::PointXYZ>(*source_cloud, inliers, *new_cloud);
页:
[1]