def calculatePlane(cam, depth, xl, yl, rl):
point_num = 0
Points = []
for i in range(len(xl)):
if depth[yl[i], xl[i]] != 0:
x, y, z = rgbdTools.getPosition(cam, depth, yl[i], xl[i])
point = [x, y, z]
Points.append(point)
point_num += 1
if point_num == 3:
break
Points = np.array(Points)
a = (Points[1, 1] - Points[0, 1]) * (Points[2, 2] - Points[0, 2]) - (
Points[1, 2] - Points[0, 2]) * (Points[2, 1] - Points[0, 1])
b = (Points[1, 2] - Points[0, 2]) * (Points[2, 0] - Points[0, 0]) - (
Points[1, 0] - Points[0, 0]) * (Points[2, 2] - Points[0, 2])
c = (Points[1, 0] - Points[0, 0]) * (Points[2, 1] - Points[0, 1]) - (
Points[1, 1] - Points[0, 1]) * (Points[2, 0] - Points[0, 0])
d = 0 - (a * Points[0, 0] + b * Points[0, 1] + c * Points[0, 2])
xlist = []
ylist = []
zlist = []
for num, x in enumerate(xl):
if num > 5:
# if 0 :
break
else:
pix_l = pointInRadius(xl[num], yl[num], rl[num])
for pix in pix_l:
m1, n1 = pix
if depth[m1, n1] != 0:
x, y, z = rgbdTools.getPosition(cam, depth, m1, n1)
xlist.append(x)
ylist.append(y)
zlist.append(z)
xarray = np.array(xlist)
yarray = np.array(ylist)
zarray = np.array(zlist)
r = optimize.leastsq(res, [a, b, c, d], args=(xarray, yarray, zarray))
a, b, c, d = r[0]
return a, b, c, d
cam2_depth = cv2.imread('./output/cam2_depth_' + str(view) + '.png',
-1)
cam2_depth_array = np.asanyarray(cam2_depth)
chessboard_found1, corners1 = cv2.findChessboardCorners(
cam1_rgb, (9, 6))
corners1 = np.asanyarray(corners1).squeeze()
chessboard_found2, corners2 = cv2.findChessboardCorners(
cam2_rgb, (9, 6))
corners2 = np.asanyarray(corners2).squeeze()
for p_2d in range(54):
# point2d_pair.append([corners1[p_2d],corners2[53-p_2d]])
m1 = int(round(corners1[p_2d][1]))
n1 = int(round(corners1[p_2d][0]))
if cam1_depth_array[m1, n1] > 0:
x1, y1, z1 = rgbdTools.getPosition(cam1, cam1_depth_array, m1,
n1)
else:
continue
m2 = int(round(corners2[53 - p_2d][1]))
n2 = int(round(corners2[53 - p_2d][0]))
if cam2_depth_array[m2, n2] > 0:
x2, y2, z2 = rgbdTools.getPosition(cam2, cam2_depth_array, m2,
n2)
else:
continue
cam1_point.append([x1, y1, z1])
cam2_point.append([x2, y2, z2])
Transformation = registration.rigid_transform_3D(np.asarray(cam1_point),
np.asarray(cam2_point))
print(Transformation)
temRGB = np.array(template_rgb)
temDepth = np.array(template_depth)
xl, yl, rl = keyPoints.getCircles(template_rgb)
tem_old_xyr = []
temPt2 = []
temPt3 = []
temPoint2 = o3d.geometry.PointCloud()
temPoint3 = o3d.geometry.PointCloud()
plane_flag = 0
for indt, x in enumerate(xl):
tem_old_xyr.append([xl[indt], yl[indt], rl[indt]])
# print(tem_old_xyr)
for xyr in tem_old_xyr:
x2, y2, z2 = rgbdTools.getPosition(RealSense, temDepth, xyr[1], xyr[0])
temPt2.append([x2, y2, z2])
temp_l = keyPoints.pointInRadius(xyr[0], xyr[1], 1)
for p in temp_l:
m2, n2 = p
x, y, z = rgbdTools.getPosition(RealSense, temDepth, m2, n2)
temPt3.append([x, y, z])
temPoint2.points = o3d.utility.Vector3dVector(np.array(temPt2))
temPoint3.points = o3d.utility.Vector3dVector(np.array(temPt3))
temFeatureList, tem_new_xyr = registration.extractFeatures(temPoint2,
tem_old_xyr,
n=3)
while True:
# Pt2 = []
depth_image = np.asanyarray(depth_frame.get_data())
color_image1 = cv2.cvtColor(color_image, cv2.COLOR_RGB2BGR)
if plane_flag == 0:
chessboard_found1, corners1 = cv2.findChessboardCorners(
color_image1, (9, 6))
corners = np.asanyarray(corners1).squeeze()
if chessboard_found1:
# cv2.drawChessboardCorners(color_image1,(9,6),corners1,chessboard_found1)
Points = []
for corner in corners:
n = int(round(corner[0]))
m = int(round(corner[1]))
if depth_image[m, n] > 0:
x, y, z = rgbdTools.getPosition(cam, depth_image, m, n)
Points.append([x, y, z])
Points = np.array(Points)
a = (Points[1, 1] - Points[0, 1]) * (Points[2, 2] - Points[
0, 2]) - (Points[1, 2] - Points[0, 2]) * (Points[2, 1] -
Points[0, 1])
b = (Points[1, 2] - Points[0, 2]) * (Points[2, 0] - Points[
0, 0]) - (Points[1, 0] - Points[0, 0]) * (Points[2, 2] -
Points[0, 2])
c = (Points[1, 0] - Points[0, 0]) * (Points[2, 1] - Points[
0, 1]) - (Points[1, 1] - Points[0, 1]) * (Points[2, 0] -
Points[0, 0])
d = 0 - (a * Points[0, 0] + b * Points[0, 1] +
c * Points[0, 2])
z_min = minl(Points[:, 2])
Pt2 = []
Pt3 = []
Pt4 = []
img = cv2.imread(path_to_rgbd + 'color/color_{}.png'.format(j))
rgb = np.array(img)
depth = cv2.imread(path_to_rgbd + 'depth/depth_{}.png'.format(j), -1)
depth = np.asarray(depth)
xl, yl, rl = keyPoints.getCircles(img)
if j == 0:
a, b, c, d = keyPoints.calculatePlane(RealSense, depth, xl, yl, rl)
TablePlane.getParam(a, b, c, d)
for ind, x in enumerate(xl):
x, y, z = rgbdTools.getPosition(RealSense, depth, yl[ind], xl[ind])
if abs(TablePlane.a * x + TablePlane.b * y + TablePlane.c * z +
TablePlane.d) / (TablePlane.a**2 + TablePlane.b**2 +
TablePlane.c**2)**0.5 < 0.008:
x, y, z = rgbdTools.getPosition(RealSense, depth, yl[ind], xl[ind])
Pt3.append([x, y, z])
p_l = keyPoints.pointInRadius(xl[ind], yl[ind], rl[ind])
for p in p_l:
m2, n2 = p
x, y, z = rgbdTools.getPosition(RealSense, depth, m2, n2)
Pt2.append([x, y, z])
for mm in range(0, 480):
for nn in range(0, 640):
x, y, z = rgbdTools.getPosition(RealSense, depth, mm, nn)
if y < (TablePlane.a * x + TablePlane.c * z + TablePlane.d) / (
