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'")