def getRF(self):
self.rf[:, 0] = self.rf[:, 0] / np.linalg.norm(self.rf[:, 0])
self.rf[:, 1] = self.rf[:, 1] / np.linalg.norm(self.rf[:, 1])
self.rf[:, 2] = self.rf[:, 2] / np.linalg.norm(self.rf[:, 2])
frame = PyKDL.Frame(PyKDL.Vector(
self.center[0], self.center[1], self.center[2]
))
q = quaternion_from_matrix(np.array([
self.rf[0, 0], self.rf[0, 1], self.rf[0, 2], 0,
self.rf[1, 0], self.rf[1, 1], self.rf[1, 2], 0,
self.rf[2, 0], self.rf[2, 1], self.rf[2, 2], 0,
0, 0, 0, 1]).reshape((4, 4)), isprecise=True
)
# self.transform = np.array([
# self.rf[0, 0], self.rf[0, 1], self.rf[0, 2], self.center[0],
# self.rf[1, 0], self.rf[1, 1], self.rf[1, 2], self.center[1],
# self.rf[2, 0], self.rf[2, 1], self.rf[2, 2], self.center[2],
# 0, 0, 0, 1
# ]).reshape((4, 4))
q = q / np.linalg.norm(q)
frame.M = PyKDL.Rotation.Quaternion(q[3], q[0], q[1], q[2])
self.transform = transformations.KLDtoNumpyMatrix(frame)
return frame # transformations.NumpyMatrixToKDL(self.transform)
def updateSVD(self, x):
x = np.array(x).reshape(4)
x = x / np.linalg.norm(x)
frame = PyKDL.Frame()
frame.M = PyKDL.Rotation.Quaternion(x[0], x[1], x[2], x[3])
mat = transformations.KLDtoNumpyMatrix(frame)
self.rf = mat[0:3, 0:3]
area = self.getBoxArea()
print("Area:", area)
return area
def build(self, options={}):
if os.path.exists(self.dest_folder) or len(self.dest_folder) == 0:
return False
os.mkdir(self.dest_folder)
frames = self.training_dataset.getAllFrames()
counter = 0
for frame in frames:
if counter % self.jumps == 0:
counter_string = '{}'.format(
str(int(counter / self.jumps)).zfill(
MaskDatasetBuilder.ZERO_PADDING_SIZE))
img = cv2.imread(frame.getImagePath())
#⬢⬢⬢⬢⬢➤ Compute Unique Labels
instances = frame.getInstances()
labels = []
for inst in instances:
labels.append(inst.label)
labels = set(labels)
#⬢⬢⬢⬢⬢➤ Masks for each label
for l in labels:
gts = frame.getInstancesBoxesWithLabels(filter_labels=[l])
pair = np.ones(img.shape, dtype=np.uint8) * 255
for inst in gts:
if not self.boxed_instances:
hull = cv2.convexHull(np.array(inst[1]))
cv2.fillConvexPoly(
pair, hull,
TrainingClass.getColorByLabel(inst[0]))
else:
hull = cv2.boundingRect(np.array(inst[1]))
cv2.rectangle(
pair, (hull[0], hull[1]),
(hull[0] + hull[2], hull[1] + hull[3]),
TrainingClass.getColorByLabel(inst[0]), -1)
mask_img_file = os.path.join(
self.dest_folder,
counter_string + "_mask_{}.png".format(l))
cv2.imwrite(mask_img_file, pair)
#⬢⬢⬢⬢⬢➤ Mask for all labels together
gts = frame.getInstancesBoxesWithLabels()
pair = np.ones(img.shape, dtype=np.uint8) * 255
for inst in gts:
if not self.boxed_instances:
hull = cv2.convexHull(np.array(inst[1]))
cv2.fillConvexPoly(
pair, hull, TrainingClass.getColorByLabel(inst[0]))
else:
hull = cv2.boundingRect(np.array(inst[1]))
cv2.rectangle(pair, (hull[0], hull[1]),
(hull[0] + hull[2], hull[1] + hull[3]),
TrainingClass.getColorByLabel(inst[0]),
-1)
mask_img_file = os.path.join(self.dest_folder,
counter_string + "_mask_all.png")
cv2.imwrite(mask_img_file, pair)
#⬢⬢⬢⬢⬢➤ RGB Image
rgb_img_file = os.path.join(self.dest_folder,
counter_string + "_rgb.jpg")
cv2.imwrite(rgb_img_file, img)
print "Writing to", rgb_img_file
#⬢⬢⬢⬢⬢➤ Depth Image
if frame.getImageDepthPath():
depth_img_file = frame.getImageDepthPath()
ext = os.path.splitext(depth_img_file)[1]
depth_img_file = os.path.join(
self.dest_folder, counter_string + "_depth." + ext)
shutil.copy(frame.getImageDepthPath(), depth_img_file)
#⬢⬢⬢⬢⬢➤ Camera Pose
camera_pose = frame.getCameraPose()
camera_pose = transformations.KLDtoNumpyMatrix(camera_pose)
pose_file = os.path.join(self.dest_folder,
counter_string + "_camerapose.txt")
np.savetxt(pose_file, camera_pose)
counter = counter + 1
return True