def init_ui(self):
self.w = QWidget()
self.w.resize(500, 150)
self.w.setWindowTitle('Parameter')
# tabにするwidget作成
tabs_dict = OrderedDict()
for key in ParamServer.get_all_params().keys(): # 'Func.Param'のkey名のOrderedDict
key_func = ParamServer.get_func_name(key)
# 新しいfuncが見つかったとき、widget作成
if key_func not in tabs_dict:
setting_widget = SettingPanel(key_func)
tabs_dict[key_func] = setting_widget
tab = QTabWidget()
for key in tabs_dict.keys():
tab.addTab(tabs_dict[key], key)
w_layout = QHBoxLayout()
w_layout.addWidget(tab)
self.w.setLayout(w_layout)
self.w.show()
def callback(self, image_msg):
self.last_image_msg = image_msg
cv_image = self.__cv_bridge.imgmsg_to_cv2(image_msg, 'bgr8')
pimg = image_process.ProcessingImage(cv_image)
# 処理負荷軽減のための事前縮小
pre_scale = 1.0 / ParamServer.get_value('system.pre_resize')
pimg.resize(pre_scale)
# 抽象化
pimg.preprocess()
# 直線検出
if ParamServer.get_value('system.detect_line'):
pre_img = pimg.get_img()
pimg.detect_line()
pimg.overlay(pre_img)
# deep learning学習データ用の縮小。 pre_resize * final_resizeの値が最終データとなる
final_scale = 1.0 / ParamServer.get_value('system.final_resize')
pimg.resize(final_scale)
if ParamServer.get_value('system.mono_output'):
self.__pub.publish(
self.__cv_bridge.cv2_to_imgmsg(pimg.get_grayimg(), 'mono8'))
else:
self.__pub.publish(
self.__cv_bridge.cv2_to_imgmsg(pimg.get_img(), 'bgr8'))
def callback(self):
key = self.name
if self.dpi == 1:
value = self.slider.value()
else:
value = (float)(self.slider.value()) / self.dpi
self.textbox.setText(str(value))
ParamServer.set_value(key, value)
def preprocess(self):
if ParamServer.get_value('system.color_filter'):
self.__color_filter()
if ParamServer.get_value('system.to_gray'):
self.__to_gray()
if ParamServer.get_value('system.blur'):
self.__blur()
if ParamServer.get_value('system.detect_edge'):
self.__detect_edge()
def __init__(self):
rospy.init_node('image_process')
self.__cv_bridge = CvBridge()
self.__sub = rospy.Subscriber('image',
Image,
self.callback,
queue_size=1)
self.__pub = rospy.Publisher('image_processed', Image, queue_size=1)
ParamServer.add_cb_value_changed(self.redraw)
def init_ui(self, name, init_value):
self.label = QLabel(ParamServer.get_param_name(name))
self.checkbox = QCheckBox()
self.checkbox.setChecked(ParamServer.get_value(name))
self.connect(self.checkbox, SIGNAL('stateChanged(int)'), self.callback)
layout = QHBoxLayout()
layout.setMargin(0)
layout.addWidget(self.label, 3)
layout.addWidget(self.checkbox, 7)
self.setLayout(layout)
def __detect_edge(self):
if ParamServer.get_value('edge.canny'):
EDGE_TH_LOW = ParamServer.get_value('edge.canny_th_low')
EDGE_TH_HIGH = ParamServer.get_value('edge.canny_th_high')
self.img = cv2.Canny(self.img, EDGE_TH_LOW, EDGE_TH_HIGH)
if ParamServer.get_value('edge.findContours'):
self.__to_gray()
# contours, hierarchy = cv2.findContours(self.img, cv2.RETR_TREE, cv2.CHAIN_APPROX_SIMPLE)
# contours, hierarchy = cv2.findContours(self.img, cv2.RETR_LIST, cv2.CHAIN_APPROX_SIMPLE)
contours, hierarchy = cv2.findContours(self.img, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
self.__to_color()
cv2.drawContours(self.img, contours, -1, (255, 255, 255), 4)
def detect_line(self, color_pre=[0, 255, 0], color_final=[0, 0, 255], thickness=4):
MASK_V1 = [0. / 640., 400. / 480.]
MASK_V2 = [0. / 640., 200. / 480.]
MASK_V3 = [640. / 640., 200. / 480.]
MASK_V4 = [640. / 640., 400. / 480.]
# image mask
if ParamServer.get_value('system.image_mask'):
vertices = np.array([[MASK_V1, MASK_V2, MASK_V3, MASK_V4]], dtype=np.float)
self.__mask(vertices)
# line detect
pre_lines = self.__houghline()
final_lines = self.__extrapolation_lines(pre_lines)
# create image
if len(self.img.shape) == 3:
line_img = np.zeros((self.img.shape), np.uint8)
else:
line_img = np.zeros((self.img.shape[0], self.img.shape[1], 3), np.uint8)
# draw pre_lines
if (pre_lines is None):
return
for x1, y1, x2, y2 in pre_lines[0]:
cv2.line(line_img, (x1, y1), (x2, y2), color_pre, thickness)
self.img = line_img
# draw final_lines
if (final_lines is None):
return
for x1, y1, x2, y2 in final_lines:
cv2.line(line_img, (x1, y1), (x2, y2), color_final, thickness)
self.img = line_img
def init_ui(self):
layout = QVBoxLayout()
# layout.setMargin(0)
layout.setAlignment(Qt.AlignTop)
# func_name.から始まるitemの設定項目を作成
for key in ParamServer.get_all_params().keys():
key_func = ParamServer.get_func_name(key)
if (key_func == self.name):
# typeごとに異なるwidgetを作成
if (ParamServer.get_type(key) == ParamServer.TYPE_BOOL):
widget = CheckboxItem(key, ParamServer.get_value(key))
elif (ParamServer.get_type(key) == ParamServer.TYPE_LINEAR):
widget = SliderItem(key,
ParamServer.get_value(key),
ParamServer.get_lower(key),
ParamServer.get_upper(key))
layout.addWidget(widget)
self.setLayout(layout)
def init_ui(self, name, init_value, range_low, range_high):
# floatの場合、スライドの値1で、反映値0.1とする
self.dpi = 1
if isinstance(init_value, float):
self.dpi = 10
self.label = QLabel(ParamServer.get_param_name(name))
self.slider = QSlider(Qt.Horizontal) # スライダの向き
self.slider.setRange(range_low * self.dpi, range_high * self.dpi) # スライダの範囲
self.slider.setValue(init_value * self.dpi)
self.slider.setTickPosition(QSlider.TicksAbove) # メモリの位置
self.connect(self.slider, SIGNAL('valueChanged(int)'), self.callback)
self.textbox = QLineEdit()
self.textbox.setText(str(init_value))
layout = QHBoxLayout()
layout.setMargin(0)
layout.addWidget(self.label, 3)
layout.addWidget(self.slider, 6)
layout.addWidget(self.textbox, 1)
self.setLayout(layout)
def __color_filter(self):
LOW_B = ParamServer.get_value('color.low_b')
LOW_G = ParamServer.get_value('color.low_g')
LOW_R = ParamServer.get_value('color.low_r')
HIGH_B = ParamServer.get_value('color.high_b')
HIGH_G = ParamServer.get_value('color.high_g')
HIGH_R = ParamServer.get_value('color.high_r')
lower = np.array([LOW_B, LOW_G, LOW_R])
upper = np.array([HIGH_B, HIGH_G, HIGH_R])
hsv_image = cv2.cvtColor(self.get_img(), cv2.COLOR_BGR2HSV)
mask_image = cv2.inRange(hsv_image, lower, upper)
self.img = cv2.bitwise_and(self.get_img(), self.get_img(), mask=mask_image)
area = cv2.countNonZero(mask_image)
return area
def __houghline(self):
THRESHOLD = ParamServer.get_value('houghline.threshold')
MIN_LINE_LENGTH = ParamServer.get_value('houghline.min_line_length')
MAX_LINE_GAP = ParamServer.get_value('houghline.max_line_gap')
self.__to_gray()
return cv2.HoughLinesP(self.img, 1, np.pi / 180, THRESHOLD, MIN_LINE_LENGTH, MAX_LINE_GAP)
def __blur(self):
FILTER_SIZE = (ParamServer.get_value('blur.gau_filter_size'),
ParamServer.get_value('blur.gau_filter_size'))
# bilateralFilterだと色の差も加味する？
# self.img = cv2.bilateralFilter(self.img, 5, 75, 75)
self.img = cv2.GaussianBlur(self.img, FILTER_SIZE, 0)
def __extrapolation_lines(self, lines):
# 検出する線の傾き範囲
EXPECT_RIGHT_LINE_M_MIN = ParamServer.get_value('extrapolation_lines.right_m_min')
EXPECT_RIGHT_LINE_M_MAX = ParamServer.get_value('extrapolation_lines.right_m_max')
EXPECT_LEFT_LINE_M_MIN = ParamServer.get_value('extrapolation_lines.left_m_min')
EXPECT_LEFT_LINE_M_MAX = ParamServer.get_value('extrapolation_lines.left_m_max')
if lines is None:
return None
right_line = np.empty((0, 6), float)
left_line = np.empty((0, 6), float)
for line in lines:
for tx1, ty1, tx2, ty2 in line:
if ty2 > ty1:
x1 = tx1
x2 = tx2
y1 = ty1
y2 = ty2
else:
x1 = tx2
x2 = tx1
y1 = ty2
y2 = ty1
m, b = self.__get_segment(x1, y1, x2, y2)
if EXPECT_RIGHT_LINE_M_MIN < m < EXPECT_RIGHT_LINE_M_MAX:
# right side
right_line = np.append(right_line, np.array([[x1, y1, x2, y2, m, b]]), axis=0)
elif EXPECT_LEFT_LINE_M_MIN < m < EXPECT_LEFT_LINE_M_MAX:
# left side
left_line = np.append(left_line, np.array([[x1, y1, x2, y2, m, b]]), axis=0)
# print 'right lines num:', right_line.size
# print 'left lines num:', left_line.size
if (right_line.size == 0) and (left_line.size == 0):
return None
extrapolation_lines = []
if (right_line.size > 0):
right_m = right_line[:, 4].mean(axis=0)
right_b = right_line[:, 5].mean(axis=0)
right_y_max = right_line[:, 3].max(axis=0)
right_y_min = right_line[:, 1].min(axis=0)
right_x_min = self.__get_point_horizontal(right_m, right_b, right_y_min)
right_x_max = self.__get_point_horizontal(right_m, right_b, right_y_max)
right_x_min = int(right_x_min)
right_x_max = int(right_x_max)
right_y_min = int(right_y_min)
right_y_max = int(right_y_max)
extrapolation_lines.append([right_x_min, right_y_min, right_x_max, right_y_max])
if (left_line.size > 0):
left_m = left_line[:, 4].mean(axis=0)
left_b = left_line[:, 5].mean(axis=0)
left_y_max = left_line[:, 3].max(axis=0)
left_y_min = left_line[:, 1].min(axis=0)
left_x_min = self.__get_point_horizontal(left_m, left_b, left_y_min)
left_x_max = self.__get_point_horizontal(left_m, left_b, left_y_max)
left_x_min = int(left_x_min)
left_x_max = int(left_x_max)
left_y_min = int(left_y_min)
left_y_max = int(left_y_max)
extrapolation_lines.append([left_x_min, left_y_min, left_x_max, left_y_max])
return extrapolation_lines
self.__cv_bridge.cv2_to_imgmsg(pimg.get_grayimg(), 'mono8'))
else:
self.__pub.publish(
self.__cv_bridge.cv2_to_imgmsg(pimg.get_img(), 'bgr8'))
def main(self):
rospy.spin()
if __name__ == '__main__':
gui_mode = True
log_mode = False
for arg in sys.argv:
if (arg == '--disable-gui'):
gui_mode = False
elif (arg == '--logmode'):
log_mode = True
if (log_mode):
ParamServer.set_value('system.detect_line', 0)
ParamServer.set_value('system.mono_output', 1)
process = RosLineDetect()
if (gui_mode):
app = QApplication(sys.argv)
gui = setting_gui.SettingWindow()
app.exec_()
else:
process.main()
def callback(self):
key = self.name
value = self.checkbox.isChecked()
ParamServer.set_value(key, value)