#include "find_decode.h"

//#define Debug
//#define lengthThres 50
//#define lengthThres1 100
//#define lengthThres2 37


ver_pair vps[100];
int L=0;
/**
  @brief 从分组线段中搜索出约成90度夹角的线段对
  @param [in] seg_mat   分组线段
  @param [in] seg_cnt   分组线段计数
  @param [in] lthThres1 线段长度下限
  @param [in] lthThres2 线段长度上限
  @param [out] vps      线段对数组
  @param [out] m        线段对计数
*/
void findVertcialLinesFromSeg_mat(line_template seg_mat[][segNumLimit/2], int seg_cnt[], int lthThres1,
                                  int lthThres2, ver_pair vps[], int &m)
{
    int cntL =0, cntB=0, cntBl=0, cntBr=0, cntBll=0, cntBrr=0;
    int v=0, vl=0, vr=0, vll=0, vrr=0;

    //float seg_theta =0.0;
    float segL_lth =0.0, segB_lth =0.0;
    const float deviAngleThres =6.0; //角度偏差阈值
    line_template *sp = NULL, *cp = NULL;
    bool b1,b2,b3,b4;
    m=0;
    for(int i=0; i<=30; i++)
    {
        v = (i+30)%60;
        vl = (i+29)%60;
        vr = (i+31)%60;
        vll = (i+28)%60;
        vrr = (i+32)%30;
        cntL = seg_cnt[i];
        cntB = seg_cnt[v];
        cntBl = seg_cnt[vl];
        cntBr = seg_cnt[vr];
        cntBll = seg_cnt[vll];
        cntBrr = seg_cnt[vrr];
        if(cntL && (cntBll || cntBl || cntB ||cntBr || cntBrr))
        {
            for(int j=0; j <cntL; j++)
            {
                sp = &seg_mat[i][j];
                segL_lth = sqrt((sp->s.col-sp->e.col)*(sp->s.col-sp->e.col) + (sp->s.row-sp->e.row)*(sp->s.row-sp->e.row));
                if(segL_lth >= lthThres1 && segL_lth <= lthThres2)
                {
                    for(int k=0; k <cntB; k++)
                    {
                        cp = &seg_mat[v][k];
                        if(fabs(fabs(sp->dir-cp->dir)-90.0) <= deviAngleThres){
                            segB_lth = sqrt((cp->s.col-cp->e.col)*(cp->s.col-cp->e.col) + (cp->s.row-cp->e.row)*(cp->s.row-cp->e.row));
                            b1 = (abs(sp->s.col-cp->s.col)+abs(sp->s.row-cp->s.row)) <= 2*lthThres1;
                            b2 = (abs(sp->s.col-cp->e.col)+abs(sp->s.row-cp->e.row)) <= 2*lthThres1;
                            b3 = (abs(sp->e.col-cp->s.col)+abs(sp->e.row-cp->s.row)) <= 2*lthThres1;
                            b4 = (abs(sp->e.col-cp->e.col)+abs(sp->e.row-cp->e.row)) <= 2*lthThres1;
                            if(segB_lth >= lthThres1 && segB_lth <= lthThres2 && (b1||b2||b3||b4)){
                                vps[m].L = *sp;
                                vps[m].B = *cp;
                                m++;
                            }
                        }
                    }
                    for(int k=0; k <cntBl; k++)
                    {
                        cp = &seg_mat[vl][k];
                        if(fabs(fabs(sp->dir-cp->dir)-90.0) <=deviAngleThres){
                            segB_lth = sqrt((cp->s.col-cp->e.col)*(cp->s.col-cp->e.col) + (cp->s.row-cp->e.row)*(cp->s.row-cp->e.row));
                            b1 = (abs(sp->s.col-cp->s.col)+abs(sp->s.row-cp->s.row)) <= 2*lthThres1;
                            b2 = (abs(sp->s.col-cp->e.col)+abs(sp->s.row-cp->e.row)) <= 2*lthThres1;
                            b3 = (abs(sp->e.col-cp->s.col)+abs(sp->e.row-cp->s.row)) <= 2*lthThres1;
                            b4 = (abs(sp->e.col-cp->e.col)+abs(sp->e.row-cp->e.row)) <= 2*lthThres1;
                            if(segB_lth >= lthThres1 && segB_lth <= lthThres2 && (b1||b2||b3||b4)){
                                vps[m].L = *sp;
                                vps[m].B = *cp;
                                m++;
                            }
                        }
                    }
                    for(int k=0; k <cntBr; k++)
                    {
                        cp = &seg_mat[vr][k];
                        if(fabs(fabs(sp->dir-cp->dir)-90.0) <=deviAngleThres){
                            segB_lth = sqrt((cp->s.col-cp->e.col)*(cp->s.col-cp->e.col) + (cp->s.row-cp->e.row)*(cp->s.row-cp->e.row));
                            b1 = (abs(sp->s.col-cp->s.col)+abs(sp->s.row-cp->s.row)) <= 2*lthThres1;
                            b2 = (abs(sp->s.col-cp->e.col)+abs(sp->s.row-cp->e.row)) <= 2*lthThres1;
                            b3 = (abs(sp->e.col-cp->s.col)+abs(sp->e.row-cp->s.row)) <= 2*lthThres1;
                            b4 = (abs(sp->e.col-cp->e.col)+abs(sp->e.row-cp->e.row)) <= 2*lthThres1;
                            if(segB_lth >= lthThres1 && segB_lth <= lthThres2 && (b1||b2||b3||b4)){
                                vps[m].L = *sp;
                                vps[m].B = *cp;
                                m++;
                            }
                        }
                    }
                    for(int k=0; k<cntBll; k++)
                    {
                        cp = &seg_mat[vll][k];
                        if(fabs(fabs(sp->dir-cp->dir)-90.0) <=deviAngleThres){
                            segB_lth = sqrt((cp->s.col-cp->e.col)*(cp->s.col-cp->e.col) + (cp->s.row-cp->e.row)*(cp->s.row-cp->e.row));
                            b1 = (abs(sp->s.col-cp->s.col)+abs(sp->s.row-cp->s.row)) <= 2*lthThres1;
                            b2 = (abs(sp->s.col-cp->e.col)+abs(sp->s.row-cp->e.row)) <= 2*lthThres1;
                            b3 = (abs(sp->e.col-cp->s.col)+abs(sp->e.row-cp->s.row)) <= 2*lthThres1;
                            b4 = (abs(sp->e.col-cp->e.col)+abs(sp->e.row-cp->e.row)) <= 2*lthThres1;
                            if(segB_lth >= lthThres1 && segB_lth <= lthThres2 && (b1||b2||b3||b4)){
                                vps[m].L = *sp;
                                vps[m].B = *cp;
                                m++;
                            }
                        }
                    }
                    for(int k=0; k<cntBrr; k++)
                    {
                        cp = &seg_mat[vrr][k];
                        if(fabs(fabs(sp->dir-cp->dir)-90.0) <=deviAngleThres){
                            segB_lth = sqrt((cp->s.col-cp->e.col)*(cp->s.col-cp->e.col) + (cp->s.row-cp->e.row)*(cp->s.row-cp->e.row));
                            b1 = (abs(sp->s.col-cp->s.col)+abs(sp->s.row-cp->s.row)) <= 2*lthThres1;
                            b2 = (abs(sp->s.col-cp->e.col)+abs(sp->s.row-cp->e.row)) <= 2*lthThres1;
                            b3 = (abs(sp->e.col-cp->s.col)+abs(sp->e.row-cp->s.row)) <= 2*lthThres1;
                            b4 = (abs(sp->e.col-cp->e.col)+abs(sp->e.row-cp->e.row)) <= 2*lthThres1;
                            if(segB_lth >= lthThres1 && segB_lth <= lthThres2 && (b1||b2||b3||b4)){
                                vps[m].L = *sp;
                                vps[m].B = *cp;
                                m++;
                            }
                        }
                    }
                }
            }
        }
    }
    //将vps尺度扩大一倍，作用于原始图像
    for(int i=0; i<m; i++)
    {
        vps[i].L.s.col *=2;
        vps[i].L.s.row *=2;
        vps[i].L.e.col *=2;
        vps[i].L.e.row *=2;
        vps[i].B.s.col *=2;
        vps[i].B.s.row *=2;
        vps[i].B.e.col *=2;
        vps[i].B.e.row *=2;
    }
}

#define testBlackThres (grayThres)
#define testWhiteThres (grayThres)
/**
  @brief 从直角对中寻找出L的左边和底边
**@brief DM码图像结构
    |口 口 口
    |       口
  L |   口
    |口     口
    |   口  口
    ——————————
        B
**@param [in] image         原始图像
**@param [in] auxSubIndex   遍历图像辅助矩阵
**@param [in] width         图像宽
**@param [in] height        图像高
**@param [out] vps          如上图，储存找到的DM的L线段、B线段
**@param [in] lthThres1     线段长度下限
*/
int find_LB_fromVerticalPairs(uchar image[], uint auxSubIndex[],ushort width, ushort height,
                              ver_pair vps[], int m, int lthThres1)
{
    //static Hposition_f L1,L2;
    position crossP, leftP, bottomP, midP, testP_in, testP_ex;
    position_f newCrossP_f;
    //position_f newCrossP;
    float lthTemp =0.0;
    uchar detDir;
    for(int i=0; i<m; i++)
    {
        vps[i].Lparam = cross_ip_fhp(vps[i].L.s, vps[i].L.e);
        vps[i].Bparam = cross_ip_fhp(vps[i].B.s, vps[i].B.e);
        //printf("s=(%d,%d), e=(%d,%d), L1=(%f,%f,%f)\n",vps[i].L.s.col, vps[i].L.s.row,vps[i].L.e.col, vps[i].L.e.row, L1.x, L1.y, L1.scale);
        //printf("s=(%d,%d), e=(%d,%d), L2=(%f,%f,%f)\n",vps[i].B.s.col, vps[i].B.s.row,vps[i].B.e.col, vps[i].B.e.row, L2.x, L2.y, L2.scale);
        //fflush(stdout);
        crossP = cross_fhp_ip(vps[i].Lparam, vps[i].Bparam);//初步确定L B交点

        if(manhattan(vps[i].L.s, crossP) > manhattan(vps[i].L.e, crossP)) //确定除L上除交点的外一点：左点
            leftP = vps[i].L.s;
        else
            leftP = vps[i].L.e;

        if(manhattan(vps[i].B.s, crossP) > manhattan(vps[i].B.e, crossP)) //确定除B上除交点的外一点：底点
            bottomP = vps[i].B.s;
        else
            bottomP = vps[i].B.e;


        midP.col = (leftP.col + bottomP.col)/2; //确定 左点 底点连线中点
        midP.row = (leftP.row + bottomP.row)/2;

        lthTemp = sqrt((midP.col-crossP.col)*(midP.col-crossP.col)+(midP.row-crossP.row)*(midP.row-crossP.row));
        testP_in.col = round(crossP.col + (midP.col-crossP.col)*5/lthTemp); //确定内测试点
        testP_in.row = round(crossP.row + (midP.row-crossP.row)*5/lthTemp);

        testP_ex.col = round(crossP.col - (midP.col-crossP.col)*5/lthTemp); //确定外测试点
        testP_ex.row = round(crossP.row - (midP.row-crossP.row)*5/lthTemp);

        if(meanGray(image, auxSubIndex, testP_in)<testBlackThres && meanGray(image, auxSubIndex, testP_ex)>testWhiteThres){
        //if(meanGray(image, auxSubIndex, testP_in)<(grayThres-10) && meanGray(image, auxSubIndex, testP_ex)>grayThres){
            //printf("L.s=(%d,%d), L.e=(%d,%d)\n",vps[i].L.s.col, vps[i].L.s.row, vps[i].L.e.col, vps[i].L.e.row);
            //printf("B.s=(%d,%d), B.e=(%d,%d)\n",vps[i].B.s.col, vps[i].B.s.row, vps[i].B.e.col, vps[i].B.e.row);
            vps[i].L.s = crossP;
            //vps[i].L.e = leftP;
            vps[i].L.e = calacE(crossP, leftP, L_lth);//根据L边的长度和起点计算两边终点
            vps[i].B.s = crossP;
            //vps[i].B.e = bottomP;
            vps[i].B.e = calacE(crossP, bottomP, L_lth);
            //printf("L is %d.\n",i);
            //printf("leftP=(%d,%d), midP=(%d,%d), bottomP=(%d,%d)\n",leftP.col, leftP.row, midP.col, midP.row, bottomP.col, bottomP.row);
            //fflush(stdout);
            adjustLB(leftP, midP, bottomP, crossP, vps[i]);

            detDir = calacDetDir(vps[i].L, vps[i].LquadC, true);
            adjustAngle(image, auxSubIndex, width, height, vps[i].L, vps[i].Lparam, detDir);

            detDir = calacDetDir(vps[i].B, vps[i].BquadC, false);
            adjustAngle(image, auxSubIndex, width, height, vps[i].B, vps[i].Bparam, detDir);//对left和bottom的角度进行调整

            newCrossP_f = cross_fhp_fp(vps[i].Lparam, vps[i].Bparam);
            crossP.col = (int)round(newCrossP_f.x);
            crossP.row = (int)round(newCrossP_f.y);
            vps[i].L.s = crossP;
            vps[i].B.s = crossP;
            vps[i].CrossP_f = newCrossP_f;

//            printf("crossP=(%d,%d)\n",crossP.col, crossP.row);
//            printf("newCrossP=(%f,%f)\n",newCrossP_f.x, newCrossP_f.y);
//            fflush(stdout);
            //if(1){
            if(adjustLBEndpoint(image, auxSubIndex, width, height, vps[i])){
                //printf("bottomP_f=(%f,%f).\n",vps[i].bottomP_f.x, vps[i].bottomP_f.y);
                //printf("leftP_f=(%f,%f).\n",vps[i].leftP_f.x, vps[i].leftP_f.y);
                //fflush(stdout);
                detDir = calacDetDir(vps[i].L, vps[i].LquadC, true);
                adjustAngle(image, auxSubIndex, width, height, vps[i].L, vps[i].Lparam, detDir);
                detDir = calacDetDir(vps[i].B, vps[i].BquadC, false);
                adjustAngle(image, auxSubIndex, width, height, vps[i].B, vps[i].Bparam, detDir);

                newCrossP_f = cross_fhp_fp(vps[i].Lparam, vps[i].Bparam);
                crossP.col = (int)round(newCrossP_f.x);
                crossP.row = (int)round(newCrossP_f.y);
                vps[i].L.s = crossP;
                vps[i].B.s = crossP;
                vps[i].CrossP_f = newCrossP_f;

                if(adjustLBEndpoint(image, auxSubIndex, width, height, vps[i])){
                    float lastL_lth = L_lth;
                    uchar lastGrayThres = grayThres;
                    //L_lth = updateL_lth(vps[i].CrossP_f, vps[i].leftP_f, vps[i].bottomP_f);
                    if(DmRegionCheck(image, auxSubIndex, width, height, vps[i], L_lth, grayThres, agvPoseFromDM)){
                        //L_lth = updateL_lth(vps[i].CrossP_f, vps[i].leftP_f, vps[i].bottomP_f);
                        cout<< L_lth <<" "<< (int)grayThres << endl;
                        samPS[19] = testP_in;
                        samPS[20] = testP_ex;
                        return i;
                    }else{
                        L_lth = lastL_lth;
                        grayThres = lastGrayThres;
                    }
//                    printf("crossP=(%f,%f)\n",vps[i].CrossP_f.x, vps[i].CrossP_f.y);
//                    printf("bottomP_f=(%f,%f).\n",vps[i].bottomP_f.x, vps[i].bottomP_f.y);
//                    printf("leftP_f=(%f,%f).\n",vps[i].leftP_f.x, vps[i].leftP_f.y);
                }else{
                    cout << "adjust endpoint failed."<<endl;
                }
            }else{
                cout << "adjust endpoint failed."<<endl;
            }
        }else
            cout<< "find no DM bar"<<endl;
    }
    return -1;
}
/**
  @brief 检查当前约成90度夹角的线段对所包含的区域\n
        是否属于DM码数据区，如果属于，则进行解码，和\n
        解算相机位姿
  @param [in] image         原始输入图像
  @param [in] auxSubIndex   用于遍历图像的辅助计算数组
  @param [in] width         图像宽
  @param [in] height        图像高
  @param [in] vp            约成90度夹角的线段对
  @param [in] L_lth         DM LB边长度
  @param binaryThres        二值化阈值
  @param [out] POSE         相机位姿
  @retval false             不属于
  @retval true              解算成功
*/
bool DmRegionCheck(uchar image[], uint auxSubIndex[],ushort width, ushort height,
                   ver_pair &vp, float L_lth, uchar &binaryThres, pose &POSE)
{
    uchar grayTest[19];
    memset(grayTest,0,19);
    float scale = L_lth/20,theta=0.0;
    int grayThres=0;
    position tempPos={0,0};
    signed char testSum=0;

    //cout<<"vp.L.dir ="<<vp.L.dir<<" vp.LquadC"<<(int)vp.LquadC<<endl;
    //float temp = sqrt(vp.Lparam.x*vp.Lparam.x+vp.Lparam.y*vp.Lparam.y);
    //float cosTheta = fabs(vp.Lparam.y)/temp;
    //float sinTheta = -vp.Lparam.x/temp;
    if(vp.LquadC==2)
        theta = 180 + vp.L.dir;
    else if(vp.LquadC==0)
        theta = -180 + vp.L.dir;
    else
        theta = vp.L.dir;

    float cosTheta = cos(theta*3.1416/180);
    float sinTheta = sin(theta*3.1416/180);

    for(int i=0;i<19;i++){
        tempPos.col = (int)round(vp.CrossP_f.x + locP[i].col*scale*cosTheta - locP[i].row*scale*sinTheta);
        tempPos.row = (int)round(vp.CrossP_f.y + locP[i].col*scale*sinTheta + locP[i].row*scale*cosTheta);
        //grayTest[i] = image[auxSubIndex[tempPos.row] + tempPos.col];
        if(tempPos.col <=0 || tempPos.col>=(width-1) || tempPos.row <=0 || tempPos.row >=(height-1))
            return false;
        grayTest[i] = meanGray(image, auxSubIndex, tempPos);
        grayThres += grayTest[i];
        samPS[i] = tempPos;
    }

    for(int i=1; i<19; i++,i++)
    {
        grayTest[i] = grayTest[i] >binaryThres;
        grayTest[i-1] = grayTest[i-1] >binaryThres;
        testSum +=grayTest[i]-grayTest[i-1];
        //printf("%d, %d\n",grayTest[i-1], grayTest[i]);
    }
    if(testSum <=7){
        //cout<<(int)testSum<<endl;
        return false;

    }else{
        grayThres /=19;
        binaryThres = grayThres; //更新灰度阈值
        //cout << "update gray thres:"<<(int)binaryThres<<endl;
    }
    uchar num[8]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};
    for(int i=0; i<64; i++)
    {
        tempPos.col = (int)round(vp.CrossP_f.x + DatP[i].col*scale*cosTheta-DatP[i].row*scale*sinTheta);
        tempPos.row = (int)round(vp.CrossP_f.y + DatP[i].col*scale*sinTheta+DatP[i].row*scale*cosTheta);
        if(tempPos.col <=0 || tempPos.col>=(width-1) || tempPos.row <=0 || tempPos.row >=(height-1))
            return false;
        num[i/8] |= (meanGray(image, auxSubIndex, tempPos) <binaryThres) <<(7-i%8);
        //samPS[i] = tempPos;
    }
    POSE.x = num[0]-130;
    POSE.y = num[1]-130;
    POSE.theta = theta;
    //坐标系变换
    POSE.x = POSE.x + 0.034/L_lth*(320.5 - vp.CrossP_f.x); //这里要乘以 系数 = 实际长度/L_lth
    POSE.y = POSE.y + 0.034/L_lth*(240.5 - vp.CrossP_f.y);
    POSE.theta = -POSE.theta;
    return true;
}

/**
  @brief 确定LB边的端点
  @param leftP      L边的端点(非与B边交点)
  @param midP       辅助判断点
  @param bottomP    B边的端点(非与L边交点)
  @param crossP     LB边交点
  @param vp         约成90度夹角的线段对
*/
void adjustLB(position &leftP, position &midP, position &bottomP, position &crossP, ver_pair &vp)
{
    vp.LquadC = (leftP.col >= crossP.col) + ((leftP.row >= crossP.row)<<1);
    vp.BquadC = (bottomP.col >= crossP.col) + ((bottomP.row >= crossP.row)<<1);
    uchar LquadM = (leftP.col >= midP.col) + ((leftP.row >= midP.row)<<1);
    //uchar BquadM = (bottomP.col >= midP.col) + ((bottomP.row >= midP.row)<<1);
    //qDebug()<<"Mquad="<<Mquad<<", Lquad="<<Lquad<<", Bquad="<<Bquad<<".";
    //printf("LquadC=%d, BquadC=%d, LquadM=%d\n",vp.LquadC, vp.BquadC, LquadM);
    bool isLine1Left = false;

    if(3 == vp.LquadC){
        if(2 == vp.BquadC)
            isLine1Left = true;
        else if(3 == vp.BquadC && 1 == LquadM)
            isLine1Left = true;
    }else if(2 == vp.LquadC){
        if(0 == vp.BquadC)
            isLine1Left = true;
        else if(2 == vp.BquadC && 3 == LquadM)
            isLine1Left = true;
    }else if(0 == vp.LquadC){
        if(1 == vp.BquadC)
            isLine1Left = true;
        else if(0 == vp.BquadC && 2 == LquadM)
            isLine1Left = true;
    }else if(1 == vp.LquadC){
        if(3 == vp.BquadC)
            isLine1Left = true;
        else if(1 == vp.BquadC && 0 == LquadM)
            isLine1Left = true;
    }
    if(!isLine1Left){
        Hposition_f temp;
        temp = vp.Lparam;
        vp.Lparam = vp.Bparam;
        vp.Bparam = temp;
        line_template temp2;
        temp2 = vp.L;
        vp.L = vp.B;
        vp.B = temp2;
        uchar temp3;
        temp3 = vp.BquadC;
        vp.BquadC = vp.LquadC;
        vp.LquadC = temp3;
    }
}
/**
  @brief 根据先验的边的长度和起点计算边终点
  @param [in] S     线段边起点
  @param [in] oldE  旧的线段的终点
  @param [in] lth   先验的边长度
  @return 返回重算过的线段的终点
*/
position calacE(position S, position oldE, float lth)
{
    if(lth>0){
        position newE;
        float k=0.0,temp=0.0;
        short diff_Y = oldE.row - S.row;
        short diff_X = oldE.col - S.col;
        if(0==diff_X){
            newE.col = oldE.col;
            if(diff_Y >0)
                newE.row = S.row + lth;
            else
                newE.row = S.row - lth;
        }else{
            k = float(diff_Y)/float(diff_X);
            temp = sqrt(lth*lth/(1+k*k));
            if(diff_X>0){
                newE.col = S.col + temp;
                newE.row = S.row + int(k*temp);
            }else{
                newE.col = S.col - temp;
                newE.row = S.row - int(k*temp);
            }

//                if(diff_Y>0)
//                    newE.row = S.row + k*abs(newE.col - S.col);
//                else
//                    newE.row = S.row - k*abs(newE.col - S.col);
        }
//        printf("k=%f, temp=%f, lth=%f, S=(%d,%d), OldE=(%d,%d),newE=(%d,%d)\n",k, temp, lth, S.col, S.row, oldE.col, oldE.row, newE.col, newE.row);
//        fflush(stdout);
        return newE;
    }else
        return oldE;
}
float updateL_lth(position_f &C, position_f &L, position_f &B)
{
    double lth = sqrt((L.x-C.x)*(L.x-C.x)+(L.y-C.y)*(L.y-C.y));

    lth += sqrt((B.x-C.x)*(B.x-C.x)+(B.y-C.y)*(B.y-C.y));
//    printf("%lf\n",lth/2);
//    fflush(stdout);
    return lth/2;
}
/**
  @brief 为获取亚像素精度，调整垂直直线对的终点(已知直线对相交点和方向)
  @param [in] image         原始图像
  @param [in] auxSubIndex   图像遍历辅助矩阵
  @param [in] width         图像宽
  @param [in] height        图像高
  @param vp                 待调整直线对（结构体）
  @param retval false       调整失败
  @param retval true        调整成功
*/
bool adjustLBEndpoint(uchar image[], uint auxSubIndex[], ushort width, ushort height, ver_pair &vp)
{
    short diff_X = vp.B.e.col - vp.B.s.col;
    short diff_Y = vp.B.e.row - vp.B.s.row;
    position detectP;
    const uchar gap =1;
    short cntUpper = abs(diff_X) > abs(diff_Y) ? abs(diff_X)/gap : abs(diff_Y)/gap;
    bool breakB_Flag = false, breakL_Flag = false;
    float temp;
    //printf("%d \n",cntUpper);
    //fflush(stdout);
//    for(int i=0; i<100; i++){
//        samPS[i].col =0;
//        samPS[i].row =0;
//    }
    for(int i=cntUpper-2; i <=2*cntUpper; i++)//注意这里从-2开始
    //for(int i=-2; i <=cntUpper; i++)//注意这里从-2开始
    {
        if(abs(diff_X)>abs(diff_Y))
        {
            if(diff_X >0.0)
                //detectP.col = vp.B.e.col + i*gap; //使用终点作为起始点检测点位置错误，还没看出原因
                temp = vp.CrossP_f.x + i*gap;
            else
                //detectP.col = vp.B.e.col - i*gap;
                temp = vp.CrossP_f.x - i*gap;
            //detectP.row = vp.B.e.row + (int)round((vp.B.e.col-detectP.col)*vp.Bparam.x/vp.Bparam.y);
            detectP.row = (int)round(vp.CrossP_f.y + (vp.CrossP_f.x-temp)*vp.Bparam.x/vp.Bparam.y);
            detectP.col = (int)round(temp);
        }else{
            if(diff_Y>0.0)
                //detectP.row = vp.B.e.row + i*gap;
                temp = (int)round(vp.CrossP_f.y + i*gap);
            else
                //detectP.row = vp.B.e.row - i*gap;
                temp = (int)round(vp.CrossP_f.y - i*gap);
            //detectP.col = vp.B.e.col + (int)round((vp.B.e.row-detectP.row)*vp.Bparam.y/vp.Bparam.x);
            detectP.col = (int)round(vp.CrossP_f.x + (vp.CrossP_f.y-temp)*vp.Bparam.y/vp.Bparam.x);
            detectP.row = (int)round(temp);
        }
//        printf("(%d,%d) ",detectP.col,detectP.row);
//        fflush(stdout);
        if(detectP.row <0 || detectP.row >=height || detectP.col <0 || detectP.col >= width)//已经到达图像边缘，剩余的点也不满足要求
        {
//            printf("adjust endpoint %d valid points.\n",i-cntUpper+2);
//            fflush(stdout);
            break;
        }
        //samPS[i+2-cntUpper] = detectP;
        //samPS[i+2] = detectP;

        if(verify(image, auxSubIndex, width, height, detectP,false, grayThres, 1)){
            if(abs(diff_X)>abs(diff_Y)){
                if(diff_X>0)
                    vp.bottomP_f.x = detectP.col-3*gap;
                else
                    vp.bottomP_f.x = detectP.col+3*gap;
                vp.bottomP_f.y = vp.CrossP_f.y + (vp.CrossP_f.x-vp.bottomP_f.x)*vp.Bparam.x/vp.Bparam.y;
            }else{
                if(diff_Y>0)
                    vp.bottomP_f.y = detectP.row-3*gap;
                else
                    vp.bottomP_f.y = detectP.row+3*gap;
                vp.bottomP_f.x = vp.CrossP_f.x + (vp.CrossP_f.y-vp.bottomP_f.y)*vp.Bparam.y/vp.Bparam.x;
            }
            vp.B.e.col = (int)round(vp.bottomP_f.x);
            vp.B.e.row = (int)round(vp.bottomP_f.y);
            breakB_Flag = true;

//            printf("adjust endpoint %d valid points.\n",i-cntUpper+3);
//            fflush(stdout);
            break;
        }
    }

//    for(int i=0; i<sampNum; i++){
//        printf("(%d,%d) ",samPS[i].col,samPS[i].row);
//    }
//    printf("\n");
//    fflush(stdout);

    diff_X = vp.L.e.col - vp.L.s.col;
    diff_Y = vp.L.e.row - vp.L.s.row;
    cntUpper = abs(diff_X) > abs(diff_Y) ? abs(diff_X)/gap : abs(diff_Y)/gap;
    for(int i=cntUpper-2; i <=2*cntUpper; i++)//注意这里从-2开始
    //for(int i=-2; i <=cntUpper; i++)//注意这里从-2开始
    {
        if(abs(diff_X)>abs(diff_Y))
        {
            if(diff_X >0.0)
                //detectP.col = vp.B.e.col + i*gap; //使用终点作为起始点检测点位置错误，还没看出原因
                temp = vp.CrossP_f.x + i*gap;
            else
                //detectP.col = vp.B.e.col - i*gap;
                temp = vp.CrossP_f.x - i*gap;
            //detectP.row = vp.B.e.row + (int)round((vp.B.e.col-detectP.col)*vp.Bparam.x/vp.Bparam.y);
            detectP.row = (int)round(vp.CrossP_f.y + (vp.CrossP_f.x-temp)*vp.Lparam.x/vp.Lparam.y);
            detectP.col = (int)round(temp);
        }else{
            if(diff_Y>0.0)
                //detectP.row = vp.B.e.row + i*gap;
                temp = (int)round(vp.CrossP_f.y + i*gap);
            else
                //detectP.row = vp.B.e.row - i*gap;
                temp = (int)round(vp.CrossP_f.y - i*gap);
            //detectP.col = vp.B.e.col + (int)round((vp.B.e.row-detectP.row)*vp.Bparam.y/vp.Bparam.x);
            detectP.col = (int)round(vp.CrossP_f.x + (vp.CrossP_f.y-temp)*vp.Lparam.y/vp.Lparam.x);
            detectP.row = (int)round(temp);
        }
//        printf("(%d,%d) ",detectP.col,detectP.row);
//        fflush(stdout);
        if(detectP.row <0 || detectP.row >=height || detectP.col <0 || detectP.col >= width)//已经到达图像边缘，剩余的点也不满足要求
        {
//            printf("adjust endpoint %d valid points.\n",i-cntUpper+2);
//            fflush(stdout);
            break;
        }
        //samPS[i+2-cntUpper] = detectP;
        //samPS[i+2] = detectP;

        if(verify(image, auxSubIndex, width, height, detectP, false, grayThres, 1)){
            if(abs(diff_X)>abs(diff_Y)){
                if(diff_X>0)
                    vp.leftP_f.x = detectP.col-3*gap;
                else
                    vp.leftP_f.x = detectP.col+3*gap;
                vp.leftP_f.y = vp.CrossP_f.y + (vp.CrossP_f.x-vp.leftP_f.x)*vp.Lparam.x/vp.Lparam.y;
            }else{
                if(diff_Y>0)
                    vp.leftP_f.y = detectP.row-3*gap;
                else
                    vp.leftP_f.y = detectP.row+3*gap;
                vp.leftP_f.x = vp.CrossP_f.x + (vp.CrossP_f.y-vp.leftP_f.y)*vp.Lparam.y/vp.Lparam.x;
            }
            vp.L.e.col = (int)round(vp.leftP_f.x);
            vp.L.e.row = (int)round(vp.leftP_f.y);
            breakL_Flag = true;
            break;
        }
    }
    if(breakB_Flag && breakL_Flag)
        return true;
    else
        return false;
}
/**
  @brief 根据线段和所属象限，计算L边或者B边的方向编码
  @param [in] l             线段
  @param [in] quad          线段在图像坐标中的象限
  @param [in] isLeftFlag    l是否属于DM定位区左边
  @param 方向编码
*/
uchar calacDetDir(line_template &l, uchar quad, bool isLeftFlag)
{
    short diff_X = l.e.col-l.s.col;
    short diff_Y = l.e.row-l.s.row;
    uchar detDir;
    if(abs(diff_X)>abs(diff_Y)){
        if(2==quad || 0==quad)
            detDir = 0x00 | (0x0F & (!isLeftFlag));
        else
            detDir = 0x00 | (0x0F & isLeftFlag);
    }else{
        if(0==quad || 1==quad)
            detDir = 0x10 | (0x0F & isLeftFlag);
        else
            detDir = 0x10 | (0x0F & (!isLeftFlag));
    }
//    printf("detDir=%d, quad=%d, isLeftFlag=%d\n", detDir, quad, isLeftFlag);
//    fflush(stdout);
    //printf("detDir = %d %d %d %d \n",0x00 | (0x0F & (!isLeftFlag)), 0x00 | (0x0F & isLeftFlag), 0x10 | (0x0F & isLeftFlag), detDir = 0x10 | (0x0F & (!isLeftFlag)));
    return detDir;
}
/**
  @brief 根据图像边缘信息，对直线的角度进行调整
  @param [in] image         原始图像
  @param [in] auxSubIndex   辅助矩阵
  @param [in] width         图像宽
  @param [in] height        图像高
  @param [in] l             线段(结构体)
  @param [in] lparam        线段齐次参数（浮点）
  @param [in] detDir        方向编码
*/
void adjustAngle(uchar image[], uint auxSubIndex[], ushort width, ushort height,
                 line_template &l, Hposition_f &lparam, uchar detDir)
{
    bool aa;
    short diff_X = l.e.col-l.s.col;
    short diff_Y = l.e.row-l.s.row;
    const uchar sampGap =3;
    uchar sampNum;
    if(abs(diff_X)>abs(diff_Y))
        sampNum = abs(diff_X)/3-2;
    else
        sampNum = abs(diff_Y)/3-2;
    int k=0;
    for(int i=0; i<100; i++){
        samPS[i].col =0;
        samPS[i].row =0;
    }
//    printf("diff_X = %d, diff_Y = %d, sample points num = %d\n",diff_X, diff_Y, sampNum);
//    fflush(stdout);
    samplePoints(l.s, l.e, sampNum, sampGap, samPS);

//    printf("line.s=(%d,%d), line.e=(%d,%d)\n", l.s.col, l.s.row, l.e.col, l.e.row);
//    fflush(stdout);
//    for(int i=0; i<sampNum; i++){
//        printf("(%d,%d) ",samPS[i].col,samPS[i].row);
//    }
//    printf("\n");
    for(int i=0; i<sampNum; i++){
        aa = verify(image, auxSubIndex, width, height, samPS[i], detDir, grayThres, 0);
//        printf("%d / %d\n",i, sampNum);
//        fflush(stdout);
        if(aa){
            samPS[k] = samPS[i];
            k++;
        }
    }
//    printf("%d valid points.\n",k);
//    fflush(stdout);
    //最小二乘法求解新角度
    LSnewAngle(samPS, k, l, lparam);
}

/*寻找周围过渡点，拟合直线*/
//signed char coor5X5[16][2] = {{0,2},{-1,2},{-2,2},{-2,1},{-2,0},{-2,-1},{-2,-2},{-1,-2},{0,-2},{1,-2},{2,-2},{2,-1},{2,0},{2,1},{2,2},{1,2}};
/**
  @brief 验证目标点是否是边缘点或者空白点
  @param image          原始图像
  @param auxSubIndex    图像遍历辅助矩阵
  @param width          图像宽
  @param height         图像宽
  @param dstP           目标点
  @param detDir         检测点局部区域横纵方向，0：竖向 1：横向
  @param binaryThres    局部二值化阈值
  @param pointType      检测点类型：0：边缘点，1：空白点
  @retval true          是边缘点
  @retval false         不是边缘点
*/
bool verify(uchar image[], uint auxSubIndex[], ushort width, ushort height, position &dstP,
            uchar detDir,uchar binaryThres, uchar pointType)
{
    //static uchar recoArr[32];
    uchar segNum=0, segLth=0, idx_s=0;
    int i, j, margin=0,margin1=0;
    //float mid1=0, mid2=0;
    position p;
    bool breakFlag = false;
    //static int k;
    if(0==pointType){
        if((detDir >>4)==0){
            margin =0;
            if((detDir & 0x0F)==1)
                margin1 = 7;
            else
                margin1 = -7;
                //margin1 = 7;
        }else{
            if((detDir & 0x0F)==1)
                margin = 7;
            else
                margin = -7;
                //margin = 7;
            margin1 = 0;
        }
    }else if(1==pointType){
        margin =0;
        margin1 =0;
    }
//    printf("detDir=%d, margin=%d, margin1=%d\n",detDir, margin, margin1);
//    fflush(stdout);
    for( i =dstP.col-margin; ;)
    {
        //if(i <(margin+1) || i >=(width-margin-1))
        if(i <=1 || i >=(width-2))
            break; //mark:"break" relpaces "continue";
        for( j =dstP.row -margin1; ;)
        {
            //if(j<(margin1+1) || j >=(height-margin1-1))
            if(j<=1 || j >=(height-2))
                break;
            p.col =i;
            p.row =j;
//            printf("%d, %d\n",i,j);
//            fflush(stdout);

//            if(1==pointType){
//                printf("%d\n",k);
//                samPS[k++] = p;
//            }

            //calcSegNumSegLth(image, auxSubIndex, p, binaryThres, 5, segNum, segLth, idx_s);
            if(0==pointType){ //边缘点
                calcSegNumSegLth(image, auxSubIndex, p, binaryThres, 5, segNum, segLth, idx_s);
                if(segNum ==1 && segLth >=7 && segLth <=9){
                    breakFlag = true;
                    dstP = p;
                    break;
                }
            }else if(1==pointType){ //空白点
                calcSegNumSegLth(image, auxSubIndex, p, binaryThres, 5, segNum, segLth, idx_s);
                if(segNum ==0)
                //if(segNum ==1 && segLth >=4 && segLth <=7)
                {
                    //mid1 = (float)idx_s + (float)segLth/2 - 0.5;
                    //calcSegNumSegLth(image, auxSubIndex, p, binaryThres, 3, segNum, segLth, idx_s); //直角点检测需要加以3Ｘ３模板辅助
                    //if(segNum ==0)
                    //if(segNum ==1 && segLth >=3 && segLth <=4)
                    {
                        //mid2 = 2*idx_s + segLth-1;
                        //printf("%.2f,%.2f\n",mid1, mid2);
                        //fflush(stdout);
                        //if(fabs(mid1-mid2) <=1){
                            breakFlag = true; //满足上述条件，通过直角点检测
                            dstP = p;
                            break;
                        //}
                    }
                }
            }
            if(j==dstP.row +margin1)
                break;
            else
                j = margin1>0? j+1:j-1;
        }
        if(breakFlag)
            break;
        if(i==dstP.col+margin)
            break;
        else
            i = margin>0? i+1:i-1;
    }
    if(breakFlag)
        return true;
    else
        return false;
}
/**
  @brief
  @param image      原始图像
  @param softSize   检测模板大小，3：3Ｘ３，5：5Ｘ5
  @param dstP       目标点
  @param binaryThres局部二值化阈值
  @param softSize   模板大小
  @param segNum     检测到的段数
  @param segLth     检测到的第一段长度
*/
void calcSegNumSegLth(uchar image[], uint auxSubIndex[], position &dstP, uchar binaryThres, uchar softSize,
                      uchar &segNum, uchar &segLth, uchar &idx_s)
{
    uchar softLth =16, lth =0;
    position p;
    uchar *recoArr = NULL;
    signed char *softP = coor5X5[0];
    segNum = 0;
    segLth = 0;
    idx_s = 0;
    if(3 == softSize){
        softLth = 8;
        softP = coor[0];
    }else if(5 == softSize){
        softLth = 16;
        softP = coor5X5[0];
    }
    recoArr = (uchar *)malloc(softLth*2);

    for(int k=0; k<softLth; k++){
        //p.row = j + coor5X5[k][0];
        //p.col = i + coor5X5[k][1];
        p.row = dstP.row + *(softP+k*2);
        p.col = dstP.col + *(softP+k*2+1);
        //cout <<"dstP=("<<dstP.col<<","<<dstP.row<<") "<< "p=("<<p.col<<","<<p.row<<")"<<endl;
        recoArr[k] = image[auxSubIndex[p.row]+p.col] < binaryThres; //0:黑 1：白
    }
    while(!recoArr[idx_s] && idx_s <softLth)
        idx_s++;

    if(idx_s>0){
        for(int k=0; k <idx_s; k++)
            recoArr[softLth+k] = recoArr[k];
    }

    lth = softLth+idx_s;
    recoArr[lth] = 1;

    for(int k=1; k <lth; k++)
    {
        if(recoArr[k-1] ==1 && recoArr[k]==0){
            segNum++;
            if(segNum ==1)
                idx_s = k; //记录起始黑色点
            else
                break;
            while(!recoArr[k]){
                segLth++;
                k++;
            }
        }
    }
//    if(5==softSize)
//    {
//    for(int k=0; k <lth; k++)
//    {
//        printf("%d ",recoArr[k]);
//        fflush(stdout);
//    }
//    printf("soft=%dX%d, segNum=%d, segLth=%d, idx_s=%d\n\n",softSize, softSize, segNum, segLth, idx_s);
//    fflush(stdout);
//    }
}
/**
  @breif 对一条直线上进行等间距点采样
  @param s          直线起点
  @param e          直线终点
  @param sampNum    采样点数
  @param sampGap    采样间隔
  @param sps        采样点存储数组
*/
void samplePoints(position &s, position &e, const uchar sampNum, const uchar sampGap, position sps[])
{
    float diff_X = e.col - s.col;
    float diff_Y = e.row - s.row;
    if(fabs(diff_X) >= fabs(diff_Y))
    {
        if(diff_X >0.0){
            for(int i=0; i<sampNum; i++)
                sps[i].col = s.col +sampGap*(i+2);
        }else{
            for(int i=0; i<sampNum; i++)
                sps[i].col = s.col -sampGap*(i+2);
        }
        for(int i=0; i<sampNum; i++)
            sps[i].row = s.row + (int)round((sps[i].col-s.col)/diff_X*diff_Y);
    }else{
        if(diff_Y>0.0){
            for(int i=0; i<sampNum; i++)
                sps[i].row = s.row +sampGap*(i+2);
        }else{
            for(int i=0; i<sampNum; i++)
                sps[i].row = s.row -sampGap*(i+2);
        }
        for(int i=0; i<sampNum; i++)
            sps[i].col = s.col + (int)round((sps[i].row - s.row)/diff_Y*diff_X);
    }
}
/**
  @brief 最小二乘法拟合线段
  @param sps    有效的采样点
  @param l      直线（结构体）
  @param lparam 直线齐次坐标参数
*/
void LSnewAngle(position sps[], int spsLth, line_template &l, Hposition_f &lparam)
{
    long A=0, B=0, C=0, D=0;
    double k, b, tmp =0.0;
    for(int i=0; i<spsLth; i++)
    {
        A += sps[i].col*sps[i].col;
        B += sps[i].col;
        C += sps[i].col*sps[i].row;
        D += sps[i].row;
    }
    tmp=A*spsLth-B*B;
    if(tmp){
        k = (double)(C*spsLth-B*D)/tmp;
        b = (double)(A*D-C*B)/tmp;
//        printf("%f, %f\n", kk, b);
        lparam.x = k;
        lparam.y = -1;
        lparam.scale = b;
        l.dir = (float)atan(k)*180/3.141593;
//        printf("%f\n",l.dir);
//        fflush(stdout);
//        printf("line new param %d, %d, %d\n",lparam.x, lparam.y, lparam.scale);
//        fflush(stdout);
    }else{
        lparam.x = 1;
        lparam.y = 0;
        lparam.scale = -sps[0].col;
        l.dir = 90.0;
    }
}
///*@brief 根据检测到的freeman直线提取Ｌ并解码
//  @param image  原始图像
//  @param seg    freeman线段数组
//  @param segCnt freeman线段个数
//  @param lthThres1  线段筛选阈值下限
//  @param lthThres2  线段筛选阈值上限
//*/
//bool locateLeft(uchar *image, uint *auxSubIndex, line_template (&seg)[segNumLimit],int &segCnt, int lthThres1, int lthThres2, position &crossP, int &leftLineIdx, int &bottomLineIdx, float &theta)
//{
////    int i=0,j=0;
////    bool finishFlag=false;
////    for( ;i<segCnt;i++)
////    {
////        if(seg[i].pixNum >lthThres1 && seg[i].pixNum < lthThres2)
////        {
////            j=i+1;
////            for( ;j<segCnt;j++)
////            {
////                if(verify(image, auxSubIndex, seg, crossP, theta, leftLineIdx, bottomLineIdx, i, j, lthThres1, lthThres2)){
////#ifdef Debug
////                    printf("line is %d and %d, cross point is (x,y)=(%d,%d)\n",i, j, crossP.col, crossP.row);
////                    fflush(stdout);
////#endif
////                    finishFlag = true;
////                    return finishFlag;
////                }
////            }
////        }
////    }
////    return finishFlag;
// }

/*@brief 解码求相机位姿
  @param image  原始图像
  @param theta  角度
  @param lth    Ｌ边准确长度
  @param crossP Ｌ边交点
  @param POSE   Ｌ解码的相机位姿
*/
//#define PLOT

bool decode(uchar *image, uint *auxSubIndex, float &theta, float &lth, position &crossP, pose &POSE)
{
    uchar grayTest[19];
    memset(grayTest,0,19);
    float scale = lth/20;
    int grayThres=0;
    //int tempCol=0, tempRow=0;
    position tempPos={0,0};
    uchar testSum=0;
    for(int i=0;i<19;i++){
        tempPos.col = crossP.col + locP[i].col*scale*cos(theta)-locP[i].row*scale*sin(theta);
        tempPos.row = crossP.row + locP[i].col*scale*sin(theta)+locP[i].row*scale*cos(theta);
        grayTest[i] = image[auxSubIndex[tempPos.row] + tempPos.col];
        grayThres += grayTest[i];

#ifdef PLOT
    drawPs[i] = tempPos;
    printf("detect point (%d,%d)\n",locP[i].col, locP[i].row);
#endif
    }
    grayThres /=19;
#ifdef Debug
    printf("gray threshold =%d\n",grayThres);
#endif
    for(int i=1; i<19; i++,i++)
    {
        grayTest[i] = grayTest[i] >grayThres;
        grayTest[i-1] = grayTest[i-1] >grayThres;
        testSum +=grayTest[i]-grayTest[i-1];
#ifdef Debug
        printf("%d, %d\n",grayTest[i-1], grayTest[i]);
    //qDebug()<<grayTest[i-1]<<" "<<grayTest[i];
#endif
    }
    if(testSum <7)
        return false;

    uchar num[8]={0x00,0x00,0x00,0x00,0x00,0x00,0x00,0x00};

    for(int i=0; i<8; i++)
    {
        tempPos.col = crossP.col + DatP[8*i].col*scale*cos(theta)-DatP[8*i].row*scale*sin(theta);
        tempPos.row = crossP.row + DatP[8*i].col*scale*sin(theta)+DatP[8*i].row*scale*cos(theta);
        num[i] |= (meanGray(image, auxSubIndex, tempPos) <grayThres) <<7;
#ifdef PLOT
        drawDPs[8*i] = tempPos;
#endif
        //grayDat[8*i] =image[preComput[tempRow]+tempCol] <grayThres;

        tempPos.col = crossP.col + DatP[8*i+1].col*scale*cos(theta)-DatP[8*i+1].row*scale*sin(theta);
        tempPos.row = crossP.row + DatP[8*i+1].col*scale*sin(theta)+DatP[8*i+1].row*scale*cos(theta);
        num[i] |= (meanGray(image, auxSubIndex, tempPos) <grayThres) <<6;
#ifdef PLOT
        drawDPs[8*i+1] = tempPos;
#endif
        //grayDat[8*i+1] =image[preComput[tempRow]+tempCol] <grayThres;

        tempPos.col = crossP.col + DatP[8*i+2].col*scale*cos(theta)-DatP[8*i+2].row*scale*sin(theta);
        tempPos.row = crossP.row + DatP[8*i+2].col*scale*sin(theta)+DatP[8*i+2].row*scale*cos(theta);
        num[i] |= (meanGray(image, auxSubIndex, tempPos) <grayThres) <<5;
#ifdef PLOT
        drawDPs[8*i+2] = tempPos;
#endif

        //grayDat[8*i+2] =image[preComput[tempRow]+tempCol] <grayThres;
        tempPos.col = crossP.col + DatP[8*i+3].col*scale*cos(theta)-DatP[8*i+3].row*scale*sin(theta);
        tempPos.row = crossP.row + DatP[8*i+3].col*scale*sin(theta)+DatP[8*i+3].row*scale*cos(theta);
        num[i] |= (meanGray(image, auxSubIndex, tempPos) <grayThres) <<4;
#ifdef PLOT
        drawDPs[8*i+3] = tempPos;
#endif
        //grayDat[8*i+3] =image[preComput[tempRow]+tempCol] <grayThres;

        tempPos.col = crossP.col + DatP[8*i+4].col*scale*cos(theta)-DatP[8*i+4].row*scale*sin(theta);
        tempPos.row = crossP.row + DatP[8*i+4].col*scale*sin(theta)+DatP[8*i+4].row*scale*cos(theta);
        num[i] |= (meanGray(image, auxSubIndex, tempPos) <grayThres) <<3;
#ifdef PLOT
        drawDPs[8*i+4] = tempPos;
#endif

        //grayDat[8*i+4] =image[preComput[tempRow]+tempCol] <grayThres;

        tempPos.col = crossP.col + DatP[8*i+5].col*scale*cos(theta)-DatP[8*i+5].row*scale*sin(theta);
        tempPos.row = crossP.row + DatP[8*i+5].col*scale*sin(theta)+DatP[8*i+5].row*scale*cos(theta);
        num[i] |= (meanGray(image, auxSubIndex, tempPos) <grayThres) <<2;
#ifdef PLOT
        drawDPs[8*i+5] = tempPos;
#endif

        //grayDat[8*i+5] =image[preComput[tempRow]+tempCol] <grayThres;

        tempPos.col = crossP.col + DatP[8*i+6].col*scale*cos(theta)-DatP[8*i+6].row*scale*sin(theta);
        tempPos.row = crossP.row + DatP[8*i+6].col*scale*sin(theta)+DatP[8*i+6].row*scale*cos(theta);
        num[i] |= (meanGray(image, auxSubIndex, tempPos) <grayThres)<<1;
#ifdef PLOT
        drawDPs[8*i+6] = tempPos;
#endif

        //grayDat[8*i+6] =image[preComput[tempRow]+tempCol] <grayThres;

        tempPos.col = crossP.col + DatP[8*i+7].col*scale*cos(theta)-DatP[8*i+7].row*scale*sin(theta);
        tempPos.row = crossP.row + DatP[8*i+7].col*scale*sin(theta)+DatP[8*i+7].row*scale*cos(theta);
        num[i] |= (meanGray(image, auxSubIndex, tempPos) <grayThres);
#ifdef PLOT
        drawDPs[8*i+7] = tempPos;
#endif

        //grayDat[8*i+7] =image[preComput[tempRow]+tempCol] <grayThres;

        //qDebug()<<grayDat[8*i]<<grayDat[8*i+1]<<grayDat[8*i+2]<<grayDat[8*i+3]<<grayDat[8*i+4]<<grayDat[8*i+5]<<grayDat[8*i+6]<<grayDat[8*i+7];
        //qDebug()<<"num"<<i<<"="<<num[i]+0;
    }
    POSE.x = num[0]-130;
    POSE.y = num[1]-130;
    POSE.theta = theta;
    return true;
}
/*@brief 计算ｐ点周围8点像素的均值
  @param image  原始图像
  @param theta  角度
  @param p      位置
*/
uchar meanGray(uchar *image, uint *auxSubIndex, position &p)
{
    int sum=image[auxSubIndex[p.row]+p.col];
    for(int i=0; i<8; i++){
        sum += image[auxSubIndex[p.row+(int)coor[i][0]] + p.col+(int)coor[i][1]];
    }
    return sum/9;
}