求点云中近似落在平面上的点

mypcl

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 ("[pcl::SampleConsensusModelPlane::selectWithinDistance] 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[nr_p] = (*indices_);

  173

       error_sqr_dists_[nr_p] = static_cast<double> (distance);

  174

       ++nr_p;

  175

     }

  176

   }

  177

   inliers.resize (nr_p);

  178

   error_sqr_dists_.resize (nr_p);

  179

}

mypcl

1. pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr dit (new pcl::SampleConsensusModelPlane<pcl::PointXYZ> (source_cloud));
   
2. 
   
3. Eigen::Vector4f coefficients = Eigen::Vector4f(a, b, c, d);
   
4. std::vector<int> inliers;
   
5. dit -> selectWithinDistance (coefficients, Threshold, inliers);
   
6. pcl::copyPointCloud<pcl::PointXYZ>(*source_cloud, inliers, *new_cloud);

复制代码