Skip to content
Snippets Groups Projects
Commit 137f7cd7 authored by jerry's avatar jerry
Browse files

Slightly more stable/accurate measurement

parent 2572ae01
No related branches found
No related tags found
No related merge requests found
# ROS_human_detection # ROS_human_detection
Human detection model that runs on ROS Human detection model that runs on ROS
This is a human detection mode that uses Intel RealSense D435 Camera and ROS Kinetic. Visit the GitHub page of This is a human detection mode that uses Intel RealSense D435 Camera and ROS Kinetic/Melodic.
Intel RealSense-ROS for more camera installation. This is still a work in progress and I am continuously updating.
If you have any questions or suggestions, please do not hesistate to contact me.
I will answer your questions to the best of my ability :D
Jerry Kuo: jerrykuo820@gmail.com
## How This Model Works
The basic pipeline looks like this.
RGB Image -> Object Detection -> 2D Bounding Box -> Use ROI to Parse Depth Image ->\
Filter Depth Info -> Get 3D Coordinates -> Show the Human Model on PointCloud
I am using <a href="https://github.com/tensorflow/models/tree/master/research/object_detection" target="_blank">
tensorflow's object detection module </a>for getting the 2D bounding box. I might consider to switch over to YOLO, PyTorch.
We will see lol.
Aside from that, everything else is hard-coded. What each function does should be pretty self-explanatory.
## Installing The Workspace ## Installing The Workspace
Run the following commands on terminal to get started. If you are using/ plan to use the D435 RGBD Camera,
visit the GitHub page of Intel RealSense-ROS for full RealSense Camera installation on ROS.
Also keep in mind that the model runs on **Python 2**. Because ros kinetic was designed more for Python 2.
Transferring over to Python 3 is definitely on my list of todo, but just not my priority.
Ok. Now we can start installing the workspace.
First we start a workspace First start by initializing a workspace
```shell ```shell
mkdir -p ~/catkin_workspace/src mkdir -p ~/catkin_workspace/src
cd catkin_workspace/src cd catkin_workspace/src
...@@ -21,7 +42,12 @@ catkin_make ...@@ -21,7 +42,12 @@ catkin_make
source ./devel/setup.bash source ./devel/setup.bash
``` ```
*Something like that. I will fix it later.* Then you are good to go! Open three terminal and run the three following commands.
```shell
roslaunch realsense2_camera rs_rgbd.laucnh
rosrun human_detection webcam.py
rosrun get_obj_dist object_to_distance.py
```
## Sample Data Files ## Sample Data Files
Below are some links to example files. These bag files are too big for GitHub so I put them on Google Drive. Below are some links to example files. These bag files are too big for GitHub so I put them on Google Drive.
...@@ -37,7 +63,7 @@ topics: /camera/color/image_raw 534 msgs : sensor_msgs/Image ...@@ -37,7 +63,7 @@ topics: /camera/color/image_raw 534 msgs : sensor_msgs/Image
``` ```
1. Indoor settings 1. Indoor settings
- *Will update as I go* - *Will update as I go. File is so big lol*
2. Outdoor settings 2. Outdoor settings
- *Will update as I go* - *Same as above*
\ No newline at end of file \ No newline at end of file
source diff could not be displayed: it is too large. Options to address this: view the blob.
File added
import numpy.ma as ma import numpy as np
import cv2
# Really basic way of separating foreground and background.
def filter_background(roi): def filter_background(roi):
ret_val = ma.masked_less(roi, 2900) # Anything further than 8000mm, we consider it as background
# roi[roi <= 2900] # Anything less than 100mm is consider noise
return ret_val ret_val = np.ma.masked_greater(roi, 8000)
ret_val = np.ma.masked_less(ret_val, 100)
unique, counts = np.unique(ret_val.mask, return_counts=True)
def mask_the_thing(image, mask): _dict = dict(zip(unique, counts))
cv2.bitwise_and(image, mask) if False in _dict:
return ret_val, _dict[False]
else:
return ret_val, 0
...@@ -17,20 +17,23 @@ font = cv2.FONT_HERSHEY_SIMPLEX ...@@ -17,20 +17,23 @@ font = cv2.FONT_HERSHEY_SIMPLEX
font_scale = 0.75 font_scale = 0.75
font_color = (255, 255, 255) font_color = (255, 255, 255)
line_type = 2 line_type = 2
size_of_moving_avg = 15
class obj_dist: class obj_dist:
def __init__(self): def __init__(self):
# Initializing ROS Topics # Declare functional variables
self.bridge = CvBridge() self.bridge = CvBridge()
self.moving_average = [3] * size_of_moving_avg
# Initializing ROS Topics
self.dist_pub = rospy.Publisher('/obj_to_dist/human_distance', Image, queue_size=10) self.dist_pub = rospy.Publisher('/obj_to_dist/human_distance', Image, queue_size=10)
self.render_pub = rospy.Publisher('/obj_to_dist/render_human', Marker, queue_size=10) self.render_pub = rospy.Publisher('/obj_to_dist/render_human', Marker, queue_size=10)
self.roi_pub = rospy.Publisher('/obj_to_dist/disappera', Image, queue_size=10) # self.roi_pub = rospy.Publisher('/obj_to_dist/disappera', Image, queue_size=10)
self.bbx_sub = message_filters.Subscriber('/human_detected_image/bounding_box', box_list) self.bbx_sub = message_filters.Subscriber('/human_detected_image/bounding_box', box_list)
self.human_image_sub = message_filters.Subscriber('/human_detected_image/image', Image) self.human_image_sub = message_filters.Subscriber('/human_detected_image/image', Image)
self.depth_image_sub = message_filters.Subscriber('/camera/depth/image_rect_raw', Image) self.depth_image_sub = message_filters.Subscriber('/camera/depth/image_rect_raw', Image)
# self.pcl_sub = message_filters.Subscriber('/camera/depth_registered/points', PointCloud2)
ts = message_filters.ApproximateTimeSynchronizer([self.bbx_sub, self.human_image_sub, ts = message_filters.ApproximateTimeSynchronizer([self.bbx_sub, self.human_image_sub,
self.depth_image_sub], self.depth_image_sub],
...@@ -49,19 +52,16 @@ class obj_dist: ...@@ -49,19 +52,16 @@ class obj_dist:
def get_human_distance(self, cv_depth, cv_image, box): def get_human_distance(self, cv_depth, cv_image, box):
roi_depth = cv_depth[box.ymin:box.ymax, box.xmin:box.xmax] roi_depth = cv_depth[box.ymin:box.ymax, box.xmin:box.xmax]
filtered_depth, _size = filter_background(roi_depth)
print(filtered_depth.data)
if _size:
self.moving_average.pop()
self.moving_average.insert(0, filtered_depth.sum() / _size / 1000.0)
jk = np.shape(roi_depth) current_avg = sum(self.moving_average) / size_of_moving_avg
jk = jk[0]*jk[1]
filtered_depth = filter_background(roi_depth)
_size = len(filtered_depth)
print('size of original: {}'.format(jk)) x_meter = self.get_x_in_meters(box.xmin, box.xmax, current_avg)
print('size of filtered: {}'.format(_size)) cv2.putText(cv_image, '{} meters'.format(round(current_avg, 2)),
print('')
avg_distance = filtered_depth.sum() / _size / 1000
x_meter = self.get_x_in_meters(box.xmin, box.xmax, avg_distance)
cv2.putText(cv_image, '{} meters'.format(avg_distance),
(box.xmin, box.ymax - 100), (box.xmin, box.ymax - 100),
font, font,
font_scale, font_scale,
...@@ -69,43 +69,69 @@ class obj_dist: ...@@ -69,43 +69,69 @@ class obj_dist:
line_type) line_type)
# self.roi_pub.publish(self.bridge.cv2_to_imgmsg(filtered_depth.data)) # self.roi_pub.publish(self.bridge.cv2_to_imgmsg(filtered_depth.data))
self.roi_pub.publish(self.bridge.cv2_to_imgmsg(roi_depth)) # self.roi_pub.publish(self.bridge.cv2_to_imgmsg(roi_depth))
self.dist_pub.publish(self.bridge.cv2_to_imgmsg(cv_image)) self.dist_pub.publish(self.bridge.cv2_to_imgmsg(cv_image))
return x_meter, 0, avg_distance return x_meter, 0, current_avg
def render_human(self, cood): def render_human(self, cood):
marker = Marker() marker = Marker()
marker.header.frame_id = 'camera_color_optical_frame' marker.header.frame_id = 'camera_color_optical_frame'
marker.header.stamp = rospy.Time.now() marker.header.stamp = rospy.Time.now()
marker.id = 0 marker.id = 1
marker.action = Marker.ADD marker.action = Marker.ADD
marker.lifetime = rospy.Duration() marker.lifetime = rospy.Duration()
marker.type = Marker.SPHERE # marker.type = Marker.SPHERE
marker.type = Marker.MESH_RESOURCE
marker.mesh_resource = 'package://get_obj_dist/human_model.STL'
marker.pose.position.x = cood[0] marker.pose.position.x = cood[0]
marker.pose.position.y = cood[1] marker.pose.position.y = cood[1]
marker.pose.position.z = cood[2] marker.pose.position.z = cood[2]
marker.scale.x = 0.5 marker.pose.orientation.x = 0.0
marker.scale.y = 0.5 marker.pose.orientation.y = 0.0
marker.scale.z = 0.5 marker.pose.orientation.z = 0.0
marker.pose.orientation.x = 0 marker.color.r = 1.0
marker.pose.orientation.y = 0
marker.pose.orientation.z = 0
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0 marker.color.g = 1.0
marker.color.b = 0.0 marker.color.b = 1.0
marker.color.a = 1.0
marker.scale.x = 1.0
marker.scale.y = 1.0
marker.scale.z = 1.0
# marker.id = 0
# marker.action = Marker.ADD
# marker.lifetime = rospy.Duration()
# marker.type = Marker.SPHERE
#
# marker.pose.position.x = cood[0]
# marker.pose.position.y = cood[1]
# marker.pose.position.z = cood[2]
#
# marker.scale.x = 0.5
# marker.scale.y = 0.5
# marker.scale.z = 0.5
#
# marker.pose.orientation.x = 0
# marker.pose.orientation.y = 0
# marker.pose.orientation.z = 0
#
# marker.color.a = 1.0
# marker.color.r = 0.0
# marker.color.g = 1.0
# marker.color.b = 0.0
self.render_pub.publish(marker) self.render_pub.publish(marker)
def get_x_in_meters(self, xmin, xmax, z_i): def get_x_in_meters(self, xmin, xmax, z_i):
# Tune z_c to get better value lol. # Tune z_c to get better value lol.
z_c = 100 z_c = 100
ret_val = (z_i*(xmax+xmin-600.0))/(2*z_c) ret_val = (z_i * (xmax + xmin - 600.0)) / (2 * z_c)
return ret_val return ret_val
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment