cost_kernel

int x = 0;
    int y = 0;
    int Dir[8][2] = {{-1,1},{0,1},{1,1},{-1,0},{1,0},{-1,-1},{0,-1},{1,-1}};
    list<Point> vec_p;
    list<Point> vec_p_b;

    ofstream outputfile;
    outputfile.open ("/home/lcy/CLionProjects/narrow_passage/map/matrixoutput.txt");

    vector<vector<Point>> vec_result(15);
    vector<vector<uint8_t>> vec_history(200,vector<uint8_t >(200,0));

    Point temp;
    temp.x = x;
    temp.y = y;
    vec_p.push_back(temp);
    int num_total = 0;
    for(int i = 1 ; i < 11 ; i ++){

        while(!vec_p.empty()) {
            Point p = vec_p.front();
            vec_p.pop_front();

            for (int j = 0; j < 8; j++) {
                int x_ = p.x + Dir[j][0];
                int y_ = p.y + Dir[j][1];


                if (abs(x_ - x) == i || abs(y_ - y) == i) {

                    Point point_result;
                    point_result.x = x_;
                    point_result.y = y_;
                    if(vec_history[point_result.x + 100][point_result.y + 100] == 0) {
                        temp.x = x_;
                        temp.y = y_;
                        vec_p_b.push_back(temp);

                        vec_history[point_result.x + 100][point_result.y + 100] = 1;
                        vec_result[i].push_back(point_result);
                        num_total++;
                        printf("{%d,%d},", x_, y_);
                        outputfile <<"{"<<x_<<","<<y_<<"}"<<",";

                    }
                }
            }
        }
        if(vec_p.empty()) {
            vec_p.swap(vec_p_b);
        }
        printf("\n");
        printf("num_total:%d\n", num_total);

        outputfile << endl;
    }
    outputfile.close();
    printf("num_total:%d\n", num_total);

    Mat temp_map = Mat::zeros(200,200,CV_8UC1);
    for(int i = 0 ; i < vec_result.size() ; i ++) {
        for (int j = 0; j < vec_result[i].size(); j++) {

            temp_map.at<uint8_t>(vec_result[i][j].x + 100, vec_result[i][j].y + 100) = i * 10 + 50;
        }

    }
    namedWindow("result", CV_WINDOW_NORMAL);
    imshow("result", temp_map);

    waitKey();