def relocation(self):
while True:
image, image_depth = self.camera.getImage()
pose, match_points_ref, match_points_cur = self.pnp.getPose(
self.refImage, image, image_depth,
self.camera.cameraCalibration.getK())
R, t = pose.toRt()
tmpPose = Pose3.fromRt(R, t)
image_warp, image_depth_warp, warped_mask = cv2.rgbd.warpFrame(
image, np.float32(image_depth), None,
tmpPose.inverse().toSE3(),
self.camera.cameraCalibration.getK(), None)
image_depth_warp = np.uint16(image_depth_warp)
self.curPose = pose.copy() # type: Pose3
self.curAFD = self.computeAFD(match_points_ref, match_points_cur)
self.writeInfo(self.data_dir, self.step, image, image_depth, pose,
self.curAFD, image_warp, image_depth_warp)
if self.stopCondition():
break
# moving platform
movingPose = self.curPose # type: Pose3
dumpMotion = self.dumpPose(movingPose)
self.platform.movePose(dumpMotion.inverse())
# rots = np.array(motion[3:]) * self.dampRatio # sequence: rz, ry, rx
# trans = np.array(motion[:3]) * self.dampRatio * -1 # sequence: tx, ty, tz
# self.platform.rotate(rots[2], rots[1], rots[0])
# self.platform.translate(trans[0], trans[1], trans[2])
# self.writeHandInfo(self.data_dir, self.step, rots, trans)
self.step = self.step + 1
def getPose(self, refImg : np.ndarray, curImg: np.ndarray, K: np.ndarray)-> (Pose3, np.ndarray, np.ndarray):
import cv2
ref_pts, cur_pts = SIFTFeature.detectAndMatch(image1=refImg,image2=curImg)
E, mask = cv2.findEssentialMat(ref_pts, cur_pts, K, method=cv2.RANSAC, prob=0.999, threshold=1.0)
points, R, t, mask = cv2.recoverPose(E, ref_pts, cur_pts, mask=mask)
pose = Pose3.fromRt(R, t)
return pose, ref_pts, cur_pts
def loadPose(cls, json_path):
with open(json_path, 'r') as f:
info = json.load(f)
R = np.array(info['R']).reshape(3, 3)
t = np.array(info['t'])
# TODO: moidify PnP, and remove t = t * 1000
t = t * 1000
pose = Pose3.fromRt(R, t)
return pose
def getPose(self, refImg: np.ndarray, curImg: np.ndarray, K: np.ndarray) -> (Pose3, np.ndarray, np.ndarray):
ref_pts, cur_pts = SIFTFeature.detectAndMatch(image1=refImg, image2=curImg) # type: np.ndarray, np.ndarray
ref_pts_list = list(ref_pts.flatten())
cur_pts_list = list(cur_pts.flatten())
K_list = list(K.flatten())
print(ref_pts[:5])
print(ref_pts_list[:10])
R, t = self._fivePointsAlg.calcRP(ref_pts_list, cur_pts_list, K_list)
R = np.array(R).reshape(3, 3)
t = np.array(t)
pose_ref_to_cur = Pose3.fromRt(R, t)
return pose_ref_to_cur, ref_pts, cur_pts
def getPose(self, refImg: np.ndarray, curImg: np.ndarray,
curImg_depth: np.ndarray, K: np.ndarray):
import cv2
ref_pts, cur_pts = SIFTFeature.detectAndMatch(image1=refImg,
image2=curImg)
ref_pts_2d, cur_pts_2d, cur_pts_3d = self.get3dPoints(
ref_pts, cur_pts, curImg_depth, K)
retval, rvec, tvec, inlier = cv2.solvePnPRansac(
cur_pts_3d, ref_pts_2d, K, None, flags=cv2.SOLVEPNP_EPNP)
if retval == True:
R, jacobian = cv2.Rodrigues(rvec)
pose = Pose3.fromRt(R, tvec)
# pose must be inversed
pose = pose.inverse()
return pose, ref_pts_2d, cur_pts_2d
else:
return Pose3(), ref_pts_2d, cur_pts_2d
def poseWithScale(cls, pose: Pose3, s: float):
R, t = pose.toRt()
t_norm = cls.normalized(t)
ts = t_norm * s
return Pose3.fromRt(R, ts)