def draw_circle(event,x,y,flags,param): global ix,iy,drawing,mode if event == cv2.EVENT_LBUTTONDOWN: drawing = True ix,iy = x,y elif event == cv2.EVENT_LBUTTONUP: drawing = False x0, y0 = x, y print 'ixiy=' + str(ix)+','+str(iy) print 'x0y0=' + str(x0) + ',' + str(y0) udist = depth_frame.get_distance(ix, iy) vdist = depth_frame.get_distance(x0, y0) print udist,vdist point1 = rs.rs2_deproject_pixel_to_point(color_intrin, [ix, iy], udist) point2 = rs.rs2_deproject_pixel_to_point(color_intrin, [x0, y0], vdist) print str(point1)+str(point2) dist = math.sqrt( math.pow(point1[0] - point2[0], 2) + math.pow(point1[1] - point2[1],2) + math.pow( point1[2] - point2[2], 2)) print 'distance: '+ str(dist) if mode == True: cv2.line(images,(ix,iy),(x,y),(0,255,0),100) else: cv2.circle(images,(x,y),5,(0,0,255),100)
def map_location(point: list, eyes: list, video_provider, depth_frame, ref, r: int = 3): da = get_depth(eyes, depth_frame, 4) # Depth of the eye db = get_depth(point, depth_frame, r) if ref == 0 else ref # Depth of the point xa, ya, za = rs.rs2_deproject_pixel_to_point( video_provider.depth_intrinsics, eyes, da) xb, yb, zb = rs.rs2_deproject_pixel_to_point( video_provider.color_intrinsics, point, db) ya = ya * math.cos(theta) + za * math.sin(theta) yb = yb * math.cos(theta) + zb * math.sin(theta) dz = db + da dy = yb - ya dx = xb - xa if dz != 0: yi = ya + (da / dz) * dy xi = xa + (da / dz) * dx if not math.isnan(xi) and not math.isnan(yi): return [round(xi), round(yi)] return [-1, -1]
def displacement(colorized_depth): img1 = cv2.imread('images/d435colored_4.png') img = cv2.imread('images/d435depth_4.png') loop = True while loop: obj = Grabcut() imageGC = obj.GC(img1) plt.imshow(imageGC[..., ::-1]) plt.show() x, y = input('Enter xy values: ').split() x1, y1 = input('Enter other xy values: ').split() depth = aligned_depth_frame.get_distance(int(x), int(y)) dx, dy, dz = rs.rs2_deproject_pixel_to_point(color_intrin, [int(x), int(y)], depth) depth1 = aligned_depth_frame.get_distance(int(x1), int(y1)) dx1, dy1, dz1 = rs.rs2_deproject_pixel_to_point( color_intrin, [int(x1), int(y1)], depth1) # euclid = cv2.norm((dx, dy, dz), (dx1, dy1, dz1), cv2.NORM_L2) # another way to calculate displacement, should be the same result as the distance variable distance = math.sqrt(((dx1 - dx)**2) + ((dy1 - dy)**2) + ((dz1 - dz)**2)) print('Displacement: ', distance, 'meters') print('Displacement: ', distance * 100, 'cm') again = input('Go again? (y or n)') if again == 'y': continue else: loop = False
def computeXYZ(profile, mask, depth_map, color): prof = profile.get_stream(rs.stream.depth) intrin = prof.as_video_stream_profile().get_intrinsics() #print("Camera Parameters:\n {}".format(intrin)) w, h = np.argwhere(mask).T pts = [] rgb = [] for x, y in zip(w, h): pts.append( np.array( rs.rs2_deproject_pixel_to_point(intrin, [x, y], depth_map[x, y]))) rgb.append(color[x, y, :]) pts = np.array(pts) rgb = np.array(rgb) a, b = mask.size xx, yy = np.meshgrid(np.arange(a), np.arange(b)) xx = xx.flatten() yy = yy.flatten() todos_pts = [] todos_rgb = [] for y, x in zip(xx, yy): todos_pts.append( np.array( rs.rs2_deproject_pixel_to_point(intrin, [x, y], depth_map[x, y]))) todos_rgb.append(color[x, y, :]) todos_pts = np.array(todos_pts) return todos_pts, pts, todos_rgb, rgb
def getnewAvgDepth(aligned_depth_frame, Point, depth_intrin, initPoint=None): depthval = aligned_depth_frame.get_distance(int(Point[0]), int(Point[1])) depth_pointCtr = rs.rs2_deproject_pixel_to_point( depth_intrin, [int(Point[0]), int(Point[1])], depthval) depth_values = [] for i in range(-3, 3): for j in range(-3, 3): depthval = aligned_depth_frame.get_distance( int(Point[0] + i), int(Point[1] + j)) depth_point = rs.rs2_deproject_pixel_to_point( depth_intrin, [int(Point[0] + i), int(Point[1] + j)], depthval) if (depth_point[2] == 0): continue depth_values.append(depth_point[2]) med = np.median(depth_values) if initPoint is not None: if (abs(initPoint[2] - med) > 0.3): med = 0 depth_pointCtr[2] = med return depth_pointCtr
def xyz(self, pxl, depth_frame, depth_int, wnd=(0, 0), z_min=10, z_max=1000): dim = np.asanyarray(depth_frame.get_data()).shape # make pixel to int pxl = np.array(pxl) pxl = pxl.astype(int) # lattice lattice = np.array([[x, y] for x in range(-wnd[0], wnd[0] + 1) for y in range(wnd[1] + 1)]) avg = np.array([0. for i in range(3)]) i = 0 for l in lattice: try: pxl_p = pxl + l pxl_n = pxl - l # make sure pixeles are not out of range if any([ any([ p[0] < 0, p[0] >= dim[1], p[1] < 0, p[1] >= dim[0] ]) for p in [pxl_p, pxl_n] ]): continue xyz_p = rs.rs2_deproject_pixel_to_point( depth_int, pxl_p.tolist(), depth_frame.get_distance(pxl_p[0], pxl_p[1])) xyz_p = 1000 * np.array(xyz_p) xyz_n = rs.rs2_deproject_pixel_to_point( depth_int, pxl_n.tolist(), depth_frame.get_distance(pxl_n[0], pxl_n[1])) xyz_n = 1000 * np.array(xyz_n) valid_point = [ xyz_p[2] < z_max and xyz_p[2] > z_min, xyz_n[2] < z_max and xyz_n[2] > z_min ] """ avg += valid_point[0]*xyz_p + valid_point[1]*xyz_n i += sum(valid_point) """ if sum(valid_point) == 2: avg += xyz_p + xyz_n i += 2 except Exception as ex: print(ex) pass if i == 0: return avg return avg / i
def render_result(skeletons, img, confidence_threshold, depth_frame, depth_scale): skeleton_color = (100, 254, 213) skeles_drawn = np.zeros(len(skeletons)) for i, skeleton in enumerate(skeletons): if skeles_drawn[i]: # skip if already drawn continue # draw_skeleton(img, skeleton, confidence_threshold, skeleton_color) # determine distance violations - mh chest1_keypoint = skeleton.joints[1] if (chest1_keypoint[0] < 0 or chest1_keypoint[0] >= 640) or ( chest1_keypoint[1] < 0 or chest1_keypoint[1] >= 480): break # chest1_distance = depth_frame.get_distance(int(chest1_keypoint[0]), int(chest1_keypoint[1])) depth = depth_image[int(chest1_keypoint[1]), int(chest1_keypoint[0])] * depth_scale depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics p1 = rs.rs2_deproject_pixel_to_point( depth_intrin, [int(chest1_keypoint[1]), int(chest1_keypoint[0])], depth) # p1 = [chest1_keypoint[0], chest1_keypoint[1], chest1_distance] for j in range(i + 1, len(skeletons)): skeleton2 = skeletons[j] chest2_keypoint = skeleton2.joints[1] if (chest2_keypoint[0] < 0 or chest2_keypoint[0] >= 640) or ( chest2_keypoint[1] < 0 or chest2_keypoint[1] >= 480): break # chest2_distance = depth_frame.get_distance(int(chest2_keypoint[0]), int(chest2_keypoint[1])) depth = depth_image[int(chest2_keypoint[1]), int(chest2_keypoint[0])] * depth_scale depth_intrin = depth_frame.profile.as_video_stream_profile( ).intrinsics p2 = rs.rs2_deproject_pixel_to_point( depth_intrin, [int(chest2_keypoint[1]), int(chest2_keypoint[0])], depth) # p2 = [chest2_keypoint[0], chest2_keypoint[1], chest1_distance] distance = math.sqrt(((p1[0] - p2[0])**2) + ((p1[1] - p2[1])**2) + ((p1[2] - p2[2])**2)) # print(distance) if distance < 1.5: # draw box red draw_skeleton(img, skeleton, confidence_threshold, (0, 0, 255)) draw_skeleton(img, skeleton2, confidence_threshold, (0, 0, 255)) skeles_drawn[i] = 1 skeles_drawn[j] = 1 break if not skeles_drawn[i]: draw_skeleton(img, skeleton, confidence_threshold, skeleton_color)
def convert_depth_to_phys_coord_using_realsense(depth_coordinates, depth): result = rs.rs2_deproject_pixel_to_point( depth_intrinsics, [depth_coordinates[0], depth_coordinates[1]], depth) center_pixel = [depth_intrinsics.ppy / 2, depth_intrinsics.ppx / 2] result_center = rs.rs2_deproject_pixel_to_point(depth_intrinsics, center_pixel, depth) return result[2], (result[1] - result_center[1]), -(result[0] - result_center[0])
def calculate_distance(self, ix, iy, x, y): udist = self.get_depth_xy(ix, iy) vdist = self.get_depth_xy(x, y) point1 = rs.rs2_deproject_pixel_to_point(self.intrin, [ix, iy], udist) point2 = rs.rs2_deproject_pixel_to_point(self.intrin, [x, y], vdist) dist = math.sqrt(math.pow(point1[0] - point2[0], 2) + \ math.pow(point1[1] - point2[1],2) + math.pow(point1[2] - point2[2], 2)) return dist
def pixels_to_landmarks(intrinsics, color_frame, depth_frame, depth_scale): """ Transforms pixel landmarks to point space :param intrinsics: Intrinsic parameters of the camera :param color_frame: color frame :param depth_frame: pyrealsense2.pyrealsense2.composite_frame -> rs2::depth_frame :param depth_scale: depth scale information :return: a list of landmarks with signature -> dict(str(name), tuple((x, y, z))) """ print("[INFO] Depth Scale is: ", depth_scale) print("[INFO] Multiplying points by:", 1 / depth_scale) scale = 1 / depth_scale color_image = np.asanyarray(color_frame.get_data()) lm = landmark_detector(color_image) points = list() for n, px in lm.items(): z = depth_frame.get_distance(px[0], px[1]) xx = rs.rs2_deproject_pixel_to_point(intrinsics, [px[0], px[1]], z)[0] yy = rs.rs2_deproject_pixel_to_point(intrinsics, [px[0], px[1]], z)[1] zz = rs.rs2_deproject_pixel_to_point(intrinsics, [px[0], px[1]], z)[2] # No scaling, no shift - landmarks are still off so it's not scaling issue # lm.update({"%s" % n: (xx, yy, zz)}) # No shift introduced lm.update({"%s" % n: (xx * scale, yy * scale, zz * scale)}) # When frames are aligned # lm.update({"%s" % n: ((xx + 0.006) * scale, (yy + 0.003) * scale, zz * scale)}) # lm.update({"%s" % n: ((xx) * scale, (yy) * scale, zz * scale)}) # When frames are not aligned # lm.update({"%s" % n: ((xx - 0.005) * scale, (yy + 0.007) * scale, zz * scale)}) # Test points alone points.append([xx * scale, yy * scale, zz * scale]) if LM_DEBUG: with open( os.path.join(SAVE_PATH, '3D_landmarks_rounded.numpy.txt'), 'w+') as f: np.savetxt(f, np.asanyarray(points), fmt='%-7.8f', delimiter=' ') print("Landmark Pixels: ", px[0], px[1], "Corresponding 3D Points: ", xx, yy, zz, "Scaled: ", xx * scale, yy * scale, zz * scale) # with open(os.path.join(SAVE_PATH, "3D_landmarks_raw.txt"), "w+") as f: # csv_writer = csv.writer(f) # csv_writer.writerows(lm) return lm
def calc(x0, y0, conf0, x1, y1, conf1, aligned_depth, frame_intrin, frame_name, depth_scale, is_right_shoulder, time_stamp): global meter_coordinates global point_coordinates pixel_1 = [x0, y0] pixel_2 = [x1, y1] depth_1 = aligned_depth[int(x0), int(y0)] depth_2 = aligned_depth[int(x1), int(y1)] # depth * depth scale = physical distance between the camera and the baby phys_depth_1 = depth_1 * depth_scale phys_depth_2 = depth_2 * depth_scale # Getting depth using this API return wrong value, no idea why # phys_depth_1 = depth_frame.get_distance(pixel_1[0], pixel_1[1]) # phys_depth_2 = depth_frame.get_distance(pixel_2[0], pixel_2[1]) # depth_1 = phys_depth_1 / depth_scale # depth_2 = phys_depth_2 / depth_scale # if it is negative then it is incorrect because it means the baby is behind the camera # max limit is the expected furthest distance between the baby and the camera, # this might be vary each time we record if (-1 < phys_depth_1 < 1.9) and (-1 < phys_depth_2 < 1.9): point1 = rse.rs2_deproject_pixel_to_point(frame_intrin, pixel_1, phys_depth_1) point2 = rse.rs2_deproject_pixel_to_point(frame_intrin, pixel_2, phys_depth_2) # This get the distance in meters, we multiply by 100 to get cm length = distance(point1[1], point1[0], point1[2], point2[1], point2[0], point2[2]) * 100 if frame_name not in meter_coordinates: meter_coordinates += '\n' + str(frame_name) + ',' + str(point1[0]) + ',' + str(point1[1]) + ',' \ + str(point1[2]) + ',' + str(conf0) + ',' + str(point2[0]) + ',' + str(point2[1]) \ + ',' + str(point2[2]) + ',' + str(conf1) else: if is_right_shoulder: meter_coordinates += ',' + str(point1[0]) + ',' + str(point1[1]) + ',' + str(point1[2]) + ',' \ + str(conf0) + ',' + str(point2[0]) + ',' + str(point2[1]) + ',' + str(point2[2]) \ + ',' + str(conf1) else: meter_coordinates += ',' + str(point2[0]) + ',' + str(point2[1]) + ',' + str(point2[2]) + ',' \ + str(conf1) if frame_name not in point_coordinates: point_coordinates += '\n' + str(frame_name) + ',' + str(y0) + ',' + str(x0) + ',' + str(depth_1) + ',' \ + str(conf0) + ',' + str(y1) + ',' + str(x1) + ',' + str(depth_2) + ',' + str(conf1) else: if is_right_shoulder: point_coordinates += ',' + str(y0) + ',' + str(x0) + ',' + str(depth_1) + ',' + str(conf0) + ',' \ + str(y1) + ',' + str(x1) + ',' + str(depth_2) + ',' + str(conf1) else: point_coordinates += ',' + str(y1) + ',' + str(x1) + ',' + str( depth_2) + ',' + str(conf1) return length else: return None
def calculate_distance(depth_frame, intrin, x1, y1, x2, y2): udist = depth_frame.get_distance(x1, y1) vdist = depth_frame.get_distance(x2, y2) # print(udist, vdist) point1 = rs.rs2_deproject_pixel_to_point(intrin, [x1, y1], udist) point2 = rs.rs2_deproject_pixel_to_point(intrin, [x2, y2], vdist) # print(point1, point2) dist = math.sqrt(math.pow(point1[0] - point2[0], 2) + math.pow(point1[1] - point2[1], 2)) # Ignore z point return dist
def get_list(self): """This method get the list A and B by rs.deproject function""" for match in self.matches: img_pixel = [int(self.featureA[match.queryIdx].pt[0]), int(self.featureA[match.queryIdx].pt[1])] depth = aligned_depth_frame.get_distance(img_pixel[0], img_pixel[1]) point_a = rs.rs2_deproject_pixel_to_point(self.intrin, img_pixel, depth) point_a = [point_a[0], point_a[2], 1] img_pixel = [int(self.featureB[match.trainIdx].pt[0]), int(self.featureB[match.trainIdx].pt[1])] depth = aligned_depth_frame.get_distance(img_pixel[0], img_pixel[1]) point_b = rs.rs2_deproject_pixel_to_point(self.intrin, img_pixel, depth) point_b = [point_b[0], point_b[2], 1] self.listA.append(point_a) self.listB.append(point_b)
def get_width_objs(self, x, y, w, h, var_limits_inside): ix1, iy1, iz1 = x + var_limits_inside, y + round(h / 2), ( self.depth_image[y + round(h / 2), x + var_limits_inside]) / 10 ix2, iy2, iz2 = x + w - var_limits_inside, y + round(h / 2), ( self.depth_image[y + round(h / 2), x + w - var_limits_inside]) / 10 point1 = rs.rs2_deproject_pixel_to_point(self.color_intrin, [ix1, iy1], iz1) point2 = rs.rs2_deproject_pixel_to_point(self.color_intrin, [ix2, iy2], iz2) return math.sqrt( math.pow(point1[0] - point2[0], 2) + math.pow(point1[1] - point2[1], 2) + math.pow(point1[2] - point2[2], 2))
def calculate_3D (xi, yi, x0, y0): udist = depth_frame.get_distance(xi, yi) vdist = depth_frame.get_distance(x0, y0) print udist, vdist point1 = rs.rs2_deproject_pixel_to_point(color_intrin, [xi, yi], udist) point2 = rs.rs2_deproject_pixel_to_point(color_intrin, [x0, y0], vdist) print 'start(x,y,z): '+ str(point1)+'\n' + 'end(x,y,z): ' +str(point2) dist = math.sqrt( math.pow(point1[0] - point2[0], 2) + math.pow(point1[1] - point2[1],2) + math.pow( point1[2] - point2[2], 2)) cm = dist * 100 decimal2 = "%.2f" % cm print 'Vermessung: ' + str(decimal2)+ 'cm' MessageBox(text='Vermessung: ' + decimal2 + ' cm')
def compute_distance(profile,depth_map,refPt): prof = profile.get_stream(rs.stream.depth) intrin = prof.as_video_stream_profile().get_intrinsics() print("Camera Parameters:\n {}".format(intrin)) a=np.array(rs.rs2_deproject_pixel_to_point(intrin,refPt[0][:2],depth_map[refPt[0][:2][0],refPt[0][:2][1]])) b=np.array(rs.rs2_deproject_pixel_to_point(intrin,refPt[1][:2],depth_map[refPt[1][:2][0],refPt[1][:2][1]])) D = np.sqrt((a-b).dot(a-b)) d = "distance between dots = {}mm".format(D) print(d) return D
def cmPerPx(depth_frame,x1,y1,x2,y2,scale): depth1 = depth_frame.get_distance(x1, y1) depth2 = depth_frame.get_distance(x2, y2) # fx,fy = rs.rs2_fov(depth_frame.profile.as_video_stream_profile().intrinsics) # print(fx,fy) # realWdith = (abs(x1-x2) ) * 2 * math.tan(math.radians(fx / 2)) * depth1 / depth_frame.width # realHeight = (abs(y1-y2) ) * 2 * math.tan(math.radians(fy / 2)) * depth1 / depth_frame.width # print(realWdith,realHeight,depth1 - depth2) intrin = depth_frame.profile.as_video_stream_profile().intrinsics x = rs.rs2_deproject_pixel_to_point(intrin,[x1,y1],depth1) y = rs.rs2_deproject_pixel_to_point(intrin,[x2,y2],depth2) print("depth1 : ",depth1) print("depth1 : ",depth2) print(((x[0]-y[0])**2 +(x[1]-y[1])**2 +(x[2]-y[2])**2)**0.5 ) # return (realWdith ** 2 + realHeight ** 2 + (depth1 - depth2)**2)**(0.5)
def mapGenerator(detections, intr): plt.ion() plt.grid(color='r', linestyle='-', linewidth=2) plt.axis([-2.5, 2.5, 0, 5]) for i in range(0, len(detections[0])): #Transform x,y,depth coordinates to new xyz: cxy = [detections[0][i], detections[1][i]] depth = detections[2][i] point = rs.rs2_deproject_pixel_to_point( intr, cxy, depth) #Outputs xyz in cameras reference frame in 3D space #Display map: if len(point) != 0: p = plt.plot(point[1], point[2], marker='o', label=detections[4][i]) plt.text(point[1], point[2], detections[4][i]) fig.canvas.draw() img = np.frombuffer(fig.canvas.tostring_rgb(), dtype=np.uint8) img = img.reshape(fig.canvas.get_width_height()[::-1] + (3, )) img = cv.cvtColor(img, cv.COLOR_RGB2BGR) cv.imshow("Labelled Map", img) plt.clf() plt.ioff()
def getRelativePos(self, pixCoord, meshSize=3): # using the center pixel coordinates of a detected target. # generate a 3x3 mask around the center pixel # deproject the aligned depth frame. assert (meshSize % 2 > 0) pixCoord = np.round(pixCoord) #xyz_array = ros_numpy.point_cloud2.get_xyz_points(self.ptCloud) x = np.linspace(pixCoord[0] - (meshSize - 1) / 2, pixCoord[0] + (meshSize - 1) / 2, meshSize, dtype=np.int32) y = np.linspace(pixCoord[1] - (meshSize - 1) / 2, pixCoord[1] + (meshSize - 1) / 2, meshSize, dtype=np.int32) # deproject the depth image pts = np.zeros([meshSize * meshSize, 3]) n = 0 for xv in x: for yv in y: pts[n, :] = rs2.rs2_deproject_pixel_to_point( self.intrinsics, [xv, yv], self.Dframe[yv, xv]) n += 1 relPos = np.sum(pts, 0) / (meshSize**2) relPosT = np.array([relPos[2], -relPos[0]]) * 0.001 return relPosT
def calculate_relative_pose(box, depth_frame, depth_intrin): x_mid = int((box[0]+box[2])/2) y_mid = int((box[1]+box[3])/2) object_depth, object_angle = detect_object_depth_orientation(box, depth_frame) if object_depth is not None: # The camera intrinsics couldn't be flipped so the other data has to be temporarily flipped if flip_camera_ver: y_mid = RESOLUTION_HEIGHT-y_mid if flip_camera_hor: x_mid = RESOLUTION_WIDTH-x_mid object_point = rs.rs2_deproject_pixel_to_point(depth_intrin, [x_mid, y_mid], object_depth) # Makes sure all objects are above ground-level object_point[1] = -object_point[1] if object_point[1] < 0: object_point[1] = -object_point[1] else: object_point[1] = object_point[1]*2 if flip_camera_ver: object_point[1] = -object_point[1] if flip_camera_hor: object_point[0] = -object_point[0] object_xyz = [object_point[0], object_point[2], object_point[1]] return object_xyz, object_depth, object_angle else: return None, None, None
def depthmatrix_to_pointcloud( matrix: np.ndarray, intrin: rs2.intrinsics, max_dist=float('Inf'), min_dist=float('-Inf') ) -> np.ndarray: """ Uses rs2_deproject_pixel_to_point to map each pixel to a point. Note: i, j are flipped 'cause realsense is stupid :param matrix: The matrix that represents the depth frame :param intrin: The intrinsics of the camera :param max_dist: the maximum distance :param min_dist: the maximum distance :return: A pointcloud via the depth frame and the rs2_deproject_pixel_to_point function """ assert len(matrix.shape) == 2 points: List = [] for i in range(matrix.shape[0]): for j in range(matrix.shape[1]): if min_dist <= matrix[i][j] <= max_dist: points.append( rs2.rs2_deproject_pixel_to_point(intrin, [j, i], matrix[i][j])) return np.array(points)
def calc_world_pos(x, y, depth_frame, tolerance_radius=5): depth_intrin = depth_frame.profile.as_video_stream_profile().intrinsics depth = 0 if tolerance_radius > 0: depth_candidates = [] for x_i in range(-tolerance_radius, tolerance_radius): for y_i in range(-tolerance_radius, tolerance_radius): x_t = x+x_i y_t = y+y_i if not DepthRoiEvaluator.is_in_bounds(x_t, y_t, depth_frame): continue depth = depth_frame.get_distance(x_t, y_t) if depth < 10.0: depth_candidates.append(depth) if len(depth_candidates) == 0: return None depth_candidates = np.array(depth_candidates) # reject outliers m = 2 depth_candidates = depth_candidates[ abs(depth_candidates - np.mean(depth_candidates)) < m * np.std(depth_candidates)] depth = np.mean(depth_candidates) else: depth = depth_frame.get_distance(x, y) return rs.rs2_deproject_pixel_to_point(depth_intrin, [x, y], depth), depth
def average_matrices_to_pointcloud( mats: List[np.ndarray], intrin: rs2.intrinsics, max_dist=float('Inf'), min_dist=float('-Inf') ) -> np.ndarray: """ Uses rs2_deproject_pixel_to_point as in the depthmatrix_to_pointcloud function. Points the have the same i,j values have their z values averaged out, iff the z value is within the range. :param mats: A list of matrices that each represents the depth frame :param intrin: The intrinsics of the camera :param max_dist: the maximum distance :param min_dist: the maximum distance :return: A pointcloud calculated via averaging the matrices and the rs2_deproject_pixel_to_point function. """ ij_to_zs: Dict[Tuple[int, int], List[float]] = defaultdict(list) for mat in mats: assert len(mat.shape) == 2 for i in range(mat.shape[0]): for j in range(mat.shape[1]): if min_dist <= mat[i][j] <= max_dist: ij_to_zs[(i, j)].append(mat[i][j]) pc = [] for (i, j), zs in ij_to_zs.items(): pc.append( rs2.rs2_deproject_pixel_to_point(intrin, [j, i], np.average(zs))) return np.array(pc)
def calculate_distance(self, x, y): color_intrin = self.color_intrin x_ratio, y_ratio = self.x_ratio, self.y_ratio ix, iy = int(self.ix/x_ratio), int(self.iy/y_ratio) x, y = int(x/x_ratio), int(y/y_ratio) udist = self.depth_frame.get_distance(ix, iy) vdist = self.depth_frame.get_distance(x, y) if udist ==0.00 or vdist ==0.00: dist = 'NaN' else: point1 = rs.rs2_deproject_pixel_to_point(color_intrin, [ix, iy], udist) point2 = rs.rs2_deproject_pixel_to_point(color_intrin, [x, y], vdist) dist = math.sqrt( math.pow(point1[0] - point2[0], 2) + math.pow(point1[1] - point2[1], 2) + math.pow( point1[2] - point2[2], 2)) return dist
def frame_to_scan(image_frame, intren, depth_scale=0.5): scan_row = int(image_frame.shape[0] / 2) row_size = image_frame.shape[1] scan_mat = np.zeros((row_size, row_size), np.uint8) MAX_WIDTH = 2 #screen covers a maximum of 10 meters of width scale_val = (row_size - 10) / (MAX_WIDTH ) #scale points to make them fit on image point_collection = [] for j in range(row_size): depth_val = image_frame[scan_row][j] point = rs.rs2_deproject_pixel_to_point(intren, [j, scan_row], depth_val) point = np.asarray(point) * depth_scale point_collection.append(point) print("x_coordinate: ", point[0]) img_point = scale_val * point img_x_val = int(img_point[0]) + int(row_size / 2) - 1 print("img_x, img_y: " + str(img_point[0]) + ", " + str(img_point[2])) #Dont draw any points that dont fall on the image plane if (img_x_val >= 0 and img_x_val < row_size) and (img_point[2] >= 0 and img_point[2] <= row_size): print("writing!") scan_mat[row_size - int(img_point[2]) - 1][img_x_val] = 255 return scan_mat
def get_depth_as_distance(box, curr_frame, depth_image, color_image, depth_intrin): (x, y, w, h) = box.astype("int") if curr_frame >= 30 and curr_frame % 30 == 0: roi_depth_image = depth_image[y:y + h, x:x + w] roi_color_image = color_image[y:y + h, x:x + w] os.system('mkdir %d' % curr_frame) cv2.imwrite('%d/depth.jpg' % curr_frame, roi_depth_image) print("save depth image") cv2.imwrite('%d/color.jpg' % curr_frame, roi_color_image) print("save color image") print("the mid position depth is:", depth_frame.get_distance(int(x + w / 2), int(y + h / 2))) # write the depth data in a depth.txt with open('%d/depth.csv' % curr_frame, 'w') as f: cols = list(range(x, x + w)) rows = list(range(y, y + h)) for i in rows: for j in cols: if (x, x + w) in (cols) and (y, y + w) in (rows): depth = depth_frame.get_distance(j, i) # print(x, x+w, y, y+h, j, i) depth_point = rs.rs2_deproject_pixel_to_point( depth_intrin, [j, i], depth) text = "%d: %.5lf, %.5lf, %.5lf\n" % ( curr_frame, depth_point[0], depth_point[1], depth_point[2]) f.write(text) print("Finish writing the depth img")
def ProccessCircle(frame, cnts, depth, ballNum, graph_iter=0): # only proceed if at least one contour was found if len(cnts) > 0: ((x, y), radius) = cv2.minEnclosingCircle(max(cnts, key=cv2.contourArea)) # only proceed if the radius meets a minimum size if radius > 5: # draw the circle, centroid and line on the frame markBallInFrame(frame, x, y, radius, ballNum) # convert from pixels coordinates to real world cordinates depth_val = depth.get_distance(int(x), int(y)) depth_intrin = depth.profile.as_video_stream_profile().intrinsics (x, y, z) = rs.rs2_deproject_pixel_to_point(depth_intrin, [x, y], depth_val) # update x , y , z according to initial state and publish update_publish_axes(x, y, z, ballNum) # build and output graphes if needed (debugging purposes) if graph_iter > 0: if iterNum == graph_iter: iters = [i for i in range(len(axes[ballNum]))] plt_axes = zip(*axes[ballNum]) plt.plot(iters, list(plt_axes[0]), 'r--', iters, list(plt_axes[1]), 'bs', iters, list(plt_axes[2]), 'g^') plt.show()
def run(self): global depth_frame_global print("motor thread starting") while not self.done: time.sleep(0.001) with depth_frame_lock: depth_frame = depth_frame_global if depth_frame == None: continue pos = self.motor_controller.position() mouse_x = int( max( min((pos[0] / 50.8 + 0.25) * WIDTH / RESCALE * ZOOM / 1.25, WIDTH / RESCALE * ZOOM - 1), 0)) mouse_y = int( max( min((pos[1] / 50.8 + 0.08) * WIDTH / RESCALE * ZOOM / 1.25 - 10, HEIGHT / RESCALE * ZOOM - 1), 0)) z = max(int(pos[2]) * 400, 1) mouse_real_world = rs.rs2_deproject_pixel_to_point( depth_frame.profile.as_video_stream_profile().intrinsics, [mouse_x // 2, mouse_y // 2], z) vec = compute_feedback_vector(depth_frame, mouse_real_world) with frame_info_lock: frame_info_global["mouse_x"] = mouse_x frame_info_global["mouse_y"] = mouse_y frame_info_global["vec"] = vec self.motor_controller.create_force(vec, pos) self.frame_counter.inc()
def imageDepthCallback(self, data): try: cv_image = self.bridge.imgmsg_to_cv2(data, data.encoding) # pick one pixel among all the pixels with the closest range: indices = np.array( np.where(cv_image == cv_image[cv_image > 0].min()))[:, 0] pix = (indices[1], indices[0]) self.pix = pix line = '\rDepth at pixel(%3d, %3d): %7.1f(mm).' % ( pix[0], pix[1], cv_image[pix[1], pix[0]]) if self.intrinsics: depth = cv_image[pix[1], pix[0]] result = rs2.rs2_deproject_pixel_to_point( self.intrinsics, [pix[0], pix[1]], depth) line += ' Coordinate: %8.2f %8.2f %8.2f.' % ( result[0], result[1], result[2]) if (not self.pix_grade is None): line += ' Grade: %2d' % self.pix_grade line += '\r' sys.stdout.write(line) sys.stdout.flush() except CvBridgeError as e: print(e) return except ValueError as e: return
def mouseRGB(event, x, y, flags, param): if event == cv2.EVENT_LBUTTONDOWN: #checks mouse left button down condition z = aligned_depth_frame.get_distance(y, x) point = rs.rs2_deproject_pixel_to_point( depth_intrinsics, [y, x], z) print("Projected point:", "x =", point[0], "y =", point[1], "z =", point[2])
def get_point(x, y): p = rs.rs2_deproject_pixel_to_point(intrinsics, [x, y], d) batch.add(2, gl.GL_LINES, None, ('v3f', [0, 0, 0] + p)) return p
def get_point(x, y): p = rs.rs2_deproject_pixel_to_point(intrinsics, [x, y], d) line3d(out, orig, view(p), color) return p