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()
