---
# System prepended metadata

title: Note code

---

# Note code #
## Function dialog ##
- Thông số trong các ô Base from Rail, Gripper from Base, và Camera from Gripper là lấy trong /data/handeye_calib/CADData.txt
- Lưu dữ liệu vào CADData.txt dùng hàm SaveCADData trong handeyecalib.cpp

## Load and show poses on TableView ##
- When open a dialog, QWidget *parent is call
- LoadData(): load from txt files,....
- DisplayDataModel();

## Stopper Detection ##
- Stopper is used to align the frame position.
- In order to detect Stopper, a color segmentation algorithm is used.
    - In which, images are converted into HSV format. 
    - Then, determine the color of the Stopper. 
    - Late on, set the HSV image in range of Hue of the Stopper's color. Actually, set ranges for H, S, V.

- After having segmentation mask, the contours of Stopper are searched. Then, based on those contours, we find the center of Stoppers.
- At the beggining step of retrieving or storing, we need to set color and ROI??? of stoppers in each grid. (Should be improved).

## Marker Detection ##
- In order to use the marker detection function, we need to collect 4 coordinates of robot where the camera can detect markers. 
    - 4 markers at 4 corners of grid: Top-left marker, Top-right marker, Bottom-right marker, and Bottom-left marker.
    - In order to collect those coordinates, control robot arm move to each marker, get coordinate, click save to ref button.
    - After having 4 coordinates of markers, save them, then compute frame marker, then save them.
    - Those coordinates are saved into /data/pose-data/robot_markers_pose.txt

- After collecting corner marker of grid, press Marker Detection button in MainWindow, it will control robot arm move to each marker and detect them.

## Virtual Function - Một hàm linh động cho nhiều ứng dụng khác nhau cùng lúc. ##
- Hàm ảo (virtual function) được sử dụng để linh động trong việc sử dụng các hàm.
- Ví dụ:
    - Một ứng dụng có thể điều khiển cho 3 loại robot khác nhau. Và các nút chức năng điều khiển chuyển động chỉ cần 1 giao diện, và tùy chọn loại robot được set up mà các hàm hoạt động khác nhau.
    - Có thể dùng hàm if? Yes. Nhưng có lẽ không hay hoặc kém linh động. Và việc khai báo các hàm trở nên phức tạp, mỗi robot điều có 1 hàm riêng, trở nên quá nhiều, khó quản lý.
        - Dùng hàm ảo. Đặt 1 class 0 chứa các hàm: move, connect, gripper.... 
            Ví dụ: `virtual bool move(std::vector<double> robot_pose) = 0`.
        - Đối với robot 1 sẽ có class 1 và sẽ đặt kế thừa từ class 0 đối với các hàm move, connect,... 
            - Trong class 1, khai báo các hàm move:
                - `bool move(std::vector<double> robot_pose)`
            - Tương tự với các robot 2 và 3 cũng sẽ có các hàm tương tự vậy.
            - Như vậy 
            - Khi gọi hàm move: robot_.move() thì tùy vào robot_ được định vị ở pointer nào của một trong ba robot mà nó sẽ gọi hàm move của robot tương ứng.
                - robot_ được declare bằng 1 shared_ptr, và khi gán địa chỉ pointer cho nó thì dùng make_shared (xem trong ASRS project, file systemgroup, tại hàm SetClassPtrs) 

## Eye-in-Hand Calibration ##
1. Find intrinsic and distortion matrix:
- Function: CalibrateCamera in camcalibration.cpp
    - Các ảnh được sử dụng được lưu trữ trong 1 thư mục. Gồm 27 ảnh.
        - thu thập tự do.

### Code_Flow ###

- ComputeExtrinsicOfChessboard:
    - output: rotation vector and translation vector.
- func: FilterVecData: `confused this function, it looks like do nothing`
    - input:
        - vec_cam_to_target_poses: get rotation and translation matrix of extrinsic matrix
        - vec_robot_poses_
- Final function: ComputeHandToCam: 
    - is located in handeyecalib file, 
    - and which is called in functionthread file.
    
    - input: 
        - ref_robot_pose_: poses which robot will move to, get from UI posedatadialog.ui, tab calib.
                - this pose is saved in robot_pose_ref.txt file
        - vec_robot_poses_: get from UI posedatadialog.ui, tab calib
                - vec_robot_poses are saved in txt file: robot_poses_calib.txt
        - ref_cam_to_chess_pose_: get rotation and translation matrix of extrinsic matrix.
            - Get from DetectChessboard function
            - Just at first step. Other steps will collect pose for vec_cam_to_target_poses_
        - vec_cam_to_target_poses_: get rotation and translation matrix of extrinsic matrix.
            - Get from CalibHandEye function.
    - final: cv::calibrateHandEye
    
## How convert from Aruco Marker to Robot arm coordinate ##
- bTo = bTe eTc cTo
    - bTo: transformation matrix from base to object (4x4)
    - bTe: transformation matrix from base to end effector (4x4)
    - eTc: transformation matrix from end effector to camera (4x4)
    - cTo: transformation matrix from camera to object (4x4)
    
- From Aruco Marker, using camera to capture marker, it will give a coordinate of marker with x_m, y_m, z_m, Rx_m, Ry_m, and Rz_m.
    - using opencv Rodrigues function to convert into 4x4 cTo matrix.

- eTc: from calibration process.
- bTe: read the end effector coordinate in handle tool (or GetPosition from UI). Then, convert to 4x4 matrix using Euler function (see calibration code)
- After having 3 matrix, we can get bTo matrix, then we also need convert into Euler angles for Rx, Ry, and Rz before send them to Robot arm for picking objects. 

## Transformation matrix ##
- Rotation matrix is using for computation.
- If we need to find rotation angle for robot grasping, we need to convert into Euler angles.

- Note:

## Retrieve magazine from Grid Process ##
- input:: vec_pose (grid pose: x,y,y, rx, ry, rz)
- 
## Euler Vector to Homogeneous Matrix ##

`CvtEulerVector2HomoginusMatrix`

- From a vector pose (6x1) convert into transformation matrix 4x4 rTg (robot transform to grid position)
    - Goal: ~~get a grid position of the target.~~

- input:: a pose vector of robot arm???
- output:: a transformation matrix from robot to grid position rTg
- The operation flow:
    - seperate translation and rotation values in 6 values
    - declare 4 rotation matrix 3x3
        - r1_mat, r2_mat, r3_mat
        - and r_mat
    - Check which order of Euler coordinates that robot use: XYZ, or ZYX, or ZYZ? (In Manual book of robot)
        - then assign rotation values in pose vector (input para) into r matrix following the Euler coordinates order of robot arm.
            - Ex: with ZYZ Euler:
                    Declare a vector 3x1: r 
                                Rx-> r[2]
                    Rodrigues(r, r1_mat): convert to 3x3 matrix
                                Ry -> r[1]
                    Rodrigues(r, r2_mat)
                                Rz->r[2]
                    Rodrigues(r, r3_mat)
                    r_mat = r1_mat * r2_mat * r3_mat
        r_mat is 3x3 rotation part in Homogeneous matrix.
        
- Then, copy translation coordinate to 3x1 translation part in Homogeneous matrix.
                    
