道格拉斯 扑克算法

#ifndef DOUGLASPEUCKER_H
#define DOUGLASPEUCKER_H
#include <iostream>
#include<fstream>
#include<vector>
#include<eigen3/Eigen/Core>
#include<string>
#include <sstream>

class DouglasPeucker
{
public:

    typedef std::vector<Eigen::Vector3d> PointList;
    typedef Eigen::Vector3d Point;
    struct Line
    {
        double a;
        double b;
        double c;
    };

public:
    DouglasPeucker();
    /**
     * @brief DouglasPeucker
     * @param point_list  直接传递点集参数
     */
    DouglasPeucker(PointList point_list)
    {
        point_list_ =point_list;
    }
    /**
     * @brief setPointList
     * @param point_list   Eigen::Vector3d 类型的有序点
     */
    void setPointList(PointList  point_list)
    {
             point_list_ = point_list;
    }

    /**
     * @brief DouglasPeucker
     * @param file_name   数据文件路径及名称
     */
    DouglasPeucker(std::string file_name);

    ~DouglasPeucker()
    {
        file_in_.close();
    }

    /**
     * @brief compute  道格拉斯-普克算法计算
     * @param p            点簇顺序表
     * @param epsilon  断点阈值
     * @return                断点组成的顺序表
     */
    PointList compute(PointList point_list,double epsilon);

    void outputTestReslut();

private:
    std::string file_name_;
    std::ifstream  file_in_; //打开文件
    PointList  point_list_;   // 存储一帧数据中点簇

   void  computeLine(const Point &A, const Point &B,Line &line);

    double perpendicularDistance(Point p, Line &line);

};

#endif // DOUGLASPEUCKER_H



#include "douglaspeucker.h"
#define OUT_TO_FILE 0
DouglasPeucker::DouglasPeucker()
{
}

DouglasPeucker:: DouglasPeucker(std::string file_name)
{
    file_name_ = file_name;
    file_in_.open(file_name_.c_str());
    std::string line;
    point_list_.clear();

    while(getline(file_in_,line))
    {
           std::istringstream is(line);
           std::vector<double> temp_vec;
           temp_vec.clear();
           double temp;
            while(is >>temp)
            {
                temp_vec.push_back(temp);
            }

            point_list_.push_back(Point(temp_vec[0],temp_vec[1],1));

    }

}

 DouglasPeucker::PointList DouglasPeucker::compute (PointList point_list,double epsilon)
 {
      PointList  ResultList;
     //Find the point with the maximum distance
     double  dmax = 0;
     double  index = 0;
     for(int i = 1; i < point_list.size()-1;++i)
      {
              Line line;
              computeLine(point_list[0],point_list[point_list.size()-1],line);
              double d = perpendicularDistance(point_list[i], line);
              if (d > dmax)
              {
                   index = i;
                   dmax = d;
              }
     }

     //If max distance is greater than epsilon, recursively simplify
     if(dmax >= epsilon)
     {
          //Recursive call
          PointList  recResults1;
          PointList point_list_1;
          PointList  recResults2;
          PointList point_list_2;
          for(int i=0;i<=index;i++)
          {
                point_list_1.push_back(point_list[i]);
          }
          for(int i=index;i<point_list.size();i++)
          {
                point_list_2.push_back(point_list[i]);
          }


          recResults1 = compute(point_list_1,epsilon);
          recResults2 = compute(point_list_2,epsilon);

          // Build the result list

          for(int i=0;i<recResults1.size()-1;i++)
          {
                ResultList.push_back(recResults1[i]);
          }

          for(int i=0;i<recResults2.size();i++)
          {
                ResultList.push_back(recResults2[i]);
          }


     }
     else
     {

        ResultList.push_back( point_list[0] );
        ResultList.push_back( point_list[point_list.size()-1] ) ;
     }

     //Return the result
     return ResultList;

 }

void DouglasPeucker:: computeLine(const Point &A, const Point &B,Line &line)
{
        line.a  = B[1] - A[1];
        line.b =  A[0] - B[0];
        line.c  = B[0]*A[1] - A[0]*B[1];
}

double DouglasPeucker::perpendicularDistance(Point p, Line &line)
{

    return    fabs( p[0] * line.a+ p[1] * line.b +line.c)/sqrt(line.a*line.a +line.b *line.b);

}

void DouglasPeucker::outputTestReslut()
{
    PointList result = compute(point_list_,0.05);
    std::ofstream out("/home/qixianyu/out.txt");
    for(PointList::iterator iter = result.begin(); iter !=result.end();++iter)
    {
            std::cout << "x = " << (*iter)[0] <<" y = " << (*iter)[1] <<" index = "<< (*iter)[2] << std::endl;

#if OUT_TO_FILE
            out << (*iter)[0]  << "   "  << (*iter)[1] <<std::endl;
            std::cout << "out to file.\n";
#endif
    }


}