def save_pose(pos_filename, vm_pose):
cfg_parser = TextParser()
# cfg_tabel = cfg_parser("data/vehicle_config.cfg")
with open(pos_filename, "w") as f:
for i, pose in enumerate(vm_pose):
a = np.cos(pose[2])
b = np.sin(pose[2])
t = np.arccos(a) if b > 0 else -np.arccos(a)
msg = "%05d %f %f %f\n" % (pose[4], pose[0], pose[1], t)
f.write(msg)
def disp_pose(self, vm_pose):
cfg_parser = TextParser()
cfg_tabel = cfg_parser("data/vehicle_config.cfg")
mapper = PosMap(cfg_tabel)
for pos in vm_pose:
pos_x, pos_y, pos_yaw, r, id_ = pos
if self.disp_start < id_ < self.disp_end:
mapper.mark_position(pos_x, pos_y, pos_yaw, False)
img_filename = self.can_dir + r"\pose_map.jpg"
mapper.disp_map(img_filename)
def insertDR(self):
cfg_parser = TextParser()
cfg_tabel = cfg_parser("data/vehicle_config.cfg")
vm = VehicleMotion(cfg_tabel["wheel_base"],cfg_tabel["vehicle_width"])
pose = []
vm.setPosition(self.start_point[0],self.start_point[1],self.start_point[2])
_vhcl_can_data = None
for _i,vhcl_can_data in enumerate(self.can_match_logs):
if _i > 0:
times = (vhcl_can_data.data["time_stamp"] - _vhcl_can_data.data["time_stamp"]) #/ time_unit
vm.traject_predict_world(vhcl_can_data, times)
if (vhcl_can_data.data["frameID"] in self.keys):
print "update %d" % vhcl_can_data.data["frameID"]
key_point = self.keypoint_table[vhcl_can_data.data["frameID"]]
vm.setPosition(key_point.x,key_point.y,key_point.yaw_deg)
pose.append((vm.pos.x, vm.pos.y, vm.theta, vm.radius, int(vhcl_can_data.data["frameID"])))
_vhcl_can_data = vhcl_can_data
return pose
def disp_pose(vm_pose):
cfg_parser = TextParser()
cfg_tabel = cfg_parser("data/vehicle_config.cfg")
mapper = PosMap(cfg_tabel)
for pos in vm_pose:
pos_x, pos_y, pos_yaw, r, id_ = pos
if id_ >= start:
mapper.mark_position(pos_x, pos_y, pos_yaw, False)
if id_ == stop:
break
for pos in vm_pose:
pos_x, pos_y, pos_yaw, r, id_ = pos
if id_ >= start:
if id_ in key_point:
print id_
mapper.mark_position(pos_x, pos_y, pos_yaw, True)
mapper.mark_pose_id(pos_x, pos_y, pos_yaw, id_)
if id_ == stop:
break
mapper.disp_map(img_filename)
def generate_pose(can_match_logs):
cfg_parser = TextParser()
cfg_tabel = cfg_parser("data/vehicle_config.cfg")
vm = VehicleMotion(cfg_tabel["wheel_base"], cfg_tabel["vehicle_width"])
pose = []
vm.setPosition(start_point[0], start_point[1], start_point[2])
_vhcl_can_data = None
for _i, vhcl_can_data in enumerate(can_match_logs):
if _i > 0:
print _i,
times = (vhcl_can_data.data["time_stamp"] -
_vhcl_can_data.data["time_stamp"]) #/ time_unit
vm.traject_predict_world(vhcl_can_data, times)
if vhcl_can_data.data["frameID"] in rot_table:
vm.theta += np.deg2rad(
rot_table[vhcl_can_data.data["frameID"]])
pose.append((vm.pos.x, vm.pos.y, vm.theta, vm.radius,
int(vhcl_can_data.data["frameID"]),
vhcl_can_data.data["time_stamp"]))
_vhcl_can_data = vhcl_can_data
return pose
#-*-coding:UTF-8-*-
'''
Created on 2017年3月1日-下午1:34:57
author: Gary-W
'''
import cv2
import numpy as np
import os
from config_parser import TextParser
parser = TextParser()
text_path = r"config.txt"
items_table = parser(text_path)
label_table = {}
cur_line_color = (0, 0, 200)
for key, item in items_table.items():
if key.find("class_") == -1:
continue
try:
id, color = item.split(' ')
id = int(id)
color = color[1:-1]
color = tuple([int(c) for c in color.split(',')])
label_table[id] = {"label": key, "color": color}
except:
pass
img_path = items_table["img_path"]
img_load = cv2.imread(img_path)
print("please input '1' to '9'")