# Draw constellation to output image
for pxl in feats:
pos = (int(pxl[0]), int(pxl[1]))
cv.circle(featimg, pos, 5, (0, 255, 0), -1)
if len(feats) == CONSTELLATION_SIZE:
vel = ibvs.execute(feats, depth)
if prev_pnts is not None:
R, trans = rigid_tf(prev_pnts, pnts)
target_pnt = target_pnt + trans.T[0]
print("Found translation: ", trans)
sim.applyVelocity(vel)
# Draw target
target_pxl = ibvs.pnt_to_feature(target_pnt)
cv.circle(featimg, tuple(target_pxl.astype(int)), 15, (0, 0, 255), -1)
prev_pnts = pnts
# now = time.time()
# print("Time taken: {}".format(now - prev_time))
# prev_time = now
# Show image and sleep for sim
cv.imshow("Output Image", featimg)
print("Frame finished")
cv.waitKey()
cv.destroyAllWindows()
sim.disconnect()
