思路参考了论文《基于特征线段匹配的救援机器人建图方法》

特征识别主要分为两个过程：区域分割与特征提取

首先要做的就是区域分割，假设激光雷达可以得到n个数据点，以这n个数据点作为一个区域，可以表示为A1{(Xi, Yi) | i = 1, 2, ..., n}，利用两点间欧氏距离公式可得到两点间距Dj，同时确定一个阈值Delta，比较delta和Dj，确定两点是否在同一个线段特征集合范围内。当Dj大于Delta时，确定点（Xj, Yj）为区域的一个分割点，以此点将激光雷达的n个数据分割成两部分，分别得到区域A{(Xi, Yi) | i = 1, 2,..., j}和A’{(Xi, Yi) |  i = j+1, j+2, ..., n}。依次类推，对A和A‘应用算法进行分割，最后能够得到N个互不相连区域{A1, A2, ..., An}。

因为做的是基于QT的程序，所以用的是QT下的模板类

#include <QCoreApplication>
#include "qvector.h"
#include "math.h"
#include "qlinkedlist.h"
#include <iostream>

using namespace std;

typedef struct Point
{
    double xpos;
    double ypos;
}Point;

QLinkedList<QVector<Point> > subset_list;    //声明全局变量，用于存储分割后的子集

/***************************将点集分割为互不相连子集********************************/
/*********************************************************************************/
/*******输入：QVector<Point>点集数组、int n为点集内点的数量、double Delta分割阈值*********/
/***********输出：QLinkedList<QVector<Point>> 链表存储每个点集的数组********************/
/*******************************返回值：n为分割子集数目*****************************/

int partition(const QVector<Point> &point_vector, int n, double Delta, QLinkedList<QVector<Point> > &sub_list)
{
    int count = 1;      //保存分割子集数,初始有1个所有点的集合
    int pivot = 0;  //记录分割点
    double dist;
    for(int i = 0, j = i + 1; i < n - 1; i++, j++)
        {
            double x_dist = fabs(point_vector[i].xpos - point_vector[j].xpos);
            double y_dist = fabs(point_vector[i].ypos - point_vector[j].ypos);
            dist = sqrt(x_dist * x_dist + y_dist * y_dist);     //求相邻两点间距离
            if(dist >= Delta)
            {
                //将集合分割
                //建立临时数组保存分割子集
                QVector<Point> tmp_array;
                //将分割出来的点保存到临时子集中
                for(int k = pivot;k <= i; k++)
                    tmp_array.append(point_vector[k]);
                //更新分割点记录
                pivot = j;
                cout << "第" << count << "个分割点为：" << pivot << endl;
                //将分割的子集加入链表中，分割子集数加1
                sub_list.append(tmp_array);
                count++;
            }
    }
    return count;
}

int main()
{
    QVector<Point> test_array;
    double set_1 = 1.0;
    double set_2 = 3.0;
    double set_3 = 5.0;
    Point p;
    p.xpos = 0.0; p.ypos = 0.0;

    for(int i = 0; i < 10; i++)
    {
        p.xpos = set_1; p.ypos = set_1;
        test_array.append(p);
        set_1 += 0.1;
    }
    for(int i = 0; i < 10; i++)
    {
        p.xpos = set_2; p.ypos = set_2;
        test_array.append(p);
        set_2 += 0.1;
    }
    for(int i = 0; i < 10; i++)
    {
        p.xpos = set_3; p.ypos = set_3;
        test_array.append(p);
        set_3 += 0.1;
    }

    int count = partition(test_array, test_array.length(), 0.5, subset_list);
    for(int i = 0; i < test_array.length(); i++)
    {
        if(i % 10 == 0 && i != 0)
            cout << endl;
        cout << "(" << test_array[i].xpos<<","<<test_array[i].ypos << ")   ";
    }
    cout << endl;
    cout<< "分割子集数为：" << count << endl;
    return 0;
}