# ELEC3210-Project Report
| Name |Student ID| Email | Contribution |
|-------------|----------|---------------------|---|
|LO Shih-heng |20654493 |[e-mail] |Keyboard control, localization, ball following, writing launch file|
|DINH Anh Dung|20565783 |[e-mail]|Image detection and positioning, rviz marker, report writing|

## Abstract
This project aims to implement Simultaneous localization and mapping (SLAM) on a mobile robot in a simulation environment. The robot can be driven using keyboard input, and by analyzing laser scan data a map of the environment can be constructed, which allows for determination of the robot's position within the map. The robot can also detect visual images on the walls using camera and determine their positions on the map. Control of the robot can be switched to automatic mode, where the robot will follow a ball moving in the environment.

## Environment setup
### Environments used
* ROS: Noetic
* Ubuntu 20.04
* V-REP 3.6.2
* OpenCV 4.2.0
* rviz 1.14.10
### Additional packages
* teleop_twist_keyboard 1.0.0
* hector_slam 0.5.2
### Setup
1. Launch roscore

    roscore

2. Use the following command to start the environment and robot

    ./V-REP_PRO_EDU_V3_3_2_64_Linux/vrep.sh 

alternatively, we can directly launch the enviroment in main.launch.

    <node name="env" pkg="elec3210_project" type="vrep.sh" args="$(find elec3210_project)/env/env.ttt"/> 

and press the "play" button in V-REP.
3. Launch the controller and rviz and stuff with the following command

    roslaunch elec3210_project main.launch

## Methodology

### Overview
<!--  -->

### Task 1: Control using keyboard
Node: teleop
Publish Topic: /vrep/cmd_vel
Subscribe Topic: None

We used the [teleop_twist_keyboard](http://wiki.ros.org/teleop_twist_keyboard) package and map the output from `/cmd_vel` to `/vrep/cmd_vel` topic in order to control the robot. This is implemented fully in the launch file `main.launch`.

### Task 2: Build rviz 2D map
Node: hector_mapping
Publish Topic: /slam_out_pose /map/
Subscribe Topic: /initialpose /vrep/scan

We used the [hector_slam](http://wiki.ros.org/hector_slam) package which generates the map and publishes to `/map`. At the same time hector_slam estimates the location of the robot and publishes to `/slam_out_pose`.

We have found some limitations of the hector slam package, e.g., it is sensitive to rapid rotation changes, which will distort map construction because it doesn't use IMU as a close-loop sensor.

### Task 3: Determine robot position in map
Node: robot_locationing
Publish Topic: None
Subscribe Topic: /slam_out_pose

Since robot position is published to `/slam_out_pose`, we can determine which room the robot is in by comparing its position against some known keypoints, found by moving the robot around and recording its position at room boundaries. For example, the robot x location is bigger than 4.7 meters, the robot must be in room D.

### Task 4: Detect and localize images
Node: face_recognizer
Publish Topic: /vrep/marker
Subscribe Topic: /vrep/image, /vrep/scan, /slam_out_pose

This task involves 3 sub-tasks: Detecting images, finding its position and putting markers on rviz.

#### Image detection
In this task, we get the camera image from `/vrep/image`. The image needs to be flipped before processing.

For detecting images on the wall, we based our implementation on [OpenCV's guide about Feature Matching + Homography to find Objects](https://docs.opencv.org/4.2.0/d1/de0/tutorial_py_feature_homography.html). [SIFT detector](https://docs.opencv.org/4.2.0/da/df5/tutorial_py_sift_intro.html) is used to extract features from the given 5 images as well as the camera image. At each time step, the features of camera input is compared with each source image, each giving a list of matching features using [FLANN based matching](https://docs.opencv.org/4.2.0/d5/d6f/tutorial_feature_flann_matcher.html) and [Lowe's ratio test](https://www.cs.ubc.ca/~lowe/papers/ijcv04.pdf) to filter only good matches. If the list of matches is good enough (at least 10 matches and average feature-distance is at most 150), the image is considered as matching. By finding the transformation from source image features to camera features, we can get the source image boundary on the camera image.

||
|---|
|*pic001.jpg detected. The 10 keypoints corresponding to the 10 most closely matching features are shown as green circles. The detected image bounding box is shown in blue. For debug purpose, average feature-distance of the 10 features is about 46.*|

#### Image localization
Using the transformation matrix found above, we can find the source image center's corresponding pixel position on the camera. Since the camera perspective parameters are known, the pitch and yaw angles from the camera to image center can be found:

$yaw=\frac{x-resolution_x/2}{resolution_x}\times angle_x=\frac{x-256}{512}\times\frac{\pi}{4}$
$pitch=\frac{y-resolution_y/2}{resolution_y}\times angle_y=-\frac{y-256}{512}\times\frac{\pi}{4}$

The distance from camera to image can be found using laser scan. The laser scan range is [-90, 90] degrees with respect to camera, so the laser ray pointing to the image center can be determined as:

$laser\ angle=\frac{\pi}{2}+yaw$

Distance to image therefore is:

$distance=\frac{laser[laser\ angle]}{cos(pitch)}$

#### rviz Marker
A sphere marker is added to rviz at the calculated position of the detected image. This is done by calculating the transformation matrix from map to image center:

$M_{map\rightarrow image}=T_{map\rightarrow camera}\times R_{map\rightarrow image}\times T_{camera\rightarrow image}$

where $T_{map\rightarrow camera}$ is the transformation matrix from map to camera, equivalent to robot position found via /slam_out_pose; and $T_{camera\rightarrow image}$ is the transformation matrix from camera to image, equal to $distance$. $R_{map\rightarrow image}$ is the rotation matrix from map to image, calculated by multiplying the quaternions of robot orientation, 90 degree yaw rotation to align to camera, and $pitch$+$yaw$ rotation:

$q_{map\rightarrow image}=q_{robot}\times q_{yaw=-\pi/2}\times q_{pitch, -yaw}$

(negative sign of yaw is due to positive camera x-axis corresponding to clockwise rotation)

The translation from this matrix is applied to the marker.

### Task 5: Visual Servoing
Node: ball_tracker
Publish Topic: /cmd_vel
Subscribe Topic: /vrep/image

#### Preprocessing
Similar to task 4, we get the camera image from /vrep/image and flip it. Next, we used a color filter to filter out the yellow object with OpenCV built-in funciton `inRange`:

yellow_mask = cv2.inRange(cv_image, (0, 100, 100), (30, 255, 255))

After filtering, we get a binary image in which the pixels are marked as 1 if they were yellow, otherwise 0. We then used another OpenCV built-in function `findContours` to find the boundary of yellow objects. Note that since the yellow color varies on the surface of the target ball, we used a loose range of filtering, which leads to some noise from the pictures on the wall or background in the filtered result. As a result, we set a threshold for the minimum area to track.

contours, hierarchy = cv2.findContours(
            yellow_mask, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
countour = max(contours, key=cv2.contourArea)
countour_area = cv2.contourArea(countour)
if countour_area > 1000:  # to avoid tracking some random thing
    (x, y, w, h) = cv2.boundingRect(countour)

After further filtering, we get a clean ball mask which is helpful for tracking.

#### Tracking method
We used PID control to achieve smooth tracking of the ball. First, we calculated the mass center of the ball and compared it with the center of the camera's frame for left-right control. We then get the area of the ball in the frame, inversely related to the distance to the ball, as the current value of the PID control in the forward-backward direction.

def control(self, current_value, target_value):
    self.error = target_value - current_value
    self.integral += self.error
    self.derivative = self.error - self.error_prev
    self.error_prev = self.error
    value = self.kp*self.error + self.ki*self.integral + self.kd*self.derivative
    value = self.constrain(value)
    return value

The function is implemented to control both the left-right velocity and the backward-forward velocity values.

#### Results
In our experiments, by using the parametes shown below, the tracking of the ball is rapid, smooth, and stable. It has been tested for more than 8 hours continuously without fail.

left_right_pid = PID_Controller(kp=-0.3, kd=0.35, min_val=-1.3, max_val=1.3)
for_backward_pid = PID_Controller(kp=0.7, kd=0.03, min_val=-1.2, max_val=1.2)

### Task 6: Write launch file
We can launch several nodes at once as shown below. (below is a simplified version of the project's actual launch file):

<launch>
   
    <node name="env" pkg="elec3210_project" type="vrep.sh" args="$(find elec3210_project)/env/env.ttt"/>

    <node name="odom2base_broadcaster" pkg="tf" type="static_transform_publisher" args="0 0 0 0 0 0 $(arg odom_frame) $(arg base_frame) 100"/>

    <node name="teleop" pkg="teleop_twist_keyboard" type="teleop_twist_keyboard.py" launch-prefix="terminator -x " output="screen" />
    
    <node name="rviz" pkg="rviz" type="rviz" args="-d $(find elec3210_project)/env/rviz.rviz"/>
    
    <include file="$(find elec3210_project)/launch/hector_slam.launch" />

    <node name="robot_locationing" pkg="elec3210_project" type="locationing" output="screen"/>

    <node name="face_recoginzer" pkg="elec3210_project" type="face_recognizer.py" output="screen"/>
    
    <node name="ball_tracker" pkg="elec3210_project" type="ball_tracker.py" output="screen"/>

</launch>

## Conclusion

### Applications
This project demonstrates that the libraries provided by ROS can be very useful for localization and mapping of robot in any arbitrary environment. Similarly, OpenCV provides useful methods to understand visual clues from the environment, which helps in the localization and mapping task. The methodology of this project can be helpful for robot navigation within areas such as houses, offices, etc. where walls and visualization can assist in robot positioning and manuever.

### Limitations & Future Work
There are some limitations in this work regarding image localization and ball tracking:
* Images are currently detected by comparing features of the given 5 source images to the camera input. While processing time can be reduced by computing source image features on initialization, this may not be as efficient with higher number of images. The detection accuracy also varies with images, with average feature-distance ranging from 50 to nearly 150 for the given images.
* For improvement, image detection can be done using OpenCV's face detection algorithms instead (such as [Haar Cascade](https://docs.opencv.org/4.2.0/db/d28/tutorial_cascade_classifier.html)), and the image can be classified using PCA and eigenfaces trained on the given dataset.
* The distance to image is found using the corresponding laser scan. This may give false result if there is obstacles in the laser path, or if the wall is not perpendicular to ground.
* A possible improvement is to calculate distance using the corners of the image, since the size of the image and camera parameters are known, such as described by [Amestoy and Ortiz](https://www.ncbi.nlm.nih.gov/pmc/articles/PMC6960959/).
* Ball tracking is done by detecting the ball center and performing necessary rotations to follow the ball at close distance. While this works for the slow-moving ball, the algorithm may not be able to follow a faster moving ball that can move out of camera range or hide behind obstacles. The robot would continue the last computed velocity until the ball is within view again.
* hector_slam sometimes freeze after running for a while which causes rviz and robot position to not be updated. This happens randomly and usually after running the simulation for about 3-5 minutes.

## References
* ROS Wiki - teleop_twist_keyboard: http://wiki.ros.org/teleop_twist_keyboard
* ROS Wiki - hector_slam: http://wiki.ros.org/hector_slam
* OpenCV - Feature Matching + Homography to find Objects: https://docs.opencv.org/4.2.0/d1/de0/tutorial_py_feature_homography.html
* OpenCV - Introduction to SIFT (Scale-Invariant Feature Transform): https://docs.opencv.org/4.2.0/da/df5/tutorial_py_sift_intro.html
* OpenCV - Feature Matching with FLANN: https://docs.opencv.org/4.2.0/d5/d6f/tutorial_feature_flann_matcher.html
* D. Lowe (2004) - Distinctive Image Features from Scale-Invariant Keypoints: https://www.cs.ubc.ca/~lowe/papers/ijcv04.pdf
* OpenCV - Cascade Classifier: https://docs.opencv.org/4.2.0/db/d28/tutorial_cascade_classifier.html
* M. Amestoy & O. Ortiz (2019) - Global Positioning from a Single Image of a Rectangle in Conical Perspective: https://www.ncbi.nlm.nih.gov/pmc/articles/PMC6960959/
* PID Controller: https://en.wikipedia.org/wiki/PID_controller