def ground2pixel(self, point):
if point.z != 0:
msg = 'This method assumes that the point is a ground point (z=0). '
msg += 'However, the point is (%s,%s,%s)' % (point.x, point.y,
point.z)
raise ValueError(msg)
ground_point = np.array([point.x, point.y, 1.0])
# An applied mathematician would cry for this
# image_point = np.dot(self.Hinv, ground_point)
# A better way:
image_point = np.linalg.solve(self.H, ground_point)
image_point = image_point / image_point[2]
pixel = Pixel()
# if not self.rectified_input:
# dtu.logger.debug('project3dToPixel')
# distorted_pixel = self.pcm.project3dToPixel(image_point)
# pixel.u = distorted_pixel[0]
# pixel.v = distorted_pixel[1]
# else:
pixel.u = image_point[0]
pixel.v = image_point[1]
return pixel
def vector2pixel(self, vec):
""" Converts a [0,1]*[0,1] representation to [0, W]x[0, H]. """
pixel = Pixel()
cw = self.ci.width
ch = self.ci.height
pixel.u = cw * vec.x
pixel.v = ch * vec.y
return pixel
def ground_to_pixel(self, point):
if point.z != 0:
raise ValueError('This method assumes that the point is a ground '
'point (z = 0). However, the point is ({0}, {1}, '
'{2})'.format(point.x, point.y, point.z))
ground_point = np.array([point.x, point.y, 1.0])
x, y, z = np.linalg.solve(self.homography, ground_point)
return Pixel(u=x / z, v=y / z)
def JoyButton(self, joy_msg):
if (joy_msg.buttons[0] == 1):
rospy.loginfo("[%s] You Press Button A " % (self.node_name))
state = BoolStamped()
state.data = False
self.pub_A.publish(state)
elif (joy_msg.buttons[1] == 1):
rospy.loginfo("[%s] You Press Button B " % (self.node_name))
state = BoolStamped()
state.data = True
self.pub_B.publish(state)
elif (joy_msg.buttons[2] == 1):
rospy.loginfo("[%s] You Press Button X " % (self.node_name))
state = BoolStamped()
state.data = True
self.pub_X.publish(state)
elif (joy_msg.buttons[3] == 1):
rospy.loginfo("[%s] You Press Button Y " % (self.node_name))
state = BoolStamped()
state.data = True
self.pub_Y.publish(state)
else:
some_active = sum(joy_msg.buttons)
if some_active:
rospy.loginfo("[%s] No binding buttons " % (self.node_name))
#gripperrrrrr_cmd
if (joy_msg.buttons[0] == 1):
rospy.loginfo("Grip Open")
g_cmd = Pixel()
g_cmd.u = 1
self.pub_Grip.publish(g_cmd)
elif (joy_msg.buttons[1] == 1):
rospy.loginfo("Grip Close")
g_cmd = Pixel()
g_cmd.u = 2
self.pub_Grip.publish(g_cmd)
else:
rospy.loginfo("Grip Stop")
g_cmd = Pixel()
g_cmd.u = 0
self.pub_Grip.publish(g_cmd)
def vector2pixel(self, vec):
pixel = Pixel()
cw = self.ci_.width
ch = self.ci_.height
pixel.u = cw * vec.x
pixel.v = ch * vec.y
if (pixel.u < 0): pixel.u = 0
if (pixel.u > cw - 1): pixel.u = cw - 1
if (pixel.v < 0): pixel.v = 0
if (pixel.v > ch - 1): pixel.v = 0
return pixel
def rectify_segment(gpg, s1):
pixels_normalized = []
for i in (0, 1):
# normalized coordinates
nc = s1.pixels_normalized[i]
# get pixel coordinates
pixels = gpg.vector2pixel(nc)
uv = (pixels.u, pixels.v)
# rectify
pr = gpg.rectify_point(uv)
# recompute normalized coordinates
t = Pixel(pr[0], pr[1])
v = gpg.pixel2vector(t)
pixels_normalized.append(v)
s2 = Segment(color=s1.color, pixels_normalized=pixels_normalized)
return s2
def ground2pixel(self, point):
ground_point = np.array([point.x, point.y, 1.0])
image_point = np.dot(self.Hinv, ground_point)
image_point = image_point / image_point[2]
pixel = Pixel()
if not self.rectified_input:
distorted_pixel = self.pcm_.project3dToPixel(image_point)
pixel.u = distorted_pixel[0]
pixel.v = distorted_pixel[1]
else:
pixel.u = image_point[0]
pixel.v = image_point[1]
def ground2pixel(self, point):
# TODO check whether z=0 or z=1.
# I think z==1 (jmichaux)
ground_point = np.array([point.x, point.y, 1.0])
image_point = self.Hinv * ground_point
image_point = np.abs(image_point / image_point[2])
pixel = Pixel()
if not self.rectified_input:
distorted_pixel = self.pcm_.project3dToPixel(image_point)
pixel.u = distorted_pixel[0]
pixel.v = distorted_pixel[1]
else:
pixel.u = image_point[0]
pixel.v = image_point[1]
def vector_to_pixel(self, vector):
camera_width = self.camera_info.width
camera_height = self.camera_info.height
return Pixel(u=camera_width * vector.x, v=camera_height * vector.y)
