def updateHistory(self):
if (self.init_history is True) and (self.trueX is not None):
self.t0_est = np.array([self.cur_t[0], self.cur_t[1], self.cur_t[2]]) # starting translation
self.t0_gt = np.array([self.trueX, self.trueY, self.trueZ]) # starting translation
self.init_history = False
if (self.t0_est is not None) and (self.t0_gt is not None):
p = [self.cur_t[0]-self.t0_est[0], self.cur_t[1]-self.t0_est[1], self.cur_t[2]-self.t0_est[2]] # the estimated traj starts at 0
self.traj3d_est.append(p)
pg = [self.trueX-self.t0_gt[0], self.trueY-self.t0_gt[1], self.trueZ-self.t0_gt[2]] # the groudtruth traj starts at 0
self.traj3d_gt.append(pg)
self.poses.append(poseRt(self.cur_R, p))
def estimatePose(self, kpn_ref, kpn_cur):
E, self.mask_match = cv2.findEssentialMat(
kpn_cur,
kpn_ref,
focal=1,
pp=(0., 0.),
method=cv2.RANSAC,
prob=kRansacProb,
threshold=kRansacThresholdNormalized)
_, R, t, mask = cv2.recoverPose(E,
kpn_cur,
kpn_ref,
focal=1,
pp=(0., 0.))
return poseRt(
R, t.T
) # Trc homogeneous transformation matrix with respect to 'ref' frame, pr_= Trc * pc_
def estimatePose(self, kpn_ref, kpn_cur):
# here, the essential matrix algorithm uses the five-point algorithm solver by D. Nister (see the notes and paper above )
E, self.mask_match = cv2.findEssentialMat(
kpn_cur,
kpn_ref,
focal=1,
pp=(0., 0.),
method=cv2.RANSAC,
prob=kRansacProb,
threshold=kRansacThresholdNormalized)
_, R, t, mask = cv2.recoverPose(E,
kpn_cur,
kpn_ref,
focal=1,
pp=(0., 0.))
return poseRt(
R, t.T
) # Trc homogeneous transformation matrix with respect to 'ref' frame, pr_= Trc * pc_