def get_orientation_ray(cls, perspective, landmark): standard_direction = Vec2(0, 1) top_primary_axes = landmark.get_top_parent().get_primary_axes() our_axis = None for axis in top_primary_axes: if axis.contains_point(perspective): our_axis = axis assert (our_axis != None) new_axis = our_axis.parallel(landmark.representation.middle) new_perspective = new_axis.project(perspective) p_segment = LineSegment.from_points( [new_perspective, landmark.representation.middle]) angle = standard_direction.angle_to(p_segment.vector) rotation = Affine.rotation(angle) o = [cls.orientation] rotation.itransform(o) direction = o[0] ori_ray = Ray(p_segment.end, direction) return ori_ray
def __init__(self, perspective, landmark, poi, orientation): super(OrientationRelation, self).__init__(perspective, landmark, poi) self.standard = Vec2(0,1) self.orientation = orientation top_primary_axes = landmark.get_top_parent().get_primary_axes() our_axis = None for axis in top_primary_axes: if axis.contains_point(perspective): our_axis = axis assert( our_axis != None ) new_axis = our_axis.parallel(self.landmark.representation.middle) new_perspective = new_axis.project(perspective) p_segment = LineSegment.from_points( [new_perspective, self.landmark.representation.middle] ) angle = self.standard.angle_to(p_segment.vector) rotation = Affine.rotation(angle) o = [self.orientation] rotation.itransform(o) direction = o[0] self.ori_ray = Ray(p_segment.end, direction) self.projected = self.ori_ray.line.project(poi) self.distance = self.ori_ray.start.distance_to(self.projected) self.measurement = Measurement(self.distance, required=False)
def calculateObjectPosition(x_pixel, y_pixel): # x_pixel offset = +5 and y_pixel offset = -5 # this is not done and should be set somewhere # Robots world coordinates seen from camera frame x_coordinate_robot_position = 34.7 y_coordinate_robot_position = 33.9 # Resolution in cm per pixel resolution_width = 70.0/611.0 resolution_height = 39.5/344.0 # Robots position seen from camera frame T1 = Affine.translation((x_coordinate_robot_position, y_coordinate_robot_position)) # Robots rotating frame opposite to camera frame - degrees rot = Affine.rotation(180) # Getting robot frame robot_frame = T1*rot # Actual distance seen from camera frame actual_distance_width = resolution_width*x_pixel actual_distance_height = resolution_height*y_pixel # Object position seen from camera frame P_yx_camera_frame = Vec2(actual_distance_height, actual_distance_width) # Object position seen from the robot frame P_xyz_robot_frame = ~robot_frame*P_yx_camera_frame return P_xyz_robot_frame
def get_orientation_ray(cls, perspective, landmark): standard_direction = Vec2(0,1) top_primary_axes = landmark.get_top_parent().get_primary_axes() our_axis = None for axis in top_primary_axes: if axis.contains_point(perspective): our_axis = axis assert( our_axis != None ) new_axis = our_axis.parallel(landmark.representation.middle) new_perspective = new_axis.project(perspective) p_segment = LineSegment.from_points( [new_perspective, landmark.representation.middle] ) angle = standard_direction.angle_to(p_segment.vector) rotation = Affine.rotation(angle) o = [cls.orientation] rotation.itransform(o) direction = o[0] ori_ray = Ray(p_segment.end, direction) return ori_ray
def _get_transform(self, dt): # Set rotation in analogue to direction of velocity rotation = Affine.rotation(self._angle) position = Affine.translation(self._position_in(dt)) return position * rotation