[A]Suppose that there are 7 points. [B]First, draw a circle with a radius around Point 1. The radius can be set by parameter ‘eps’. There are another parameter ‘min_samples’ which means minimum number of points in the circle of Point. In this case, the min_sample is set to 3. Because circle of Point 1 does not contain 3 points, Point 1 is not core point. [C]Second, draw a circle with a radius around Point 2. Circle of Point 2 contain 4 points and Point 2 is defined as the core point. Also, Point 1 is included in the circle of core point. Point 1 is defined as border point. The same processes are repeated on all points. [D]Point 7 does not include 3 points and is not included in other circles. Point 7 is defined as noise point. [E]Point 1~6 are recognized as an object.
- eps: The maximum distance between two samples for one to be considered as in the neighborhood of the other, default=0.5
- min_samples: The number of samples in a neighborhood for a point to be considered as a core point, default=5
- metric: The metric to use when calculating distance between insatances in a feature array, default='euclidean'
- core_sample_indices_: Indices of core samples
- components_: Copy of each core sample found by training
- labels_: Cluster labels for each point in the dataset given to fit(). Noisy samples are give the label -1
from sklearn.cluster import DBSCAN
import numpy as np
model = DBSCAN(eps=0.5, min_samples=1)
# vector array
data = np.array([[1,1],[2,0],[3,0],[1,0]])
Clustering = model.fit_predict(data)
print(Clustering)
https://scikit-learn.org/stable/modules/clustering.html#dbscan (2.3.7. DBSCAN)
https://scikit-learn.org/stable/modules/generated/sklearn.cluster.DBSCAN.html
Radar Tracking version 1 is in radar_tracking/src/ti_mmwave_rospkg/src/RadarTrackingVer1.py
This Flowchart shows the algorithm of Radar Tracking version 1.
There are 4 steps.
Each steps are explained below.
This Process diagram also shows the algorithm of Radar Tracking version 1.
Each steps are explained below.
The values (point id, x position, y position, time) of detected points are subscribed from the ti_mmwave_rospkg.
* Package name: ti_mmwave_rospkg
* Launch name: 6843_multi_3d_0.launch
* Topic name: /ti_mmwave/radar_scan
* msg Type : ti_mmwave_rospkg/RadarScan
point_id - The number of scaned points
x - x-coordinate of the point
y - y-coordinate of the point
header.stamp.secs - second
header.stamp.nsecs - nano second
-
Detection range in radial axis : 175(car), 98(person) [m]
-
Range resolution : 0.039 ~ 0.044 [m]
-
Azimuth angle of detection : ±60 [deg]
-
Elevation angle of detection : ±20 [deg]
-
Angular resolution (Azimuth) : 20 [deg]
-
Angular resolution (Elevation) : 58 [deg]
-
Maximum radial velocity : 9.59 [m/s]
(Note: Radial velocity represents how fast object go close or far from RADAR. It does not contain transverse velocity.)
Point id =0 is the criterian for separating current and previous points.
When the point id is 0, the getPoint (Array) is clustered.
The function of 'Clustering' is detecting objects and finding the center of each object.
The eps(radius) for Clustering should be the size of one object.
In the "good" case, the eps is almost same with the size of one person.
In the "bad" case, the eps is bigger than the size of one person. Two person can be perceived as one person.
Clustering(eps = objectSize, min_samples = 2)
objectSize = the size of one object
min_samples = The minimum number of detected points from object. if you increase this number, accuracy would increase.
currentTime (from radar) is the average of the time of current points
Current points and Previous points are clusterd together.
The function of 'ClusteringVel' is identifying the same objects in current points and previous points and calculating the velocity.
The eps(radius) for ClusteringVel should be the size of object moving.
ClusteringVel(eps = objectMovingRange, min_samples = 1)
objectMovingRange = The range of object moving during time difference + "Radar Error"
min_samples = 1 # ClusteringVel judges whether it's the same object or not. So, 1 is enough.
Because radar detect the points within the object randomly, the detected center of current points is not always equal to the real center of person.
Velocity is obtaining by following equation.
The position and velocity of current center point of the cluster is assigned to the 'Data'.
This figure shows the reason of using moving average filter
In case 1, the detected velocity is bigger than real velocity.
In case 2, the detected velocity is smaller than real velocity.
The average of detected velocities of case 1 and 2 becomes almost same with real velocity and the radar error decreases.
For making the moving average filter, "Window set" is defined.
Window set contain k number of "window".
The number of window is equal to the number of objects currently observed.
One window has n number of "Elements".
Element is 1 x 4 matrix which contains X, Y position and X, Y veloctiy of Data.
When the Data (current position and current velocity) is obtained from Second and Third step(of Tutorial), the Data is clustered with first element of each window.
The function of 'ClusteringFilter' is finding the window where Data belongs.
This is same with ClusteringVel.
ClusteringFilter(eps = filterRange, min_samples = 2)
filterRange = The range of object moving for time difference + Radar Error
min_samples = 2 # ClusteringFilter judges whether Data and window is same or not.
If the matched window is founded, the Data becomes the first element and the previous elements are going down one by one.
Object(X,Y,Vx,Vy) (position and velocity of Object) is obtained by applying a moving average filter to that window.
There are three formula to calculate the average of window.
Wieghted moving average and Current-Weighted moving average consider more weight to the recently observed values.
You can set the proper weight value by doing many experiments.
# You can select one fomula and weight by adjusting the PARAMETER in RadarTrackingVer1.py
https://en.wikipedia.org/wiki/Moving_average
If the matched window is not founded, new window is created.
Because Noise always makes new window, window that have not been updated for a long time will be deleted by counting the "Skip number".
When window is updated, skip count becomes 0. When window is not updated, 1 is added to skip count.
When the skip count exceeds the maximum number of skip count, the window would be deleted.
If you set the large window size, than it would be good to "straight" case, but it's not good to "turning" case.
If you set the small window size, than it would be not good to decrease radar error.
Therefore, Window size is very important value of this Radar Tracking.
You should set the value somewhere between decreasing radar error and good to turning case.
For example, the time difference of point id =0 is around 0.03sec.
n (size of window) is 15.
The time interval of filtering is 0.45sec.
People doesn't move very fast (example: turning) in 0.45sec.
Radar Tracking was tested on people moving mainly along the Y-axis direction. The topic from Radar Tracking version 1 and 2 is recorded by rosbag. The bag file is converted into the csv file and the position and velocity graph of person is drawn by using matplotlib.
The below figure is top view. The cell size is 0.5m. The time of experiment is 5 seconds.
The green line is original data which is Data(X,Y,Vx,Vy).
The blue line is filterd data which is Object(X,Y,Vx,Vy).
-
Position
The original has the fluctuation and noise
After filtering, fluctuation is smoothed and noise is removed. -
Velocity
The original has very big fluctuation because of radar error.
After filtering, the tendency is confirmed.
By comparing with the slope of position, we can judge that the obtained velocity is not good.
https://colab.research.google.com/drive/1FAUvLSvKU4EWGs6MzC3Q67TUwbQ0ep9M#scrollTo=D06d4Hf05jRH
TI mmWave's IWR6843ISK is used for this radar tracking.
install numpy and sklearn!
$ sudo apt install python3-pip
$ sudo pip3 install numpy
$ sudo pip3 install scikit-learn
git clone
$ cd ~
$ git clone https://github.com/nabihandres/RADAR_Cluster-and-tracking.git
catkin make
$ cd ~/RADAR_Cluster-and-tracking/radar_tracking
$ catkin_make
$ source devel/setup.bash
add dialout to have acess to the serial ports on Linux
$ sudo adduser <your_username> dialout
allow permission to serial port. (with connecting radar by usb port)
$ sudo chmod a+rw /dev/ttyUSB0
you have to set the parameters depending on what you want to detect.
For example, if you want to detect car, then objectSize would be big.
If you want to detect rabbit, then the objectSize would be small
# You can set these values by adjusting the PARAMETER in RadarTrackingVer1.py
- objectSize = eps for Clustering
- objectMovingRange = eps for ClusteringVel
- filteringRange = eps for ClusteringFilter
- sizeOfWindow = number of elements in window
- maxNumOfSkip = the number of maximum skip count of window. If skip count over this number, that window will be deleted.
- filterMode =
- 0: Simple moving average
- 1: Weighted moving average
- 2: Current-Weighted moving average
- weight = weight for Current-Weighted moving average
launch and get the data from radar
$ roslaunch ti_mmwave_rospkg 6843_multi_3d_0.launch
rosrun the RadarTracking.py
$ rosrun ti_mmwave_rospkg RadarTrackingVer1.py
RadarTracking.py publish following topic
Published Topic : /object_tracking
msg Type : object_msgs/Objects
Objects - [Object 1, Object 2, ... ]
msg Type : Object_msgs/Object
name - object number in Window Set
position - [X, Y, Z]
X: x-coordinate of the object
Y: y-coordinate of the object
Z: z-coordinate of the object (= 0)
velocity - [Vx, Vy, Vz]
Vx: x-velocity of the object
Vy: y-velocity of the object
Vz: z-velocity of the object (= 0)
https://dev.ti.com/ (Explore/ Resource Explorer/ Software/ mmWave Sensors/ Industrial Toolbox/ Labs/ Robotics/ ROS Driver)