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
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
