简介:同样的物体在不同的光照环境中会出现色彩偏差,为了减少色彩偏差对系统检测造成的影响,在对图像进行识别前,需要先评估图像明暗度,计算出明暗阈值,通过对图像的色彩平衡处理,可以校正图像色偏,过饱和或饱和度不足的情况。
注:本节内容在虚拟仿真环境中意义不大,在实际应用场景中实用性较强。
本文以下图(虚拟仿真环境截图存为colorBanlance.png)为例,编码讲解具体的实现过程。
1、计算明暗阈值
这一步简单来说就是图像拆分为红绿蓝3通道,每个通道取最大值做为白色参考,取指定比例位置的值做为黑色参考,低于黑色参考值的部分一律认定为黑色。
image:待处理图像
scale:图像缩放比例,缩小图像可减少计算量,增加鲁棒性
percentage:下限取值百分比
def calculate_color_balance_thresholds(image, scale, percentage):
#缩小图像,减少计算量
resized_image = cv2.resize(image, (0, 0), fx=scale, fy=scale)
#计算图像高度
H = resized_image.shape[0]
#道路在图像的下半部分,裁剪图像,保留下边2/3,去除部分影响因素
cropped_image = resized_image[int(H * 0.3): (H - 1), :, :]
#将图像拆分为红、绿、蓝三通道
channels = cv2.split(cropped_image)
lower_threshold = []
higher_threshold = []
#遍历3个通道,取每个通道最大值做为白色参考值,取通道指定比例处的值做为黑色参考值
for idx, channel in enumerate(channels):
height, width = channel.shape
num_pixels = width * height
flattened = channel.reshape(num_pixels)
flattened = np.sort(flattened)
lower_threshold.append(flattened[int(math.floor(num_pixels * percentage))])
higher_threshold.append(flattened[int(math.floor(num_pixels-1))])
return lower_threshold, higher_threshold
2、根据明暗阈值调整图像颜色
图像拆分为3通道,每个通道对比黑白参考值,大于白参考值的统一设置为白参考值,小于黑参考值的统一设置为黑参考值,然后将调整过的通道缩放到0~255之间。
def apply_mask(matrix, mask, fill_value):
masked = np.ma.array(matrix, mask=mask, fill_value=fill_value)
return masked.filled()
def apply_threshold(matrix, low_value, high_value):
low_mask = matrix < low_value
matrix = apply_mask(matrix, low_mask, low_value)
high_mask = matrix > high_value
matrix = apply_mask(matrix, high_mask, high_value)
return matrix
def apply_color_balance(lower_threshold, higher_threshold, image):
if lower_threshold is None:
return None
#将图像拆分为红、绿、蓝三通道
channels = cv2.split(image)
out_channels = []
#遍历3个通道,将每个通道进行色彩平衡处理
for idx, channel in enumerate(channels):
#通道中不在阈值范围内的点赋值为高低阈值参数
thresholded = apply_threshold(channel, lower_threshold[idx], higher_threshold[idx])
#归一化处理,通道数值缩放到指定的范围(0~255)
normalized = cv2.normalize(thresholded, thresholded.copy(), 0, 255, cv2.NORM_MINMAX)
out_channels.append(normalized)
return cv2.merge(out_channels)
3、测试
image = cv2.imread("images/colorBanlance.png")
cv2.imshow("image", image)
lower_threshold, higher_threshold = calculate_color_balance_thresholds(image, 0.2, 0.4)
cb_image = apply_color_balance(lower_threshold, higher_threshold, image)
cv2.imshow("cb_image", cb_image)
cv2.waitKey(0)
cv2.destroyAllWindows()
左侧为原图,右侧为处理后的图片,可见处理后色彩对比更加明显,方便后续图像识别处理。
4、加入ROS系统
将色彩平衡功能做为一个节点加入ROS系统,因为环境光照参数变化不会太明显,所以色彩阈值参数不需要对每一张图片都进行处理,我们可以间隔一定时间计算一次,发布给相关节点使用。
进入工作空间目录:
$ cd ~/myros/catkin_ws/src
创建功能包:
$ catkin_create_pkg anti_instagram rospy sensor_msgs duckietown_msgs
新建源码文件:
$ touch anti_instagram/src/anti_instagram_node.py
编辑编译配置文件
$ gedit anti_instagram/CMakeLists.txt
修改为:
创建启动脚本:
$ mkdir -p anti_instagram/launch
$ touch anti_instagram/launch/start.launch
$ gedit anti_instagram/launch/start.launch
<launch>
<arg name="veh"/>
<arg name="pkg_name" value="anti_instagram"/>
<arg name="node_name" value="anti_instagram_node"/>
<arg name="param_file_name" default="default" doc="Specify a param file. ex:megaman"/>
<arg name="required" default="false" />
<group ns="$(arg veh)">
<remap from="anti_instagram_node/image/compressed" to="duckiebot_node/image/compressed"/>
<node name="anti_instagram_node" pkg="$(arg pkg_name)" type="$(arg node_name).py" respawn="true" respawn_delay="10" output="screen" required="$(arg required)"></node>
</group>
</launch>
编辑源码文件:
$ gedit anti_instagram/src/anti_instagram_node.py
附源码:
#!/usr/bin/env python3
import rospy
import math
import cv2
import numpy as np
from multiprocessing import Lock
from cv_bridge import CvBridge
from sensor_msgs.msg import CompressedImage
from duckietown_msgs.msg import AntiInstagramThresholds
class AntiInstagramNode():
def __init__(self):
rospy.init_node('anti_instagram_node', anonymous=False)
# 每隔5秒重新计算阈值参数
self.interval = 5
self.percentage = 0.8
self.output_scale = 0.2
self.bridge = CvBridge()
self.image_msg = None
#初始化阈值参数
self.lower_thresholds = [0,0,0]
self.higher_thresholds = [255,255,255]
self.mutex = Lock()
#发布阈值参数话题
self.pub = rospy.Publisher("~thresholds", AntiInstagramThresholds, queue_size=1)
#订阅图像话题
rospy.Subscriber(
"~image/compressed",
CompressedImage,
self.store_image_msg,
buff_size=10000000,
queue_size=1,
)
#启动一个定时器,定时重新计算阈值参数
rospy.Timer(rospy.Duration(self.interval), self.calculate_new_parameters)
#图像存储函数,由于图像不是每帧都处理,所以先做存储,处理时直接调用最近存储的图像
#调试时,函数中注释的内容打开,可看到原始图像和处理后的图像,实际使用时不需要显示
def store_image_msg(self, image_msg):
#image = self.bridge.compressed_imgmsg_to_cv2(image_msg, "bgr8")
#cb_image = self.apply_color_balance(self.lower_thresholds, self.higher_thresholds, image)
#cv2.imshow("image", image)
#cv2.imshow("cb_image", cb_image)
#cv2.waitKey(1)
with self.mutex:
self.image_msg = image_msg
#图像解码函数
def decode_image_msg(self):
with self.mutex:
try:
image = self.bridge.compressed_imgmsg_to_cv2(self.image_msg, "bgr8")
except ValueError as e:
print(f"Anti_instagram cannot decode image: {e}")
return image
#取最近的图像,计算色彩平衡阈值参数,并发布相关话题
def calculate_new_parameters(self, event):
if self.image_msg is None:
print("Waiting for first image!")
return
image = self.decode_image_msg()
(self.lower_thresholds, self.higher_thresholds) = self.calculate_color_balance_thresholds(
image, self.output_scale, self.percentage
)
msg = AntiInstagramThresholds()
msg.low = self.lower_thresholds
msg.high = self.higher_thresholds
self.pub.publish(msg)
def calculate_color_balance_thresholds(self, image, scale, percentage):
resized_image = cv2.resize(image, (0, 0), fx=scale, fy=scale)
H = resized_image.shape[0]
cropped_image = resized_image[int(H * 0.3): (H - 1), :, :]
channels = cv2.split(cropped_image)
lower_threshold = []
higher_threshold = []
for idx, channel in enumerate(channels):
height, width = channel.shape
num_pixels = width * height
flattened = channel.reshape(num_pixels)
flattened = np.sort(flattened)
lower_threshold.append(flattened[int(math.floor(num_pixels * percentage))])
higher_threshold.append(flattened[int(math.floor(num_pixels-1))])
return lower_threshold, higher_threshold
def apply_mask(self, matrix, mask, fill_value):
masked = np.ma.array(matrix, mask=mask, fill_value=fill_value)
return masked.filled()
def apply_threshold(self, matrix, low_value, high_value):
low_mask = matrix < low_value
matrix = self.apply_mask(matrix, low_mask, low_value)
high_mask = matrix > high_value
matrix = self.apply_mask(matrix, high_mask, high_value)
return matrix
def apply_color_balance(self, lower_threshold, higher_threshold, image):
if lower_threshold is None:
return None
channels = cv2.split(image)
out_channels = []
for idx, channel in enumerate(channels):
thresholded = self.apply_threshold(channel, lower_threshold[idx], higher_threshold[idx])
normalized = cv2.normalize(thresholded, thresholded.copy(), 0, 255, cv2.NORM_MINMAX)
out_channels.append(normalized)
return cv2.merge(out_channels)
if __name__ == "__main__":
node = AntiInstagramNode()
rospy.spin()
返回工作空间:
$ cd ~/myros/catkin_ws
编译:
$ catkin_make
运行:同时启动多个ROS节点时,我们目前的做法是开启多个终端,非常的不方便,所以将常用节点放到同一个启动脚本中,同时需要修改部分脚本与源码,具体修改内容如下:
$ cd ~/myros/catkin_ws
新建并编辑启动脚本:
$ gedit start.launch
<launch>
<arg name="veh" default="duckiebot"/>
<group>
<include file="$(find duckiebot)/launch/duckiebot.launch">
<arg name="veh" value="$(arg veh)"/>
</include>
<include file="$(find anti_instagram)/launch/start.launch">
<arg name="veh" value="$(arg veh)"/>
</include>
</group>
</launch>
修改部分之前面课程文件内容:
$ gedit src/duckiebot/launch/duckiebot.launch
修改 <arg name="veh" value="duckietown"/> 为 <arg name="veh"/>
$ gedit src/duckiebot/src/duckiebot_node.py
批量替换 “/duckietown/duckiebot_node/” 为 “~”
$ gedit src/key_control/src/key_control_node.py
批量替换“/duckietown”为“/duckiebot”
运行需要2个终端,一个运行roscore与duckiebot节点以及色彩平衡节点,一个开key_control节点:
注:每新开一个终端,都要执行环境变量设置命令
$ source ~/myros/catkin_ws/devel/setup.bash
终端1:$ roslaunch start.launch
终端2:$ rosrun key_control key_control_node.py
通过键盘上的w/s/a/d键控制小车移动方向,空格键急停切换,q键退出
终端1打开的图像窗口可以看到图像处理前后的差别,可通过终端3控制小车行走:
注:如果没有图像显示,查看源码41~45行,将注释打开