def cameraCallback(frame): #⬢⬢⬢⬢⬢➤ Grabs image from Frame img = frame.rgb_image.copy() #⬢⬢⬢⬢⬢➤ Detects markers markers = marker_detector.detectMarkersMap( frame.rgb_image, markers_map=markers_map) #⬢⬢⬢⬢⬢➤ draw markers for id, marker in markers.iteritems(): marker.draw(img) node.broadcastTransform( marker, "marker_{}".format(id), "world", node.getCurrentTime() ) if id == 401: vo = VirtualObject(marker, size=np.array([0.04, 0.04, 0.04])) img_points = vo.getImagePoints(camera=camera) VirtualObject.drawBox(img_points, img) c = cv2.waitKey(1) if c == 32: v = transformations.KDLtoNumpyVector(marker) print(v) #⬢⬢⬢⬢⬢➤ Show cv2.imshow("output", img) cv2.waitKey(1)
def shortRepresentation(self): box = self.getBBox() sx = math.fabs(box[0, 0] - box[0, 1]) sy = math.fabs(box[1, 0] - box[1, 1]) sz = math.fabs(box[2, 0] - box[2, 1]) size = np.array([sx, sy, sz]).reshape((1, 3)) box_rf = transformations.KDLtoNumpyVector(self.getBoxRF()).reshape( (1, 7)) whole = np.concatenate( (np.array([self.heavier_label]), box_rf.ravel(), size.ravel())) return whole
def findBestPoses(self, markers_id_list, x0=np.array([-0.00, 0, 0.0, 0, 0, 0])): res_map = {"markers": {}} for id in markers_id_list: print("Optimizing marker:", id) res = self.findBestPose(id, x0) frame = transformations.KDLFromArray(res.x, fmt='RPY').Inverse() pose = transformations.KDLtoNumpyVector(frame, fmt='RPY') res_map["markers"][id] = { "pose": pose.reshape(6).tolist(), "size": self.marker_size } print pose # res_map[id] = return res_map
def cameraCallback(frame): global current_arp_pose, collected_boxes, windows_config if not windows_config: windows_config = True cv2.namedWindow("img", cv2.WINDOW_NORMAL) #⬢⬢⬢⬢⬢➤ Grabs image from Frame img = frame.rgb_image.copy() camera_pose = node.retrieveTransform( "/comau_smart_six/link6", "/comau_smart_six/base_link", -1) if camera_pose == None: print("No camera pose available") return camera_pose = camera_pose * camera_extrinsics arp_pose = arp.detect(img, debug_draw=False, contributes_output=None) if arp_pose: current_arp_pose = arp_pose rod_p = PyKDL.Frame(PyKDL.Vector(0, 0, 0.06)) vob_rod = VirtualObject( arp_pose, size=[0.005, 0.005, rod_p.p.z()]) vob_body = VirtualObject( arp_pose * rod_p, size=[0.05, 0.05, 0.22 - rod_p.p.z()]) vob_rod.draw(img, camera=camera, color=(54, 67, 244), thickness=1) vob_body.draw(img, camera=camera, color=(54, 67, 244), thickness=2) for p in collected_points: center = cvutils.reproject3DPoint( p[0], p[1], p[2], camera=camera, camera_pose=PyKDL.Frame()) cv2.circle(img, center, 5, (243, 150, 33), -1) for b in collected_boxes: vo = b.buildVirtualObject() vo.draw(img, camera=camera, color=(243, 150, 33)) print "B", transformations.KDLtoNumpyVector(vo), b.size cv2.imshow("img", img) c = cv2.waitKey(1) if c == 32: evt = Bool() evt.data = True new_point_publisher.publish(evt)
def cameraCallback(frame): global current_arp_pose, collected_boxes, first_camera_pose, windows_config if not windows_config: windows_config = True cv2.namedWindow("img", cv2.WINDOW_NORMAL) #⬢⬢⬢⬢⬢➤ Grabs image from Frame img = frame.rgb_image.copy() camera_pose = node.retrieveTransform("/comau_smart_six/link6", "/comau_smart_six/base_link", -1) if camera_pose == None: print("No camera pose available") return camera_pose = camera_pose * camera_extrinsics if first_camera_pose == None: first_camera_pose = camera_pose for o in collected_objects: print o rf = transformations.NumpyVectorToKDL(o['rf']) size = o['size'] rf = first_camera_pose * rf vo = VirtualObject(frame=rf, size=size) vo.draw(img, camera=camera, camera_frame=camera_pose, color=(243, 150, 33)) print "B", transformations.KDLtoNumpyVector(rf), size cv2.imshow("img", img) c = cv2.waitKey(1) if c == 32: pass
pose_files.append(f) rgb_files = sorted(rgb_files) depth_files = sorted(depth_files) pose_files = sorted(pose_files) poses = [] for i in range(0, len(rgb_files)): image_name = str(i).zfill(zeros) + ".png" rgb_basename = os.path.basename(rgb_files[i]) depth_basename = os.path.basename(depth_files[i]) shutil.copy(rgb_files[i], os.path.join(rgb_path, image_name)) shutil.copy(depth_files[i], os.path.join(depth_path, image_name)) pose_raw = np.loadtxt(pose_files[i]) pose = transformations.NumpyMatrixToKDL(pose_raw) pose_q = transformations.KDLtoNumpyVector(pose).ravel() print pose_q poses.append(pose_q) poses = np.array(poses) print poses.shape np.savetxt(pose_path, poses) np.savetxt(extr_path, np.array([0, 0, 0, 0, 0, 0, 1])) np.savetxt(intr_path, np.array([640, 480, 570.3, 570.3, 320, 240, 0, 0, 0, 0])) # img_folder = os.path.join(dataset_folder, "images") # label_folder = os.path.join(dataset_folder, "labels") # ids_folder = os.path.join(dataset_folder, "ids") # if not os.path.exists(img_folder): # print("Invalud path '{}'".format(img_folder))
for filename in files: basename = os.path.basename(filename) base = basename.split('.')[0] img = cv2.imread(filename) markers = marker_detector.detectMarkersMap(img, 0.04) out_list = [] for id, marker in markers.iteritems(): if debug: marker.draw(img) vob = VirtualObject(marker, size=[0.04, 0.04, 0.01]) vob.draw(img, camera=camera) out_list.append( np.hstack((np.array(id).reshape(1, 1), transformations.KDLtoNumpyVector(marker).reshape(1, 7)))) s = len(out_list) out_list = np.array(out_list).reshape(s, 8) output_file = os.path.join(output_folder, base + ".txt") np.savetxt(output_file, out_list, fmt='%d %.8f %.8f %.8f %.8f %.8f %.8f %.8f') if debug: control = np.loadtxt(output_file) if control.shape[0] > 0: for c in control: m = transformations.KDLFromArray(c[1:8], fmt='XYZQ') vob2 = VirtualObject(m, size=[0.04, 0.04, 0.01]) vob2.draw(img, camera=camera, color=(0, 255, 0))
coverage_map = {} filtered_ids = {} for id, frame in matched_frames.iteritems(): print "#########" print id, frame.getCameraPose() for inst in frame.getInstances(): i_frame = PyKDL.Frame() i_frame.M = inst.M i_frame.p = inst.p i_frame = i_frame.Inverse() i_frame = i_frame * frame.getCameraPose() label = inst.label if label not in coverage_map: coverage_map[label] = [] trans = transformations.KDLtoNumpyVector(i_frame) sph = asSpherical(trans) if label == 0 and sph[2] > 0 and sph[2] < 90: filtered_ids[id] = 1 output_ids = os.path.join(output_folder, "coverage_filtered_ids.txt") f = open(output_ids, 'w') for id, v in filtered_ids.iteritems(): f.write(id + "\n") f.close()