def get_point(a, b, ratio): """get point from a to b with ratio""" p = Point3D() p.x = a[0] + ratio * (b[0] - a[0]) p.y = a[1] + ratio * (b[1] - a[1]) p.z = a[2] + ratio * (b[2] - a[2]) return p
def generate_polygon(point, heading, length, width): """ Generate polygon """ points = [] half_l = length / 2.0 half_w = width / 2.0 sin_h = math.sin(heading) cos_h = math.cos(heading) vectors = [(half_l * cos_h - half_w * sin_h, half_l * sin_h + half_w * cos_h), (-half_l * cos_h - half_w * sin_h, - half_l * sin_h + half_w * cos_h), (-half_l * cos_h + half_w * sin_h, - half_l * sin_h - half_w * cos_h), (half_l * cos_h + half_w * sin_h, half_l * sin_h - half_w * cos_h)] for x, y in vectors: p = Point3D() p.x = point.x + x p.y = point.y + y p.z = point.z points.append(p) return points
def get_velocity(theta, speed): """get velocity from theta and speed""" point = Point3D() point.x = math.cos(theta) * speed point.y = math.sin(theta) * speed point.z = 0.0 return point
def process(self, message): """Processing radar message.""" if not check_stationary_pole(self._pose, self._is_primary): return radar = ContiRadar() radar.ParseFromString(message) transform = get_world_coordinate(self._transform, self._pose) for point in radar.contiobs: point3d = Point3D() point3d.x = point.longitude_dist point3d.y = point.lateral_dist point3d.z = 0 convert_to_world_coordinate(point3d, transform, None) radar_point = frame_pb2.RadarPoint() radar_point.type = self._type radar_point.position.x = point3d.x radar_point.position.y = point3d.y radar_point.position.z = point3d.z #radar_point.position.z = 0 point3d.x = point.longitude_dist + point.longitude_vel point3d.y = point.lateral_dist + point.lateral_vel point3d.z = 0 convert_to_world_coordinate(point3d, transform, None) radar_point.direction.x = point3d.x - radar_point.position.x radar_point.direction.y = point3d.y - radar_point.position.y radar_point.direction.z = point3d.z - radar_point.position.z #radar_point.direction.z = 0 self._current_list.append(radar_point) if len(self._current_list) > self._accumulated_num: self._current_list.pop(0)
def initialize_transform(self): if self._extrinsics is None: return q = Quaternion() q.qw = self._extrinsics['transform']['rotation']['w'] q.qx = self._extrinsics['transform']['rotation']['x'] q.qy = self._extrinsics['transform']['rotation']['y'] q.qz = self._extrinsics['transform']['rotation']['z'] d = Point3D() d.x = self._extrinsics['transform']['translation']['x'] d.y = self._extrinsics['transform']['translation']['y'] d.z = self._extrinsics['transform']['translation']['z'] self._transform = generate_transform(q, d)
def process(self, message): """Processing image message.""" if not check_stationary_pole(self._pose, self._is_primary): return image = CompressedImage() image.ParseFromString(message) camera_image = frame_pb2.CameraImage() camera_image.timestamp = image.header.timestamp_sec dump_img_bin(image.data, self._output_dir, ImageSensor._image_seq) camera_image.image_url = '{}/{}/pic-{}.jpg'.format( self._image_url, os.path.basename(self._output_dir), ImageSensor._image_seq) ImageSensor._image_seq += 1 camera_image.k1 = self._intrinsics['D'][0] camera_image.k2 = self._intrinsics['D'][1] camera_image.k3 = self._intrinsics['D'][4] camera_image.p1 = self._intrinsics['D'][2] camera_image.p2 = self._intrinsics['D'][3] camera_image.skew = self._intrinsics['K'][1] camera_image.fx = self._intrinsics['K'][0] camera_image.fy = self._intrinsics['K'][4] camera_image.cx = self._intrinsics['K'][2] camera_image.cy = self._intrinsics['K'][5] point = Point3D() point.x = 0 point.y = 0 point.z = 0 transform = get_world_coordinate(self._transform, self._pose) convert_to_world_coordinate(point, transform, None) camera_image.position.x = point.x camera_image.position.y = point.y camera_image.position.z = point.z rotation = get_rotation_from_tranform(transform) q = rotation_to_quaternion(rotation) # TODO: either apply it to all or do not apply it #q = apply_scale_rotation(q) camera_image.heading.x = q.qx camera_image.heading.y = q.qy camera_image.heading.z = q.qz camera_image.heading.w = q.qw camera_image.channel = self._channel self._current_list.append(camera_image) if len(self._current_list) > self._accumulated_num: self._current_list.pop(0)
def offer_data(self, frame): """Provide data to current frame.""" for vector4 in self._current_list: point = frame.points.add() point.CopyFrom(vector4) if not self._is_primary: return point = Point3D() point.x = 0 point.y = 0 point.z = 0 transform = get_world_coordinate(self._transform, self._pose) convert_to_world_coordinate(point, transform, None) frame.device_position.x = point.x frame.device_position.y = point.y frame.device_position.z = point.z rotation = get_rotation_from_tranform(transform) q = rotation_to_quaternion(rotation) # TODO: either apply it to all or do not apply it #q = apply_scale_rotation(q) frame.device_heading.x = q.qx frame.device_heading.y = q.qy frame.device_heading.z = q.qz frame.device_heading.w = q.qw