SCBT_Edge.cpp

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <opencv2/opencv.hpp>

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/SVD>
#include <eigen3/Eigen/Dense>

#include <boost/config.hpp>
#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <boost/graph/graph_traits.hpp>
#include <boost/graph/iteration_macros.hpp>
#include <boost/graph/properties.hpp>
#include <boost/property_map/property_map.hpp>

#include "Timer.h"
//#include "LS.h"

#include <iostream>
#include <fstream>
#include <vector>
#include <cmath>
#include <map>
#include <ctime>
#include <sstream>

//#include "Timer.h"
#include "EdgeMap.h"
#include "EDLib.h"

#include "CommonFunctions.h"

#define DivideErrThre_e 1.4//edge breaking threshold
#define DivideErrThre_m 5
#define BufferThre 20//Bowl(30) GarbageBin(30) buffer area width
#define edgeLenThre 12//Bowl(15) GarbageBin(25)//edges longer than 12 pixels are saved
#define areaCnst 0.9//Bowl(0.9) GarbageBin(0.9) area similarity threshold
#define lenCnst 0.9//Bowl(0.9) GarbageBin(0.9) length similarity threshold
#define ddOverLen 0.7 //Bowl(0.7) GarbageBin(0.7) MarkCup(1) BookStnd(1) MarkCupContour 0.7
#define searchLineThre 1.0//possible threhosld: GarbagBin(0.3)

//(w_len*length+w_dd*distDiff)/length = w_len + w_dd*distDiff/length
#define w_dd 1.0
#define w_len 1.0


using namespace cv;
using namespace std;
//using namespace Eigen;


struct lessXY{
    bool operator()(const int* lhs, const int*rhs) const{
        return (lhs[0] == rhs[0]) ? (lhs[1] < rhs[1]) : (lhs[0] < rhs[0]);
    }
};

struct EdgeFragment{//divided edge fragments from detected edge segments
    //index
    int seg_id;//edge fragements belong to the seg_id th detected segment
    int start_idx;// its start index in the seg_id th detected segment
    int end_idx;// its end index in the seg_id th detected segment
};

struct GraphEdge{//edge constructed by detected edge fragments

    EdgeFragment EF;

    Pixel sp;
    Pixel ep;

    float length;
    float distDiff;
};

struct GraphEdgeDT{//edge constructed generated by delaunay triangulation

    int s_se;//start or end point of the GraphEdge
    int e_se;

    float length;
    float distDiff;
};

struct Graph_GE_GEDT{
    vector<GraphEdge> graphEdge;
    vector<GraphEdgeDT> graphEdgeDT;
    std::map<int*, int, lessXY> mapGEDT;
};

struct Graph_GEDT{//store the GEDT part of the graph optimization result
    //int idx;//indicates the prior EF index in EF vector
    Pixel sp;
    Pixel ep;
};

struct Graph_RSLT{
    vector<EdgeFragment> EFs;
    vector<Graph_GEDT> GEDTs;
    float area;
    float length;
};


struct GraphEdgeUniform{
    int seg_id1;
    int pixel_idx1;

    int seg_id2;
    int pixel_idx2;

    float length;
    float distDiff;
};

double totalTime = 0;

int video_proc = 0;//video_proc: 0 read from webcam, 1 read from video file

int k, pk=1;
int pki=1;
bool tracking_flg = false;

vector<Point> points; //contour of pre-shape
Point pt_lbd, pt_mv;
int mbd = 0;//flag of right button click

double ave_fps = 0;//average fps

void CallBackFunc(int event, int x, int y, int flags, void* userdata)
{
    if  ( event == EVENT_LBUTTONDOWN )
    {
        //Point* pt = (Point*) userdata;
        if(!tracking_flg){
            if(mbd == 0){
                pt_lbd.x = x;
                pt_lbd.y = y;
                points.push_back(pt_lbd);
            }
        }
        //cout << "Left button of the mouse is clicked - position (" << x << ", " << y << ")" << endl;
    }
    else if  ( event == EVENT_RBUTTONDOWN )
    {

        //cout << "Right button of the mouse is clicked - position (" << x << ", " << y << ")" << endl;
    }
    else if  ( event == EVENT_MBUTTONDOWN )
    {
        mbd = 1;//stop polygon selection
        //cout << "Middle button of the mouse is clicked - position (" << x << ", " << y << ")" << endl;
    }
    else if ( event == EVENT_MOUSEMOVE )
    {

        if(!tracking_flg){
            pt_mv.x = x;
            pt_mv.y = y;
        }

    }

    return;
}

//measure the least square error of line fitting
int EdgeBreakFit(float xyh[3][4]){
    /*
    //normalize coordinates of pixels
    VectorXf centroid = xyh.rowwise().mean();

    MatrixXf trslt_xyh = xyh.colwise() - centroid;
    double mean_dist = (trslt_xyh.array() * trslt_xyh.array()).colwise().sum().sqrt().mean();
    double norm_scale = sqrt(2) / mean_dist;
    MatrixXf norm_xyh;
    norm_xyh = trslt_xyh*norm_scale;

    //save inverse of normalizing transformation matrix
    MatrixXf inv_norm_mat(3,3);
    inv_norm_mat.setIdentity();

    inv_norm_mat(0, 0) = inv_norm_mat(1, 1) = (1.0 / norm_scale);edgeLenThre
    inv_norm_mat(0, 2) = centroid(0);
    inv_norm_mat(1, 2) = centroid(1);

    MatrixXf A;
    A = MatrixXf::Ones(3, norm_xyh.cols());
    A.row(0) = norm_xyh.row(0);
    A.row(1) = norm_xyh.row(1);

    //JacobiSVD<MatrixXf> svd(A.transpose(), ComputeFullU | ComputeFullV);
    //MatrixXf V = svd.matrixV();
    */

    //Timer timer;
    //timer.Start();

    //Vector3f v1(xyh[0][0],xyh[1][0],1);
    //Vector3f v2(xyh[0][2],xyh[1][2],1);
    //Vector3f V = v1.cross(v2);
    //MatrixXf abcX(1,1);
    float abcXe = 0;//end point test
    float abcXm = 0;//middle point test

    //Vector3f edpt(xyh[0][3],xyh[1][3],1);

    float a=0, b=0, c=0;
    a = xyh[1][0]-xyh[1][2];
    b = xyh[0][2]-xyh[0][0];
    c = xyh[0][0]*xyh[1][2]-xyh[1][0]*xyh[0][2];

    //timer.Stop();
    //totalTime = totalTime + timer.ElapsedTime();
    //cout<<" time of each leastSquareLineFit "<<totalTime<<endl;

    //Timer timer;
    //timer.Start();

    //abcX = V.transpose()*edpt;//(1*1);
    abcXe = a*xyh[0][3]+b*xyh[1][3]+c;
    abcXm = a*xyh[0][1]+b*xyh[1][1]+c;

    //timer.Stop();
    //cout<<" time of each leastSquareLineFit "<<timer.ElapsedTime()<<endl;


    //float err = (abcX.cwiseAbs().sum())/sqrt(pow(V(0),2)+pow(V(1),2));
    //float err = (abcX.cwiseAbs().sum())/(xyh.cols()*sqrt(pow(V(0),2)+pow(V(1),2)));

    float erre = abs(abcXe)/sqrt(pow(a,2)+pow(b,2));
    float errm = abs(abcXm)/sqrt(pow(a,2)+pow(b,2));

    //cout<<" abc: "<<a<<" "<<b<<" "<<c;
    //cout<<" xe: "<<xyh[0][3]<<" "<<xyh[1][3];
    //cout<<" xm: "<<xyh[0][1]<<" "<<xyh[1][1];
    //cout<<" erre: "<<erre;
    //cout<<" errm: "<<errm<<endl;

    //float *err;
    //err = new float[2];
    //err[0] = erre;
    //err[1] = errm;

    if(erre<DivideErrThre_e && errm<DivideErrThre_m){
        return 1;
    }else{
        return 0;
    }

    //return err;
}

//divide an edge segment into fragements
/*vector<EdgeFragment> DivideIntoEdgeFragements(EdgeMap *map){

    vector<EdgeFragment> dividedEF;//dmapGEDTivided Edge Fragments

    for (int i = 0; i < map->noSegments; i++){

        int iEgStrt = 0;
        int newEdge = 1;// start a new small edge segment searching

        //vector<Pixel> Pixel_tmp;
        EdgeFragment EF_tmp;
        //MatrixXf pts;//3*4
        float PT[3][4];

        if(map->segments[i].noPixels < 5){
            EF_tmp.seg_id = i;
            EF_tmp.start_idx = iEgStrt;
            EF_tmp.end_idx = map->segments[i].noPixels-1;
            dividedEF.push_back(EF_tmp);
        }else{
            //every two new pixels will be tested by line fitframe_edgeting
            for (int iEgEnd = 4; iEgEnd < map->segments[i].noPixels;){

                if(newEdge){//start a new small edge segment searching

                    EF_tmp.seg_id = i;
                    EF_tmp.start_idx = iEgStrt;

                    //add the start pixel
                    //pts = MatrixXf::OmapGEDTnes(3, 4);//start pixel, middle pixel, the thir/*d last pixel and the last pixel
                    //pts(0,0) = map->segments[i].pixels[iEgStrt].r;
                    //pts(1,0) = map->segments[i].pixels[iEgStrt].c;
                    //pts(2,0) = 1;
                    PT[0][0] = map->segments[i].pixels[iEgStrt].r;
                    PT[1][0] = map->segments[i].pixels[iEgStrt].c;
                    PT[2][0] = 1;
                }

                PT[0][1] = map->segments[i].pixels[(iEgEnd-iEgStrt)/2].r;
                PT[1][1] = map->segments[i].pixels[(iEgEnd-iEgStrt)/2].c;
                PT[2][1] = 1;

                PT[0][2] = map->segments[i].pixels[iEgEnd-2].r;
                PT[1][2] = map->segments[i].pixels[iEgEnd-2].c;
                PT[2][2] = 1;

                PT[0][3] = map->segments[i].pixels[iEgEnd].r;
                PT[1][3] = map->segments[i].pixels[iEgEnd].c;
                PT[2][3] = 1;


                //add the middle pixel
                //pts(0,1) = map->segments[i].pixels[(iEgEnd-iEgStrt)/2].r;
                //pts(1,1) = map->segments[i].pixels[(iEgEnd-iEgStrt)/2].c;
                //pts(2,1) = 1;

                //pts(0,2) = map->segments[i].pixels[iEgEnd-2].r;
                //pts(1,2) = map->segments[i].pixels[iEgEnd-2].c;
                //pts(2,2) = 1;

                //pts(0,3) = map->segments[i].pixels[iEgEnd].r;
                //pts(1,3) = map->segments[i].pixels[iEgEnd].c;
                //pts(2,3) = 1;

                if(EdgeBreakFit(PT)<DivideErrThre){

                    if(iEgEnd < map->segments[i].noPixels-2){
                        newEdge = 0;
                        iEgEnd +=2;
                    }else{
                        newEdge = 1;
                        EF_tmp.end_idx = map->segments[i].noPixels-1;

                        dividedEF.push_back(EF_tmp);// find the last small edge segment
                        break;
                    }
nlines
                }else{
                    newEdge = 1;//start a new small edge
                    EF_tmp.end_idx = iEgEnd - 2;
                    dividedEF.push_back(EF_tmp);// find a new small edge segment

                    iEgStrt = iEgEnd-1;
                    EF_tmp.start_idx =  iEgStrt;
                    if(map->segments[i].noPixels-iEgStrt < 5){// there are less than 5 remaining pixels in this edge segment
                        EF_tmp.end_idx = map->segments[i].noPixels-1;
                        dividedEF.push_back(EF_tmp);
                        break;
                    }else{// there are less than 3 remaining pixels in this edge segment and start a new small edge segment searching
                        iEgEnd = iEgStrt + 4;
                    }
                }//end if(LeastSquaresLineFit < DividedErrThre){} else{}
            } //end-for map->noSegments
        }//if(map->segments[i].noPixels < 5)
    } //end-for map->segments[i]
return dividedEF;
}
*/

vector<EdgeFragment> EdgeFilterBreak(EdgeMap *map, Mat Dist){

    vector<EdgeFragment> EdgeFilters;//divided Edge Fragments
//cout<<" Debug----------------3.1 "<<endl;
    //edge segments filtering according to buffer region of the prior shape
    for(int i = 0; i < map->noSegments; i++){

        int iEgStrt = -1;
        EdgeFragment EdgeFilter;
        //float PT[3][4];

        for (int j = 0; j < map->segments[i].noPixels; j++){
            if(Dist.at<float>(map->segments[i].pixels[j].r,map->segments[i].pixels[j].c) < BufferThre){
                if(iEgStrt==-1){
                    iEgStrt = j;
                }else{
                    if(j == (map->segments[i].noPixels-1)){
                        EdgeFilter.seg_id = i;
                        EdgeFilter.start_idx = iEgStrt;
                        EdgeFilter.end_idx = j;

                        EdgeFilters.push_back(EdgeFilter);
                        iEgStrt = -1;
                    }
                }
            }else{
                if(iEgStrt!=-1){
                    EdgeFilter.seg_id = i;
                    EdgeFilter.start_idx = iEgStrt;
                    EdgeFilter.end_idx = j - 1;

                    EdgeFilters.push_back(EdgeFilter);
                    iEgStrt = -1;
                }
            }
        }//for(int j)mapGEDT
    }//for(int i)


    //output for paper editing===========================
/*
                int rc, gc, bc;
                int lowest=100, highest=255;
                int range=(highest-lowest)+1;

                Mat all_edges;
                all_edges = Mat::zeros(Dist.size().height, Dist.size().width, CV_8UC3);////frame.clone();
                for(int i = 0; i<EdgeFilters.size(); i++){

                    int tmpi = EdgeFilters[i].seg_id;

                    rc = lowest+int(rand()%range);
                    gc = lowest+int(rand()%range);
                    bc = lowest+int(rand()%range);

                    for( int j = EdgeFilters[i].start_idx; j<EdgeFilters[i].end_idx + 1;j++){
                        int r = map->segments[tmpi].pixels[j].r;
                        int c = map->segments[tmpi].pixels[j].c;

                        Vec3b & color = all_edges.at<Vec3b>(r,c);
                        color[0] = bc;
                        color[1] = gc;
                        color[2] = rc;
                    }
                }

                imwrite("Buffer_edges.png",all_edges);
                cvWaitKey(0);
*/    //output for paper editing--------------------------

//cout<<" Debug----------------3.2 "<<endl;
//--------------------------------------------------------------------------
    //edge segments breaking according to "turning distance"
    vector<EdgeFragment> dividedEF;//divided Edge Fragments
    for(int i = 0; i < EdgeFilters.size(); i++){

        int newEdge = 1;// start a new small edge segment searching

        int iEgStrt = EdgeFilters[i].start_idx;
        EdgeFragment EF_tmp;
        //EF_tmp.seg_id = EdgeFilters[i].seg_id;

        float PT[3][4];

        int noPixels = EdgeFilters[i].end_idx-EdgeFilters[i].start_idx+1;

        if(noPixels < 5 && noPixels > 1){
            /*EF_tmp.seg_id = EdgeFilters[i].seg_id;
            EF_tmp.start_idx = iEgStrt;
            EF_tmp.end_idx = EdgeFilters[i].end_idx;noSegments
            dividedEF.push_back(EF_tmp);*/
        }else{

            //every two new pixels will be tested by line fitting
            for (int iEgEnd = iEgStrt + 4; iEgEnd < EdgeFilters[i].end_idx + 1;){

                if(newEdge){//start a new small edge segment searching

                    EF_tmp.seg_id = EdgeFilters[i].seg_id;
                    EF_tmp.start_idx = iEgStrt;

                    PT[0][0] = map->segments[EdgeFilters[i].seg_id].pixels[iEgStrt].r;
                    PT[1][0] = map->segments[EdgeFilters[i].seg_id].pixels[iEgStrt].c;
                    PT[2][0] = 1;
                }

                PT[0][1] = map->segments[EdgeFilters[i].seg_id].pixels[(iEgEnd+iEgStrt)/2].r;
                PT[1][1] = map->segments[EdgeFilters[i].seg_id].pixels[(iEgEnd+iEgStrt)/2].c;
                PT[2][1] = 1;

                PT[0][2] = map->segments[EdgeFilters[i].seg_id].pixels[iEgEnd-2].r;
                PT[1][2] = map->segments[EdgeFilters[i].seg_id].pixels[iEgEnd-2].c;
                PT[2][2] = 1;

                PT[0][3] = map->segments[EdgeFilters[i].seg_id].pixels[iEgEnd].r;
                PT[1][3] = map->segments[EdgeFilters[i].seg_id].pixels[iEgEnd].c;
                PT[2][3] = 1;

                if(EdgeBreakFit(PT)==1/*<DivideErrThre_e*/){

                    if(iEgEnd < EdgeFilters[i].end_idx - 1 ){
                        newEdge = 0;
                        iEgEnd +=2;
                    }else{
                        newEdge = 1;
                        EF_tmp.end_idx = EdgeFilters[i].end_idx;
                        if((EF_tmp.end_idx-EF_tmp.start_idx+1)>edgeLenThre/*EF_tmp.end_idx!=EF_tmp.start_idx*/){//
                            dividedEF.push_back(EF_tmp);// find the last small edge segment
                        }
                        break;
                    }

                }else{
                    newEdge = 1;//start a new small edge
                    EF_tmp.end_idx = iEgEnd - 2;
                    if((EF_tmp.end_idx-EF_tmp.start_idx+1)>edgeLenThre){//
                        dividedEF.push_back(EF_tmp);// find a new small edge segment
                    }//


                    iEgStrt = iEgEnd-1;
                    EF_tmp.start_idx =  iEgStrt;
                    if(EdgeFilters[i].end_idx-iEgStrt < 4){// there are less than 5 remaining pixels in this edge segment
                        EF_tmp.end_idx = EdgeFilters[i].end_idx;
                        if((EF_tmp.end_idx-EF_tmp.start_idx+1)>edgeLenThre/*EF_tmp.end_idx!=EF_tmp.start_idx*/){
                            dividedEF.push_back(EF_tmp);
                        }
                        break;
                    }else{// there are less than 3 remaining pixels in this edge segment and start a new small edge segment searching
                        iEgEnd = iEgStrt + 4;
                    }
                }//end if(LeastSquaresLineFit < DividedErrThre){} else{}

            }//for(int iEgEnd)
        }//if(noPixels < 5)
    }//for(int i)


    //output for paper editing===========================
/*
                Mat broken_edges;
                broken_edges = Mat::zeros(Dist.size().height, Dist.size().width, CV_8UC3);////frame.clone();
                for(int i = 0; i<dividedEF.size(); i++){

                    int tmpi = dividedEF[i].seg_id;

                    rc = lowest+int(rand()%range);
                    gc = lowest+int(rand()%range);
                    bc = lowest+int(rand()%range);

                    for( int j = dividedEF[i].start_idx; j<dividedEF[i].end_idx + 1;j++){
                        int r = map->segments[tmpi].pixels[j].r;
                        int c = map->segments[tmpi].pixels[j].c;

                        Vec3b & color = broken_edges.at<Vec3b>(r,c);
                        color[0] = bc;
                        color[1] = gc;
                        color[2] = rc;
                    }
                }

                imwrite("broken_edges.png",broken_edges);
                cvWaitKey(0);
*/    //output for paper editing--------------------------

//cout<<" Debug----------------3.3 "<<endl;

//=============handle overlapping end points=================
    Mat dst;//test overlapping pixels
    dst = Mat::zeros(Dist.size().height,Dist.size().width,CV_8UC1);
//cout<<" Debug----------------3.4 "<<endl;
    for(int i = 0; i < dividedEF.size(); i++){
//cout<<i<<" of "<<dividedEF.size()<<endl;
        int tmpID = dividedEF[i].seg_id;
        int sidx = dividedEF[i].start_idx;
        int eidx = dividedEF[i].end_idx;
//cout<<" Debug----------------3.5 "<<endl;
        int rs = map->segments[tmpID].pixels[sidx].r;
        int cs = map->segments[tmpID].pixels[sidx].c;
//cout<<" Debug----------------3.6 "<<endl;
        if(dst.at<unsigned char>(rs,cs)==0){
            dst.at<unsigned char>(rs,cs)=1;
        }else{
            while(dst.at<unsigned char>(rs,cs)==1 && sidx<eidx){
                sidx++;
                rs = map->segments[tmpID].pixels[sidx].r;
                cs = map->segments[tmpID].pixels[sidx].c;
            }//while
            dividedEF[i].start_idx = sidx;
            dst.at<unsigned char>(rs,cs)=1;
        }

        int re = map->segments[tmpID].pixels[eidx].r;
        int ce = map->segments[tmpID].pixels[eidx].c;
//cout<<" Debug----------------3.7 "<<endl;
        if(dst.at<unsigned char>(re,ce)==0){
            dst.at<unsigned char>(re,ce)=1;
        }else{
            while(dst.at<unsigned char>(re,ce)==1 && eidx>sidx){
                eidx--;
                re = map->segments[tmpID].pixels[eidx].r;
                ce = map->segments[tmpID].pixels[eidx].c;
            }//while
            dividedEF[i].end_idx = eidx;
            dst.at<unsigned char>(re,ce)=1;
        }//else
//cout<<" Debug----------------3.8 "<<endl;
    }//for
//cout<<" Debug----------------3.9 "<<endl;

return dividedEF;
}


Graph_GE_GEDT GraphConstruct(EdgeMap *map, vector<EdgeFragment> edgeFragments, Mat Dist,Mat frame_edge){
//transform the edge fragments into graph edges and compute their weights: length and distance differences
//map: detected edges by Edge Drawing, edgeFragments: filtered and broken edge fragments, Dist: distance tranform of the prior shape

//Timer timer_solid;
//timer_solid.Start();

    Graph_GE_GEDT graph_GE_GEDT;//store graph edge created by detected edges and Delaunay Triangulation
    vector<GraphEdge> graphEdges;
    //end points of edges for create delaunay triangulation
    vector<Point2f> points;

    //create a map between points and their indices
    std::map<int*, int, lessXY> mappts;
    int* spoint;
    int* epoint;
//cout<<" =======================solid edges======================"<<endl;
    int num_EF = 0;
    for(int i = 0; i < edgeFragments.size(); i++){
        //cout<<" edge len: "<<(edgeFragments[i].end_idx - edgeFragments[i].start_idx + 1)<<endl;
        //if((edgeFragments[i].end_idx - edgeFragments[i].start_idx + 1) > edgeLenThre){
            int tmp_id = edgeFragments[i].seg_id;

            float tmp_l = 0;//temp length
            float tmp_dd = 0;//temp distance difference
            for(int j = edgeFragments[i].start_idx; j < edgeFragments[i].end_idx; j++){
                int rj = map->segments[tmp_id].pixels[j].r;
                int cj = map->segments[tmp_id].pixels[j].c;

                int rj1 = map->segments[tmp_id].pixels[j+1].r;
                int cj1 = map->segments[tmp_id].pixels[j+1].c;

                tmp_dd = tmp_dd + abs(Dist.at<float>(rj,cj)-Dist.at<float>(rj1,cj1));

                if((abs(rj-rj1)+abs(cj-cj1)) == 2){//compute the length
                    tmp_l = tmp_l + sqrt(2);
                }else{
                    tmp_l = tmp_l + 1;
                }
            }//for(int j)

            if(tmp_dd/tmp_l < ddOverLen){

            GraphEdge tmp_GE;
            tmp_GE.EF = edgeFragments[i];
            tmp_GE.sp = map->segments[tmp_id].pixels[edgeFragments[i].start_idx];
            tmp_GE.ep = map->segments[tmp_id].pixels[edgeFragments[i].end_idx];

            //get end points for delaunay triangulation
            points.push_back(Point2f(float(tmp_GE.sp.c),float(tmp_GE.sp.r)));
            points.push_back(Point2f(float(tmp_GE.ep.c),float(tmp_GE.ep.r)));
            //cout<<" i: "<<i<<endl;

            //map of i and end point
            spoint = new int[2];
            spoint[0] = tmp_GE.sp.c;//x
            spoint[1] = tmp_GE.sp.r;//y
            mappts[spoint] = 2*num_EF;//2*i;

//cout<<"endpt: "<<2*i<<" "<<spoint[0]<<" "<<spoint[1]<<endl;
            epoint = new int[2];
            epoint[0] = tmp_GE.ep.c;
            epoint[1] = tmp_GE.ep.r;
            mappts[epoint] = 2*num_EF+1;//2*i+1;
//cout<<"endpt: "<<2*i+1<<" "<<epoint[0]<<" "<<epoint[1]<<endl;
            //cout<<" i: "<<i<<" 2*i: "<<2*i<<" 2*i+1: "<<2*i+1<<endl;

/*            float tmp_l = 0;//temp length
            float tmp_dd = 0;//temp distance difference
            for(int j = edgeFragments[i].start_idx; j < edgeFragments[i].end_idx; j++){
                int rj = map->segments[tmp_id].pixels[j].r;
                int cj = map->segments[tmp_id].pixels[j].c;

                int rj1 = map->segments[tmp_id].pixels[j+1].r;
                int cj1 = map->segments[tmp_id].pixels[j+1].c;

                tmp_dd = tmp_dd + abs(Dist.at<float>(rj,cj)-Dist.at<float>(rj1,cj1));

                if((abs(rj-rj1)+abs(cj-cj1)) == 2){//compute the length
                    tmp_l = tmp_l + sqrt(2);
                }else{
                    tmp_l = tmp_l + 1;
                }
            }//for(int j)*/
            tmp_GE.length = tmp_l;
            tmp_GE.distDiff = tmp_dd;
            graphEdges.push_back(tmp_GE);
            num_EF++;
            }
        //}

    }//for(int i)
//cout<<" mappts.size(): "<<mappts.size()<<endl;

//timer_solid.Stop();
//cout<<" Graph Construction Solid: "<<timer_solid.ElapsedTime()<<endl;


    //----------------------------------------------------------------------
    //Use Delaunay Triangulation to generate gap filling line edge fragments
//Timer timer_DT;
//timer_DT.Start();

    Rect rect(0,0,Dist.size().width,Dist.size().height);
    Subdiv2D subdiv(rect);
    subdiv.insert(points);

    vector<Vec4f> edgeList;
    subdiv.getEdgeList(edgeList);//edgeList[i]: sp.x,sp.y,epx,epy

//timer_DT.Stop();
//cout<<" Delaunay Triangualtion: "<<timer_DT.ElapsedTime()<<endl;


Timer timer_dashed;
timer_dashed.Start();

    int width = Dist.size().width;
    int height = Dist.size().height;

    vector<GraphEdgeDT> graphEdgesDT;//save edges generated by delaunay triangulation
    std::map<int*, int, lessXY> mapGEDT;//map nodes id to index, given node id retrive the corresponding edge id
    int *nid;
    //cout<<" points.size: "<<points.size()<<" mappts size: "<<mappts.size()<<endl;
    int* p1;
    int* p2;
//cout<<" ==============================dashed edges======================="<<endl;
    //Mat frame_DT;//debugging-----------------
    for(int i = 0; i < edgeList.size(); i++){
//cout<<" i "<<i<<" size(): "<<edgeList.size()<<endl;
        //frame_DT = frame_edge.clone();//debugging--------------

        GraphEdgeDT tmp_GEDT;

        p1 = new int[2];
        p2 = new int[2];
        p1[0] = int(edgeList[i][0]);//x
        p1[1] = int(edgeList[i][1]);//y
        p2[0] = int(edgeList[i][2]);
        p2[1] = int(edgeList[i][3]);

        if(mappts[p1]/2 != mappts[p2]/2){
            tmp_GEDT.s_se = mappts[p1];
            tmp_GEDT.e_se = mappts[p2];

            nid = new int[2];
            if(tmp_GEDT.s_se < tmp_GEDT.e_se){
                nid[0] = tmp_GEDT.s_se;
                nid[1] = tmp_GEDT.e_se;
            }else{
                nid[0] = tmp_GEDT.e_se;
                nid[1] = tmp_GEDT.s_se;
            }

            mapGEDT[nid] = graphEdgesDT.size();

            tmp_GEDT.length = distance(float(points[tmp_GEDT.s_se].x),float(points[tmp_GEDT.s_se].y),
                    float(points[tmp_GEDT.e_se].x),float(points[tmp_GEDT.e_se].y));

            LineIterator it(Dist, points[tmp_GEDT.s_se], points[tmp_GEDT.e_se], 8);

            float tmp_distDiff = 0;
            for(int l = 0; l < it.count - 1; l++){
                Point pt_tmp1 = it.pos();
                it++;
                Point pt_tmp2 = it.pos();
                tmp_distDiff = tmp_distDiff + abs(Dist.at<float>(pt_tmp1.y,pt_tmp1.x) - Dist.at<float>(pt_tmp2.y,pt_tmp2.x));
            }
            tmp_GEDT.distDiff = tmp_distDiff;
            graphEdgesDT.push_back(tmp_GEDT);
        }
    }//for(int i)

    graph_GE_GEDT.graphEdge = graphEdges;
    graph_GE_GEDT.graphEdgeDT = graphEdgesDT;
    graph_GE_GEDT.mapGEDT = mapGEDT;

//timer_dashed.Stop();
//cout<<" Dashed Edges: "<<timer_dashed.ElapsedTime()<<endl;

    return graph_GE_GEDT;

}

//Graph_RSLT
Graph_RSLT GraphOptimize(EdgeMap *map, Graph_GE_GEDT graph_GE_GEDT,float areaPrior,float lenPrior, Mat frame_edge){//optimization

//=================================Debugging==================================
    //test weight of each edgeedgeLenThre
/*    Mat frame_wef,frame_wdt;
    frame_wef = frame_edge.clone();
    for(int i = 0; i< graph_GE_GEDT.graphEdge.size(); i++){
        for(int tt = graph_GE_GEDT.graphEdge[i].EF.start_idx; tt < graph_GE_GEDT.graphEdge[i].EF.end_idx + 1; tt++){
            int tmpi = graph_GE_GEDT.graphEdge[i].EF.seg_id;

            int r = map->segments[tmpi].pixels[tt].r;
            int c = map->segments[tmpi].pixels[tt].c;

            Vec3b & color = frame_wef.at<Vec3b>(r,c);
            color[0] = 0;
            color[1] = 0;
            color[2] = 255;

        }
        cout<<"Edge len: "<<graph_GE_GEDT.graphEdge[i].length<<" distDiff: "<<graph_GE_GEDT.graphEdge[i].distDiff<<" r: "<<
              graph_GE_GEDT.graphEdge[i].distDiff/graph_GE_GEDT.graphEdge[i].length<<" dd/r^2: "
           <<graph_GE_GEDT.graphEdge[i].distDiff/(graph_GE_GEDT.graphEdge[i].length*graph_GE_GEDT.graphEdge[i].length)<<endl;
        namedWindow("frame_wef",1);
        imshow("frame_wef",frame_wef);
        cvWaitKey(0);
    }

    for(int i = 0; i< graph_GE_GEDT.graphEdgeDT.size(); i++){

        frame_wdt = frame_edge.clone();

        int s_se,e_se;
        int r1,c1,r2,c2;
        s_se = graph_GE_GEDT.graphEdgeDT[i].s_se;
        e_se = graph_GE_GEDT.graphEdgeDT[i].e_se;

        if(s_se%2==0){
            r1 = graph_GE_GEDT.graphEdge[s_se/2].sp.r;
            c1 = graph_GE_GEDT.graphEdge[s_se/2].sp.c;
        }else{
            r1 = graph_GE_GEDT.graphEdge[s_se/2].ep.r;
            c1 = graph_GE_GEDT.graphEdge[s_se/2].ep.c;
        }
        if(e_se%2==0){
            r2 = graph_GE_GEDT.graphEdge[e_se/2].sp.r;
            c2 = graph_GE_GEDT.graphEdge[e_se/2].sp.c;
        }else{
            r2 = graph_GE_GEDT.graphEdge[e_se/2].ep.r;
            c2 = graph_GE_GEDT.graphEdge[e_se/2].ep.c;
        }

        line(frame_wdt,Point(c1,r1),Point(c2,r2),Scalar(255,0,0),1,8,0);

        cout<<"EdgeDT len: "<<graph_GE_GEDT.graphEdgeDT[i].length<<" distDiff: "<<graph_GE_GEDT.graphEdgeDT[i].distDiff<<" r: "<<
              graph_GE_GEDT.graphEdgeDT[i].distDiff/graph_GE_GEDT.graphEdgeDT[i].length<<endl;

        namedWindow("frame_wdt",1);
        imshow("frame_wdt",frame_wdt);
        cvWaitKey(0);
    }
*/
//-----------------------------------Debugging------------------------------------

    //Timer timer5;
    //timer5.Start();

    Graph_RSLT Optm_EF_GEDT;//results of graph optimization

    //vector<GraphEdgeUniform> edge_a;//new
    //vector<GraphEdgeUniform> edge_b;//new

    //vector<GraphEdgeUniform> edge_a_tmp;//new
    //vector<GraphEdgeUniform> edge_b_tmp;//new

    vector<int> GE_idx;
    vector<int> GEDT_idx;

    vector<int> GE_idx_tmp;
    vector<int> GEDT_idx_tmp;

    int *nid;

    typedef boost::property<boost::edge_weight_t, float> EdgeWeightProperty;

    typedef boost::adjacency_list < boost::listS, boost::vecS, boost::undirectedS,
            boost::no_property, EdgeWeightProperty > Graph;

    typedef boost::graph_traits < Graph >::vertex_descriptor vertex_descriptor;
    typedef boost::graph_traits < Graph >::edge_descriptor edge_descriptor;
    typedef std::pair<int, int> Edge;

    //Create a graph
    Graph g;

    vector<Graph::vertex_descriptor> Vtx;
    //add end points of graph_GE_GEDT.graphEdge as vertexes
    //graph_GE_GEDT.graphEdge has two vertexes
    for(int i = 0; i < graph_GE_GEDT.graphEdge.size(); i++){
        Vtx.push_back(boost::add_vertex(g));
        Vtx.push_back(boost::add_vertex(g));
    }

//cout<<" finished Dijkstra: graph_GE_GEDT.graphEdge.size() "<< graph_GE_GEDT.graphEdge.size()<<endl;
//cout<<" finished Dijkstra: graph_GE_GEDT.graphEdgeDT.size() "<< graph_GE_GEDT.graphEdgeDT.size()<<endl;
    //add edges to the Graph g

    for(int i = 0; i < graph_GE_GEDT.graphEdge.size(); i++){
        //>>>>>>>>>>>>>>>weights setting<<<<<<<<<<<<<
        EdgeWeightProperty weighti(w_dd*graph_GE_GEDT.graphEdge[i].distDiff);
        //EdgeWeightProperty weighti(w_dd*graph_GE_GEDT.graphEdge[i].distDiff/graph_GE_GEDT.graphEdge[i].length);
        boost::add_edge(Vtx[2*i],Vtx[2*i+1],weighti,g);
    }

    for(int i = 0; i< graph_GE_GEDT.graphEdgeDT.size(); i++){
        //>>>>>>>>>>>>>>>>>>>>weights setting<<<<<<<<<<<<<<<<<<<<
        EdgeWeightProperty weighti(w_len*graph_GE_GEDT.graphEdgeDT[i].length+w_dd*graph_GE_GEDT.graphEdgeDT[i].distDiff);
        //(w_len*length+w_dd*distDiff)/length
        //EdgeWeightProperty weighti(w_len+w_dd*graph_GE_GEDT.graphEdgeDT[i].distDiff/graph_GE_GEDT.graphEdgeDT[i].length);//weights are gap length
        boost::add_edge(Vtx[graph_GE_GEDT.graphEdgeDT[i].s_se],Vtx[graph_GE_GEDT.graphEdgeDT[i].e_se],weighti,g);
    }

    vector<int> cycle_id_a,cycle_id_b,cycle_id;//ID of vertex
    //vector<int> cycle_id_a,cycle_id_b,cycle_id;//ID of vertex
    vector<Point2f> cycle_points;
    vector<Point2f> cycle_pts;//For compute area

    float cycle_gap = 0;
    float cycle_distDiff = 0;
    float cycle_length = 0;
    float cycle_area = 0;
    float cycle_ratio = 0;
    int num_Cadit = 0;

int BDSP_dubug = 0;
Mat frame_test;//Debugging
int CurEdgeDistDiff = 0;
    for(int i = 0; i < graph_GE_GEDT.graphEdge.size(); i++){
        if(graph_GE_GEDT.graphEdge[i].distDiff/graph_GE_GEDT.graphEdge[i].length < searchLineThre){
//cout<<" graphEdge.size(): "<<graph_GE_GEDT.graphEdge.size()<<endl;
//cout<<" i "<<i<<endl;
        //set the edge weight of the current line to infinite
        boost::remove_edge(Vtx[2*i], Vtx[2*i+1],g);
        //>>>>>>>>>>>>>>>>>>>>weights setting<<<<<<<<<<<<<<<<<<<<
        EdgeWeightProperty weightii(100000);
        boost::add_edge(Vtx[2*i], Vtx[2*i+1], weightii, g);

        //Bidirectional shortest path
        //the shortest path of one direction
        // Create things for Dijkstra
        std::vector<vertex_descriptor> parents_a(boost::num_vertices(g)); // To store parents
        std::vector<int> distances_a(boost::num_vertices(g)); // To store distances
        // Compute shortest paths from v(2i) to all vertices, and store the output in parents and distances
        boost::dijkstra_shortest_paths(g, Vtx[2*i], boost::predecessor_map(&parents_a[0]).distance_map(&distances_a[0]));

        //the shortest path of the other direction
        // Create things for Dijkstra
        std::vector<vertex_descriptor> parents_b(boost::num_vertices(g)); // To store parents
        std::vector<int> distances_b(boost::num_vertices(g)); // To store distances
        // Compute shortest paths from v(2*i+1) to all vertices, and store the output in parents and distances
        boost::dijkstra_shortest_paths(g, Vtx[2*i+1], boost::predecessor_map(&parents_b[0]).distance_map(&distances_b[0]));

        //set the edge weight of the current line back to zero
        boost::remove_edge(Vtx[2*i], Vtx[2*i+1],g);
        //>>>>>>>>>>>>>>>>>>>>weights setting<<<<<<<<<<<<<<<<<<<<
        EdgeWeightProperty weightiii(w_dd*graph_GE_GEDT.graphEdge[i].distDiff);
        //EdgeWeightProperty weightiii(w_dd*graph_GE_GEDT.graphEdge[i].distDiff/graph_GE_GEDT.graphEdge[i].length);
        boost::add_edge(Vtx[2*i], Vtx[2*i+1], weightiii, g);

        cycle_distDiff += graph_GE_GEDT.graphEdge[i].distDiff;
        cycle_length += graph_GE_GEDT.graphEdge[i].length;
        CurEdgeDistDiff = graph_GE_GEDT.graphEdge[i].distDiff;
//--------------------Debugging------------------------
if(BDSP_dubug){
cout<<" draw current edge "<<endl;
cout<<2*i<<" "<<2*i+1<<" cycle_gap: "<<cycle_gap<<endl;

frame_test = frame_edge.clone();

for(int tt = graph_GE_GEDT.graphEdge[i].EF.start_idx; tt < graph_GE_GEDT.graphEdge[i].EF.end_idx + 1; tt++){
    int tmpi = graph_GE_GEDT.graphEdge[i].EF.seg_id;

    int r = map->segments[tmpi].pixels[tt].r;
    int c = map->segments[tmpi].pixels[tt].c;

    Vec3b & color = frame_test.at<Vec3b>(r,c);
    color[0] = 0;
    color[1] = 0;
    color[2] = 255;

}
//namedWindow("Testing",1);
//imshow("Testing",frame_test);
//cvWaitKey(0);
}//--------------------Debugging-------------------------
        for(int j = 0; j < 2*graph_GE_GEDT.graphEdge.size();){

            int end = j;
            int tmpa = end;
            int tmpb = end;

            if(end%2 == 0){
                cycle_pts.push_back(Point2f(float(graph_GE_GEDT.graphEdge[tmpa/2].sp.c),float(graph_GE_GEDT.graphEdge[tmpa/2].sp.r)));
            }
            else if(end%2 == 1){
                cycle_pts.push_back(Point2f(float(graph_GE_GEDT.graphEdge[tmpa/2].ep.c),float(graph_GE_GEDT.graphEdge[tmpa/2].ep.r)));
            }

            //-------------------Debugging------------------------

if(BDSP_dubug){
            frame_test = frame_edge.clone();
}
            //namedWindow("Testing",1);
            ///imshow("Testing",frame_test);
            //cvWaitKey(0);
            //-------------------Debugging------------------------

            if(j!=2*i){
                cycle_gap = w_dd*CurEdgeDistDiff + cycle_gap + distances_a[end] + distances_b[end];//store the gap length of the whole cycle
//                cout<<" cycle_gap: +a+b "<<cycle_gap<<endl;
                //-------------------Debugging------------------------
                //draw the current edge
if(BDSP_dubug){
                cout<<"==========i: "<<i<<endl;
                cout<<"----------j: "<<j<<endl;

                cout<<" draw current edge "<<endl;
                cout<<" current edge: "<<" len: "<<graph_GE_GEDT.graphEdge[i].length<<" distDiff: "<<graph_GE_GEDT.graphEdge[i].distDiff<<endl;

                for(int tt = graph_GE_GEDT.graphEdge[i].EF.start_idx; tt < graph_GE_GEDT.graphEdge[i].EF.end_idx + 1; tt++){
                    int tmpi = graph_GE_GEDT.graphEdge[i].EF.seg_id;

                    int r = map->segments[tmpi].pixels[tt].r;
                    int c = map->segments[tmpi].pixels[tt].c;

                    Vec3b & color = frame_test.at<Vec3b>(r,c);
                    color[0] = 0;
                    color[1] = 0;
                    color[2] = 255;

                }

                //namedWindow("Testing",1);
                //imshow("Testing",frame_test);
                //cvWaitKey(0);

                circle(frame_test,Point(graph_GE_GEDT.graphEdge[j/2].sp.c,graph_GE_GEDT.graphEdge[j/2].sp.r),3, Scalar(0,255,0),1, 8, 0);
}
                //namedWindow("Testing",1);
                //imshow("Testing",frame_test);
                //cvWaitKey(0);
               //-------------------Debugging------------------------
                GE_idx_tmp.push_back(i);

                while(tmpa!=parents_a[tmpa]){

                    if(tmpa/2 == parents_a[tmpa]/2){//it is a detected edge
                        GE_idx_tmp.push_back(tmpa/2);
                        cycle_distDiff +=  graph_GE_GEDT.graphEdge[tmpa/2].distDiff;
                        cycle_length += graph_GE_GEDT.graphEdge[tmpa/2].length;
                        //-------------------Debugging---------------------
                            //cout<<"a: draw EF "<<endl;
if(BDSP_dubug){
                            cout<<" a EF: "<<tmpa<<" "<<parents_a[tmpa]<<" length: "<<graph_GE_GEDT.graphEdge[tmpa/2].length<<
                                  " distDiff: "<< graph_GE_GEDT.graphEdge[tmpa/2].distDiff<<endl;
                            for(int tt = graph_GE_GEDT.graphEdge[tmpa/2].EF.start_idx; tt < graph_GE_GEDT.graphEdge[tmpa/2].EF.end_idx; tt++){
                                int tmpi = graph_GE_GEDT.graphEdge[tmpa/2].EF.seg_id;

                                int r = map->segments[tmpi].pixels[tt].r;
                                int c = map->segments[tmpi].pixels[tt].c;

                                Vec3b & color = frame_test.at<Vec3b>(r,c);
                                color[0] = 255;
                                color[1] = 0;
                                color[2] = 0;
                            }
}                            //namedWindow("Testing",1);
                            //imshow("Testing",frame_test);
                            //cvWaitKey(0);
                        //-------------------Debugging---------------------
                    }else{//it is a generated edge
                        nid = new int[2];
                        if(tmpa<parents_a[tmpa]){
                            nid[0] = tmpa;
                            nid[1] = parents_a[tmpa];
                        }else{
                            nid[0] = parents_a[tmpa];
                            nid[1] = tmpa;
                        }
                        GEDT_idx_tmp.push_back(graph_GE_GEDT.mapGEDT[nid]);
                        cycle_distDiff +=  graph_GE_GEDT.graphEdgeDT[graph_GE_GEDT.mapGEDT[nid]].distDiff;
                        cycle_length += graph_GE_GEDT.graphEdgeDT[graph_GE_GEDT.mapGEDT[nid]].length;
                        //-------------------Debugging---------------------

                        //cout<<"a: draw DT "<<endl;
if(BDSP_dubug){
                        //cout<<" ------------ "<<endl;
                        cout<<" a DT: "<<tmpa<<" "<<parents_a[tmpa]<<" gap: "<<graph_GE_GEDT.graphEdgeDT[graph_GE_GEDT.mapGEDT[nid]].length<<
                                                             " distDiff: "<< graph_GE_GEDT.graphEdgeDT[graph_GE_GEDT.mapGEDT[nid]].distDiff<<endl;
                        //cout<<graph_GE_GEDT.graphEdgeDT[graph_GE_GEDT.mapGEDT[nid]].
                        //cout<<" ------------- "<<endl;
                        int s_se,e_se;
                        int r1,c1,r2,c2;
                        s_se = graph_GE_GEDT.graphEdgeDT[graph_GE_GEDT.mapGEDT[nid]].s_se;
                        e_se = graph_GE_GEDT.graphEdgeDT[graph_GE_GEDT.mapGEDT[nid]].e_se;

                        if(s_se%2==0){
                            r1 = graph_GE_GEDT.graphEdge[s_se/2].sp.r;
                            c1 = graph_GE_GEDT.graphEdge[s_se/2].sp.c;
                        }else{
                            r1 = graph_GE_GEDT.graphEdge[s_se/2].ep.r;
                            c1 = graph_GE_GEDT.graphEdge[s_se/2].ep.c;
                        }
                        if(e_se%2==0){
                            r2 = graph_GE_GEDT.graphEdge[e_se/2].sp.r;
                            c2 = graph_GE_GEDT.graphEdge[e_se/2].sp.c;
                        }else{
                            r2 = graph_GE_GEDT.graphEdge[e_se/2].ep.r;
                            c2 = graph_GE_GEDT.graphEdge[e_se/2].ep.c;
                        }

                        line(frame_test,Point(c1,r1),Point(c2,r2),Scalar(255,0,0),3,8,0);
}                        //namedWindow("Testing",1);
                        //imshow("Testing",frame_test);
                        //cvWaitKey(0);
                        //-------------------Debugging---------------------
                    }
                    //namedWindow("Testing",1);//----------------------
                    //imshow("Testing",frame_test);//---------------------
                    //cvWaitKey(0);//---------------------

                    if(parents_a[tmpa]%2 == 0){
                        cycle_pts.push_back(Point2f(float(graph_GE_GEDT.graphEdge[parents_a[tmpa]/2].sp.c),float(graph_GE_GEDT.graphEdge[parents_a[tmpa]/2].sp.r)));
                    }
                    else if(parents_a[tmpa]%2 == 1){
                        cycle_pts.push_back(Point2f(float(graph_GE_GEDT.graphEdge[parents_a[tmpa]/2].ep.c),float(graph_GE_GEDT.graphEdge[parents_a[tmpa]/2].ep.r)));
                    }

                    tmpa = parents_a[tmpa];
                }//while(tmpa


            //trace the other half cycle vertex
            while(tmpb!=parents_b[tmpb]/*2*i+1*/){
                if(tmpb/2 == parents_b[tmpb]/2){
                    GE_idx_tmp.push_back(tmpb/2);
                    cycle_distDiff +=  graph_GE_GEDT.graphEdge[tmpb/2].distDiff;
                    cycle_length += graph_GE_GEDT.graphEdge[tmpb/2].length;

                    //-------------------Debugging---------------------
                        //cout<<"b: draw EF "<<endl;ted
if(BDSP_dubug){
                    cout<<" b EF: "<<tmpb<<" "<<parents_b[tmpb]<<" length: "<<graph_GE_GEDT.graphEdge[tmpb/2].length
                       <<" distDiff: "<<graph_GE_GEDT.graphEdge[tmpb/2].distDiff<<endl;
                        for(int tt = graph_GE_GEDT.graphEdge[tmpb/2].EF.start_idx; tt < graph_GE_GEDT.graphEdge[tmpb/2].EF.end_idx; tt++){
                            int tmpi = graph_GE_GEDT.graphEdge[tmpb/2].EF.seg_id;

                            int r = map->segments[tmpi].pixels[tt].r;
                            int c = map->segments[tmpi].pixels[tt].c;

                            Vec3b & color = frame_test.at<Vec3b>(r,c);
                            color[0] = 0;
                            color[1] = 255;
                            color[2] = 255;
                        }
}                        //namedWindow("Testing",1);
                        //imshow("Testing",frame_test);
                        //cvWaitKey(0);
                    //-------------------Debugging---------------------

                }else{
                    nid = new int[2];
                    if(tmpb<parents_b[tmpb]){
                        nid[0] = tmpb;
                        nid[1] = parents_b[tmpb];
                    }else{
                        nid[0] = parents_b[tmpb];
                        nid[1] = tmpb;
                    }
                    GEDT_idx_tmp.push_back(graph_GE_GEDT.mapGEDT[nid]);
                    cycle_distDiff +=  graph_GE_GEDT.graphEdgeDT[graph_GE_GEDT.mapGEDT[nid]].distDiff;
                    cycle_length += graph_GE_GEDT.graphEdgeDT[graph_GE_GEDT.mapGEDT[nid]].length;

                    //-------------------Debugging---------------------

                    //cout<<"b: draw DT "<<endl;
if(BDSP_dubug){
                    cout<<" b: DT "<<tmpb<<" "<<parents_b[tmpb]<<" gap: "<<graph_GE_GEDT.graphEdgeDT[graph_GE_GEDT.mapGEDT[nid]].length<<
                       " distDiff: "<<graph_GE_GEDT.graphEdgeDT[graph_GE_GEDT.mapGEDT[nid]].distDiff<<endl;

                    int s_se,e_se;
                    int r1,c1,r2,c2;
                    s_se = graph_GE_GEDT.graphEdgeDT[graph_GE_GEDT.mapGEDT[nid]].s_se;
                    e_se = graph_GE_GEDT.graphEdgeDT[graph_GE_GEDT.mapGEDT[nid]].e_se;

                    if(s_se%2==0){
                        r1 = graph_GE_GEDT.graphEdge[s_se/2].sp.r;
                        c1 = graph_GE_GEDT.graphEdge[s_se/2].sp.c;
                    }else{
                        r1 = graph_GE_GEDT.graphEdge[s_se/2].ep.r;
                        c1 = graph_GE_GEDT.graphEdge[s_se/2].ep.c;
                    }
                    if(e_se%2==0){
                        r2 = graph_GE_GEDT.graphEdge[e_se/2].sp.r;
                        c2 = graph_GE_GEDT.graphEdge[e_se/2].sp.c;
                    }else{
                        r2 = graph_GE_GEDT.graphEdge[e_se/2].ep.r;
                        c2 = graph_GE_GEDT.graphEdge[e_se/2].ep.c;
                    }

                    line(frame_test,Point(c1,r1),Point(c2,r2),Scalar(0,255,255),3,8,0);
}                    //namedWindow("Testing",1);
                    //imshow("Testing",frame_test);
                    //cvWaitKey(0);
                    //-------------------Debugging---------------------

                }
                cycle_id_b.push_back(parents_b[tmpb]);
                tmpb = parents_b[tmpb];
            }
//namedWindow("Testing",1);//---------------------------
//imshow("Testing",frame_test);//-------------------------
//cvWaitKey(0);//--------------------------
            //store corresponding points of another half cycle inverse order
            for(int t = cycle_id_b.size()-1; t > -1; t--){
                int tmpt = cycle_id_b[t];
                //cycle_id.push_back(tmpt);
                if(int(tmpt)%2==0){
                    cycle_pts.push_back(Point2f(float(graph_GE_GEDT.graphEdge[tmpt/2].sp.c),float(graph_GE_GEDT.graphEdge[tmpt/2].sp.r)));
                }
                else if(int(tmpt)%2==1){
                    cycle_pts.push_back(Point2f(float(graph_GE_GEDT.graphEdge[tmpt/2].ep.c),float(graph_GE_GEDT.graphEdge[tmpt/2].ep.r)));
                }
            }

            if(cycle_pts.size()>0){
                cycle_area = contourArea(cycle_pts);
                cycle_length = arcLength(cycle_pts,true);

                //cout<<" cycle_length: "<<cycle_length;
                //cout<<" lenPrior: "<<lenPrior;
                //cout<<" c/p: "<<cycle_length/lenPrior;
                //cout<<" p/c: "<<lenPrior/cycle_length<<endl;
            }
            //----------Debugging--------------------------------
if(BDSP_dubug){
                        cout<<" ------------------ "<<endl;
                        cout<<" cycle_a: "<<distances_a[end]<<endl;
                        cout<<" cycle_b: "<<distances_b[end]<<endl;

                        cout<<" cycle_weights: "<<cycle_gap<<endl;
                        cout<<" cycle_length: "<<cycle_length<<endl;

                        cout<<endl;
                        cout<<" lenPrior: "<<lenPrior<<endl;

                        cout<<endl;

                        cout<<" areaPrior: "<<areaPrior<<endl;
                        cout<<" cycle_area: "<<cycle_area<<endl;

                        cout<<endl;

                        cout<<" p/c: "<<cycle_area/areaPrior<<endl;
                        cout<<" c/p: "<<areaPrior/cycle_area<<endl;

                        cout<<endl;

                        cout<<" ratio_last: "<<cycle_ratio<<endl;
                        cout<<" ratio_curr: "<<cycle_gap/cycle_area<<endl;

                        for(int tt = graph_GE_GEDT.graphEdge[i].EF.start_idx; tt < graph_GE_GEDT.graphEdge[i].EF.end_idx + 1; tt++){
                            int tmpi = graph_GE_GEDT.graphEdge[i].EF.seg_id;

                            int r = map->segments[tmpi].pixels[tt].r;
                            int c = map->segments[tmpi].pixels[tt].c;

                            Vec3b & color = frame_test.at<Vec3b>(r,c);
                            color[0] = 0;
                            color[1] = 0;
                            color[2] = 255;

                        }

                        namedWindow("Testing",1);
                        imshow("Testing",frame_test);
                        cvWaitKey(0);
}
                        //cout<<" ==================== "<<endl;
            //----------Debugging--------------------------------
//            cout<<" areaCurrent: "<<cycle_area;
//            cout<<" areaPrior: "<<areaPrior<<endl;
//            cout<<" p/c: "<<cycle_area/areaPrior;
//            cout<<" c/p: "<<areaPrior/cycle_area<<endl;
            if(cycle_area/areaPrior > areaCnst && areaPrior/cycle_area > areaCnst &&
                    cycle_length/lenPrior > lenCnst && lenPrior/cycle_length > lenCnst){//length variation constrain
                if(num_Cadit==0){//indicate the first cycle
                    cycle_ratio = (cycle_gap/* + cycle_distDiff*/)/cycle_area/*cycle_length*/;
                    //GE_idx_tmp.push_back(i);
                    GE_idx.swap(GE_idx_tmp);
                    GEDT_idx.swap(GEDT_idx_tmp);
                    Optm_EF_GEDT.area = cycle_area;
                    Optm_EF_GEDT.length = cycle_length;

                    cycle_points.swap(cycle_pts);
                }
                else{

                    if(cycle_ratio > (cycle_gap/*+cycle_distDiff*/)/cycle_area/*cycle_length*/){

                        cycle_ratio = (cycle_gap/* +cycle_distDiff*/)/cycle_area/*cycle_length*/;

                        GE_idx.swap(GE_idx_tmp);
                        GEDT_idx.swap(GEDT_idx_tmp);
                        Optm_EF_GEDT.area = cycle_area;
                        Optm_EF_GEDT.length = cycle_length;
//cout<<" cycle_length: "<<cycle_length<<endl;
                        cycle_points.swap(cycle_pts);
                    }
                }

                num_Cadit++;
            }//if(cycle_length/lenProir)

            cycle_gap = 0;
            cycle_distDiff = 0;
            //cycle_length = 0;
            //cycle_ratio = 0;

            cycle_id_b.clear();
            vector<int>().swap(cycle_id_b);

            cycle_pts.clear();
            vector<Point2f>().swap(cycle_pts);

            GE_idx_tmp.clear();
            vector<int>().swap(GE_idx_tmp);
            GEDT_idx_tmp.clear();
            vector<int>().swap(GEDT_idx_tmp);
            }//if(j!=2*i)
            j = j + 2;
        }//for(int j)
    //i = i + 1;
        }
    }//for(int i)

    //Optm_EF_GEDT
    for(int i = 0; i < GE_idx.size(); i++){
        Optm_EF_GEDT.EFs.push_back(graph_GE_GEDT.graphEdge[GE_idx[i]].EF);
    }

    for(int i = 0; i < GEDT_idx.size(); i++){
        Graph_GEDT gedt_tmp;
        int s_se = graph_GE_GEDT.graphEdgeDT[GEDT_idx[i]].s_se;
        int e_se = graph_GE_GEDT.graphEdgeDT[GEDT_idx[i]].e_se;

        if(s_se%2 == 0){
            gedt_tmp.sp = graph_GE_GEDT.graphEdge[s_se/2].sp;
        }else{
            gedt_tmp.sp = graph_GE_GEDT.graphEdge[s_se/2].ep;
        }

        if(e_se%2 == 0){
            gedt_tmp.ep = graph_GE_GEDT.graphEdge[e_se/2].sp;
        }else{
            gedt_tmp.ep = graph_GE_GEDT.graphEdge[e_se/2].ep;
        }

        Optm_EF_GEDT.GEDTs.push_back(gedt_tmp);
    }

    //timer5.Stop();
    //cout<<" Time of Dijkstra: "<<timer5.ElapsedTime()<<endl;

    return Optm_EF_GEDT;
}


int main(int argc, char* argv[]){

    if(argc==4){
        video_proc = atoi(argv[1]);
    }
    else if(argc==3){
        video_proc = atoi(argv[1]);
    }

    VideoCapture cap;

    if(video_proc==0){
        cap = VideoCapture(0);
    }
    else if(video_proc==1){
        const char* video_path = argv[2];
        cap = VideoCapture(video_path);
    }

    if(!cap.isOpened())  // check if we succeeded
        return -1;

    //Write Video_frame
    int frame_width=   cap.get(CV_CAP_PROP_FRAME_WIDTH);
    int frame_height=   cap.get(CV_CAP_PROP_FRAME_HEIGHT);

    VideoWriter video;
    if(video_proc==0){
        video = VideoWriter("RECORDED_VIDEO.avi",CV_FOURCC('M','J','P','G'),30,Size(frame_width,frame_height),true);
    }

    //Write Video_results

    VideoWriter videor;
    videor = VideoWriter("RECORDED_RESULTS.avi",CV_FOURCC('M','J','P','G'),30,Size(frame_width,frame_height),true);

    Mat frame;
    Mat grayscaleFrame;
    int width, height;

    int frame_whole_id=0;
    int frame_id=0;

    remove("TRACKING_RESULTS.txt");//remove prevoius results

    int lowest=100, highest=255;
    int range=(highest-lowest)+1;
    int rc, gc, bc; //the color of edges

    float priorBoundaryArea = 0;
    float priorBoundaryLen = 0;
    //Graph_RSLT RSLT_last;
    //EdgeMap *mapPrior;

    Mat lastPoly_img, last_tmp, last_bw, lastDis_img,curtPoly_img;

    int start_frame_id = 0;

    if(video_proc){
        for(int i = 0; i<start_frame_id; i++){
            cout<<" ignore frame: "<<i<<endl;
            if(!cap.read(frame)){
                break;
            }
        }
    }

    Graph_RSLT RSLT_Curt;
    vector<vector<Pixel> > RSLT_ESs;

    //statistic parameters
    float ED_time = 0;
    float ED_time_tmp = 0;

    float EBF_time = 0;
    float EBF_time_tmp = 0;

    float Graph_time = 0;
    float Graph_time_tmp = 0;

    float Opti_time = 0;
    float Opti_time_tmp = 0;

    float Other_time = 0;
    float other_time_tmp = 0;

    float EFNum = 0;//Edge Fragments number
    float GPNum = 0;//Gap Gilling segments number


    for(;;){
        //cap >> frame; // get a new frame from camera
        if(video_proc){
            if(frame_whole_id==0||tracking_flg){
                if(!cap.read(frame)){
                    break;
                }
            }
        }else{
            if(!cap.read(frame)){
                break;
            }
        }

        cvtColor(frame,grayscaleFrame, CV_BGR2GRAY);//convert rgb image to gray image
        width = grayscaleFrame.size().width;
        height = grayscaleFrame.size().height;

        //Mat curtPoly_img;
        //curtPoly_img = Mat::zeros(height, width, CV_8UC1);

        unsigned char *srcImg;
        srcImg = grayscaleFrame.data;

        Mat dst, frame_edge;
        dst = Mat::zeros(height,width,CV_32F);
        frame_edge =  frame.clone();

        //-------------------------------- Edge detection--------------------------------------
        //Timer timer;
        //timer.Start();


        Timer ED_timer;
        ED_timer.Start();

        EdgeMap *map = DetectEdgesByED(srcImg, width, height, SOBEL_OPERATOR, 25, 8, 1.0);//36 8 1.0

        ED_timer.Stop();
        ED_time_tmp = ED_timer.ElapsedTime();

        //printf("ED detects <%d> edge segments in <%4.2lf> ms\n\n", map->noSegments, timer_map.ElapsedTime());

        //Timer timer2;
        //timer2.Start();
//        vector<EdgeFragment> edgeFragments;//seg_id, start_idx, end_idx
//        edgeFragments = DivideIntoEdgeFragements(map);
        //timer2.Stop();
        //printf("EF detects <%d> edge segments in <%4.2lf> ms\n\n", edgeFragments.size(), timer2.ElapsedTime());

        //for(int i = 0; i<edgeFragments.size(); i++){
        //    cout<<" i "<< i << " seg_id " <<edgeFragments[i].seg_id<< " start_idx " << edgeFragments[i].start_idx<< " end_idx " <<edgeFragments[i].end_idx
        //       <<" ED size " <<map->segments[edgeFragments[i].seg_id].noPixels<<endl;
        //}

        // This is how you access the pixels of the edge segments returned by ED


//====================Debugging========================
/*for(int i = 0; i < map->noSegments; i++){
    for(int j = 0; j < map->segments[i].noPixels; j++){
        int r = map->segments[i].pixels[j].r;
        int c = map->segments[i].pixels[j].c;
        dst.at<float>(r,c) += 1;
    }
}


vector<Pixel> overPixel;
for(int i =  0; i < height; i++){
    for(int j = 0; j < width; j++){
        if(dst.at<float>(i,j)>1){
            struct Pixel tmPixel = {i,j};
            overPixel.push_back(tmPixel);
        }
    }
}
cout<<" overPixel size: "<<overPixel.size()<<endl;

for(int t = 0; t<overPixel.size(); t++){
    cout<<t<<" "<<overPixel[t].r<<" "<<overPixel[t].c<<endl;
}

cvWaitKey(0);*/
//--------------------Debugging------------------------

/*        Mat all_edges;
        all_edges = frame.clone();
//        Timer timer3;
//        timer3.Start();
//        //vector<vector<Pixel> > EFPixels;
        for(int i = 0; i < map->noSegments; i++){
            rc = lowest+int(rand()%range);
            gc = lowest+int(rand()%range);
            bc = lowest+int(rand()%range);
            //vector<Pixel> temp_Pixels;
            for(int j = 0; j < map->segments[i].noPixels; j++){

                //int tmpi = edgeFragments[i].seg_id;

                int r = map->segments[i].pixels[j].r;
                int c = map->segments[i].pixels[j].c;

                Vec3b & color = all_edges.at<Vec3b>(r,c);
                color[0] = bc;
                color[1] = gc;
                color[2] = rc;

                //struct Pixel temp_pixel = {r,c};
                //temp_Pixels.push_back(temp_pixel);
            }
            //EFPixels.push_back(temp_Pixels);
            //vector<Pixel>().swap(temp_Pixels);
        }

        //timer3.Stop();
        //printf("Draw <%d> edge segments itempan <%4.2lf> ms\n\n", edgeFragments.size(), timer3.ElapsedTime());

        namedWindow("All_edges",1);
        imshow("All_edges",all_edges);
        cvWaitKey(1);
*/
        //delete map;

        //=================initialization==================
        //cout<<"frame_whole_id: "<<frame_whole_id<<endl;
        //if(frame_whole_id > 669){
        if(!tracking_flg){

            Mat frame_ini;
            frame_ini = frame_edge.clone();

            if(points.size()>1){
                for(int i = 0;i<points.size()-1; i++){
                    line(frame_ini, points[i], points[i+1], Scalar(0,255,255), 2, CV_AA);
                }
            }

            if(mbd == 0){
                if(points.size()>0){
                    line(frame_ini, pt_mv, points[points.size()-1], Scalar(0,255,255), 2, CV_AA);
                    line(frame_ini, pt_mv, points[0], Scalar(0,255,255), 2, CV_AA);
                }
            }
            else
            {
                line(frame_ini, points[points.size()-1], points[0], Scalar(0,255,255), 2, CV_AA);
                //-----------------------------edit paper-----------------------
//                            imwrite("initial_frame.png",frame_edge);
//                            imwrite("initial_polygon.png",frame_ini);
            }




            namedWindow("Display",1);
            imshow("Display",frame_ini);
            k = cvWaitKey(pki) & 255;

            //initialize the tracker
            if(k == 105||video_proc){//"i", fix the frame and initialize trackers
                //pki = 0;
                //set the callback function for any mouse event
                setMouseCallback("Display", CallBackFunc, &points);
            }

            //if(pki == 0 ){//space key, initialize tracker
            if(k == 32){//space key
                mbd = 0;
                tracking_flg = true;

                k = 1;
                pk = 1;

            }
            else if(k == 115){// s
                tracking_flg = true;
                k = 1;
                pk = 0;

            }
            //}
        }//end_if initialization

        //======================start tracking======================
        if(tracking_flg){//frame_id start from 0
            cout<<" --------------------frame_id: "<<frame_id<<" ------------------------"<<endl;

            ED_time = ED_time + ED_time_tmp;
            cout<<"Edge segments: "<<map->noSegments<<endl;
            cout<<"ED time: "<<ED_time_tmp<<endl;
            //printf("ED detects <%d> edge segments in <%4.2lf> ms\n\n", map->noSegments, ED_time_tmp.ElapsedTime());

//output for paper editing===========================
/*            Mat all_edges;
            all_edges = Mat::zeros(height, width, CV_8UC3);////frame.clone();
            for(int i = 0; i<map->noSegments; i++){

                rc = lowest+int(rand()%range);
                gc = lowest+int(rand()%range);
                bc = lowest+int(rand()%range);

                for( int j = 0; j<map->segments[i].noPixels;j++){
                    int r = map->segments[i].pixels[j].r;
                    int c = map->segments[i].pixels[j].c;

                    Vec3b & color = all_edges.at<Vec3b>(r,c);
                    color[0] = bc;
                    color[1] = gc;
                    color[2] = rc;
                }
            }

            imwrite("all_edges.png",all_edges);
            cvWaitKey(0);
*/
//output for paper editing--------------------------


            if(video_proc==0){//record video when read video stream from webcam
                video.write(frame);
            }

            //draw the tracked shape of last frame and get the buffering area on the current frame
            //create distance transform image
//cout<<" Debug----------------1 "<<endl;
            //Mat lastPoly_img, last_tmp, last_bw, lastDis_img;
            //lastPoly_img = Mat::zeros(height, width, CV_8UC1);

            Timer EBF_timer;
            EBF_timer.Start();

            if(frame_id==0){//first frame we use the manually selected polygon as the prior shape
                lastPoly_img = Mat::zeros(height, width, CV_8UC1);//
                const Point* pptl[1] = {&points[0]};
                int nptl[] = {(int)points.size()};
                polylines(lastPoly_img, pptl, nptl, 1, 1, Scalar(100), 1, 8, 0);
                priorBoundaryArea = contourArea(points);
                priorBoundaryLen = arcLength(points,true);
                //cout<<" priorBoundaryLen: "<<priorBoundaryLen<<endl;
                //for(int pp = 0; pp < points.size(); pp++){
                //    cout<<"point "<<pp<<" "<<points[pp].x<<" "<<points[pp].y<<endl;
                //}
            }
/*            else{//other frames take the shape tracked from the last frame as prior shape
cout<<" Debug----------------2 "<<endl;
cout<<" RSLT_last.EFs.size():"<<RSLT_last.EFs.size()<<endl;
cout<<" mapPrior.nosegments: "<<mapPrior->noSegments<<endl;
                for(int i = 0; i < RSLT_last.EFs.size(); i++){
                    for( int j = RSLT_last.EFs[i].start_idx; j< RSLT_last.EFs[i].end_idx+1; j++){
cout<<" i: "<<i<<" j: "<<j<<" noPixels: "<<RSLT_last.EFs[i].end_idx+1-RSLT_last.EFs[i].start_idx<<endl;
                        int tmpi = RSLT_last.EFs[i].seg_id;

                        int r = mapPrior->segments[tmpi].pixels[j].r;
                        int c = mapPrior->segments[tmpi].pixels[j].c;

                        lastPoly_img.at<char>(r,c) = 100;
                    }
                }
imshow("Binary Image", lastPoly_img);
cout<<" Debug----------------2.5 "<<endl;
                for(int i = 0; i < RSLT_last.GEDTs.size(); i++){
                    line(lastPoly_img,Point(RSLT_last.GEDTs[i].sp.c, RSLT_last.GEDTs[i].sp.r),Point(RSLT_last.GEDTs[i].ep.c, RSLT_last.GEDTs[i].ep.r),Scalar(100),1,8,0);
                }
imshow("Binary Image", lastPoly_img);
            }//if(frame_id==0)

*/

//cout<<" Debug----------------1 "<<endl;
            //Timer timer_threshold;
            //timer_threshold.Start();

            last_tmp = 255 - lastPoly_img;//155
            threshold(last_tmp, last_bw, 160, 255, CV_THRESH_BINARY | CV_THRESH_OTSU);

            //timer_threshold.Stop();
            //printf("Threshold detects <%d> edge segments in <%4.2lf> ms\n\n", map->noSegments, timer_threshold.ElapsedTime());

//cout<<" Debug----------------2 "<<endl;
            //Timer timer_dist;
            //timer_dist.Start();

            distanceTransform(last_bw, lastDis_img, CV_DIST_L2, 3);//distance transform


/*            namedWindow("Dis_Test",1);
            normalize(lastDis_img, lastDis_img, 0, 255., NORM_MINMAX);
            imshow("Dis_Test",lastDis_img);
            imwrite("bw_img.png",last_bw);
            imwrite("dist_img.png",lastDis_img);
            cout<<"------dist image--------"<<endl;
            cvWaitKey(0);
*/

            //timer_dist.Stop();
            //printf("Distance Transform detects <%d> edge segments in <%4.2lf> ms\n\n", map->noSegments, timer_dist.ElapsedTime());
//cout<<" Debug----------------3 "<<endl;

//            vector<EdgeFragment>& edgeFragments;//seg_id, start_idx, end_idx
//cout<<" Debug----------------4-0.1 "<<endl;

            //Timer timer_filter;
            //timer_filter.Start();

            vector<EdgeFragment> edgeFragments = EdgeFilterBreak(map, lastDis_img);

            EBF_timer.Stop();
            EBF_time_tmp = EBF_timer.ElapsedTime();
            EBF_time = EBF_time + EBF_time_tmp;


            cout<<"Edge Fragments Split: "<<edgeFragments.size()<<endl;
            cout<<"Edge Fragments Time: "<<EBF_time_tmp<<endl;

            //printf("Edge fragments: <%d> , EF filter and break: <%4.2lf> ms\n\n", edgeFragments.size(), EBF_time_tmp.ElapsedTime());

            //timer_filter.Stop();
            //printf("Edge Filter detects <%d> edge segments in <%4.2lf> ms\n\n", map->noSegments, timer_filter.ElapsedTime());
//cout<<" Debug----------------4 "<<endl;

            //Timer timer_construct;
            //timer_construct.Start();

            Timer Graph_timer;
            Graph_timer.Start();

            Graph_GE_GEDT GE_GEDT_edges;//grah edges from filtered and broken edge fragments
            GE_GEDT_edges = GraphConstruct(map, edgeFragments, lastDis_img, frame_edge);

            Graph_timer.Stop();
            Graph_time_tmp = Graph_timer.ElapsedTime();
            Graph_time = Graph_time + Graph_time_tmp;

            EFNum = EFNum + GE_GEDT_edges.graphEdge.size();
            GPNum = GPNum + GE_GEDT_edges.graphEdgeDT.size();

            cout<<"Graph Nodes: "<<2*GE_GEDT_edges.graphEdge.size()<<endl;
            cout<<"Edge Fragments Filtered: "<<GE_GEDT_edges.graphEdge.size()<<endl;
            cout<<"Gap Filling: "<<GE_GEDT_edges.graphEdgeDT.size()<<endl;
            cout<<"Graph Construct Time: "<<Graph_time_tmp<<endl;

            //printf("Edge fragments: <%d> , EF filter and break: <%4.2lf> ms\n\n", GE_GEDT_edges.graphEdge.size(), GE_GEDT_edges.graphEdgeDT.size(), Graph_time_tmp.ElapsedTime());
            //output for paper editing===========================
/*                        Mat filtered_edges;//,chans[3];
                        filtered_edges = Mat::zeros(height, width, CV_8UC3);////frame.clone();
                        //split(filtered_edges,chans);
                        //chans[0] = 255-chans[0];
                        //chans[1] = 255-chans[1];
                        //chans[2] = 255-chans[2];
                        //merge(chans,3,filtered_edges);

                        for(int i = 0; i<GE_GEDT_edges.graphEdge.size(); i++){

                            int tmpi = GE_GEDT_edges.graphEdge[i].EF.seg_id;

                            rc = lowest+int(rand()%range);
                            gc = lowest+int(rand()%range);
                            bc = lowest+int(rand()%range);

                            for( int j = GE_GEDT_edges.graphEdge[i].EF.start_idx; j<GE_GEDT_edges.graphEdge[i].EF.end_idx+1;j++){
                                int r = map->segments[tmpi].pixels[j].r;
                                int c = map->segments[tmpi].pixels[j].c;

                                Vec3b & color = filtered_edges.at<Vec3b>(r,c);
                                color[0] = bc;
                                color[1] = gc;
                                color[2] = rc;
                            }
                        }

                        imwrite("filtered_edges.png",filtered_edges);
                        cvWaitKey(0);
*/            //output for paper editing--------------------------


            //timer_construct.Stop();
            //printf("Graph Construction detects <%d> edge segments in <%4.2lf> ms\n\n", map->noSegments, timer_construct.ElapsedTime());


           for(int i = 0; i < GE_GEDT_edges.graphEdge.size(); i++){

                rc = lowest+int(rand()%range);
                gc = lowest+int(rand()%range);
                bc = lowest+int(rand()%range);

                for(int j = GE_GEDT_edges.graphEdge[i].EF.start_idx; j < GE_GEDT_edges.graphEdge[i].EF.end_idx+1; j++){

                    int tmpi = GE_GEDT_edges.graphEdge[i].EF.seg_id;

                    int r = map->segments[tmpi].pixels[j].r;
                    int c = map->segments[tmpi].pixels[j].c;

                    Vec3b & color = frame_edge.at<Vec3b>(r,c);
                    color[0] = bc;
                    color[1] = gc;
                    color[2] = rc;
                }
            }
/*
            namedWindow("Display_edges", WINDOW_AUTOSIZE);
            imshow("Display_edges",frame_edge);
            cvWaitKey(1);

            cout<<" GE_GEDT_edges: "<<GE_GEDT_edges.graphEdge.size()<<endl;
*/

            //Timer timer_optimize;
            //timer_optimize.Start();

            Timer Opti_timer;
            Opti_timer.Start();

            Graph_RSLT RSLT_Curt_tmp;
            RSLT_Curt_tmp = GraphOptimize(map, GE_GEDT_edges, priorBoundaryArea, priorBoundaryLen, frame_edge);

            Opti_timer.Stop();
            Opti_time_tmp = Opti_timer.ElapsedTime();
            Opti_time = Opti_time + Opti_time_tmp;

            cout<<"Optimization Time: "<<Opti_time_tmp<<endl;

            cout<<"Total Time: "<<ED_time_tmp+EBF_time_tmp+Graph_time_tmp+Opti_time_tmp<<endl;

            //timer_optimize.Stop();
            //printf("Graph Optimization detects <%d> edge segments in <%4.2lf> ms\n\n", map->noSegments, timer_optimize.ElapsedTime());

//timer_start.Stop();
//cout<<" edgeFragments "<< edgeFragments.size()<<" GE "<<GE_GEDT_edges.graphEdge.size()<<" DT "
//   <<GE_GEDT_edges.graphEdgeDT.size()<<" time graph edges "<<timer_start.ElapsedTime()<<endl;

            Timer other_timer;
            other_timer.Start();

            vector<vector<Pixel> > RSLT_ESs_tmp;
            for(int i = 0; i < RSLT_Curt_tmp.EFs.size(); i++){
                vector<Pixel> es_tmp;
                for( int j = RSLT_Curt_tmp.EFs[i].start_idx; j< RSLT_Curt_tmp.EFs[i].end_idx+1; j++){
                    int tmpi = RSLT_Curt_tmp.EFs[i].seg_id;

                    int r = map->segments[tmpi].pixels[j].r;
                    int c = map->segments[tmpi].pixels[j].c;

                    struct Pixel tmppt = {r,c};
                    es_tmp.push_back(tmppt);

                }

                RSLT_ESs_tmp.push_back(es_tmp);
                es_tmp.clear();
                vector<Pixel>().swap(es_tmp);
            }

            if(RSLT_Curt_tmp.EFs.size()>1){
                //cout<<" RSLT_Curt_tmp.length: "<<RSLT_Curt_tmp.length<<endl;
                RSLT_Curt = RSLT_Curt_tmp;
                RSLT_ESs.swap(RSLT_ESs_tmp);
            }

//cout<<" Debug----------------5 "<<endl;
            //Mat curtPoly_img;
            curtPoly_img = Mat::zeros(height, width, CV_8UC1);//
//cout<<" Debug----------------6 "<<endl;
            //cout<<" 2 EFs.size(): "<<RSLT_Curt.EFs.size()<<endl;
            //cout<<" 2 DTs.size(): "<<RSLT_Curt.GEDTs.size()<<endl;


            for(int i = 0; i < RSLT_ESs.size(); i++){
                for( int j = 0; j< RSLT_ESs[i].size(); j++){

                    int r = RSLT_ESs[i][j].r;
                    int c = RSLT_ESs[i][j].c;
//cout<<" rslt: "<<" R "<<r<<" C "<<c<<endl;
                    if(i == 0){
                        Vec3b & color = frame_edge.at<Vec3b>(r,c);
                        color[0] = 0;
                        color[1] = 0;
                        color[2] = 255;
                    }else{
                        Vec3b & color = frame_edge.at<Vec3b>(r,c);
                        color[0] = 0;
                        color[1] = 0;
                        color[2] = 255;
                    }

                    curtPoly_img.at<unsigned char>(r,c) = 100;
                }
                //namedWindow("Display", WINDOW_AUTOSIZE);
                //imshow("Display",frame_edge);
                //cvWaitKey(0);
            }

            other_timer.Stop();
            other_time_tmp = other_timer.ElapsedTime();
            Other_time = Other_time + other_time_tmp;
            cout<<"other time: "<<other_time_tmp<<endl;



//---------------------------------------
//            imwrite("grouped_edges.png",curtPoly_img);

            for(int i = 0; i < RSLT_Curt.GEDTs.size(); i++){

                line(frame_edge,Point(RSLT_Curt.GEDTs[i].sp.c, RSLT_Curt.GEDTs[i].sp.r),Point(RSLT_Curt.GEDTs[i].ep.c, RSLT_Curt.GEDTs[i].ep.r),Scalar(0,0,255),1,8,0);
                line(curtPoly_img,Point(RSLT_Curt.GEDTs[i].sp.c, RSLT_Curt.GEDTs[i].sp.r),Point(RSLT_Curt.GEDTs[i].ep.c, RSLT_Curt.GEDTs[i].ep.r),Scalar(100),1,8,0);

            }
//cout<<" Debug----------------7 "<<endl;
//            imwrite("tracked_boundary.png",curtPoly_img);//-----------------------------------

            namedWindow("Display", WINDOW_AUTOSIZE);
            imshow("Display",frame_edge);
            cvWaitKey(1);

            char frmNameOLP[20];
            sprintf(frmNameOLP, "../RESULTS_IROS2017_BMVC2017/EGF_New_Tests/MarkCupPourWater/%04d.png",frame_id);
            string frmNameStrOLP;
            frmNameStrOLP = frmNameOLP;
            imwrite(frmNameStrOLP,frame_edge);

//cout<<" Debug----------------8 "<<endl;
//cout<<" lastPoly_img "<<lastPoly_img.size().width<<" "<<lastPoly_img.size().height<<endl;
//cout<<" curtPoly_img "<<curtPoly_img.size().width<<" "<<curtPoly_img.size().height<<endl;

//namedWindow("DDD", WINDOW_AUTOSIZE);
//imshow("DDD",curtPoly_img);
//cvWaitKey(0);

            lastPoly_img = curtPoly_img.clone();
/*
            char frmNameR[20];
            sprintf(frmNameR, "../Edge_Track_Results/MarkCupPourWater/%04d.png",frame_id);
            string frmNameStr;
            frmNameStr = frmNameR;
            imwrite(frmNameStr,lastPoly_img);


            string fname = "../Edge_Track_Results/MarkCupPourWater.txt";
            const char* filename = fname.c_str();

            FILE *fpt = fopen(filename, "a");
            fprintf(fpt, "ES %d EFsplit %d EFNum %d GPNum %d EDTime %f EBFTime %f GraphConst %f OtherTime %f OptimTime %f",
                    map->noSegments,edgeFragments.size(),GE_GEDT_edges.graphEdge.size(),GE_GEDT_edges.graphEdgeDT.size(),
                    ED_time_tmp, EBF_time_tmp,Graph_time_tmp, other_time_tmp, Opti_time_tmp);
            fprintf(fpt,"\n");
            fclose(fpt);
*/
            cout<<"============================================================================"<<endl;


//namedWindow("LLL", WINDOW_AUTOSIZE);
//imshow("LLL",curtPoly_img);
//cvWaitKey(0);

//cout<<" Debug----------------9 "<<endl;
            priorBoundaryArea = RSLT_Curt.area;
            //cout<<" lastLength: "<<priorBoundaryLen;
            priorBoundaryLen = RSLT_Curt.length;
            //cout<<" currentLength: "<<priorBoundaryLen<<endl;


            //namedWindow("frame_edge",1);
            //imshow("frame_edge",frame_edge);
            //cvWaitKey(10000);


            //vector<GraphEdge> graphEdgeDetected;
            //graphEdgeDetected = GraphEdgeDetected(map, edgeFragments, Dist);

            //timer4.Stop();
            //printf("Distance Transform <%d> edge segments in <%4.2lf> ms\n\n", edgeFragments.size(), timer4.ElapsedTime());


            //namedWindow("Dis_Test",1);
            //normalize(lastDis_img, lastDis_img, 0, 1., NORM_MINMAX);
            //imshow("Dis_Test",lastDis_img);

            //cout<<" dist pixel "<<lastDis_img.at<float>(map->segments[1].pixels[1].r,map->segments[1].pixels[1].c)<<endl;
            //cout<<"  second pixel "<<lastDis_img.at<float>(map->segments[1].pixels[1].r+1,map->segments[1].pixels[1].c+1)<<endl;

            //imwrite("dis_test.png",lastPoly_img);
            //cout<<"----distance transform---4--"<<endl;
            //cvWaitKey(10000);

            //tracking scheme

            //draw_contour(frame_edge, points, Scalar(255,255,0), 1);//draw tracked contour of the current frame
            //namedWindow("Display", WINDOW_AUTOSIZE);
            //imshow("Display",frame_edge);

            //save_track_results(points,  "TRACKING_RESULTS.txt");

            videor.write(frame_edge);
            frame_id++;
            k = cvWaitKey(pk) & 255;

            if(k == 115){// "s"
                pk = 0;
            }
            else if(k == 114){// "r"
                pk = 1;
            }
        }//if(!tracking_flag){}else{
//}
        frame_whole_id++;
        if(k == 27){
            break;
            //return 0;
        }
        else if(k == 32){

        }

        delete map;

    }//end for(;;)

    cout<<"=======================statistic summation========================="<<endl;
    cout<<"Average Edge Drawing Time: "<< ED_time/frame_id<<endl;
    cout<<"Average Edge Fragments Time: "<<EBF_time/frame_id<<endl;
    cout<<"Average Graph Construct Time: "<<Graph_time/frame_id<<endl;
    cout<<"Average Edge Fragments: "<<EFNum/frame_id<<endl;
    cout<<"Average Gap Filling: "<<GPNum/frame_id<<endl;
    cout<<"Average Optimization Time: "<<Opti_time/frame_id<<endl;
    cout<<"Overall average Time: "<<(ED_time+EBF_time+Graph_time+Opti_time+Other_time)/frame_id<<endl;
    cout<<"FPS: "<<1000.0/((ED_time+EBF_time+Graph_time+Opti_time+Other_time)/frame_id)<<endl;
    cout<<"==================================================================="<<endl;

    return 0;
}