PCL拟合多条直线

手写ransac拟合多条直线

投影＋提取边缘点后的点云，然后拟合，直接上代码。
因为是投影到xy平面上的点云，所有只有k b 两个直线参数。
还是先看效果图吧。。。
恩，，每次选择拟合直线的点数放在了函数里，代码写的较差，见谅哈哈哈

当时只是上传代码，没做过多介绍，没想到有朋友关注评论，这里对原理做下解释，大家可以根据各自具体的情况和需求进行改进。
输入的点云类似矩形，我的目标是提取4条直线参数。
每一条直线提取通过迭代若干次得到，并选择当前直线上点数最多的作为第n条直线，具体过程是每次随机选取m（m>=3,可以自行选择不同参数调整观察效果）个点进行最小二乘拟合得到直线L，并记录L上一定阈值内的点数和点索引，最后选取最优直线。得到一条拟合直线后，会把属于这条L上的点索引去除掉。
然后继续进行下一条直线的拟合过程，最后根据拟合得到的直线数和剩余的点数判断是否停止迭代。

#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <iostream>
#include <string>     
#include<cmath>
#include <time.h>

using namespace std;

class fitLineRansac
{
  
public:
  vector<vector<float>> ransac_line(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, float dist, int iterate)
  {
    /*** 
     *		dist: 点到直线距离小于dist 即当前点在直线上
     *      iterate: 迭代次数
    ***/
    int allPts = cloud->points.size();
    vector<int> cur_ptsIdx(allPts);			//提取当前直线后，不在当前直线上的其他点索引
    for(int i=0; i<allPts; i++)
      cur_ptsIdx[i] = i;

    int r=0;
    vector<int> line_ptsIdx, all_ptsIdx(allPts);
    vector<vector<float>> lines;			//所有直线参数 [0]: k, [1]: b
    vector<float> cur_line(2);
    Eigen::Vector3f line_model, best_lineModel;
    while (1)
    { 
      int line_pts=0, tmp_pts;
      if(r >= 2) iterate = iterate / 3;
      if(cur_ptsIdx.size()<10 && cur_ptsIdx.size()>3) iterate = 4;
      for(int i=0; i<iterate; i++){
        line_model = leastSquare(cloud, cur_ptsIdx, dist);
        tmp_pts = line_model[2] / 1;
        if(tmp_pts > line_pts){
          line_pts = tmp_pts;
          best_lineModel = line_model;
          line_ptsIdx = tmp_ptsIdx;
        }
        tmp_ptsIdx.clear();
      }
      
      cur_line[0] = best_lineModel[0]; cur_line[1] = best_lineModel[1];
      lines.push_back(cur_line);
      cout<<"第　"<<r++<<"　次循环,直线参数:  "<<best_lineModel<<endl;
    //   cout<<"所有点的个数:  "<<cur_ptsIdx.size()<<endl;
    //   cout<<"当前直线上的点数："<<line_ptsIdx.size()<<endl;
	
	//得到剩余点的索引
    for(int i=0; i<line_ptsIdx.size(); i++)
      all_ptsIdx[line_ptsIdx[i]] = 1;
    cur_ptsIdx.clear();
    for(int j=0; j<allPts; j++)
      if(!all_ptsIdx[j]) cur_ptsIdx.push_back(j);
      
      if(cur_ptsIdx.size() < 5){
        cout<<"点数剩余过少......."<<endl;
        break;  
      }
    } 
    view(cloud, lines);
    return lines;
  }

private:
  vector<int> tmp_ptsIdx;			//当前直线上的点数
  Eigen::Vector3f leastSquare(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, vector<int> pIdx, float dist)
  {
    //求解给定若干点的直线方程
    float a=0, B=0, c=0, d=0;          //a: x之和　b: y之和  c: x平方和  d: x*y之和  e: 样本数量
    int s = pIdx.size();
    vector<int> cur_ptsIdx = rangedRand(0, s, 4);					//４：每次选４点用最小二乘拟合直线
    int e = cur_ptsIdx.size();
    for(int i=0; i<e; i++){
      a += cloud->points[pIdx[cur_ptsIdx[i]]].x;
      B += cloud->points[pIdx[cur_ptsIdx[i]]].y;
      c += cloud->points[pIdx[cur_ptsIdx[i]]].x * cloud->points[pIdx[cur_ptsIdx[i]]].x;
      d += cloud->points[pIdx[cur_ptsIdx[i]]].x * cloud->points[pIdx[cur_ptsIdx[i]]].y;
    }
    float k, b;
    float tmp = e * c - a * a;
    if(abs(tmp) > 0.0005){
      b = (c*B - a*d) / tmp;
      k = (e*d - a*B) / tmp;
    }
    else{
      k=1;b=0;
    }
    
    //求每一个点到直线的距离，小于dist, 即在直线上
    int line_pnum=0;
    for(int i=0; i<s; i++){
      float d, numerator, denominator;             //分子分母        点到直线的距离　d = |kx - y + b| / sqrt(k^2 + 1)
      numerator = abs(k*cloud->points[pIdx[i]].x - cloud->points[pIdx[i]].y + b);
      denominator = sqrt(k*k + 1);
      d = numerator / denominator;
      if(d < dist){
        line_pnum++;
        tmp_ptsIdx.push_back(pIdx[i]);
      }
    }
    Eigen::Vector3f line_model;              
    line_model[0] = k;line_model[1] = b; line_model[2] = line_pnum;
    return line_model;
  }

  vector<int> rangedRand( int range_begin, int range_size, int n )
  {
    int i;vector<int> indices;
    // srand((unsigned)time(NULL));           //生成随机数种子
    for ( i = 0; i < n; i++ )
    {
      int u = rand()%range_size + range_begin; //生成[range_begin, range_begin+range_size]内的随机数
      // cout<<i<<": "<<u<<endl;
      indices.push_back(u);
    }
    return indices;
  }

  void view(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, vector<vector<float>> lines)
  { 
    cout<<"共找到   "<<lines.size()<<"  条直线!!!!!!!!"<<endl;
    pcl::visualization::PCLVisualizer viewer("Viewer");
    viewer.setBackgroundColor(0.5, 0.5, 0.5, 0);
    viewer.addPointCloud(cloud, "pc");
    viewer.setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "pc"); 
    for(int i=0; i<lines.size(); i++){
        pcl::PointXYZ p1(200, 200*lines[i][0]+lines[i][1], 0);
        pcl::PointXYZ p2(-500, -500*lines[i][0]+lines[i][1], 0);
        cout<<"直线     "<<i+1<<"   上两点坐标： "<<p1<<",  "<<p2<<endl;
        viewer.addLine(p1, p2, 240-40*i, 0, 0, "line"+to_string(i), 0);
    }
    while (!viewer.wasStopped()) {
    	viewer.spinOnce();
    }
  }

};

int main(int argc, char**argv) {
  // 加载点云模型
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
   pcl::io::loadPCDFile (argv[1], *cloud);
   
  fitLineRansac ransac;
  ransac.ransac_line(cloud, 7, 50);
  return 0;
}