放到win的写字板下

//=====================================================================================//
// Program:  基于detect_allimg_line03代码进行改编，将显示直线代码编成了一个函数，整合卡尔曼滤波
// process： 要不只显示中间的导航线利用卡尔曼滤波
// Data:2021.6.30
// Author:JQ
// Version:V4.0
//=====================================================================================//

#include <opencv2/core.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/imgproc.hpp>
#include <stdio.h>

#include <algorithm>
#include <fstream>
#include <iostream>
#include <math.h>
#include <stack>
#include <string.h>
#include <vector>
#include"CKalmanFilter.h"

#define PI 3.14159265
using namespace cv;
using namespace std;



int Otsu(cv::Mat &src, cv::Mat &dst, int thresh);
void ransac(std::vector<Point2f> temporary, cv::Vec4f &line, int iterations,float sigma);
void Least_squares_method(std::vector<Point2f> temporary, cv::Vec4f &line);
void drawLines(Mat roi, vector<Line_param> line_parameters,string save_image_name);
cv::Scalar icvprGetRandomColor() 
{
  uchar r = 300 * (rand() / (0.8 + RAND_MAX));
  uchar g = 300 * (rand() / (0.9 + RAND_MAX));
  uchar b = 400 * (rand() / (1.0 + RAND_MAX));
  return cv::Scalar(b, g, r);
}

int main() {

  //单个图像名称
  string name;
  //单个图片全名地址
  string input_image_name;
  //单个图片保存地址
  string save_image_name;
  //存储图片文件夹的路径
  string input_image_path = "/home/robot/pictures/experiment_img/返青期pc_121-240/";
  //图片处理后的保存图片的文件夹的路径
  string save_image_path = "/home/robot/pictures/experiment_img/返青期pc_121-240_result/";
  //图片名称列表
  string image_list = "/home/robot/pictures/imgname.txt";
  // 创建文件流接口
  ifstream str_file(image_list);
  bool iflineisfrist = true;
  std::vector<Line_param> last_line_parameters;
  while (getline(str_file, name)) {
    //单个图片全名
    input_image_name = input_image_path + name;
    cout << input_image_name << endl;
    Mat src = imread(input_image_name); ///读取图像
    //img process
    
    // roi image
    int roi_top = 80;
    int roi_height = 400;
    Mat roi = src(Rect(50, roi_top, src.cols - 50, roi_height));
    // EXG image
    Mat gray_img = Mat(roi.rows, roi.cols, CV_8UC1);
    float R, G, B, value;
    for (int y = 0; y < roi_height; y++) {
      Vec3b *ptr = roi.ptr<Vec3b>(y);
      uchar *data1 = gray_img.ptr<uchar>(y);

      for (int x = 0; x < roi.cols; x++) {
        B = ptr[x][0];
        G = ptr[x][1];
        R = ptr[x][2];
        value = G * 2 - B - R;
        if (value >= 0 && value <= 255)
          data1[x] = value;
        else if (value < 0)
          data1[x] = 0;
        else
          data1[x] = 255;
      }
    }
    // OTSU image
    int thresh = 0;
    Mat bin_img;
    double t2 = (double)cv::getTickCount();
    thresh = Otsu(gray_img, bin_img, thresh);

    // Morphological operation
    clock_t start = clock();
    Mat open_img, close_img;

    Mat element = getStructuringElement(MORPH_RECT, Size(2, 2));
    morphologyEx(bin_img, open_img, MORPH_OPEN, element);
    morphologyEx(open_img, close_img, MORPH_CLOSE, element);

    //水平分割图片，获取特征点
    int img_width = close_img.cols;
    int img_height = close_img.rows;
    int img_split_distance = 20;               //水平条的高度值为20px
    int num = img_height / img_split_distance; //分割图像的水平条数
    //cout << "横向分割的个数：" << num << endl;

    Mat image_cut;
    vector<Point2f> centers; //存放特征点
    Point pt1;

    int start_point_num; //起始种子总数
    int signl = 1;

    //寻找图像的所有的特征点
    for (int i = num - 1; i > 0; i--) {
      //在图像上画水平线

      Rect rect(0, img_split_distance * i, img_width, img_split_distance);
      image_cut = Mat(close_img, rect);

      vector<vector<Point>> contours;
      vector<Vec4i> hierarchy;

      findContours(image_cut, contours, hierarchy, CV_RETR_EXTERNAL,
                   CV_CHAIN_APPROX_NONE, Point(0, 0));
      // CV_RETR_EXTERNAL只检测外部轮廓，可根据自身需求进行调整
      //计算轮廓矩，计算轮廓的质心，一个横条中所有的轮廓的质心
      if (contours.size() == 0) {
        continue;
      }
      vector<Moments> mu(contours.size());
      vector<Point2f> mc(contours.size());

      for (int num = 0; num < contours.size(); num++) {
        if (contours[num].size() < 20) //很小轮廓不计算特征点
          continue;
        mu[num] = moments(contours[num], false);
        mc[num] = Point2d(static_cast<float>(mu[num].m10 / mu[num].m00),
                          static_cast<float>(mu[num].m01 / mu[num].m00));
        pt1.x = mc[num].x;
        pt1.y = img_split_distance * i + mc[num].y;

        cv::circle(roi, pt1, 1, (255, 0, 0), 2, 8, 0); //绘制特征点
        centers.push_back(pt1);
      }

      if (signl < 6) {
        start_point_num = centers.size();
        // cout<<"起始种子的个数是："<<start_point_num<<endl;
      }
      signl = signl + 1;
    }

    vector<Point2f> temporary;
    int h_distance; //横坐标的左右阈值
    vector<vector<Point>> temp;

    Point pointxc;
    float x_left, x_right;
    cv::RNG random;
    cv::Vec4f lineParam;
    std::vector<Line_param> line_parameters;
    
    float threshold;
    float distance;
    // ofstream fout("/home/robot/pictures/old/a.txt");
    // //将输出在终端的内容输出到文档中

    //特征点分类
    //以初始种子为外壳，去找与初始种子一类的特征点
    for (int d = 0; d < start_point_num; d++) {

      //起始种子的坐标
      if (centers[d].x < 0) //判断起始种子是否已经归类，如果是则去除
        continue;

      pointxc.x = centers[d].x;
      pointxc.y = centers[d].y;

      cv::Scalar color = icvprGetRandomColor(); //绘制点的颜色

      //将起始种子加入分类集合中，temporary是分类集合
      vector<Point2f> temporary;
      temporary.push_back(pointxc);
      x_left = x_right = pointxc.x;
      // fout << "起始种子的横坐标pointxc.x---------------------------------："
      //<< pointxc.x << endl;

      //先判断是否在横坐标的范围内，然后再判断到容器任意一点的距离
      for (int e = 0; e < centers.size(); e++) {
        // fout << "-------------要判断的容器中的一点的坐标：" << centers[e].x<<
        // "  " << centers[e].y << endl;
        if (centers[e].x <
            0) //判断当前点是否已经归类，以横坐标是否小于0为标记判断是否归类
          continue;
        double subband_position =
            centers[e].y / img_split_distance; //预判断特征点的所在条行图的位置
        double position = ceil(subband_position);

        //根据预分类特征点的position来设置横坐标扩展阈值和距离阈值
        int aindex = 0;
        if (position > 11) {
          threshold = 200 - (num - position) * 10;
          h_distance = 80 - (num - position) * 2;
        } else {
          threshold = position * 10;
          h_distance = 40;
        }

        if (position > 11 || fabs(centers[e].x - img_width / 2) < 100) {
          if (centers[e].x > x_left - h_distance &&
              centers[e].x < x_right + h_distance)
            aindex = 1;
        } else if (position < 11 && centers[e].x > img_width / 2 &&
                   centers[e].x > x_left - h_distance && centers[e].x < x_right)
          aindex = 1;
        else if (position < 11 && centers[e].x < img_width / 2 &&
                 centers[e].x > x_left && centers[e].x < x_right + h_distance)
          aindex = 1;

        if (aindex == 1) {
          //判断特征点与分类集合中点的距离，迭代100次
          for (int index = 0; index < 100; index++) {

            int randomNum = random(temporary.size()); //分类集合中随机一点

            distance = sqrt(pow(centers[e].x - temporary[randomNum].x, 2) +pow(centers[e].y - temporary[randomNum].y,2)); //计算预判断特征点与分类集合中一点的距离
            cv::RNG rng(time(0));

            if (distance < threshold) {
              //把这一点加入到与分类集合中，清空或者是改变这个值，修改横坐标的值
              cv::circle(roi, centers[e], 1, color, 2, 8, 0);
              temporary.push_back(centers[e]);
              //更新边界参数
              // x_left = min(centers[e].x,x_left);
              min(centers[e].x, x_left) > 0 ? x_left = min(centers[e].x, x_left)
                                            : x_left = 0;
              x_right = max(centers[e].x, x_right);

              centers[e].x = -5;
              break;
            }
          }
        }
      }
      //进一步判断得到的点群,进行直线拟合
      // cout<<"点集的大小"<<temporary.size()<<endl;

      if (temporary.size() < 7)
        continue;

      //优化2：返回这个数组后，再用最小二乘法进行拟合，因为最佳的直线可能不会一定穿过特征点；也可以直接在函数中进行
      // Least_squares_method(temporary,lineParam);
      ///最小二乘法，当初写这个是为了最小二乘法和随机一致性算法的对比

      ransac(temporary, lineParam, 2000, 2); //随机一致性算法
      float k1 = lineParam[0] / lineParam[1];
      Line_param line_temp;
      line_temp.k = k1;
      line_temp.b = lineParam[2]-k1*lineParam[3];
      line_parameters.push_back(line_temp);
    }

    if(iflineisfrist){
      
      last_line_parameters.assign(line_parameters.begin(), line_parameters.end());
      iflineisfrist = false;
    }
    cout << "直线参数向量中的值：" << line_parameters.size() << endl;
    save_image_name = save_image_path + name;

    /*
    判断如果导航线没有的话就用卡尔曼滤波，那么给卡尔曼滤波输入的参数是什么呢，是斜率和截距
    目前已经把是斜率和截距保存在line_parameters中，只要它的大小小于3代表检测到的直线的数量是小于3条
    小于3条的话就要使用卡尔曼滤波了
    
    if(line_parameters.size()<3){
        //根据上一次的点的参数来进行画线
        drawLines(roi,last_line_parameters,save_image_name);
        continue;
    }
    */
    //打算卡尔曼滤波的也写一个函数
    CKalmanFilter KF2(last_line_parameters);  //
    vector<Line_param> pp = KF2.predict();
    vector<Line_param> lines2Final = KF2.update(line_parameters);
    last_line_parameters = lines2Final;
    drawLines(roi,line_parameters,save_image_name);
  }
  return 0;
}

int Otsu(cv::Mat &src, cv::Mat &dst, int thresh) {
  const int Grayscale = 256;
  int graynum[Grayscale] = {0};
  int r = src.rows;
  int c = src.cols;
  for (int i = 0; i < r; ++i) {
    const uchar *ptr = src.ptr<uchar>(i);
    for (int j = 0; j < c; ++j) { //直方图统计
      graynum[ptr[j]]++;
    }
  }

  double P[Grayscale] = {0};
  double PK[Grayscale] = {0};
  double MK[Grayscale] = {0};
  double srcpixnum = r * c, sumtmpPK = 0, sumtmpMK = 0;
  for (int i = 0; i < Grayscale; ++i) {
    P[i] = graynum[i] / srcpixnum; //每个灰度级出现的概率
    PK[i] = sumtmpPK + P[i];       //概率累计和
    sumtmpPK = PK[i];
    MK[i] = sumtmpMK + i * P[i]; //灰度级的累加均值
    sumtmpMK = MK[i];
  }

  //计算类间方差
  double Var = 0;
  for (int k = 0; k < Grayscale; ++k) {
    if ((MK[Grayscale - 1] * PK[k] - MK[k]) *
            (MK[Grayscale - 1] * PK[k] - MK[k]) / (PK[k] * (1 - PK[k])) >
        Var) {
      Var = (MK[Grayscale - 1] * PK[k] - MK[k]) *
            (MK[Grayscale - 1] * PK[k] - MK[k]) / (PK[k] * (1 - PK[k]));
      thresh = k;
    }
  }

  //阈值处理
  src.copyTo(dst);
  for (int i = 0; i < r; ++i) {
    uchar *ptr = dst.ptr<uchar>(i);
    for (int j = 0; j < c; ++j) {
      if (ptr[j] > thresh)
        ptr[j] = 255;
      else
        ptr[j] = 0;
    }
  }
  return thresh;
}

//给图像画线
void drawLines(Mat roi, vector<Line_param> line_parameters,string save_image_name){

    cv::Point point01, point02, point03;
    vector<Point2f> line_point;

    float k, b, best_dis;
    float point_center_dis;

    for (int r = 0; r < line_parameters.size(); r++) 
    {
      k = line_parameters[r].k;
      b = line_parameters[r].b;

      point01.y = -10;
      point01.x = k * point01.y + b;

      point02.y = 600;
      point02.x = k * point02.y + b;

      point03.y = 400;
      point03.x = k * point03.y + b;

      point_center_dis = abs(point03.x - roi.cols / 2);   //计算的是绝对值
      if (r == 0)
          best_dis = point_center_dis;

      if (point_center_dis <= best_dis) {
          line_point.insert(line_point.begin(), point01);
          line_point.insert(line_point.begin(), point02);
          best_dis = point_center_dis;
      }
      //显示3条直线
      cv::line(roi, point01, point02, cv::Scalar(0,255,0),2);
    }
    //显示导航线
    cv::line(roi,line_point[0],line_point[1],cv::Scalar(255,0,0),2,8,0);
    imwrite(save_image_name, roi);
}

void ransac(std::vector<Point2f> temporary, cv::Vec4f &line, int iterations,float sigma) {
  unsigned int n = temporary.size();

  if (n < 2) {
    return;
  }
  cv::RNG rng;
  double bestScore = -1.;

  //进行迭代
  for (int k = 0; k < iterations; k++) {
    int i1 = 0, i2 = 0;
    while (i1 == i2) {
      i1 = rng(n);
      i2 = rng(n);
    }
    const cv::Point2f &p1 = temporary[i1];
    const cv::Point2f &p2 = temporary[i2];
    vector<Point2f> fit_temp_point;
    cv::Point2f dp = p2 - p1;
    dp *= 1. / norm(dp);
    double score = 0;
    //点集中的
    //点到直线的距离，
    ///如果进行修改的话，还可以是当距离小于阈值时，将点加入到数组中，一旦score >
    ///bestScore，返回这个数组，可以不返回数组直接在这个算法中进行最小二乘法的拟合

    for (int i = 0; i < n; i++) {
      cv::Point2f v = temporary[i] - p1;
      //向量a与b叉乘，可以得到向量a与b组成的平行四边形的面积，平行四边形的面积又等于底成高
      double d = v.y * dp.x - v.x * dp.y; //向量a与b叉乘/向量b的模.||b||=1./norm(dp)
      // score += exp(-0.5*d*d/(sigma*sigma));//误差定义方式的一种
      if (fabs(d) < sigma) {
        score += 1;
        fit_temp_point.push_back(temporary[i]);
      }
    }

    if (score > bestScore) {
      //内点集 ：fit_temp_point
      line = cv::Vec4f(dp.x, dp.y, p1.x, p1.y);
      //cv::fitLine(fit_temp_point, line, cv::DIST_L2, 0, 1e-2, 1e-2);
      
      bestScore = score;
    }
  }
}

void Least_squares_method(std::vector<Point2f> temporary, cv::Vec4f &line) {
  cv::fitLine(temporary, line, cv::DIST_L2, 0, 1e-5, 1e-5);
}