2d 点云匹配算法

 #include "dbtype.h"
 #include "dbkdtree.h"

 #include <pcl/point_cloud.h>
 #include <pcl/io/pcd_io.h>
 #include <pcl/point_types.h>
 #include <pcl/visualization/pcl_visualizer.h>
 #include <pcl/common/transforms.h>

 #include <pcl/registration/icp.h>

 #include <iostream>
 #include <time.h>
 #include <algorithm>
 #include <Eigen/Dense>
 #include <valarray>
 #include <random>
 #include <vector>
 #include <set>

     //二维点转平面
 void toProjecImage(float& x, float& y, unsigned int& rows, unsigned int& cols)    //  640X640的图像
 {
     //    int nthrds = 4;
     //    omp_set_num_threads(nthrds);
     //#pragma omp parallel for
     /*int rows = 0;
     int cols = 0;*/

     if (x >= -16.0 && x<0.0)
     {
         cols = (unsigned int)((x + ) * );
         if (y >= -16.0 && y<0.0)
         {
             rows = (unsigned int)(y*(-)) + ;
         }
         if (y >= 0.0 && y <= 16.0)
         {
             rows = (unsigned int)(( - y) * );
         }
     }
     else if (x >= 0.0 && x <= 16.0)
     {
         cols = (unsigned int)((x) * ) + ;
         if (y >= -16.0 && y<0.0)
         {
             rows = (unsigned int)(y*(-)) + ;
         }
         if (y >= 0.0 && y <= 16.0)
         {
             rows = (unsigned int)(( - y) * );
         }
     }
 }

 //输入一系列点的坐标，输出这些点所拟合的线的k b值 最小二乘法
 std::vector<double> LineFitting(std::vector<db::Point2f> &rPoints)
 {
     // y = Ax + B，根据最小二乘法求出A，B
     std::vector<double > resLine();
     int size = rPoints.size();
     float *x = new float[size];
     float *y = new float[size];
     float A=1.0, B=0.0;
     float xmean = 0.0f;
     float ymean = 0.0f;

     for (int i = ; i < size; i++)
     {
         x[i] = rPoints[i].x;
         y[i] = rPoints[i].y;
     }

     for (int i = ; i < size; i++)
     {
         xmean += x[i];
         ymean += y[i];
     }
     xmean /= size;
     ymean /= size;
     float sumx2 = 0.0f;
     float sumxy = 0.0f;
     for (int i = ; i < size; i++)
     {
         sumx2 += (x[i] - xmean) * (x[i] - xmean);
         sumxy += (y[i] - ymean) * (x[i] - xmean);
     }

     if (sumx2!=)
     {
         A = sumxy / sumx2;
         B = ymean - A*xmean;
     }
     else
     {
         A = 1.0;
         B = 0.0;
     }

     resLine[] = A;
     resLine[] = B;
     return resLine;
 }
 std::vector<double> leastSquareFitting(std::vector<db::Point2f> &rPoints)
 {
     std::vector<double > resLine();
     int num_points = rPoints.size();
     std::valarray<float> data_x(num_points);
     std::valarray<float> data_y(num_points);
     for (int i = ; i < num_points; i++)
     {
         data_x[i] = rPoints[i].x;
         data_y[i] = rPoints[i].y;
     }
     float A = 0.0;
     float B = 0.0;
     float C = 0.0;
     float D = 0.0;
     A = (data_x*data_x).sum();
     B = data_x.sum();
     C = (data_x*data_y).sum();
     D = data_y.sum();
     float k, b, tmp = ;
     if (tmp = (A*data_x.size() - B*B))   //temp！=0
     {
         k = (C*data_x.size() - B*D) / tmp;
         b = (A*D - C*B) / tmp;
     }
     else
     {
         k = ;
         b = ;
     }
     resLine[] = k;
     resLine[] = b;
     return resLine;
 }

 //已知直线方程  线外一点  求其垂足
 db::Point2f getFootPoint(std::vector<double>& kb, const db::Point2f& P1)
 {
     db::Point2f FootPoint;
     double x = (kb[]* P1.y+ P1.x- kb[]*kb[])/(+ kb[]* kb[]);
     double y = x*kb[] + kb[];
     FootPoint.x = (float)x;
     FootPoint.y = (float)y;

     return FootPoint;
 }
 //已知直线方程  线外一点  求点到直线距离
 double getPointToline_Distance(std::vector<double>& kb, const db::Point2f& P1)
 {
     db::Point2f pointtemp = getFootPoint(kb, P1);
     double distance=0.0;
     distance = sqrt((pointtemp.x - P1.x)*(pointtemp.x - P1.x) + (pointtemp.y - P1.y)*(pointtemp.y - P1.y));
     return distance;
 }
 //求平面 两点间距离
 double getPointToPoint_Distance(const db::Point2f& P1, const db::Point2f& P2)
 {
     double distance = 0.0;
     distance = sqrt((P2.x - P1.x)*(P2.x - P1.x) + (P2.y - P1.y)*(P2.y - P1.y));
     return distance;
 }

 //获取pointset中的直线段
 void getLinesFromPointset(const std::vector<db::Point2f> &rPoints, std::vector<db::line> &lineSets)    //2018.11.7 修正
 {

     if (rPoints.size()<)
     {
         std::cout << "point cloud have NuLL data !!!" << std::endl;
         return;
     }

     double avg = 0.0,sum=0.0;
     std::vector<double> kb;
     std::vector<db::Point2f> tempPoints;
     db::line tempLine;
     std::vector<double> kb1;

     std::vector<double> kb2;
     std::vector<double> kb3;

     for (size_t i = ; i < rPoints.size(); i++)
     {
         if (tempPoints.size()==)
         {
             if (i == )
             {
                 double distance_0 = getPointToPoint_Distance(rPoints[i], rPoints[i + ]);
                 if (distance_0 <0.5)
                 {
                     tempPoints.push_back(rPoints[i]);

                     tempLine.Points.push_back(rPoints[i]);
                     tempLine.PointsIndex.push_back(i);
                 }
                 continue;
             }
             if (i == rPoints.size()-)
             {
                 double distance_0 = getPointToPoint_Distance(rPoints[i], rPoints[i -]);
                 if (distance_0 <0.5)
                 {
                     tempPoints.push_back(rPoints[i]);

                     tempLine.Points.push_back(rPoints[i]);
                     tempLine.PointsIndex.push_back(i);
                 }
                 continue;
             }

             //判断孤点
             double distance_1 = getPointToPoint_Distance(rPoints[i], rPoints[i - ]);
             double distance_2 = getPointToPoint_Distance(rPoints[i], rPoints[i + ]);
             if (distance_1<0.5 || distance_2<0.5)
             {
                 tempPoints.push_back(rPoints[i]);

                 tempLine.Points.push_back(rPoints[i]);
                 tempLine.PointsIndex.push_back(i);
             }

             continue;
         }
         else
         {
             tempPoints.push_back(rPoints[i]);
         }

         if (tempPoints.size() < )
         {
             double distance_1 = getPointToPoint_Distance(rPoints[i], rPoints[i - ]);
             if (distance_1>0.5)
             {
                 tempLine.Points.swap(std::vector<db::Point2f>());
                 tempLine.PointsIndex.swap(std::vector<int>());
                 tempLine.kb.swap(std::vector<double>());

                 tempPoints.swap(std::vector<db::Point2f>());
                 continue;
             }
             kb = leastSquareFitting(tempPoints);
             kb1 = kb;
             kb2 = kb1;
             kb3 = kb1;

             tempLine.Points.push_back(rPoints[i]);
             tempLine.PointsIndex.push_back(i);
             continue;
         }

         //点到直线的距离
         double dis_1 = getPointToline_Distance(kb1, rPoints[i]);
         double dis_2 = getPointToline_Distance(kb2, rPoints[i]);
         double dis_3 = getPointToline_Distance(kb3, rPoints[i]);

         double distance_2 = getPointToPoint_Distance(rPoints[i], rPoints[i - ]);

         if (dis_1 > 0.05|| distance_2 > 0.5)      //偏差较大的 点20cm
         {
             //sum = 0.0;
             if (i!=)
                 i--;

             tempLine.kb = kb1;
             if (atan(tempLine.kb[]) >= )
                 tempLine.sigma = atan(tempLine.kb[]);   //0-180度
             else
                 tempLine.sigma = atan(tempLine.kb[]) + 3.1415926;

             lineSets.push_back(tempLine);

             tempLine.Points.swap(std::vector<db::Point2f>());
             tempLine.PointsIndex.swap(std::vector<int>());
             tempLine.kb.swap(std::vector<double>());

             tempPoints.swap(std::vector<db::Point2f>());
         }
         else
         {
             if (dis_2 > 0.08)
             {
                 if (i != )
                     i--;

                 tempLine.kb = kb2;
                 if (atan(tempLine.kb[]) >= )
                     tempLine.sigma = atan(tempLine.kb[]);   //0-180度
                 else
                     tempLine.sigma = atan(tempLine.kb[]) + 3.1415926;

                 lineSets.push_back(tempLine);

                 tempLine.Points.swap(std::vector<db::Point2f>());
                 tempLine.PointsIndex.swap(std::vector<int>());
                 tempLine.kb.swap(std::vector<double>());

                 tempPoints.swap(std::vector<db::Point2f>());
                 continue;
             }
             if (dis_3 > 0.08)
             {
                 if (i != )
                     i--;

                 tempLine.kb = kb3;
                 if (atan(tempLine.kb[]) >= )
                     tempLine.sigma = atan(tempLine.kb[]);   //0-180度
                 else
                     tempLine.sigma = atan(tempLine.kb[]) + 3.1415926;

                 lineSets.push_back(tempLine);

                 tempLine.Points.swap(std::vector<db::Point2f>());
                 tempLine.PointsIndex.swap(std::vector<int>());
                 tempLine.kb.swap(std::vector<double>());

                 tempPoints.swap(std::vector<db::Point2f>());
                 continue;
             }
             kb = LineFitting(tempPoints);
             //kb = leastSquareFitting(tempPoints);
             kb3 = kb2;
             kb2 = kb1;
             kb1 = kb;

             tempLine.Points.push_back(rPoints[i]);
             tempLine.PointsIndex.push_back(i);

             if (i == (rPoints.size() - ))
             {
                 tempLine.kb = kb1;
                 if (atan(tempLine.kb[]) >= )
                     tempLine.sigma = atan(tempLine.kb[]);   //0-180度
                 else
                     tempLine.sigma = atan(tempLine.kb[]) + 3.1415926;

                 lineSets.push_back(tempLine);
             }
         }
     }
     return;
 }

 void getLinesFromPointset2(const std::vector<db::Point2f> &rPoints, std::vector<db::line> &lineSets)
 {
     //std::vector<db::line> lineSets;
     double avg = 0.0, sum = 0.0;
     std::vector<double> kb;
     std::vector<db::Point2f> tempPoints;
     db::line tempLine;
     std::vector<double> tempkb;

     //利用最小二乘拟合直线
     for (size_t i = ; i < rPoints.size(); i++)
     {
         if (tempPoints.size()<)
         {
             tempPoints.push_back(rPoints[i]);

             tempLine.Points.push_back(rPoints[i]);
             tempLine.PointsIndex.push_back(i);
             continue;
         }
         tempPoints.push_back(rPoints[i]);
         kb = leastSquareFitting(tempPoints);

         //残差
         double def = 0.0;
         if (tempPoints.size()==)
         {
             def = abs(kb[] * rPoints[i - ].x + kb[] - rPoints[i].y);    //点到直线的位置
             sum += def;
             def = abs(kb[] * rPoints[i - ].x + kb[] - rPoints[i].y);    //点到直线的位置
             sum += def;
             def = abs(kb[] * rPoints[i - ].x + kb[] - rPoints[i].y);    //点到直线的位置
             sum += def;
             def = abs(kb[] * rPoints[i-].x + kb[] - rPoints[i].y);    //点到直线的位置
             sum += def;
             def = abs(kb[] * rPoints[i-].x + kb[] - rPoints[i].y);    //点到直线的位置
             sum += def;
         }

         def = abs(kb[] * rPoints[i].x + kb[] - rPoints[i].y);    //点到直线的位置
         sum += def;
         avg = sum / tempPoints.size();

         if (1.2* avg < def)      //偏差较大的 点
         {
             sum = 0.0;
             tempPoints.swap(std::vector<db::Point2f>());
             //tempPoints.push_back(rPoints[i]);
             if (i != )
                 i--;

             if (tempkb.size()==)
             {
                 tempLine.kb = kb;
             }
             else
             {
                 tempLine.kb = tempkb;
             }

             if (atan(tempLine.kb[]) >= )
                 tempLine.sigma = atan(tempLine.kb[]);   //0-180度
             else
                 tempLine.sigma = atan(tempLine.kb[]) + 3.1415926;

             lineSets.push_back(tempLine);

             tempLine.Points.swap(std::vector<db::Point2f>());
             tempLine.PointsIndex.swap(std::vector<int>());
             tempLine.kb.swap(std::vector<double>());
         }
         else
         {
             tempkb = kb;
             tempLine.Points.push_back(rPoints[i]);
             tempLine.PointsIndex.push_back(i);

             if (i == (rPoints.size() - ))
             {
                 tempLine.kb = tempkb;
                 if (atan(tempLine.kb[]) >= )
                     tempLine.sigma = atan(tempLine.kb[]);   //0-180度
                 else
                     tempLine.sigma = atan(tempLine.kb[]) + 3.1415926;

                 lineSets.push_back(tempLine);
             }
         }
     }

 }

 // 利用ransac 进行直线拟合
 int ransacLiner_2(
     std::vector<db::Point2f>& pstData,
     int minCnt,
     double maxIterCnt,
     int consensusCntThreshold,
     double maxErrorThreshod,
     double& k, double& b,
     double& modelMeanError)
 {
     int dataCnt = pstData.size();
     if (dataCnt<)
     {
         return -;
     }

     default_random_engine rng;
     uniform_int_distribution<unsigned> uniform(, dataCnt - );
     rng.seed(); // 固定随机数种子
     set<unsigned int> selectIndexs;     // 选择的点的索引
     vector<db::Point2f> selectPoints;      // 选择的点
     set<unsigned int> consensusIndexs;  // 满足一致性的点的索引

     double A = ;
     double B = ;
     double C = ;
     modelMeanError = ;
     int isNonFind = ;
     unsigned int      bestConsensusCnt = ; // 满足一致性估计的点的个数
     int iter = ;
     while (iter < maxIterCnt)
     {
         selectIndexs.clear();
         selectPoints.clear();
         // Step1: 随机选择minCnt个点
         while ()
         {
             unsigned int index = uniform(rng);
             selectIndexs.insert(index);
             if (selectIndexs.size() == minCnt)
             {
                 break;
             }
         }
         // Step2: 进行模型参数估计 (y2 - y1)*x - (x2 - x1)*y + (y2 - y1)x2 - (x2 - x1)y2= 0
         set<unsigned int>::iterator selectIter = selectIndexs.begin();
         while (selectIter != selectIndexs.end())
         {
             unsigned int index = *selectIter;
             selectPoints.push_back(pstData[index]);
             selectIter++;
         }
         double deltaY = (selectPoints[]).y - (selectPoints[]).y;
         double deltaX = (selectPoints[]).x - (selectPoints[]).x;
         A = deltaY;
         B = -deltaX;
         C = -deltaY * (selectPoints[]).x + deltaX * (selectPoints[]).y;
         // Step3: 进行模型评估: 点到直线的距离
         int dataIter = ;
         double meanError = ;
         set<unsigned int> tmpConsensusIndexs;
         while (dataIter < dataCnt)
         {
             double distance = (A * pstData[dataIter].x + B * pstData[dataIter].y + C) / sqrt(A*A + B*B);
             distance = distance >  ? distance : -distance;
             if (distance < maxErrorThreshod)
             {
                 tmpConsensusIndexs.insert(dataIter);
             }
             meanError += distance;
             dataIter++;
         }
         // Step4: 判断一致性: 满足一致性集合的最小元素个数条件 + 至少比上一次的好
         if (tmpConsensusIndexs.size() >= bestConsensusCnt && tmpConsensusIndexs.size() >= consensusCntThreshold)
         {
             bestConsensusCnt = consensusIndexs.size();  // 更新一致性索引集合元素个数
             modelMeanError = meanError / dataCnt;
             consensusIndexs.clear();
             consensusIndexs = tmpConsensusIndexs;        // 更新一致性索引集合
             isNonFind = ;
         }
         iter++;

         if (B != )
         {
             k = -A / B;
             b = -C / B;
         }
         else
         {
             k = ;
             b = -C;
         }
     }
     return isNonFind;
 }
 // 利用ransac 进行直线拟合    获取直线
 void getLinesFromPointset_ransac(const std::vector<db::Point2f> &rPoints, std::vector<db::line> &lineSets)
 {
     std::vector<db::Point2f> pointTemp;
     int num = , numTemp=;
     double k=0.0, b=0.0;
     std::vector<double> kb;

     while(numTemp < rPoints.size())
     {
         std::vector<double> kbTemp;
         for (size_t step = ; step < rPoints.size(); )
         {
             pointTemp.swap(std::vector<db::Point2f>());
             //1、进行点集分段（判断条件距离d，步长10 、20、 30、...）
             int iTemp;
             if ((num + step)<rPoints.size())
             {
                 for (size_t i = num; i < (num + step); i++)
                 {
                     pointTemp.push_back(rPoints[i]);
                     iTemp = i;
                 }
                 double meanError;
                 if (!ransacLiner_2(pointTemp, , , , 0.1, k, b, meanError))
                 {
                     kbTemp.push_back(k);
                     kbTemp.push_back(b);
                     double PL_Distance = getPointToline_Distance(kbTemp, pointTemp[pointTemp.size() - ]);

                     if (PL_Distance>0.3)   //0.3   30cm  断开
                     {
                         db::line  Ltemp;
                         for (size_t j = ; j < pointTemp.size() - ; j++)
                         {
                             Ltemp.Points.push_back(pointTemp[j]);
                             Ltemp.PointsIndex.push_back(num + j);
                         }
                         Ltemp.kb = kbTemp;
                         lineSets.push_back(Ltemp);

                         num = num + step - ;
                         break;
                     }
                     else
                     {
                         kb = kbTemp;
                         step += ;
                     }

                 }
                 else
                 {
                     step += ;
                 }
             }
             else {
                 numTemp = num + step;
                 break;
             }
         }
     }

 }

 //直线段的优化处理  合并零散线段
 int reprocessLineset(std::vector<db::line> &lineSets, std::vector<db::line>& finalLineSets)
 {
     if(lineSets.size()==)
         return -;

     std::vector<db::line> templineSets;
     std::vector<db::line> temp;
     bool errorline = false;

     for (size_t i = ; i < lineSets.size(); i++)
     {
         if (lineSets[i].Points.size() < )
         {
             temp.push_back(lineSets[i]);
             errorline = true;
         }
         else
         {
             if (errorline && temp.size()>)
             {
                 db::line templine;
                 double k=0.0, b=0.0, sigma=0.0;
                 for (size_t j = ; j < temp.size(); j++)
                 {
                     templine.Points.insert(templine.Points.end(), temp[j].Points.begin(), temp[j].Points.end());
                     templine.PointsIndex.insert(templine.PointsIndex.end(), temp[j].PointsIndex.begin(), temp[j].PointsIndex.end());
                     k += temp[j].kb[];
                     b += temp[j].kb[];
                     sigma += temp[j].sigma;
                 }
                 templine.kb.push_back(k/ temp.size());
                 templine.kb.push_back(b / temp.size());
                 templine.sigma = sigma / temp.size();

                 finalLineSets.push_back(templine);

                 errorline = false;
                 temp.swap(std::vector<db::line>());
             }
             finalLineSets.push_back(lineSets[i]);
         }
     }
 }
 int reprocessLineset2(std::vector<db::line> &lineSets, std::vector<db::line>& finalLineSets)
 {
     if (lineSets.size()==)
     {
         return -;
     }
     //合并近似平行直线
     std::vector<db::line> lineSets_temp;
     double distance_R = 100.0;
     for (size_t i = ; i < lineSets.size(); i++)
     {
         double distance_L=100.0;
         if (i!= (lineSets.size()-))
         {
             distance_L = sqrt(
                 (lineSets[i].LRPoints.endPoint.x - lineSets[i + ].LRPoints.firstPoint.x)
                 *(lineSets[i].LRPoints.endPoint.x - lineSets[i + ].LRPoints.firstPoint.x)
                 + (lineSets[i].LRPoints.endPoint.y - lineSets[i + ].LRPoints.firstPoint.y)
                 *(lineSets[i].LRPoints.endPoint.y - lineSets[i + ].LRPoints.firstPoint.y));
         }

         if ((lineSets[i].LRangle.leftangle ==  && distance_L <= 0.5)||
             (lineSets[i].LRangle.rightangle ==  && distance_R <= 0.5))
         {
             lineSets_temp.push_back(lineSets[i]);
         }
         else
         {
             if (lineSets_temp.size()>)
             {
                 db::line templine;
                 double k = 0.0, b = 0.0, sigma = 0.0;
                 for (size_t j = ; j < lineSets_temp.size(); j++)
                 {
                     templine.Points.insert(templine.Points.end(), lineSets_temp[j].Points.begin(), lineSets_temp[j].Points.end());
                     templine.PointsIndex.insert(templine.PointsIndex.end(), lineSets_temp[j].PointsIndex.begin(), lineSets_temp[j].PointsIndex.end());
                     k += lineSets_temp[j].kb[];
                     b += lineSets_temp[j].kb[];
                     sigma += lineSets_temp[j].sigma;
                 }
                 templine.kb.push_back(k / lineSets_temp.size());
                 templine.kb.push_back(b / lineSets_temp.size());
                 templine.sigma = sigma / lineSets_temp.size();

                 finalLineSets.push_back(templine);

                 lineSets_temp.swap(std::vector<db::line>());
             }

             finalLineSets.push_back(lineSets[i]);
         }

         distance_R = distance_L;
     }
     return ;
 }

 // 获取直线KB参数
 void getLinePara(float& x1, float& y1, float& x2, float& y2, db::LinePara & LP)
 {
     double m = ;
     // 计算分子
     m = x2 - x1;
     if ( == m)
     {
         LP.k = 10000.0;
         LP.b = y1 - LP.k * x1;
     }
     else

     {
         LP.k = (y2 - y1) / (x2 - x1);
         LP.b = y1 - LP.k * x1;
     }

 }

 //求解两个向量的夹角
 void getTwoVector_angle(double (&a)[], double (&b)[], double & sigema)
 {
     double ab, a1, b1, cosr;
     ab = a[] * b[] + a[] * b[];
     a1 = sqrt(a[] * a[] + a[] * a[]);
     b1 = sqrt(b[] * b[] + b[] * b[]);
     cosr = ab / a1 / b1;
     sigema = acos(cosr);//* 180 / 3.1415926;
 }

 //求解两直线的交点
 db::Point2f getTwoline_cornerPoint(db::line firstline, db::line secondline)
 {
     db::Point2f point;
     point.x = (firstline.kb[]- secondline.kb[]) / (firstline.kb[] - secondline.kb[]);
     point.y = firstline.kb[] * point.x + firstline.kb[];
     return point;
 }

 //求解一个向量与x轴正方向的逆时针夹角
 void getVector_Xangle(double (&a)[], double &sigema)
 { 

     double b[] = { , };
     if (a[]> && a[]>=)
     {
         getTwoVector_angle(a, b, sigema);
     }
     else if (a[]<= && a[] >)
     {
         getTwoVector_angle(a, b, sigema);
     }
     else if (a[] <  && a[] <=)
     {
         getTwoVector_angle(a, b, sigema);
         sigema += 3.1415926;
     }
     else if (a[] >=  && a[] < )
     {
         getTwoVector_angle(a, b, sigema);
         sigema += 3.1415926;
     }
 }

 // 求解2d 点云中 直线左右相邻夹角  以及左右直线的交点
 void LRangleFromLines(std::vector<db::line> &lineSets)
 {
     //功能：：求解直线的左右夹角特征。
     //定理：两个向量与X轴正方向的同向夹角，大夹角减去小夹角 一定等于这两个向量的方向夹角 或 夹角+PI。
     //****  在数学中斜率夹角是逆时针的，方向射线，左值空间与右值空间  来判断向量的同向性？
     //****  在这里规定 绕原点逆时针 循环 前个直线段i 在后个直线段i+1的左值空间内。 

     db::line firstline;
     db::line secondline;

     for (size_t i = ; i < lineSets.size(); i++)
     {
         if (i==)
         {
             firstline = lineSets[i];
             db::Point2f endFootPoint = getFootPoint(firstline.kb, firstline.Points[]);   //第1条直线的第一个点 垂足
             lineSets[i].LRPoints.firstPoint = endFootPoint;
             continue;
         }
         secondline = lineSets[i];

         // 判断是否平行   求取交点
         if (abs(firstline.kb[]- secondline.kb[]) > 0.5)//0.5
         {
             //交点
             db::Point2f pointtemp;
             pointtemp.x = (secondline.kb[] - firstline.kb[]) / (firstline.kb[] - secondline.kb[]);
             pointtemp.y = firstline.kb[] * pointtemp.x + firstline.kb[];

             db::Point2f R_endFootPoint = getFootPoint(firstline.kb, firstline.Points[firstline.Points.size() - ]);  //第一条直线的最后一个点 垂足
             db::Point2f L_endFootPoint = getFootPoint(secondline.kb, secondline.Points[]);   //第二条直线的第一个点 垂足

             //判断第二个向量在第一个向量的左值空间 还是右值空间
             //R L向量     x轴正方向的夹角比较
             double R[] = { R_endFootPoint.x - pointtemp.x, R_endFootPoint.y - pointtemp.y },
                 L[] = { L_endFootPoint.x - pointtemp.x, L_endFootPoint.y - pointtemp.y };
             double R_sigema=0.0;
             double L_sigema=0.0;   //x轴正方向的夹角
             getVector_Xangle(R, R_sigema);
             getVector_Xangle(L, L_sigema);

             double sigema_temp=0.0;      //(0-PI)
             if ((L_sigema- R_sigema)> && (L_sigema - R_sigema)<3.1415926)
             {
                 getTwoVector_angle(R, L, sigema_temp);
             }
             else
             {
                 double Ltemp[]= { pointtemp.x -L_endFootPoint.x, pointtemp.y -L_endFootPoint.y};
                 getTwoVector_angle(R, Ltemp, sigema_temp);
             }

             lineSets[i - ].LRangle.leftangle = sigema_temp;
             lineSets[i].LRangle.rightangle = sigema_temp;

             lineSets[i - ].LRPoints.endPoint = pointtemp;
             lineSets[i].LRPoints.firstPoint = pointtemp;
             //lineSets[i - 1].LRPoints.endPoint = R_endFootPoint;
             //lineSets[i].LRPoints.firstPoint = L_endFootPoint;
             firstline = secondline;
         }
         else    //  如果前后两条直线近似平行
         {
             db::Point2f R_endFootPoint = getFootPoint(firstline.kb, firstline.Points[firstline.Points.size() - ]);  //第一条直线的最后一个点 垂足
             db::Point2f L_endFootPoint = getFootPoint(secondline.kb, secondline.Points[]);   //第二条直线的第一个点 垂足

             lineSets[i - ].LRPoints.endPoint = R_endFootPoint;
             lineSets[i].LRPoints.firstPoint = L_endFootPoint;

             firstline = secondline;
         }

         if (i == (lineSets.size()-))
         {
             db::Point2f endFootPoint = getFootPoint(secondline.kb, secondline.Points[secondline.Points.size() - ]);   //第1条直线的第一个点 垂足
             lineSets[i].LRPoints.endPoint = endFootPoint;
         }
     }

     //
     return;
 }
 //  2018.11.7 修正
 void LRangleFromLines2(std::vector<db::line> &lineSets)
 {
     //功能：：求解直线的左右夹角特征。
     //定理：两个向量与X轴正方向的同向夹角，大夹角减去小夹角 一定等于这两个向量的方向夹角 或 夹角+PI。
     //****  在数学中斜率夹角是逆时针的，方向射线，左值空间与右值空间  来判断向量的同向性？
     //****  在这里规定 绕原点逆时针 循环 前个直线段i 在后个直线段i+1的左值空间内。 

     db::line firstline;
     db::line secondline;

     for (size_t i = ; i < lineSets.size(); i++)
     {
         if (i == )
         {
             firstline = lineSets[i];
             db::Point2f endFootPoint = getFootPoint(firstline.kb, firstline.Points[]);   //第1条直线的第一个点 垂足
             lineSets[i].LRPoints.firstPoint = endFootPoint;
             continue;
         }
         secondline = lineSets[i];

         double distance = getPointToPoint_Distance(firstline.Points[firstline.Points.size()-], secondline.Points[]);   // 2018.11.7加入
         bool have_little_line = !((firstline.Points.size() < ) && (secondline.Points.size() < ));
         // 判断是否平行   求取交点
         if (abs(firstline.kb[] - secondline.kb[]) > 1.0 && distance< 0.3 && have_little_line)//0.5
         {
             //交点
             db::Point2f pointtemp;
             pointtemp.x = (secondline.kb[] - firstline.kb[]) / (firstline.kb[] - secondline.kb[]);
             pointtemp.y = firstline.kb[] * pointtemp.x + firstline.kb[];

             db::Point2f R_endFootPoint = getFootPoint(firstline.kb, firstline.Points[firstline.Points.size() - ]);  //第一条直线的最后一个点 垂足
             db::Point2f L_endFootPoint = getFootPoint(secondline.kb, secondline.Points[]);   //第二条直线的第一个点 垂足

                                                                                               //判断第二个向量在第一个向量的左值空间 还是右值空间
                                                                                               //R L向量     x轴正方向的夹角比较
             double R[] = { R_endFootPoint.x - pointtemp.x, R_endFootPoint.y - pointtemp.y },
                 L[] = { L_endFootPoint.x - pointtemp.x, L_endFootPoint.y - pointtemp.y };
             double R_sigema = 0.0;
             double L_sigema = 0.0;   //x轴正方向的夹角
             getVector_Xangle(R, R_sigema);
             getVector_Xangle(L, L_sigema);

             double sigema_temp = 0.0;      //(0-PI)
             if ((L_sigema - R_sigema)> && (L_sigema - R_sigema)<3.1415926)
             {
                 getTwoVector_angle(R, L, sigema_temp);
             }
             else
             {
                 double Ltemp[] = { pointtemp.x - L_endFootPoint.x, pointtemp.y - L_endFootPoint.y };
                 getTwoVector_angle(R, Ltemp, sigema_temp);
             }

             lineSets[i - ].LRangle.leftangle = sigema_temp;
             lineSets[i].LRangle.rightangle = sigema_temp;

             lineSets[i - ].LRPoints.endPoint = pointtemp;
             lineSets[i].LRPoints.firstPoint = pointtemp;
             //lineSets[i - 1].LRPoints.endPoint = R_endFootPoint;
             //lineSets[i].LRPoints.firstPoint = L_endFootPoint;
             firstline = secondline;
         }
         else    //  如果前后两条直线近似平行
         {
             db::Point2f R_endFootPoint = getFootPoint(firstline.kb, firstline.Points[firstline.Points.size() - ]);  //第一条直线的最后一个点 垂足
             db::Point2f L_endFootPoint = getFootPoint(secondline.kb, secondline.Points[]);   //第二条直线的第一个点 垂足

             lineSets[i - ].LRPoints.endPoint = R_endFootPoint;
             lineSets[i].LRPoints.firstPoint = L_endFootPoint;

             firstline = secondline;
         }

         //db::Point2f R_endFootPoint = getFootPoint(firstline.kb, firstline.Points[firstline.Points.size() - 1]);  //第一条直线的最后一个点 垂足
         //db::Point2f L_endFootPoint = getFootPoint(secondline.kb, secondline.Points[0]);   //第二条直线的第一个点 垂足
         //lineSets[i - 1].LRPoints.endPoint = R_endFootPoint;
         //lineSets[i].LRPoints.firstPoint = L_endFootPoint;
         //firstline = secondline;

         if (i == (lineSets.size() - ))
         {
             db::Point2f endFootPoint = getFootPoint(secondline.kb, secondline.Points[secondline.Points.size() - ]);   //第1条直线的第一个点 垂足
             lineSets[i].LRPoints.endPoint = endFootPoint;
         }
     }

     //
     return;
 }

 //求解两个平面两向量的旋转角
 /*
 * 两个向量之间的旋转角
 * 首先明确几个数学概念：
 * 1. 极轴沿逆时针转动的方向是正方向
 * 2. 两个向量之间的夹角theta， 是指(A^B)/(|A|*|B|) = cos(theta)，0<=theta<=180 度， 而且没有方向之分
 * 3. 两个向量的旋转角，是指从向量p1开始，逆时针旋转，转到向量p2时，所转过的角度， 范围是 0 ~ 360度
 * 计算向量p1到p2的旋转角，算法如下：
 * 首先通过点乘和arccosine的得到两个向量之间的夹角
 * 然后判断通过差乘来判断两个向量之间的位置关系
 * 如果p2在p1的逆时针方向, 返回arccose的角度值, 范围0 ~ 180.0(根据右手定理,可以构成正的面积)
 * 否则返回 360.0 - arecose的值, 返回180到360(根据右手定理,面积为负)
 */
 double getRotateAngle(double x1, double y1, double x2, double y2)

 {
     const double epsilon = 1.0e-6;
     const double nyPI = acos(-1.0);
     double dist, dot, degree, angle;

     // normalize
     dist = sqrt(x1 * x1 + y1 * y1);
     x1 /= dist;
     y1 /= dist;
     dist = sqrt(x2 * x2 + y2 * y2);
     x2 /= dist;
     y2 /= dist;

     // dot product
     dot = x1 * x2 + y1 * y2;
     if (fabs(dot - 1.0) <= epsilon)
         angle = 0.0;
     else if (fabs(dot + 1.0) <= epsilon)
         angle = nyPI;
     else {
         double cross;
         angle = acos(dot);
         //cross product
         cross = x1 * y2 - x2 * y1;
         // vector p2 is clockwise from vector p1
         // with respect to the origin (0.0)
         if (cross < ) {
             angle =  * nyPI - angle;   //p2在p1的顺时针方向
         }
     }
     //degree = angle *  180.0 / nyPI;
     return angle;
 }

 // 变换矩阵估计 2dlidar 匹配
 void registration_2dLidar(
     std::vector<db::Point2f>& src_points,
     std::vector<db::Point2f>& targ_points,
     Eigen::Matrix<float, , >& RT,
     pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_in,
     pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud_targ)
 {
     if (src_points.size()==|| targ_points.size()==)
     {
         return;
     }
     std::vector<db::line> src_lineSets;
     std::vector<db::line> src_templineSets;

     getLinesFromPointset2(src_points, src_templineSets);
     LRangleFromLines(src_templineSets);
     //reprocessLineset2(src_lineSets, src_templineSets);
     //LRangleFromLines(src_templineSets);

     std::vector<db::line> targ_lineSets;
     std::vector<db::line> targ_templineSets;

     getLinesFromPointset2(targ_points, targ_templineSets);
     LRangleFromLines(targ_templineSets);
     //reprocessLineset2(targ_lineSets, targ_templineSets);
     //LRangleFromLines(targ_templineSets);

     //1、寻找最优匹配线段
     struct paraLines
     {
         int src_line;
         int targ_line;
         int score=;    //if one match 1   two match 2
     };
     std::vector<paraLines> matchLines;
     bool bestFind = false;

     for (size_t i = ; i < src_templineSets.size(); i++)
     {
         for (size_t j = ; j < targ_templineSets.size(); j++)
         {
             if (src_templineSets[i].LRangle.leftangle!=0.0 && targ_templineSets[j].LRangle.leftangle!=0.0
                 && abs(src_templineSets[i].LRangle.leftangle - targ_templineSets[j].LRangle.leftangle) < 0.05)    //0-PI
             {
                 paraLines ml;
                 ml.src_line = i;
                 ml.targ_line = j;
                 ml.score = ;
                 if (src_templineSets[i].LRangle.rightangle !=0.0 && targ_templineSets[j].LRangle.rightangle !=0.0 &&
                     abs(src_templineSets[i].LRangle.rightangle - targ_templineSets[j].LRangle.rightangle) < 0.2)
                 {
                     ml.score = ;

                     //line corner
                     db::Point2f firstPoint;
                     db::Point2f secondPoint;
                     firstPoint = src_templineSets[i].LRPoints.firstPoint;
                     secondPoint = src_templineSets[i].LRPoints.endPoint;
                     float src_distance = sqrt((firstPoint.x - secondPoint.x)*(firstPoint.x - secondPoint.x) +
                         (firstPoint.y - secondPoint.y)*(firstPoint.y - secondPoint.y));

                     firstPoint = src_templineSets[i].LRPoints.firstPoint;
                     secondPoint = src_templineSets[i].LRPoints.endPoint;
                     float targ_distance = sqrt((firstPoint.x - secondPoint.x)*(firstPoint.x - secondPoint.x) +
                         (firstPoint.y - secondPoint.y)*(firstPoint.y - secondPoint.y));
                     if ((src_distance- targ_distance)<0.1)     //线段长度对比
                     {
                         ml.score = ;
                     }
                 }
                 matchLines.push_back(ml);
             }
         }
     }

     if (matchLines.size()==)
     {
         std::cout << "cannot find match lines" << std::endl;
         return;
     }

     int numTemp=;
     size_t i_temp;

     for (size_t i = ; i < matchLines.size(); i++)
     {
         //1、变换矩阵 估计
         //db::Point2f src_firtpoint = src_templineSets[matchLines[i].src_line].LRPoints.firstPoint;
         db::Point2f src_firtpoint = getFootPoint(src_templineSets[matchLines[i].src_line].kb, src_templineSets[matchLines[i].src_line].Points[]);
         db::Point2f src_secondpoint = src_templineSets[matchLines[i].src_line].LRPoints.endPoint;
         db::Point2f firstvector;
         firstvector.x = src_firtpoint.x - src_secondpoint.x;
         firstvector.y = src_firtpoint.y - src_secondpoint.y;

         //db::Point2f targ_firtpoint = targ_templineSets[matchLines[i].targ_line].LRPoints.firstPoint;
         db::Point2f targ_firtpoint = getFootPoint(targ_templineSets[matchLines[i].targ_line].kb, targ_templineSets[matchLines[i].targ_line].Points[]);
         db::Point2f targ_secondpoint = targ_templineSets[matchLines[i].targ_line].LRPoints.endPoint;
         db::Point2f secondvector;
         secondvector.x = targ_firtpoint.x - targ_secondpoint.x;
         secondvector.y = targ_firtpoint.y - targ_secondpoint.y;

         Eigen::Matrix<float, , > RT_temp;
         double theta=getRotateAngle(firstvector.x, firstvector.y, secondvector.x, secondvector.y);
         RT_temp(, ) = cos(theta);
         RT_temp(, ) = -sin(theta);
         RT_temp(, ) = sin(theta);
         RT_temp(, ) = cos(theta);

         float x0 = src_secondpoint.x*cos(theta) - src_secondpoint.y*sin(theta);
         float y0 = src_secondpoint.x*sin(theta) + src_secondpoint.y*cos(theta);

         RT_temp(, ) = targ_secondpoint.x- x0;
         RT_temp(, ) = targ_secondpoint.y- y0;

         //2、匹配精度结果验证
         struct Tnode * root = NULL;
         root = build_kdtree(targ_points);  //创建kdtree

         int num = ;
         std::vector<double> distanceVector;
         for (size_t j = ; j < src_points.size(); j++)
         {
             db::Point2f target;
             target.x = RT_temp(, )*src_points[j].x + RT_temp(, )*src_points[j].y + RT_temp(, );
             target.y = RT_temp(, )*src_points[j].x + RT_temp(, )*src_points[j].y + RT_temp(, );

             db::Point2f nearestpoint;
             double distance = 0.0;
             searchNearest(root, target, nearestpoint, distance);    //临近搜索  最近点

             if (distance<0.3)    //统计距离小于30cm的点个数
             {
                 num++;
             }
             distanceVector.push_back(distance);
         }

         if (numTemp < num)
         {
             numTemp = num;
             i_temp = i;
             RT = RT_temp;
         }
     }
     //i_temp = 1;
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_001(new pcl::PointCloud<pcl::PointXYZ>);
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_002(new pcl::PointCloud<pcl::PointXYZ>);
     for (size_t i = ; i < src_templineSets[matchLines[i_temp].src_line].PointsIndex.size(); i++)
     {
         pcl::PointXYZ point;
         point.x = src_points[src_templineSets[matchLines[i_temp].src_line].PointsIndex[i]].x;
         point.y = src_points[src_templineSets[matchLines[i_temp].src_line].PointsIndex[i]].y;
         point.z = 1.5;
         cloud_001->points.push_back(point);
     }
     for (size_t i = ; i < targ_templineSets[matchLines[i_temp].targ_line].PointsIndex.size(); i++)
     {
         pcl::PointXYZ point;
         point.x = targ_points[targ_templineSets[matchLines[i_temp].targ_line].PointsIndex[i]].x;
         point.y = targ_points[targ_templineSets[matchLines[i_temp].targ_line].PointsIndex[i]].y;
         point.z = 1.5;
         cloud_002->points.push_back(point);
     }

     //显示匹配的直线的点
     pcl::visualization::PCLVisualizer viewer;
     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> inColorHandler(cloud_in, , , );// 白色
     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> targColorHandler(cloud_in, , , );// 白色
     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_001Handler(cloud_001, , , ); // 红色
     pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> cloud_002Handler(cloud_002, , , ); // 红色
     viewer.addPointCloud(cloud_targ, targColorHandler, "targ");
     viewer.addPointCloud(cloud_in, inColorHandler, "In");
     viewer.addPointCloud(cloud_001, cloud_001Handler, "");
     viewer.addPointCloud(cloud_002, cloud_002Handler, "");

     viewer.addCoordinateSystem(1.0, "cloud", );

     while (!viewer.wasStopped())
     {
         viewer.spinOnce();
     }
     return;
 }

 pcl::PointXYZ _2dpointTo3dPoint(db::Point2f& temp2dpoint)
 {
     pcl::PointXYZ temp3dpoint;
     temp3dpoint.x = temp2dpoint.x;
     temp3dpoint.y = temp2dpoint.y;
     temp3dpoint.z = 1.5;
     return temp3dpoint;
 }

 void viewLines(std::vector<db::line> &lineSets, pcl::visualization::PCLVisualizer& viewer)
 {
     for (size_t i = ; i < lineSets.size(); i++)
     {
         std::stringstream ss_line;
         ss_line << "line_" << i;
         pcl::PointXYZ first_point= _2dpointTo3dPoint(lineSets[i].LRPoints.firstPoint);
         pcl::PointXYZ end_point = _2dpointTo3dPoint(lineSets[i].LRPoints.endPoint);

         viewer.addLine<pcl::PointXYZ, pcl::PointXYZ>(first_point, end_point, , , , ss_line.str());
     };

 }

 int main()
 {
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>);  // 原始点云
     pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_targ(new pcl::PointCloud<pcl::PointXYZ>);  // 原始点云
     if (pcl::io::loadPCDFile("C:/Users/18148/Desktop/room_test/020.pcd", *cloud_in) == -)
     {
         return -;
     }
     if (pcl::io::loadPCDFile("C:/Users/18148/Desktop/angelcomm/r2.pcd", *cloud_targ) == -)
     {
         return -;
     }

     std::vector<db::Point2f> src_points;
     std::vector<db::Point2f> targ_points;
     Eigen::Matrix<float, , > RT= Eigen::Matrix<float, , >::Zero();
     //MatrixXf RT(2,3)
     for (size_t i = ; i < cloud_in->size();)
     {
         db::Point2f Point2f;
         Point2f.x = cloud_in->points[i].x;
         Point2f.y = cloud_in->points[i].y;
         src_points.push_back(Point2f);
         i += ;
     }
     for (size_t i = ; i < cloud_targ->size(); )
     {
         db::Point2f Point2f;
         Point2f.x = cloud_targ->points[i].x;
         Point2f.y = cloud_targ->points[i].y;
         targ_points.push_back(Point2f);
         i += ;
     }

     std::vector<db::line> src_templineSets;
     std::vector<db::line> templineSets;

     //getLinesFromPointset_ransac(src_points, templineSets);

     getLinesFromPointset(src_points, templineSets);
     LRangleFromLines2(templineSets);
     //reprocessLineset2(templineSets, src_templineSets);
     //LRangleFromLines(src_templineSets);

     //registration_2dLidar(src_points, targ_points, RT, cloud_in, cloud_targ);

     //Eigen::Matrix4f transform_1 = Eigen::Matrix4f::Identity();      // 单位阵
     //transform_1(0, 0) = RT(0, 0);
     //transform_1(0, 1) = RT(0, 1);
     //transform_1(0, 3) = RT(0, 2);

     //transform_1(1, 0) = RT(1, 0);
     //transform_1(1, 1) = RT(1, 1);
     //transform_1(1, 3) = RT(1, 2);

     //transform_1(2, 2) = 1;
     //transform_1(3, 3) = 1;

     //cout << transform_1 << endl;
     // // 执行变换
     //pcl::PointCloud<pcl::PointXYZ>::Ptr pPointCloudOut(new pcl::PointCloud<pcl::PointXYZ>());
     //pcl::transformPointCloud(*cloud_in, *pPointCloudOut, transform_1);

     //pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_icp(new pcl::PointCloud<pcl::PointXYZ>());

     //pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
     //icp.setMaxCorrespondenceDistance(30); //设置对应点对之间的最大距离（此值对配准结果影响较大）
     //icp.setTransformationEpsilon(1e-10); //设置两次变化矩阵之间的差值（一般设置为1e-10即可）；
     //icp.setEuclideanFitnessEpsilon(0.00001); //设置收敛条件是均方误差和小于阈值， 停止迭代
     //icp.setMaximumIterations(1000);     //设置最大迭代次数iterations=true
     //icp.setInputSource(cloud_in);   //设置输入的点云
     //icp.setInputTarget(cloud_targ);    //目标点云
     //icp.align(*cloud_icp, transform_1);          //匹配后源点云
     //Eigen::Matrix4f transformation_matrix = Eigen::Matrix4f::Identity();  //初始化
     //if (icp.hasConverged())                 //icp.hasConverged ()=1（true）输出变换矩阵的适合性评估
     //{
     //    std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
     //    transformation_matrix = icp.getFinalTransformation(); //.cast<double>();
     //}

     // 3. 可视窗口初始化   点云可视化
     pcl::visualization::PCLVisualizer viewer;
     viewLines(templineSets, viewer);

     //pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> inColorHandler(cloud_in, 255, 255, 255);// 白色
     //pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> targColorHandler(cloud_targ, 0, 0, 255);// 白色
     //pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> outColorHandler(cloud_icp, 230, 20, 20); // 红色
     //viewer.addPointCloud(cloud_targ, targColorHandler, "targ");
     //viewer.addPointCloud(cloud_in, inColorHandler, "In");
     //viewer.addPointCloud(cloud_icp, outColorHandler, "Out");
     //viewer.addCoordinateSystem(1.0, "In", 0);
     while (!viewer.wasStopped())
     { // 显示，直到‘q’键被按下
         viewer.spinOnce();
     }
     return ;
 }

总结：实现单线2dlidar  的匹配算法。