###### tags: `東京威力 TEL`
{%hackmd DzaUykeiRfWOMkjFL_QkCQ %}

# II. Webcam
## <font color="orange"> 02. 座標轉換 Coordinate Transformation</font>

:star: 這章節的出生一定要感謝我的好朋友 **江鎮宇**，帶領我走入校正的世界 :star:

**總流程是：辨識方塊座標 -> 轉換座標 -> 手臂得到對於它的相對座標 -> 開夾**

:::warning
其實正規流程應該是先對相機進行**相機校正**，使魚眼鏡頭拍的廣角(扭曲)畫面校正成水平鉛直的2D平面，而一些原圖的邊緣會因校正而過濾掉。接著在將相機和手臂原點的位置**轉移矩陣**，乘上校正後的像素座標，得到相對於手臂原點的方塊座標。
$\;\;\;\;\;$然而這次比賽時間很有限，且看到學長姐和別組的相機校正都不太堪用，因此改為手刻。
雖然會標得很累，但手刻等於是將**相機校正和轉移矩陣兩個步驟合而為一**。缺點是如果相機辨識位置改變，就必須重標。還有標點真的很煩很累 XDD
:::

### <font color="pink"> 2-1. 拍照片</font>

此次拍攝的方格紙一個方格大小是 $1.5cm * 1.5cm$。

:::success

幾點注意事項：
1. 校正照片盡量在要辨識時的高度、角度拍攝。
2. 方格越小校正越精確，但如果太小照片清晰度不夠，會標到眼花和心累。
3. 確保清晰的方格有超過夾取範圍一些，太邊緣模糊的就不用標點了。
:::

### <font color="pink"> 2-2. 標點</font>
<font color="yellow">Step 1. 確認手臂和像素座標的XY關係。</font>


<font color="yellow">Step 2. 可用小畫家或OpenCV，根據游標位置找像素點。</font>
:::spoiler {state=open} OpenCV 程式碼

#include <iostream>
#include <opencv2/opencv.hpp>

using namespace cv;

void on_Mouse(int event, int x, int y, int flags, void* param){
    if (event == 0) std::cout << "x: " << x << "  y: " << y << std::endl;
}

int main(){
    Mat src = imread("/home/ditrobotics/TEL/src/opencv/src/first.png");
    cvtColor(src, src, COLOR_RGB2GRAY);
    // src.convertTo(src, -1, 2, 0); //increase the contrast by 2
    namedWindow("src");
    setMouseCallback("src", on_Mouse);
    imshow("src", src);
    waitKey();
}

:::

我覺得用小畫家會更方便些。

<font color="yellow">Step 3. 將數據紀錄到 Excel，格式要一致喔。</font>


<font color="yellow">Step 4. 在 Vscode 中新建 coordinate.csv。</font>
下載延伸模組 **Excel Viewer** 和 **Edit csv** 後，複製 Excel 數據(不包含欄位名稱)到 .csv 中。

 

<font color="yellow">Step 5. 將x_scara由小到大排列。</font>


<font color="yellow">Step 6. 寫轉換座標程式，完成。</font>

:::spoiler 轉換座標程式，只需要填入 csv 的檔案地址、目標點的像素座標。

#include <iostream>
#include <cmath>

#define numOfRow 880
#define pixel_x_min 105
#define pixel_x_max 423
#define pixel_y_min 16 
#define pixel_y_max 448 
#define path "/home/pomelo925/POMELO/TEL/src/opencv/src/coordinate_first.csv"

typedef struct{
    float x_scara;
    float y_scara;
    float x_pixel;
    float y_pixel;
    bool adjacent;
} Coordinate;

void tranform_coordinate(const char *file_name, int goal_x, int goal_y){
    /**** Import and Transforn Data ****/
    FILE *file;
    file=fopen(file_name, "r");

    Coordinate COR[numOfRow];
    bool adjacent[numOfRow]={false};

    int records=0;
    int row=0;
    do{
        records = fscanf(file, "%f%f%f%f\n", &COR[row].x_scara, \
            &COR[row].y_scara, &COR[row].x_pixel, &COR[row].y_pixel);
        if(records==4) row++;
        else return;
    } while(!feof(file));


    /**** transformation ****/
    /* Step 1. reachable goal*/
    if(goal_x < pixel_x_min || goal_y < pixel_y_min \
        || goal_x > pixel_x_max || goal_y < pixel_y_min){
        printf("Out of Range !! Goal pixel: %d, %d\n", goal_x, goal_y);
        return;
    }


    /* Step 2. find adjacent point of X-coordinate*/    
    for(int i=0; i<numOfRow; i++){
        if(goal_x<=COR[i].x_pixel && goal_x>=COR[i+1].x_pixel 
        && fabs(COR[i+1].x_pixel-COR[i].x_pixel)<50) COR[i].adjacent=true;
    } 


    /* Step 3. determine nearest point by the distance of Y-coordinate*/ 
    double distance[numOfRow]={0};
    int nst=0;
    bool flag=true;
    for(int i=0; i<numOfRow; i++){
        if(COR[i].adjacent==true){
            distance[i] = COR[i].y_pixel - goal_y;  
            if(distance[i]<0)distance[i]=0;
            if(flag){
                nst=i;
                flag=false;
            }
            if(distance[nst]>distance[i] && distance[i]!=0) nst=i;
        }
    }


    /* Step 4. find adjacent four point*/
    /*  (x1,y1)                 (x2,y2)
                (goal_x,goal_y)
        (x3,y3)                 (x4,y4)*/ 
    double x,y,x1,y1,x2,y2,x3,y3,x4,y4;
    for(int i=0; i<numOfRow; i++){
        if(COR[i].x_scara==COR[nst].x_scara+1.5 \
            && COR[i].y_scara==COR[nst].y_scara){
            x2 = COR[i].x_pixel;
            y2 = COR[i].y_pixel;
            x1 = COR[i+1].x_pixel;
            y1 = COR[i+1].y_pixel;
            break;
        }
    }
    x4 = COR[nst].x_pixel;
    y4 = COR[nst].y_pixel;
    x3 = COR[nst+1].x_pixel;
    y3 = COR[nst+1].y_pixel;
    
    /* Step 5. bilinear interpolation*/ 
    double scx=0, scy=0;
    scx += ((y1-goal_y)/(y1-y3))*COR[nst].x_scara + ((goal_y-y3)/(y1-y3))*(COR[nst].x_scara+1.5);
    scx += ((y2-goal_y)/(y2-y4))*COR[nst].x_scara + ((goal_y-y4)/(y2-y4))*(COR[nst].x_scara+1.5);
    scx /= 2;

    scy += ((goal_x-x1)/(x2-x1))*COR[nst].y_scara + ((x2-goal_x)/(x2-x1))*(COR[nst].y_scara+1.5);
    scy += ((goal_x-x3)/(x4-x3))*COR[nst].y_scara + ((x4-goal_x)/(x4-x3))*(COR[nst].y_scara+1.5);
    scy /= 2;
    printf("\n*** SCARA ***\nX: %.3f\tY:%.3f\n", scx, scy);

    fclose(file);
    file = NULL;
}


int main(int argc, char **argv){
    double goal_x, goal_y;
    std::cin >> goal_x >> goal_y;
    tranform_coordinate(path, goal_x, goal_y);
    return 0;
}
:::
    
---
<font color="lightblue">這坨程式碼的簡介：</font>
* <font color="lightblue">Excel 標點不需要每行每列都固定的個數。</font>
* <font color="lightblue">Excel 標點的單位可是任意長度。</font>
* <font color="lightblue">要改變的參數都在 `#define` 裡面：</font>
    * <font color="lightblue">csv 檔案地址：`path`，即 `tranform_coordinate()` 的參數。</font>
    * <font color="lightblue">標點邊界值：`const float pixel_x_min`、`const float pixel_x_max`、`const float pixel_y_min`、`const float pixel_y_max`。</font>
    * <font color="lightblue">資料總筆數：`numOfRow`。</font>
* <font color="lightblue">標點時的軸向必須和上面一致，要改軸向等最後判斷完在調整 `scx`、 `scy` 的值就好。</font>