def image_callback(self, data):
global theta
global beta
global tx
global ty
try:
# Convert ROS image to OpenCV image
raw_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# Flip the image 180 degrees
cv_image = cv2.flip(raw_image, -1)
# Draw a black line on the image
cv2.line(cv_image, (0,50), (640,50), (0,0,0), 5)
# cv_image is normal color image
blob_image_center = blob_search(cv_image, self.detector)
# Given world coordinate (xw, yw)
xw = 0.2875
yw = 0.1125
# Only two blob center are found on the image
if(len(blob_image_center) == 2):
x1 = int(blob_image_center[0].split()[0])
y1 = int(blob_image_center[0].split()[1])
x2 = int(blob_image_center[1].split()[0])
y2 = int(blob_image_center[1].split()[1])
print("Blob Center 1: ({0}, {1}) and Blob Center 2: ({2}, {3})".format(x1, y1, x2, y2))
################################# Your Code Start Here #################################
# Calculate beta, tx and ty, given x1, y1, x2, y2
theta = 0
beta = abs(x1-x2)/0.1
if(x1 < x2):
c = x1
r = y1
else:
c = x2
r = y2
tx = (r-Or)/beta - xw
ty = (c-Oc)/beta - yw
################################## Your Code End Here ##################################
print("theta = {0}\nbeta = {1}\ntx = {2}\nty = {3}\n".format(theta, beta, tx, ty))
else:
print("No Blob found! ")
def image_callback(self, data):
global theta
global beta
global tx
global ty
try:
# Convert ROS image to OpenCV image
raw_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# Flip the image 180 degrees
cv_image = cv2.flip(raw_image, -1)
# Draw a black line on the image
cv2.line(cv_image, (0, 50), (640, 50), (0, 0, 0), 5)
# cv_image is normal color image
blob_image_center = blob_search(cv_image, self.detector)
# Given world coordinate (xw, yw)
xw = 0.2875
yw = 0.1125
# Only two blob center are found on the image
if (len(blob_image_center) == 2):
x1 = int(blob_image_center[0].split()[1])
y1 = int(blob_image_center[0].split()[0])
x2 = int(blob_image_center[1].split()[1])
y2 = int(blob_image_center[1].split()[0])
print("Blob Center 1: ({0}, {1}) and Blob Center 2: ({2}, {3})".
format(x1, y1, x2, y2))
Or = 480 / 2
Oc = 640 / 2
beta = np.sqrt((x2 - x1)**2 + (y2 - y1)**2) / 0.1
if (y1 < y2):
tx = -xw + (x1 - Or) / beta
ty = -yw + (y1 - Oc) / beta
elif (y2 < y1):
tx = -xw + (x2 - Or) / beta
ty = -yw + (y2 - Oc) / beta
################################## Your Code End Here ##################################
print("theta = {0}\nbeta = {1}\ntx = {2}\nty = {3}\n".format(
theta, beta, tx, ty))
else:
print("No Blob found! ")
def image_callback(self, data):
global theta
global beta
global tx
global ty
try:
# Convert ROS image to OpenCV image
raw_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# Flip the image 180 degrees
cv_image = cv2.flip(raw_image, -1)
# Draw a black line on the image
cv2.line(cv_image, (0, 50), (640, 50), (0, 0, 0), 5)
# cv_image is normal color image
blob_image_center = blob_search(cv_image, self.detector)
# Given world coordinate (xw, yw)
xw = 0.2875
yw = 0.1125
# Only two blob center are found on the image
if (len(blob_image_center) < 2):
print("Not enough blobs found. " + str(len(blob_image_center)) +
" found.")
elif (len(blob_image_center) == 2):
c1 = int(blob_image_center[0].split()[0])
r1 = int(blob_image_center[0].split()[1])
c2 = int(blob_image_center[1].split()[0])
r2 = int(blob_image_center[1].split()[1])
print("Blob Center 1: ({0}, {1}) and Blob Center 2: ({2}, {3})".
format(c1, r1, c2, r2))
################################# Your Code Start Here #################################
# Calculate beta, tx and ty, given c1, r1, c2, r2
beta = 73 / 0.1
################################## Your Code End Here ##################################
print("theta = {0}\nbeta = {1}\ntx = {2}\nty = {3}\n".format(
theta, beta, tx, ty))
else:
print("To many points returned " + str(len(blob_image_center)) +
" found.")
def image_callback(self, data):
global theta
global beta
global tx
global ty
try:
# Convert ROS image to OpenCV image
raw_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# Flip the image 180 degrees
cv_image = cv2.flip(raw_image, -1)
# Draw a black line on the image
cv2.line(cv_image, (0, 50), (640, 50), (0, 0, 0), 5)
# cv_image is normal color image
blob_image_center = blob_search(cv_image, self.detector)
if (len(blob_image_center) == 0):
print("No blob found!")
self.coord_pub.publish("")
else:
c = int(blob_image_center[0].split()[0])
r = int(blob_image_center[0].split()[1])
# print("Blob found! ({0}, {1})".format(c, r))
################################ Your Code Start Here ################################
# Given theta, beta, tx, ty, calculate the world coordinate of c,r namely xw, yw
theta = 0.0
beta = 730.0
tx = -0.282020547945
ty = -0.150856164384
position_c = np.array([[(r - 249) / beta], [(c - 369) / beta]])
Rot = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
position_w = np.dot(np.linalg.inv(Rot),
position_c - np.array([[tx], [ty]]))
xw = position_w[0][0]
yw = position_w[1][0]
print(xw)
print(yw)
################################# Your Code End Here #################################
xy_w = str(xw) + str(' ') + str(yw)
# print(xy_w)
self.coord_pub.publish(xy_w)
def image_callback(self, data):
global theta
global beta
global tx
global ty
try:
# Convert ROS image to OpenCV image
raw_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# Flip the image 180 degrees
cv_image = cv2.flip(raw_image, -1)
# Draw a black line on the image
cv2.line(cv_image, (0,50), (640,50), (0,0,0), 5)
# cv_image is normal color image
blob_image_center = blob_search(cv_image, self.detector)
if(len(blob_image_center) == 0):
print("No blob found!")
self.coord_pub.publish("")
else:
x = int(blob_image_center[0].split()[0])
y = int(blob_image_center[0].split()[1])
# print("Blob found! ({0}, {1})".format(x, y))
################################ Your Code Start Here ################################
# Given theta, beta, tx, ty, calculate the world coordinate of x,y namely xw, yw
################################# Your Code End Here #################################
xy_w = str(xw) + str(' ') + str(yw)
# print(xy_w)
self.coord_pub.publish(xy_w)
def image_callback(self, data):
global theta
global beta
global tx
global ty
try:
raw_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
cv_image = cv2.flip(raw_image, -1)
cv2.line(cv_image, (0,50), (640,50), (0,0,0), 5)
blob_image_center = blob_search(cv_image, self.detector)
if(len(blob_image_center) == 0):
print("No blob found!")
self.coord_pub.publish("")
elif(len(blob_image_center) == 1):
x1 = int(blob_image_center[0][0])
y1 = int(blob_image_center[0][1])
# x2 = int(blob_image_center[1][0])
# y2 = int(blob_image_center[1][1])
# if x1 > x2:
# T = [x1,x2, y1, y2]
# x2 = T[0]
# x1 = T[1]
# y2 = T[2]
# y1 = T[3]
A = np.array([[x1/beta], [y1/beta]])
Rz = np.array([[math.cos(theta), -1*math.sin(theta)], [math.sin(theta), math.cos(theta)]])
B = Rz.dot(A) + np.array([[tx], [ty]])
xw = B[1,0]
yw = B[0,0]
xy_w = str(xw) + str(' ') + str(yw)
print(xy_w)
self.coord_pub.publish(xy_w)
def image_callback(self, data):
global theta
global beta
global tx
global ty
try:
# Convert ROS image to OpenCV image
raw_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# Flip the image 180 degrees
cv_image = cv2.flip(raw_image, -1)
# Draw a black line on the image
cv2.line(cv_image, (0, 50), (640, 50), (0, 0, 0), 5)
# cv_image is normal color image
blob_image_center, green_center, pink_center = blob_search(
cv_image, self.detector)
print(blob_image_center)
if (len(blob_image_center) == 0):
#print("No blob found!")
self.coord_pub.publish("")
else:
x = int(blob_image_center[0].split()[1])
y = int(blob_image_center[0].split()[0])
xw = (x - 240) / beta - tx
yw = (y - 320) / beta - ty
xy_w = str(xw) + str(' ') + str(yw)
print(xy_w)
self.coord_pub.publish(xy_w)
def image_callback(self, data):
global theta
global beta
global tx
global ty
try:
# Convert ROS image to OpenCV image
raw_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# Flip the image 180 degrees
cv_image = cv2.flip(raw_image, -1)
# Draw a black line on the image
cv2.line(cv_image, (0, 50), (640, 50), (0, 0, 0), 5)
# cv_image is normal color image
blob_image_center = blob_search(cv_image, self.detector)
# Given world coordinate (xw, yw)
xw = 0.2875
yw = 0.1125
# Only two blob center are found on the image
if (len(blob_image_center) == 2):
x1 = int(blob_image_center[0][0])
y1 = int(blob_image_center[0][1])
x2 = int(blob_image_center[1][0])
y2 = int(blob_image_center[1][1])
print("Blob Center 1: ({0}, {1}) and Blob Center 2: ({2}, {3})".
format(x1, y1, x2, y2))
################################# Your Code Start Here #################################
# Calculate beta, tx and ty, given x1, y1, x2, y2
d = ((x1 - x2)**2 + (y1 - y2)**2)**0.5
if x1 > x2:
T = [x1, x2, y1, y2]
x2 = T[0]
x1 = T[1]
y2 = T[2]
y1 = T[3]
beta = d / 0.1 # Pixels per meter
theta = math.asin((y2 - y1) / d)
Rz = np.array([[math.cos(theta), -1 * math.sin(theta)],
[math.sin(theta), math.cos(theta)]])
# Calculate Tx, Ty
A = Rz.dot(np.array([[x1 / beta], [y1 / beta]]))
cam_origin_x = yw - A[0, 0]
cam_origin_y = xw - A[1, 0]
tx = cam_origin_x
ty = cam_origin_y
################################## Your Code End Here ##################################
print("theta = {0}\nbeta = {1}\ntx = {2}\nty = {3}\n".format(
theta, beta, tx, ty))
else:
print("No Blob found! ")
def image_callback(self, data):
global theta
global beta
global tx
global ty
try:
# Convert ROS image to OpenCV image
raw_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# Flip the image 180 degrees
cv_image = cv2.flip(raw_image, -1)
# Draw a black line on the image
cv2.line(cv_image, (0, 50), (640, 50), (0, 0, 0), 5)
# cv_image is normal color image
blob_image_center = blob_search(cv_image, self.detector)
# Given world coordinate (xw, yw)
xw = 0.2875
yw = 0.1125
print(blob_image_center, " BLOB_TEST")
# Only two blob center are found on the image
if (len(blob_image_center) < 2):
print("Not enough blobs found. " + str(len(blob_image_center)) +
" found.")
elif (len(blob_image_center) == 2):
c1 = int(blob_image_center[0].split()[0])
r1 = int(blob_image_center[0].split()[1])
c2 = int(blob_image_center[1].split()[0])
r2 = int(blob_image_center[1].split()[1])
print("Blob Center 1: ({0}, {1}) and Blob Center 2: ({2}, {3})".
format(c1, r1, c2, r2))
################################# Your Code Start Here #################################
# Calculate beta, tx and ty, given c1, r1, c2, r2
crop_top_row = 130
crop_bottom_row = 350
crop_top_col = 130
crop_bottom_col = 450
O_r = -(.5 * (crop_top_col - crop_bottom_col)) # O_r = .5*height
O_c = -(.5 * (crop_top_row - crop_bottom_row)) # O_c = .5*width
beta = 770
theta = .019682 #radians
tx = 7 / beta #meters
ty = 15 / beta #meters
X_c_1 = (r1 - O_r) / beta
Y_c_1 = (c1 - O_c) / beta
X_c_2 = (r2 - O_r) / beta
Y_c_2 = (c2 - O_c) / beta
rotation = np.array(
[[cos(theta * np.pi / 180), -sin(theta * np.pi / 180)],
[sin(theta * np.pi / 180),
cos(theta * np.pi / 180)]])
translation = np.array([[tx], [ty]])
camera_1 = np.array([[X_c_1], [Y_c_1]])
camera_2 = np.array([[X_c_2], [Y_c_2]])
world_1 = np.dot(np.linalg.inv(translation),
np.subtract(camera_1 - translation))
world_2 = np.dot(np.linalg.inv(translation),
np.subtract(camera_2 - translation))
################################## Your Code End Here ##################################
print("theta = {0}\nbeta = {1}\ntx = {2}\nty = {3}\n".format(
theta, beta, tx, ty))
else:
print("To many points returned " + str(len(blob_image_center)) +
" found.")
def image_callback(self, data):
global theta
global beta
global tx
global ty
try:
# Convert ROS image to OpenCV image
raw_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# Flip the image 180 degrees
cv_image = cv2.flip(raw_image, -1)
# Draw a black line on the image
cv2.line(cv_image, (0, 50), (640, 50), (0, 0, 0), 5)
# cv_image is normal color image
blob_image_center = blob_search(cv_image, self.detector)
# Given world coordinate (xw, yw)
xw = 0.2875
yw = 0.1125
# Only two blob center are found on the image
if (len(blob_image_center) < 2):
print("Not enough blobs found. " + str(len(blob_image_center)) +
" found.")
elif (len(blob_image_center) == 2):
c1 = int(blob_image_center[0].split()[0])
r1 = int(blob_image_center[0].split()[1])
c2 = int(blob_image_center[1].split()[0])
r2 = int(blob_image_center[1].split()[1])
print("Blob Center 1: ({0}, {1}) and Blob Center 2: ({2}, {3})".
format(c1, r1, c2, r2))
################################# Your Code Start Here #################################
if c1 < c2:
c = c1
r = r1
else:
c = c2
r = r2
# Calculate beta, tx and ty, given c1, r1, c2, r2
theta = np.arctan((r1 - r2) / (c1 - c2))
theta = theta * 180 / np.pi
beta = np.sqrt((c1 - c2)**2 + (r1 - r2)**2) / 0.1
Ttmp = np.array([[0], [0]])
xc_mat = np.array([[(r - 249) / beta * 1.0],
[(c - 369) / beta * 1.0]])
xw_mat = np.array([[xw], [yw]])
Rot = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
Ttmp = xc_mat - np.dot(Rot, xw_mat)
tx = Ttmp[0][0]
ty = Ttmp[1][0]
################################## Your Code End Here ##################################
print("theta = {0}\nbeta = {1}\ntx = {2}\nty = {3}\n".format(
theta, beta, tx, ty))
else:
print("To many points returned " + str(len(blob_image_center)) +
" found.")
def image_callback(self, data):
global theta
global beta
global tx
global ty
try:
# Convert ROS image to OpenCV image
raw_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# Flip the image 180 degrees
cv_image = cv2.flip(raw_image, -1)
# Draw a black line on the image
cv2.line(cv_image, (0, 50), (640, 50), (0, 0, 0), 5)
# cv_image is normal color image
blob_image_center = blob_search(cv_image, self.detector)
# Given world coordinate (xw, yw)
xw = 0.2875
yw = 0.1125
# Only two blob center are found on the image
if (len(blob_image_center) < 2):
print("Not enough blobs found. and we are here " +
str(len(blob_image_center)) + " found.")
elif (len(blob_image_center) == 2):
c1 = int(blob_image_center[0].split()[0])
r1 = int(blob_image_center[0].split()[1])
c2 = int(blob_image_center[1].split()[0])
r2 = int(blob_image_center[1].split()[1])
print("Blob Center 1: ({0}, {1}) and Blob Center 2: ({2}, {3})".
format(c1, r1, c2, r2))
################################# Your Code Start Here #################################
# Calculate beta, tx and ty, given c1, r1, c2, r2
O_r = cv_image.shape[0] / 2
O_c = cv_image.shape[1] / 2
# O_c -= 200
print("O_r = ", O_r, ", O_c = ", O_c)
dist = ((c1 - c2)**2 + (r1 - r2)**2)**0.5
beta = dist / 0.1
theta = np.arcsin((r1 - r2) / dist)
if (r1 < r2):
r = r1
c = c1
else:
r = r2
c = c2
x_c = (r - O_r) / beta
y_c = (c - O_c) / beta
tx = x_c - xw * np.cos(theta) + yw * np.sin(theta)
ty = y_c - xw * np.sin(theta) - yw * np.cos(theta)
################################## Your Code End Here ##################################
print("theta = {0}\nbeta = {1}\ntx = {2}\nty = {3}\n".format(
theta, beta, tx, ty))
else:
print("To many points returned " + str(len(blob_image_center)) +
" found.")
def image_callback(self, data):
global theta
global beta
global tx
global ty
try:
# Convert ROS image to OpenCV image
raw_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# Flip the image 180 degrees
cv_image = cv2.flip(raw_image, -1)
# Draw a black line on the image
cv2.line(cv_image, (0,50), (640,50), (0,0,0), 5)
# cv_image is normal color image
blob_image_center = blob_search(cv_image, self.detector)
if(len(blob_image_center) == 0):
print("No blob found!")
self.coord_pub.publish("")
else:
c = int(blob_image_center[0].split()[0]) # - 98
r = int(blob_image_center[0].split()[1])
# print("Blob found! ({0}, {1})".format(c, r))
################################ Your Code Start Here ################################
# Given theta, beta, tx, ty, calculate the world coordinate of c,r namely xw, yw
O_r = cv_image.shape[0] / 2
O_c = cv_image.shape[1] / 2
print("O_r = ", O_r, ", O_c = ", O_c)
x_c = (r - O_r) / beta
y_c = (c - O_c) / beta
R = np.array([[np.cos(theta), -np.sin(theta)], [np.sin(theta), np.cos(theta)]])
R_inv = np.linalg.inv(R)
P_c = np.array([[x_c], [y_c]])
T = np.array([[tx], [ty]])
P_w = np.matmul(R_inv, (P_c - T))
xw = P_w[0][0]
yw = P_w[1][0]
# xw = x_c - tx
# yw = y_c - ty
################################# Your Code End Here #################################
xy_w = str(xw) + str(' ') + str(yw)
print(xy_w)
self.coord_pub.publish(xy_w)
def image_callback(self, data):
global theta
global beta
global tx
global ty
try:
# Convert ROS image to OpenCV image
raw_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# Flip the image 180 degrees
cv_image = cv2.flip(raw_image, -1)
# Draw a black line on the image
cv2.line(cv_image, (0,50), (640,50), (0,0,0), 5)
# cv_image is normal color image
blob_image_center = blob_search(cv_image, self.detector)
if(len(blob_image_center) == 0):
print("No blob found!")
self.coord_pub.publish("")
else:
c = float(blob_image_center[0].split()[0])
r = float(blob_image_center[0].split()[1])
print("Blob found! ({0}, {1})".format(str(c), str(r)))
################################ Your Code Start Here ################################
# Given theta, beta, tx, ty, calculate the world coordinate of c,r namely xw, yw
beta = 770
theta = .019682 #radians
tx = -0.272974314536
ty = -0.200657008695
X_c_1 = (r - 220)/beta # width = 220
Y_c_1 = (c - 320)/beta # height = 320
rotation = np.array([[math.cos(theta), -math.sin(theta)],[math.sin(theta),math.cos(theta)]])
translation = np.array([[tx],[ty]])
camera_1 = np.array([[X_c_1],[Y_c_1]])
world_1 = np.matmul(np.linalg.inv(rotation),(camera_1 - translation))
xw = world_1[0][0]
yw = world_1[1][0]
# R = np.array([[np.cos(theta),-np.sin(theta)],[np.sin(theta),np.cos(theta)]])
# Or = 240
# Oc = 320
# xc = (r-Or)/beta
# yc = (c-Oc)/beta
# C = np.array([[xc],[yc]])
# T = np.array([[tx],[ty]])
# R_inv = np.linalg.inv(R)
# M = C-T
# W = np.matmul(R_inv,M)
# xw = W[0][0]
# yw = W[1][0]
#print(world_1, "WORLD_1")
#print(type(world_1), "WORLD_1 FORMAT")
################################# Your Code End Here #################################
xy_w = str(xw) + str(' ') + str(yw)
print(xy_w)
self.coord_pub.publish(xy_w)