if not constellation.is_empty(): print("Number of features used: {}".format(len(feats))) # 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()