# 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.