8月8日程序备份——找挡板角度及平移误差

效果图

程序

/**************************************************************************************
该程序功能：放置挡板


*  flag    1    前进
*  flag   -1    后退

*  flag    2   横走右
*  flag   -2   横走左


*  flag    3   找中心

Forward_flag = -2;//发送开环左走指令
*  flag    4    爪子直立起来的，且有磁性，此时手动往机械臂上放置挡板
*  flag   -4    爪子到了位置，开始放置，发送一次即可

*  flag    9    右转90度
*  flag   -9    左转90度

*  flag   18    右转180度
*  flag  -18    左转180度

*  flag   18    右转180度
*  flag  -100    放下棋子


方向dir： 0.从下往上看
1.从左往右看
2.从上往下看
3.从右往左看

3 开启找棋子中心模式
4 代表先打开电磁铁  -4放下 针对于障碍
****************************************************************************************/
#include
#include   
#include   
#include
#include
#include

//#include "UART.h"
#include "findline.h"
//#include "Number.h"
//#include "Process.h"
//#include "Locate.h"
//#include "Manipulator_positioning.h"

using namespace cv;
using namespace std;

#define PI 3.1415926
RNG g_rng(12345);


//RGB转HSV
void RGB2HSV(double red, double green, double blue, double& hue, double& saturation, double& intensity)
{

    double r, g, b;
    double h, s, i;

    double sum;
    double minRGB, maxRGB;
    double theta;

    r = red / 255.0;
    g = green / 255.0;
    b = blue / 255.0;

    minRGB = ((rg) ? (r) : (g));
    maxRGB = (maxRGB>b) ? (maxRGB) : (b);

    sum = r + g + b;
    i = sum / 3.0;

    if (i<0.001 || maxRGB - minRGB<0.001)
    {
        h = 0.0;
        s = 0.0;
    }
    else
    {
        s = 1.0 - 3.0*minRGB / sum;
        theta = sqrt((r - g)*(r - g) + (r - b)*(g - b));
        theta = acos((r - g + r - b)*0.5 / theta);
        if (b <= g)
            h = theta;
        else
            h = 2 * PI - theta;
        if (s <= 0.01)
            h = 0;
    }

    hue = (int)(h * 180 / PI);
    saturation = (int)(s * 100);
    intensity = (int)(i * 100);
}
//
///***** 求两点间距离*****/
//float getDistance(CvPoint pointO, CvPoint pointA)
//{
//  float distance;
//  distance = powf((pointO.x - pointA.x), 2) + powf((pointO.y - pointA.y), 2);
//  distance = sqrtf(distance);
//  return distance;
//}

//原点到直线的距离
//输入一堆直线，返回直线到坐标原点的距离
vector <float> get_0_distance(vector lines)
{
    vector <float> diatance;

    for (unsigned int i = 0; i < lines.size(); i++)
    {
        double k = (double)(lines[i][1] - lines[i][3]) / (double)(lines[i][0] - lines[i][2]);//斜率
        double b = (double)lines[i][1] - (double)(lines[i][0])* k; //截距
        double diatance_temp = fabs(b / sqrt(1 + k*k));
        diatance.push_back(diatance_temp);
    }
    return diatance;

}

/***** 点到直线的距离:P到AB的距离*****/
//P为线外一点，AB为线段两个端点,有正负，点在左边为负，右边为正
float getDist_P2L_zhengfu(CvPoint pointP, CvPoint pointA, CvPoint pointB)
{
    //求直线方程
    float A = 0, B = 0, C = 0;
    A = pointA.y - pointB.y;
    B = pointB.x - pointA.x;
    C = pointA.x*pointB.y - pointA.y*pointB.x;
    //代入点到直线距离公式
    float distance = 0;


    if (pointP.x>(-(B*pointP.y + C) / A))
    {//点在线右边
        distance = ((float)abs(A*pointP.x + B*pointP.y + C)) / ((float)sqrtf(A*A + B*B));
    }
    else
    {//点在线左边
        distance = -((float)abs(A*pointP.x + B*pointP.y + C)) / ((float)sqrtf(A*A + B*B));
    }
    return distance;
}


/***** 点到直线的距离:P到AB的距离*****/
//P为线外一点，剩下三个参数为直线一般式参数
float getDist_P2L_zhengfu_2(CvPoint pointP, float Param_A1, float Param_A2, float Param_A3)
{
    //求直线方程
    float A = 0, B = 0, C = 0;
    A = Param_A1;
    B = Param_A2;
    C = Param_A3;
    //代入点到直线距离公式
    float distance = 0;


    if (pointP.x>(-(B*pointP.y + C) / A))
    {//点在线右边
        distance = ((float)abs(A*pointP.x + B*pointP.y + C)) / ((float)sqrtf(A*A + B*B));
    }
    else
    {//点在线左边
        distance = -((float)abs(A*pointP.x + B*pointP.y + C)) / ((float)sqrtf(A*A + B*B));
    }
    return distance;
}


//输入一堆直线，返回每条直线与水平直线的角度，为弧度
vector <float> get_lines_arctan(vector lines)
{
    float k = 0; //直线斜率
    vector <float> lines_arctan;//直线斜率的反正切值
    for (unsigned int i = 0; idouble)(lines[i][3] - lines[i][1]) / (double)(lines[i][2] - lines[i][0]); //求出直线的斜率
        lines_arctan.push_back(atan(k));
    }
    return lines_arctan;
}

/***** 点到一堆直线的距离:P到lines的距离*****/
vector <float> getDist_P2L_yibanshi(Point2f pointP, vector lines)
{
    //求直线方程
    Point2f pointA, pointB;
    vector <float>  distance;
    for (unsigned int i = 0; i < lines.size(); i++)
    {

        pointA.x = (float)lines[i][0];
        pointA.y = (float)lines[i][1];
        pointB.x = (float)lines[i][2];
        pointB.x = (float)lines[i][3];

        float A = 0, B = 0, C = 0;
        A = pointA.y - pointB.y;
        B = pointB.x - pointA.x;
        C = pointA.x*pointB.y - pointA.y*pointB.x;
        //代入点到直线距离公式
        float distance_temp;
        distance_temp = ((float)abs(A*pointP.x + B*pointP.y + C)) / ((float)sqrtf(A*A + B*B));

        distance.push_back(distance_temp);
    }
    return distance;
}



//输入一堆直线，返回每条直线的斜率和截距
//Vec2f为2个点的float，参照存储直线的数据结构
vector  get_lines_fangcheng(vector lines)
{
    double k = 0; //直线斜率
    double b = 0; //直线截距
    vector  lines_fangcheng;//

    for (unsigned int i = 0; idouble)(lines[i][3] - lines[i][1]) / (double)(lines[i][2] - lines[i][0]); //求出直线的斜率// -3.1415926/2-----+3.1415926/2
        b = (double)lines[i][1] - k * (double)lines[i][0]; //求出直线的截距
        lines_fangcheng.push_back(Point2d(k, b));
    }

    return lines_fangcheng;
}


//填充
void fillHole(const Mat srcBw, Mat &dstBw)
{
    Size m_Size = srcBw.size();
    Mat Temp = Mat::zeros(m_Size.height + 2, m_Size.width + 2, srcBw.type());//延展图像
    srcBw.copyTo(Temp(Range(1, m_Size.height + 1), Range(1, m_Size.width + 1)));

    cv::floodFill(Temp, Point(0, 0), Scalar(255, 255, 255));//填充区域

    Mat cutImg;//裁剪延展的图像
    Temp(Range(1, m_Size.height + 1), Range(1, m_Size.width + 1)).copyTo(cutImg);

    dstBw = srcBw | (~cutImg);
}


输入一堆直线，返回每条直线的一般式方程
Vec2f为2个点的float，参照存储直线的数据结构
//vector > get_lines_yibanshi_fangcheng(vector lines)
//{
//  //1.分别求角点0、1与角点2、3所对应的两根直线
//  vector > fangcheng_PARAM;
//  vector fangcheng_temp;
//  float A0 = 0, B0 = 0, C0 = 0;
//  for (unsigned int i = 0; i
//  {
//      A0 = (float)(lines[i][1] - lines[i][3]);
//      B0 = (float)(lines[i][2] - lines[i][0]);
//      C0 = (float)(lines[i][0] * lines[i][3] - lines[i][1] * lines[i][2]);
//
//      //A0 = (float)(Connor[0].y - Connor[1].y);
//      //B0 = (float)(Connor[1].x - Connor[0].x);
//      //C0 = (float)(Connor[0].x*Connor[1].y - Connor[0].y*Connor[1].x);
//      fangcheng_temp.push_back(A0);
//      fangcheng_temp.push_back(B0);
//      fangcheng_temp.push_back(C0);
//
//      fangcheng_PARAM.push_back(fangcheng_temp);
//
//  }
//  return fangcheng_PARAM;
//}


//输入一堆直线，返回每条直线的一般式方程
//Vec2f为2个点的float，参照存储直线的数据结构
vector <float> get_lines_yibanshi_fangcheng(vector lines)
{
    //1.分别求角点0、1与角点2、3所对应的两根直线
    //vector > fangcheng_PARAM;
    vector<float> fangcheng_temp;

    for (unsigned int i = 0; ifloat A0 = 0, B0 = 0, C0 = 0;
        A0 = (float)(lines[i][1] - lines[i][3]);
        B0 = (float)(lines[i][2] - lines[i][0]);
        C0 = (float)(lines[i][0] * lines[i][3] - lines[i][1] * lines[i][2]);

        //A0 = (float)(Connor[0].y - Connor[1].y);
        //B0 = (float)(Connor[1].x - Connor[0].x);
        //C0 = (float)(Connor[0].x*Connor[1].y - Connor[0].y*Connor[1].x);
        fangcheng_temp.push_back(A0);
        fangcheng_temp.push_back(B0);
        fangcheng_temp.push_back(C0);

        //  fangcheng_PARAM.push_back(fangcheng_temp);

    }
    return fangcheng_temp;
}




/*******************************************************************************************
*函数功能 ： 输入两条直线（每条直线以斜率和截距确定），返回两直线夹角，0为弧度，1为角度
*输入参数 ： line_1_k为一条直线斜率，line_2_k为另一条直线斜率，aaa为0则为弧度，反之则为角度
*返 回 值 ： float型弧度或者角度,有正负，为直线2相对于直线1的角度
*编写时间 ： 2018.8.5
*作    者 ： 毛哥
********************************************************************************************/
float get_lines_arctan(float line_1_k, float line_2_k, int aaa)
{
    if (aaa == 0)
    {
        float tan_k = 0; //直线夹角正切值
        float lines_arctan;//直线斜率的反正切值
        tan_k = (line_2_k - line_1_k) / (1 + line_2_k*line_1_k); //求直线夹角的公式
        lines_arctan = atan(tan_k);
        return lines_arctan;
    }
    else
    {
        float tan_k = 0; //直线夹角正切值
        float lines_arctan;//直线斜率的反正切值
        tan_k = (line_2_k - line_1_k) / (1 + line_2_k*line_1_k); //求直线夹角的公式
        lines_arctan = atan(tan_k)* 180.0 / 3.1415926;

        return lines_arctan;
    }
}


/*******************************************************************************************
*函数功能 ： 输入两条直线(输入一般式三个参数)，返回两直线夹角，0为弧度
*输入参数 ： Param_A1、Param_B1、Param_C1 \Param_2、Param_B2、Param_C2输入一般式三个参数
*返 回 值 ： double 型角度,有正负，为直线2相对于直线1的角度
*编写时间 ： 2018.8.6
*作    者 ： 毛哥
********************************************************************************************/
double get_lines_yibanshi_arctan(double Param_A1, double Param_B1, double Param_C1, double Param_A2, double Param_B2, double Param_C2)
{

    double tan_k = 0; //直线夹角正切值
    double lines_arctan;//直线斜率的反正切值
    double line_1_k = 0;
    double line_2_k = 0;

    //  tan_k = (line_2_k - line_1_k) / (1 + line_2_k*line_1_k); //求直线夹角的公式

    if (fabs(Param_B1) > 0.001 && fabs(Param_B2) > 0.001)
    {
        line_1_k = -Param_A1 / Param_B1;
        line_2_k = -Param_A2 / Param_B2;

        //若两直线垂直，即k1k2 = －1，此时夹角为90°；
        if (line_1_k*line_2_k != -1)
        {
            tan_k = (line_2_k - line_1_k) / (1 + line_2_k*line_1_k); //求直线夹角的公式
            //tan_k = (Param_A1*Param_A2 + Param_B1*Param_B2) / (sqrt(Param_A1*Param_A1 + Param_B1*Param_B1)*sqrt(Param_A2*Param_A2 + Param_B2*Param_B2));
            lines_arctan = atan(tan_k)* 180.0 / 3.1415926;;
        }
    }
    else if (Param_B1 < 0.0001&& Param_B2>0.0001)
    {

        tan_k = -Param_A2 / Param_B2;
        lines_arctan = atan(tan_k);
        lines_arctan = (3.1415926 / 2 - lines_arctan)* 180.0 / 3.1415926;
    }
    else if (Param_B1 > 0.0001&&Param_B2 < 0.0001)
    {

        tan_k = -Param_A1 / Param_B1;
        lines_arctan = atan(tan_k);
        lines_arctan = (lines_arctan - 3.1415926 / 2)* 180.0 / 3.1415926;

    }
    else
    {
        lines_arctan = 0;
    }
    return lines_arctan;
}






//输入一直线的两个端点，返回该直线的一般式方程
vector <float> get_lines_yibanshi_fangcheng_2(Point2f Point_A, Point2f Point_B)
{
    //1.分别求角点0、1与角点2、3所对应的两根直线
    //vector > fangcheng_PARAM;
    vector<float> fangcheng_temp;
    float A0 = 0, B0 = 0, C0 = 0;

    //A0 = (float)(lines[i][1] - lines[i][3]);
    //B0 = (float)(lines[i][2] - lines[i][0]);
    //C0 = (float)(lines[i][0] * lines[i][3] - lines[i][1] * lines[i][2]);

    A0 = (float)(Point_A.y - Point_B.y);
    B0 = (float)(Point_B.x - Point_A.x);
    C0 = (float)(Point_A.x*Point_B.y - Point_A.y*Point_B.x);
    fangcheng_temp.push_back(A0);
    fangcheng_temp.push_back(B0);
    fangcheng_temp.push_back(C0);

    //fangcheng_PARAM.push_back(fangcheng_temp);

    return fangcheng_temp;
}




Mat frame1; //全局变量前方摄像头图像
Mat frame2; //全局变量顶部摄像头图像

VideoCapture capcap_1(1); //前方摄像头
VideoCapture capcap_2(0); //顶部  摄像头


int main()
{

    capcap_2.set(CAP_PROP_FRAME_WIDTH, 1280.0);//设置摄像头采集图像分辨率
    capcap_2.set(CAP_PROP_FRAME_HEIGHT, 720.0);

    capcap_1.set(CAP_PROP_FRAME_WIDTH, 1280.0);//设置摄像头采集图像分辨率
    capcap_1.set(CAP_PROP_FRAME_HEIGHT, 720.0);

    if (!capcap_2.isOpened())return 0;
    if (!capcap_1.isOpened())return 0;
    for (int i = 0; i < 5; i++)
    {
        capcap_2 >> frame2;
    }

    while (1)
    {
        printf("\n\n\t\t\t  当前使用的OpenCV版本为" CV_VERSION"\n");

        Mat srcImage0;

        capcap_2 >> frame2;


        //载入原图，并找到红色挡板
        //  Mat srcImage = imread("13.jpg", 1);

        Mat srcImg;
        resize(frame2, srcImg, Size(800, 600));//重定义图片大小

        //namedWindow("原图", 0);
        //imshow("原图", srcImg);
        //waitKey(1);

        int width = srcImg.cols;
        int height = srcImg.rows;

        int x, y;
        double B = 0.0, G = 0.0, R = 0.0, H = 0.0, S = 0.0, V = 0.0;
        Mat vec_rgb = Mat::zeros(srcImg.size(), CV_8UC1);
        for (x = 0; x < height; x++)
        {
            for (y = 0; y < width; y++)
            {
                B = srcImg.at(x, y)[0];
                G = srcImg.at(x, y)[1];
                R = srcImg.at(x, y)[2];
                RGB2HSV(R, G, B, H, S, V);
                //红色范围，范围参考的网上。可以自己调
                if ((H >= 312 && H <= 360 || H >= 0 && H <= 20) && (S >= 17 && S <= 100) && (V>18 && V < 100))
                    vec_rgb.at(x, y) = 255;
            }
        }

        namedWindow("hsv空间图像", 0);
        imshow("hsv空间图像", vec_rgb);
        waitKey(1);

        Mat element = getStructuringElement(MORPH_ELLIPSE, Size(2 * 1 + 1, 2 * 1 + 1), Point(1, 1));
        Mat element1 = getStructuringElement(MORPH_ELLIPSE, Size(2 * 3 + 1, 2 * 3 + 1), Point(3, 3));

        dilate(vec_rgb, vec_rgb, element1);//膨胀
        /*namedWindow("膨胀", 0);
        imshow("膨胀", vec_rgb);
        waitKey(1);*/

        erode(vec_rgb, vec_rgb, element1);//腐蚀
        /*namedWindow("腐蚀", 0);
        imshow("腐蚀", vec_rgb);
        waitKey(1);*/

        vector<vector>contours, max_contours; //轮廓
        vector hierarchy;//分层
        Mat drawing_text = Mat::zeros(vec_rgb.size(), CV_8UC3);
        findContours(vec_rgb, contours, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);//找轮廓
        if (contours.size() == 0)
        {
            cout << "警告：没有红色挡板（第一次找轮廓）" << endl;
            continue;
        }
        cout << "第一次找轮廓结束，共找到轮廓：" << contours.size() << " 条" << endl;

        vector<vector> hull(contours.size());//用于存放凸包
        vector<float> length(contours.size());
        vector<float> Area_contours(contours.size()), Area_hull(contours.size()), Rectangularity(contours.size()), circularity(contours.size());
        for (int i = 0; i < contours.size(); i++)
        {//历遍所有的轮廓

            length[i] = arcLength(contours[i], true);

            cout << "轮廓长度为" << length[i] << endl;

            if (length[i] >285 && length[i] < 2300)
            {//通过长度匹配滤除小轮廓
                convexHull(Mat(contours[i]), hull[i], false);//把凸包找出来
                max_contours.push_back(hull[i]);//把提取出来的方框导入到新的轮廓组
            }
        }
        if (max_contours.size() == 0)
        {
            cout << "警告：y有红色物体，但是可能没有红色挡板，或者红色挡板被遮挡严重" << endl;
            continue;
        }

        /*************************************************************************
        2、切割出红色挡板附近的区域
        *************************************************************************/
        //多边形逼近轮廓 + 获取矩形和圆形边界框
        vector<vector > contours_poly(max_contours.size());
        vector boundRect(max_contours.size());
        vectorcenter(max_contours.size());
        vector<float>radius(max_contours.size());

        //一个循环，遍历所有部分，进行本程序最核心的操作
        for (unsigned int i = 0; i < max_contours.size(); i++)
        {
            approxPolyDP(Mat(max_contours[i]), contours_poly[i], 3, true);//用指定精度逼近多边形曲线 
            boundRect[i] = boundingRect(Mat(contours_poly[i]));//计算点集的最外面（up-right）矩形边界
            minEnclosingCircle(contours_poly[i], center[i], radius[i]);//对给定的 2D点集，寻找最小面积的包围圆形 
        }



        Mat image_cut;      //从img中按照rect进行切割，此时修改image_cut时image中对应部分也会修改，因此需要copy  
        Mat image_copy_C3;   //clone函数创建新的图片 彩色
        Mat image_copy_C1;   //clone函数创建新的图片 黑白

        int width1 = abs(boundRect[0].tl().x - boundRect[0].br().x);
        int height1 = abs(boundRect[0].tl().y - boundRect[0].br().y);

        //Rect rect(boundRect[0].tl().x - 50, boundRect[0].tl().y - 120, width1 + 110, height1 + 200);   //创建一个Rect框，属于cv中的类，四个参数代表x,y,width,height  
        Rect rect(boundRect[0].tl().x - 50, 90, width1 + 110, vec_rgb.rows-130);   //创建一个Rect框，属于cv中的类，四个参数代表x,y,width,height  

        image_cut = Mat(srcImg, rect);      //从img中按照rect进行切割，此时修改image_cut时image中对应部分也会修改，因此需要copy  
        image_copy_C3 = image_cut.clone();   //clone函数创建新的图片 

        image_cut = Mat(vec_rgb, rect);      //从img中按照rect进行切割，此时修改image_cut时image中对应部分也会修改，因此需要copy  
        image_copy_C1 = image_cut.clone();   //clone函数创建新的图片  

        namedWindow("裁剪后彩色", 0);
        imshow("裁剪后彩色", image_copy_C3);
        waitKey(1);
        namedWindow("裁剪后黑白", 0);
        imshow("裁剪后黑白", image_copy_C1);
        waitKey(1);

        cout << "已裁剪" << endl;

        vector<vector>contours_1, RectContours; //轮廓
        Mat drawing_1 = Mat(image_copy_C1.size(), CV_8UC3, Scalar(0, 0, 0));
        findContours(image_copy_C1, contours_1, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);//找轮廓

        if (contours_1.size() == 0)
        {
            cout << "警告：没有在裁剪后的图片里找到轮廓（第二次找轮廓）" << endl;
            continue;
        }

        cout << "第二次找轮廓结束，共找到轮廓：" << contours_1.size() << " 条" << endl;
        //cout << "第二次找轮廓结束" << endl;
        if (contours_1.size() > 0)//如果没有找到轮廓退出
        {
            vector<vector> hull(contours_1.size());//用于存放凸包
            int i = 0;
            vector<float> length(contours_1.size());
            vector<float> Area_contours(contours_1.size()), Area_hull(contours_1.size()), Rectangularity(contours_1.size()), circularity(contours_1.size());
            for (i = 0; i < contours_1.size(); i++)
            {//历遍所有的轮廓

                length[i] = arcLength(contours_1[i], true);

                if (length[i] >100 && length[i] < 3500)
                {//通过长度匹配滤除小轮廓
                    convexHull(Mat(contours_1[i]), hull[i], false);//把凸包找出来

                    RectContours.push_back(hull[i]);//把提取出来的方框导入到新的轮廓组

                }

            }
            cout << "已找到裁剪后的新凸包" << endl;

            for (int i = 0; i < RectContours.size(); i++)
            {
                Scalar color = (255, 255, 255);//蓝色线画轮廓
                drawContours(drawing_1, RectContours, i, color, 1, 8, hierarchy, 0, Point());//根据轮廓点集contours_poly和轮廓结构hierarchy画出轮廓
                //画圆形
            }

            fillHole(drawing_1, drawing_1);//填充
            namedWindow("填充", 0);
            imshow("填充", drawing_1);
            waitKey(1);


            dilate(drawing_1, drawing_1, element1);//膨胀
            namedWindow("膨胀1", 0);
            imshow("膨胀1", drawing_1);
            waitKey(1);

            erode(drawing_1, drawing_1, element1);//腐蚀
            namedWindow("腐蚀1", 0);
            imshow("腐蚀1", drawing_1);
            waitKey(1);


        }


        namedWindow("红色挡板凸包", 0);
        imshow("红色挡板凸包", drawing_1);
        waitKey(1);

        /********再次找轮廓******************************************************************************************/
        cvtColor(drawing_1, drawing_1, CV_BGR2GRAY);
        /*  namedWindow("灰度化", 0);
        imshow("灰度化", drawing_1);
        waitKey(1);
        */
        drawing_1 = drawing_1 >160;//二值化
        /*  namedWindow("二值化", 0);
        imshow("二值化", drawing_1);
        waitKey(1);*/



        vector<vector>contours_2, RectContours_2; //轮廓
        Mat drawing_2 = Mat(image_copy_C1.size(), CV_8UC3, Scalar(0, 0, 0));
        findContours(drawing_1, contours_2, CV_RETR_LIST, CV_CHAIN_APPROX_NONE);//找轮廓

        if (contours_2.size() == 0)
        {
            cout << "警告：没有在膨胀处理后的的图片里找到轮廓（第三次找轮廓）" << endl;
            continue;
        }



        cout << "第三次找轮廓结束，共找到轮廓：" << contours_2.size() << " 条" << endl;
        if (contours_2.size() > 0)//如果没有找到轮廓退出
        {
            vector<vector> hull(contours_2.size());//用于存放凸包
            int i = 0;
            vector<float> length(contours_2.size());
            vector<float> Area_contours(contours_2.size()), Area_hull(contours_2.size()), Rectangularity(contours_2.size()), circularity(contours_2.size());
            for (i = 0; i < contours_2.size(); i++)
            {//历遍所有的轮廓

                length[i] = arcLength(contours_2[i], true);

                if (length[i] >100 && length[i] < 3500)
                {//通过长度匹配滤除小轮廓
                    convexHull(Mat(contours_2[i]), hull[i], false);//把凸包找出来

                    RectContours_2.push_back(hull[i]);//把提取出来的方框导入到新的轮廓组

                }

            }


            for (int i = 0; i < RectContours_2.size(); i++)
            {
                Scalar color = (0, 255, 255);//蓝色线画轮廓
                drawContours(drawing_2, RectContours_2, i, color, 1, 8, hierarchy, 0, Point());//根据轮廓点集contours_poly和轮廓结构hierarchy画出轮廓
                //画圆形
            }


            /**************************************************************************************************/
        }

        namedWindow("哈哈哈", 0);
        imshow("哈哈哈", drawing_2);
        waitKey(1);









        /***** 4、ConnorPoint_Output 把所有排好序的角点输出，0号是左上角，顺时针输出(综合1、2、3)*****/
        vector ConnorPoint(4);
        vector ConnorPoint_ordered(4);
        vector<vector> ConnorPoint_Output(4);

        for (int i = 0; i < RectContours_2.size(); i++)
        {
            ConnorPoint = FindConnor(RectContours_2[i]);
            ConnorPoint_ordered = FindFirstPoint(ConnorPoint);
            ConnorPoint_Output[0].push_back(ConnorPoint_ordered[2]);
            ConnorPoint_Output[1].push_back(ConnorPoint_ordered[1]);
            ConnorPoint_Output[2].push_back(ConnorPoint_ordered[0]);
            ConnorPoint_Output[3].push_back(ConnorPoint_ordered[3]);
        }

        Point A_top, B_bottom;  //红色挡板顶部中点，红色挡板底部中点

        A_top.x = (ConnorPoint_Output[0][0].x + ConnorPoint_Output[1][0].x) / 2;
        A_top.y = (ConnorPoint_Output[0][0].y + ConnorPoint_Output[1][0].y) / 2;

        B_bottom.x = (ConnorPoint_Output[3][0].x + ConnorPoint_Output[2][0].x) / 2;
        B_bottom.y = (ConnorPoint_Output[3][0].y + ConnorPoint_Output[2][0].y) / 2;



        //上面没什么大问题，














        //再次通过找轮廓，然后拟合出直线，通过斜率滤除剩下四条数线（或横线），然后通过长度滤除挡板的边缘直线，剩下白色过道的直线方程，一切问题得到解决。
        Mat midImage, dstImage;//临时变量和目标图的定义
        Mat image_copy_C3_1;
        cvtColor(image_copy_C3, image_copy_C3_1, CV_BGR2GRAY);//灰度化
        //【3】srcImage取大于阈值119的那部分
        image_copy_C3_1 = image_copy_C3_1 > 150;

        namedWindow("取阈值后的图", 0);
        imshow("取阈值后的图", image_copy_C3_1);

        //blur(image_copy_C3_1, image_copy_C3_1, Size(3, 3));//进行模糊

        /*******  检测直线优化 开始 ****************************************************************/
        int cannyThreshold = 80;
        float factor = 2.5;


        vector lines, lines_final;//定义一个矢量结构lines用于存放得到的线段矢量集合
        //HoughLinesP(midImage, lines, 1, CV_PI / 180, 320, 240, 30);
        Canny(image_copy_C3_1, midImage, cannyThreshold, cannyThreshold * factor);
        HoughLinesP(midImage, lines, 1, CV_PI / 180, 70, 40, 80);
         最多的直线
        //while (lines.size() >= 10)
        //{

        //  cout << "正在循环，直线数大于10条"  << endl;
        //  //cannyThreshold += 2;
        //  //Canny(image_copy_C3_1, midImage, cannyThreshold, cannyThreshold * factor); 


        //  HoughLinesP(midImage, lines, 1, CV_PI / 180, 50, 20, 50);
        //}
        最少的直线
        //while (lines.size() <= 2)
        //{
        //  cout << "正在循环，直线数小于2条" << endl;
        //  //cannyThreshold -= 2;
        //  //Canny(image_copy_C3_1, midImage, cannyThreshold, cannyThreshold * factor);
        //  HoughLinesP(midImage, lines, 1, CV_PI / 180, 50, 20, 50);
        //}

        cout << "canny边缘检测阈值为:" << cannyThreshold << endl;

        /*Canny(image_copy_C3_1, midImage, cannyThreshold, cannyThreshold * factor);
        HoughLinesP(midImage, lines, 1, CV_PI / 180, 60, 50, 250);*/

        /*******  检测直线优化 结束 ****************************************************************/


        cvtColor(midImage, dstImage, COLOR_GRAY2BGR);//转化边缘检测后的图为灰度图
        //画出挡板中轴线
        line(dstImage, A_top, B_bottom, Scalar(0, 255, 0), 1, LINE_AA);//画出挡板中轴线

        cout << "\n共检测到原始  直线" << lines.size() << "条" << endl;


        //【4】依次计算出直线长度
        float  zhixian_changdu;
        //原点到直线的距离
        //输入一堆直线，返回直线到坐标原点的距离
        vector <float>      get_0_distance1;
        /***** 点到一堆直线的距离:P到lines的距离*****/
        //  vector  getDist_P2L_yibanshi(Point pointP, vector lines)
        get_0_distance1 = getDist_P2L_yibanshi(Point2f(0.0, 0.0), lines);
        for (size_t i = 0; i < lines.size(); i++)
        {
            Vec4i l = lines[i];
            zhixian_changdu = getDistance(Point(l[0], l[1]), Point(l[2], l[3]));

            cout << "\n直线长度为" << zhixian_changdu << endl;
            cout << "\n直线到原点距离为" << get_0_distance1[i] << endl;
            line(image_copy_C3, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(g_rng.uniform(0, 255), g_rng.uniform(0, 255), g_rng.uniform(0, 255)), 2, LINE_AA);
            float line_k;
            int a;
            a = lines[i][2] - lines[i][0];
            cout << "\n直线斜率的参数" << a << endl;
            if (fabs((double)(lines[i][2] - lines[i][0]))>0.001)
            {
                line_k = (double)(lines[i][3] - lines[i][1]) / (double)(lines[i][2] - lines[i][0]); //求出直线的斜率
                cout << "\n直线的斜率为" << line_k << endl;
                //斜率k以及b很接近的保留一条，两根直线应该是距离恒定的（一个比较稳定的范围）
                //通过直线长度滤除，只剩下两条最长的,斜率要大于45度的直线
                if (zhixian_changdu>70 && (lines[i][1]<60 || lines[i][3]<60) &&fabs(line_k)>1.414)
                {
                    lines_final.push_back(lines[i]);

                }

            }
            else
            {
                if (zhixian_changdu>70 && (lines[i][1] < 60 || lines[i][3] < 60))
                {   
                    lines_final.push_back(lines[i]);    
                }

            }


        }
        //Scalar color = Scalar(g_rng.uniform(0, 255), g_rng.uniform(0, 255), g_rng.uniform(0, 255));//随机设置颜色
        Point pt1, pt2;

        double Param_A1, Param_B1, Param_C1;//过道直线
        double Param_A2, Param_B2, Param_C2;//挡板中轴线

        vector <float> fangcheng_PARAM_1, fangcheng_PARAM_2;
        float theta, A_top_diatance, B_bottom_diatance;
        //最终只能检测出两条直线
        if (lines_final.size() == 2)
        {
            //【4】依次在图中绘制出每条线段
            for (size_t i = 0; i < lines_final.size(); i++)
            {
                Vec4i l = lines_final[i];
                line(dstImage, Point(l[0], l[1]), Point(l[2], l[3]), Scalar(g_rng.uniform(0, 255), g_rng.uniform(0, 255), g_rng.uniform(0, 255)), 2, LINE_AA);
            }


            //先求最终直线的方程的三个参数 A_top, B_bottom
            fangcheng_PARAM_1 = get_lines_yibanshi_fangcheng(lines_final);
            fangcheng_PARAM_2 = get_lines_yibanshi_fangcheng_2((Point2f)A_top, (Point2f)B_bottom);

            Param_A1 = (fangcheng_PARAM_1[0] + fangcheng_PARAM_1[3]) / 2.0;
            Param_B1 = (fangcheng_PARAM_1[1] + fangcheng_PARAM_1[4]) / 2.0;
            Param_C1 = (fangcheng_PARAM_1[2] + fangcheng_PARAM_1[5]) / 2.0;

            Param_A2 = fangcheng_PARAM_2[0];
            Param_B2 = fangcheng_PARAM_2[1];
            Param_C2 = fangcheng_PARAM_2[2];


            /***** 7、输入A，B，C,画出线在dstImage*****/
            draw_line(Param_A1, Param_B1, Param_C1, dstImage);






            //最终求夹角（旋转误差）和平移误差

            //两直线夹角，挡板相对于白色过道的夹角，因此白色过道直线斜率为第一个参数
            theta = get_lines_yibanshi_arctan(Param_A1, Param_B1, Param_C1, Param_A2, Param_B2, Param_C2);

            //点到直线距离
            //P为线外一点，AB为线段两个端点
            A_top_diatance = getDist_P2L_zhengfu_2(A_top, Param_A1, Param_B1, Param_C1);
            B_bottom_diatance = getDist_P2L_zhengfu_2(B_bottom, Param_A1, Param_B1, Param_C1);

            cout << "\n最终结果\n挡板中轴线和白色过道中轴线夹角为： " << theta << " 度" << endl;
            cout << "挡板顶部中点与白色过道中轴线误差为： " << A_top_diatance << endl;
            cout << "挡板底部中点与白色过道中轴线误差为： " << B_bottom_diatance << endl;
        }
        else
        {
            cout << "没有正确的找到最终的两条直线"  << endl;
            continue;
        }

        //【6】边缘检测后的图 
        //namedWindow("【边缘检测后的图】", 0);//参数为零，则可以自由拖动
        //imshow("【边缘检测后的图】", midImage);

        namedWindow("直线未滤除前", 0);
        imshow("直线未滤除前", image_copy_C3);
        waitKey(1);

        namedWindow("【检测直线效果图】", 0);//参数为零，则可以自由拖动
        imshow("【检测直线效果图】", dstImage);

        waitKey(10);


    }


}








//
//void Open_dicitie(void)
//{
//  char Forward_flag = 4;//打开电磁铁
//  int error1 = 0, error2 = 0, text = 0;
//  text = error1 + error2 + Forward_flag;
//  char g_UartRxBuffer[8] = { 123, -122, -121, 120, Forward_flag, (char)error1, (char)error2, (char)text };//针头（4），1个数据，一个验证
//  WriteChar(g_UartRxBuffer, 8);//可以自行设置分部发送
//}
//
//
//
//
//Mat frame1; //全局变量前方摄像头图像
//Mat frame2; //全局变量顶部摄像头图像
//
//
//VideoCapture capcap_2(2); //顶部  摄像头
//VideoCapture capcap_1(1); //前方摄像头
//
//int main()
//{
//
//  capcap_2.set(CAP_PROP_FRAME_WIDTH, 1280.0);//设置摄像头采集图像分辨率
//  capcap_2.set(CAP_PROP_FRAME_HEIGHT, 720.0);
//
//
//
//  capcap_1.set(CAP_PROP_FRAME_WIDTH, 1280.0);//设置摄像头采集图像分辨率
//  capcap_1.set(CAP_PROP_FRAME_HEIGHT, 720.0);
//
//  if (!capcap_2.isOpened())return 0;
//  if (!capcap_1.isOpened())return 0;
//  
//
//  Ready_Pic();
//
//
//  //Catch_chess();
//  //Go_to_Number(27);
//  //Put_chess(35);
//
//  //第一块挡板
//  Open_dicitie();
//  Sleep(1200); //程序挂起两秒
//  Sleep(1200); //程序挂起两秒
//  Go_to_Number(35, 0);
//  cout << "开始放置第一块挡板" << endl;
//  Put_Dangban(35, 0);
//  cout << "第一块挡板放置成功" << endl;
//  Sleep(2200); //程序挂起两秒
//
//  Go_to_Home();
//
//
//
//  //第二块挡板
//  Go_to_Number(35, 1);
//  cout << "开始放置第二块挡板" << endl;
//  Put_Dangban(35, 1);
//  cout << "第二块挡板放置成功" << endl;
//  //Sleep(2200); //程序挂起两秒
//  Sleep(2200); //程序挂起两秒
//  //Sleep(1200); //程序挂起两秒
//  Go_to_Home();
//
//  第三块挡板
//  Go_to_Number(28, 0);
//  cout << "开始放置第三块挡板" << endl;
//  Put_Dangban(28, 0);
//  cout << "第三块挡板放置成功" << endl;
//  Sleep(2200); //程序挂起两秒
//  Sleep(2200); //程序挂起两秒
//  Sleep(1200); //程序挂起两秒
//  Go_to_Home();
//
//  //第四块挡板
//
//  Go_to_Number(36, 1);
//  cout << "开始放置第四块挡板" << endl;
//  Put_Dangban(36, 1);
//  cout << "第四块挡板放置成功" << endl;
//  Sleep(2200); //程序挂起两秒
//  Sleep(2200); //程序挂起两秒
//  Sleep(1200); //程序挂起两秒
//  Go_to_Home();
//
//
//  //第五块 挡板
//  Go_to_Number(37, 0);
//  cout << "开始放置第五块挡板" << endl;
//  Put_Dangban(37, 0);
//  cout << "第五块挡板放置成功" << endl;
//  Sleep(2200); //程序挂起两秒
//  Sleep(2200); //程序挂起两秒
//  Sleep(1200); //程序挂起两秒
//
//
//
//
//  //最终退场 
//  //1、后退20cm左右，
//  for (int i = 0; i <2; i++)
//  {
//      char Forward_flag2 = -1;//发送开环后退指令
//      char g_UartRxBuffer1[8] = { 123, -122, -121, 120, Forward_flag2, (char)(0), (char)(0), (char)Forward_flag2 };//针头（4），1个数据，一个验证
//      WriteChar(g_UartRxBuffer1, 8);
//  }
//  Sleep(1200); //程序挂起两秒
//  Sleep(1200); //程序挂起两秒
//  Sleep(1200); //程序挂起两秒
//
//  //3、转向180度
//  for (int i = 0; i <2; i++)
//  {
//      char Forward_flag2 = 18;//发送开环后退指令
//      char g_UartRxBuffer1[8] = { 123, -122, -121, 120, Forward_flag2, (char)(0), (char)(0), (char)Forward_flag2 };//针头（4），1个数据，一个验证
//      WriteChar(g_UartRxBuffer1, 8);
//  }
//  Sleep(1200); //程序挂起两秒
//  Sleep(2200); //程序挂起两秒   
//  Sleep(2200); //程序挂起两秒
//  //3、前进
//  for (int i = 0; i <4; i++)
//  {
//      char Forward_flag2 = 1;//发送开环后退指令
//      char g_UartRxBuffer1[8] = { 123, -122, -121, 120, Forward_flag2, (char)(0), (char)(0), (char)Forward_flag2 };//针头（4），1个数据，一个验证
//      WriteChar(g_UartRxBuffer1, 8);
//  }
//  Sleep(1200); //程序挂起两秒
//  Sleep(1200); //程序挂起两秒   
//  Sleep(1200); //程序挂起两秒
//
//  //3、停止
//  for (int i = 0; i <4; i++)
//  {
//      char Forward_flag2 = 0;//发送开环后退指令
//      char g_UartRxBuffer1[8] = { 123, -122, -121, 120, Forward_flag2, (char)(0), (char)(0), (char)Forward_flag2 };//针头（4），1个数据，一个验证
//      WriteChar(g_UartRxBuffer1, 8);
//  }
//  Sleep(1200); //程序挂起两秒
//  Sleep(1200); //程序挂起两秒   
//  Sleep(1200); //程序挂起两秒
//
//  printf("Finish!");
//  waitKey(0);
//  return 0;
//}