def computeRelativeOrientation(tie_pts, cam_m, distor):
# computes relative orientation using OpenCV 5 point algorithms
tp1, tp2, tp1_u, tp2_u = UndistorTiePoints(tie_pts, cam_m, distor)
# eight point algorithm
#F, mask = cv2.findFundamentalMat(tp1[0, :], tp2[0, :], param1=0.1, param2=0.95, method = cv2.FM_RANSAC)
#em = cam_m.T.dot(F).dot(cam_m)
em, mask = cv2.findEssentialMat(tp1[0, :], tp2[0, :],
threshold=0.05,
prob=0.95,
focal=cam_m[0, 0],
pp=(cam_m[0, 2], cam_m[1, 2]))
F = np.linalg.inv(cam_m.T).dot(em).dot(np.linalg.inv(cam_m))
# optimal solution for triangulation of object points
p1, p2 = cv2.correctMatches(em, tp1_u, tp2_u)
pts, R, t, mask = cv2.recoverPose(em, p1, p2)
P = np.hstack((R, t))
pm1 = np.eye(3, 4)
pts_sps = compute3Dpoints(pm1, P, p1, p2)
return pts_sps, P, p1, p2
def find_transform(K, p1, p2):
# 根据匹配点求本征矩阵，使用RANSAC进一步消除失配点
# 基于五点算法 mask用来标记RANSAC计算得来的内点 只有内点才被用于后续步骤
E, mask = cv2.findEssentialMat(p1, p2, K, cv2.RANSAC, 0.999, 1.0)
# 分解本征矩阵E, 得到位姿R, T
pass_count, R, T, mask = cv2.recoverPose(E, p1, p2, K, mask)
return R, T, mask
def Rt(self):
'''This function finds the rotation matrix R and the translation matrix t using corresponding points and a given camera matrix.'''
pts_1 = np.array(self.ptsL, dtype=np.float) #Convert points to float
pts_2 = np.array(self.ptsR, dtype=np.float)
if self.essential_matrix != []:
poseval, self.R, self.t, mask = cv.recoverPose(
self.essential_matrix, pts_1, pts_2, self.camera_matrix, 0.4
) #Find image's pose using corresponding points, the essential matrix, the camera matrix and a ratio.
else:
poseval, self.R, self.t, mask = cv.recoverPose(
self.fundamental_matrix, pts_1, pts_2, self.camera_matrix, 0.4)
self.ptsL = self.ptsL[
mask.ravel() ==
255] #Keep only the points which are used for the pose recover procedure.
self.ptsR = self.ptsR[mask.ravel() == 255]
self.colours = self.colours[
mask.ravel() ==
255] #Keep only the colours of points which are used for the pose recover calculation.
def get_pose(self, image1, idx1, image2, idx2, K):
Rt = np.eye(4)
E, mask = cv2.findEssentialMat(image1.kps[idx1], image2.kps[idx2], K,
cv2.RANSAC)
_, rot, trans, mask = cv2.recoverPose(E, image1.kps[idx1],
image2.kps[idx2], K)
Rt[:3, :3] = rot
Rt[:3, 3] = trans.squeeze()
return Rt
def recoverPose(E, points1, points2, K=KMatrix()):
'''
takes E, points1, points2, K, returns r, t, newMask
r, t transform from camera 2 coordinates to camera 1 coordinates
'''
points, r, t, newMask = cv2.recoverPose(E, np.array(points1), np.array(points2), pp=K.principalPoint)
r = np.linalg.inv(r)
t = t * -1
return points, r, t, newMask
def find_transform(K, p1, p2):
focal_length = 0.5 * (K[0,0] + K[1, 1])
principle_point = (K[0, 2], K[1, 2])
E,mask = cv2.findEssentialMat(p1, p2, focal_length, principle_point, cv2.RANSAC, 0.999, 1.0)
cameraMatrix = np.array([[focal_length, 0, principle_point[0]], [0, focal_length, principle_point[1]], [0, 0, 1]])
pass_count, R, T, mask = cv2.recoverPose(E, p1, p2, cameraMatrix, mask)
return R, T, mask
def caluclate_transformation(self):
E, mask_e = cv2.findEssentialMat(self.kp1, self.kp2, focal=1.0, pp=(0., 0.),
method=cv2.RANSAC, prob=0.999, threshold=0.001)
print ("Essential matrix: used ",np.sum(mask_e) ," of total ",len(good),"matches")
points, R, t, mask_RP = cv2.recoverPose(E, kp1_match_ud, kp2_match_ud, mask=mask_e)
print("points:",points,"\trecover pose mask:",np.sum(mask_RP!=0))
print("R:",R,"t:",t.T)
def recover_pose(config, kp1, desc1, kp2, desc2):
flann_params = dict(
algorithm=FLANN_INDEX_LSH,
table_number=6, # 12
key_size=12, # 20
multi_probe_level=1) #2
matcher = cv2.FlannBasedMatcher(
flann_params, {}) # bug : need to pass empty dict (#1329)
raw_matches = matcher.knnMatch(desc1, trainDescriptors=desc2, k=2) #2
logging.info('原始匹配数目： %s', len(raw_matches))
mi, p1, p2, kp_pairs = filter_matches(kp1, kp2, raw_matches)
# p1 = cv2.KeyPoint_convert(kp_pairs[0])
# p1 = cv2.KeyPoint_convert(kp_pairs[1])
if len(p1) < 4:
logging.info('过滤之后匹配数目（ %s ）小于4个，无法定位', len(p1))
return
logging.info('过滤之后匹配数目： %s', len(kp_pairs))
# H, status = cv2.findHomography(p1, p2, cv2.RANSAC, 5.0)
# F, status = cv2.findFundamentalMat(p1, p2)
# H, status = cv2.findHomography(p1, p2, 0)
H, status = None, None
if config.homography:
logging.info("使用 finHomography 过滤匹配结果")
H, status = cv2.findHomography(p1, p2, cv2.RANSAC, 3.0)
elif config.fundamental:
logging.info("使用 findFundamentalMat 过滤匹配结果")
H, status = cv2.findFundamentalMat(p1, p2)
if config.show:
explore_match(config, kp_pairs, status, H)
if status is None:
status = np.ones(len(kp_pairs), np.bool_)
pts1 = np.float64(
[kpp[0].pt for kpp, flag in zip(kp_pairs, status) if flag])
pts2 = np.float64(
[kpp[1].pt for kpp, flag in zip(kp_pairs, status) if flag])
n = len(np.unique(pts2, axis=0))
if n < 9:
logging.info('最终匹配数目（ %s ）小于9个，无法定位', n)
return
K = camera_matrix(config)
E, mask = cv2.findEssentialMat(pts1, pts2, K)
retval, R, t, mask = cv2.recoverPose(E, pts1, pts2, K)
nv = np.matrix(R) * np.float64([0, 0, 1]).reshape(3, 1)
yaw = np.arctan2(nv[0], nv[2])
return yaw, t
def test_triangulation(self):
np.set_printoptions(suppress=True,
formatter={'float_kind': '{:0.2f}'.format})
src, dst, expected_mask = create_point_pair(3)
src0 = cv2.undistortPoints(np.expand_dims(src, axis=1),
cameraMatrix=self.calibration,
distCoeffs=None)
dst0 = cv2.undistortPoints(np.expand_dims(dst, axis=1),
cameraMatrix=self.calibration,
distCoeffs=None)
src = np.expand_dims(src, axis=1)
dst = np.expand_dims(dst, axis=1)
# F_, mask= cv2.findFundamentalMat(src,dst,cv2.FM_RANSAC,0.004,0.99)
E, mask = cv2.findEssentialMat(dst,
src,
cameraMatrix=self.calibration,
method=cv2.RANSAC,
prob=0.999,
threshold=0.004)
actual_E_norm = E / np.linalg.norm(E)
print('Actual E:', actual_E_norm)
print(mask)
_, local_R, local_t, mask = cv2.recoverPose(
E, dst, src, cameraMatrix=self.calibration)
print("R:", local_R)
print("t:", local_t)
s = 1.38 / local_t[0]
local_t[0] *= s
local_t[2] *= s
print("t:", local_t)
# self.P_0 = np.dot(self.calibration,np.hstack((np.identity(3),np.zeros((3,1)))))
# P = np.dot(self.calibration,np.hstack((local_R.T,-np.dot(local_R.T,local_t))))
P = np.hstack((local_R.T, -np.dot(local_R.T, local_t)))
# points4D = cv2.triangulatePoints(self.P_0,P,src, dst)
points4D = cv2.triangulatePoints(self.P_0, P, src0, dst0)
points4D = points4D / np.tile(points4D[-1, :], (4, 1))
pt3d = points4D
# points4D = points4D / np.tile(points4D[-1, :], (4, 1))
# pt3d = points4D[:3, :].T
# for i,good in enumerate(mask):
# if good ==255:
# print("Recover Points:\n",pt3d[:3].T[i])
s = np.dot(self.P_0, points4D)
s /= s[2]
d = np.dot(P, points4D)
d /= d[2]
for i, good in enumerate(mask):
if good == 255:
print("pt3d", pt3d.T[i])
print("src", src0[i, 0, :])
print("src", s.T[i])
print("dst", dst0[i, 0, :])
print("dst", d.T[i])
def mainLoop(self, _event):
if self.ok == False:
return
else:
if self.status == "init":
self.map.pushFrame(self.cur_frame)
self.status = "default"
return
else:
print len(self.cur_frame.keypoints)
ref_frame = self.map.popFrame()
try:
cur_frame = self.cur_frame
#ref_frame = self.map.popFrame()
matches = matchBF(cur_frame.descriptors,
ref_frame.descriptors)
cur_kp, ref_kp = convertKeypoint(cur_frame.keypoints,
ref_frame.keypoints,
matches)
K = np.array(cur_frame.getK()).reshape([3, 3])
print len(cur_kp), len(ref_kp)
focal, pp = cur_frame.getFocalPp()
#print focal, pp
#E, _ = cv2.findEssentialMat(cur_kp, ref_kp, K, cv2.RANSAC, threshould=1.0)
E, _ = cv2.findEssentialMat(cur_kp,
ref_kp,
focal[0],
pp,
cv2.RANSAC,
threshold=1.0)
_, R, t, mask = cv2.recoverPose(E, cur_kp, ref_kp, K, 1.0)
dR = R - np.eye(3, dtype=float)
d = np.sum(dR * dR)
print d
if d > 1:
#self.map.pushFrame(cur_frame)
raise Exception()
dt = t - np.array([0, 0, 0])
d = np.sum(dt * dt)
print d
if d > 5:
raise Exception()
pre_R, pre_t = ref_frame.getRt()
cur_R = np.dot(pre_R, R)
cur_t = pre_t + t
print cur_R
print cur_t
cur_frame.setRt(cur_R, cur_t)
self.lossframe = 0
self.map.pushFrame(cur_frame)
except:
self.map.pushFrame(ref_frame)
self.lossframe = self.lossframe + 1
print "loss frame ", self.lossframe
def run(self):
self.tmap.start()
time.sleep(3)
# Load first image
str_ptr = self.dataSets_PATH + 'I1_%06d.png' % (0)
img_ptr = cv2.imread(str_ptr, 0)
kp = self.fast.detect(img_ptr, None)
kp_ptr = np.array([kp[idx].pt for idx in range(len(kp))], np.float32)
# main loop
for frame_id in range(1, 371):
str_cur = self.dataSets_PATH + 'I1_%06d.png' % frame_id
img_cur = cv2.imread(str_cur, 0)
img_with_kp = cv2.drawKeypoints(
img_ptr, kp, None, None,
cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
cv2.imshow('image', img_with_kp)
kp_cur, st, err = cv2.calcOpticalFlowPyrLK(img_ptr, img_cur,
kp_ptr, None,
**self.lk_params)
kp_ptr = np.delete(
kp_ptr,
np.where((st == 0)
| ((kp_cur[:, [1]] < 0) | (kp_cur[:, [0]] < 0)))[0],
0)
kp_cur = np.delete(
kp_cur,
np.where((st == 0)
| ((kp_cur[:, [1]] < 0) | (kp_cur[:, [0]] < 0)))[0],
0)
E, mask = cv2.findEssentialMat(kp_cur, kp_ptr, self.focal,
(self.offsetx, self.offsety),
cv2.RANSAC, 0.999, 1.0)
res = cv2.recoverPose(E, kp_cur, kp_ptr, self.focal,
(self.offsetx, self.offsety))
dR = np.matrix(res[1])
dt = np.matrix(res[2])
si = dt.item(0) + dt.item(1) + dt.item(2)
if (si < 0):
dt = -dt
self.t = self.t + self.R * dt
self.R = dR * self.R
if (kp_ptr.shape[0] < self.minFeatureNum) & (frame_id != 1):
kp = self.fast.detect(img_ptr, None)
kp_ptr = np.array([kp[idx].pt for idx in range(len(kp))],
np.float32)
kp_cur, st, err = cv2.calcOpticalFlowPyrLK(
img_ptr, img_cur, kp_ptr, None, **self.lk_params)
kp_cur = np.delete(
kp_cur,
np.where((st == 0) | (
(kp_cur[:, [1]] < 0) | (kp_cur[:, [0]] < 0)))[0], 0)
img_ptr = img_cur
kp_ptr = kp_cur
self.tmap.setPoint(self.t[0], self.t[2])
cv2.waitKey(30)
def handle_stereo_images(config):
focal = [float(s) for s in config.focal.split(',')]
rotate = config.yaw * math.pi / 180
offset = [float(s) for s in config.offset.split(',')]
filename1, filename2 = config.images
feature, count = config.feature, config.nFeatures
asift = config.asift
homography = config.homography
imgLeft, imgRight = cv2.imread(filename1, 0), cv2.imread(filename2, 0)
h, w = imgLeft.shape[:2]
# R, _jacob = cv2.Rodrigues(rotation_matrix(rotate, axis='y').A)
R = rotation_matrix(rotate, axis='y').A
T = np.float64(offset)
distCoeffs = np.zeros(4)
fx, fy = focal
cx, cy = (w - 1) / 2, (h - 1) / 2
K = np.float64([[w * fx, 0, cx], [0, h * fy, cy], [0, 0, 1]])
pts1, pts2, keypoints, descriptors = match_images(imgLeft,
imgRight,
feature + '-flann',
asift=asift,
n=count,
homography=homography)
# if len(keypoints) < kp_stereo_match_threshold:
# raise RuntimeError('双目照片匹配的关键点个数 %d 少于 %d' % (len(keypoints), kp_stereo_match_threshold))
if config.myself:
points3d = calculate_points3d(K, R, T, pts1, pts2)
else:
if config.correct:
E, mask = cv2.findEssentialMat(pts1, pts2, K)
retval, R, t, mask = cv2.recoverPose(E, pts1, pts2, K)
nv = np.matrix(R) * np.float64([0, 0, 1]).reshape(3, 1)
yaw = np.arctan2(nv[0], nv[2])
print("自动校正得到的相机水平偏转角度为 %8.2f" % (yaw[0] / math.pi * 180))
nv = np.matrix(R) * np.float64([0, 1, 0]).reshape(3, 1)
pitch = np.arctan2(nv[2], nv[1])
print("自动校正得到的相机仰角为 %8.2f" % (pitch[0] / math.pi * 180))
nv = np.matrix(R) * np.float64([1, 0, 0]).reshape(3, 1)
roll = np.arctan2(nv[1], nv[0])
print("自动校正得到的相机旋转角度为 %8.2f" % (roll[0] / math.pi * 180))
distCoeffs = np.zeros(4)
P1, P2, Q = stereo_rectify(K, distCoeffs, K, distCoeffs, (h, w), R, T)
# print (h, w)
# print ('R:', R)
# print ('T:', T)
# print ('K:', K)
# print ('P1:', P1)
# print ('P2:', P2)
points3d = triangulate_points(P1, P2, pts1.T, pts2.T)
return keypoints, points3d
def Motion_detect(Image1, Image2, Height, K):
'''
Image1: image 1
Image2: image 2
Height: Height of ego car in metres
K: 3x3 camera intrinsic matrix
'''
orb = cv2.ORB_create()
Keypoints1, Descriptors1 = orb.detectAndCompute(Image1, None)
Keypoints2, Descriptors2 = orb.detectAndCompute(Image2, None)
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
# Match descriptors.
matches = bf.match(Descriptors1,Descriptors2)
# Sort them in the order of their distance.
# matches = sorted(matches, key = lambda x:x.distance)
kp1_list = np.mat([])
kp2_list = np.mat([])
k = 0
number_of_matches = 3000
for m in matches:
img1Idx = m.queryIdx
img2Idx = m.trainIdx
(img1x, img1y) = Keypoints1[img1Idx].pt
(img2x, img2y) = Keypoints2[img2Idx].pt
if k == 0:
kp1_list = [[img1x,img1y,1]]
kp2_list = [[img2x,img2y,1]]
k = 1
else:
kp1_list = np.append(kp1_list,[[img1x,img1y,1]],axis = 0)
kp2_list = np.append(kp2_list,[[img2x,img2y,1]],axis = 0)
k+=1
if k == number_of_matches:
break
F = cv2.findFundamentalMat(kp1_list[:,0:2], kp2_list[:,0:2], method = cv2.FM_RANSAC)
E = K.T @ F[0] @ K
Image3 = cv2.drawMatches(Image1,kp1_list,Image2,kp2_list,matches[:10]ls
)
plt.imshow(Image3)
plt.show()
Transformation_info = cv2.recoverPose(E, (kp1_list[:,0:2]), (kp2_list[:,0:2]), K)
Rotation = Transformation_info[1]
Translation = Transformation_info[2]
Translation = Translation# / Translation[2]
return Rotation, Translation
def getRT(self, pts1, pts2):
E = self.K.T @ self.F @ self.K
_, R, T, mask = cv2.recoverPose(E, pts1, pts2, self.K)
### TODO : test
pts1 = pts1[mask[:, 0] == 1]
pts2 = pts2[mask[:, 0] == 1]
return R, T
def findDecomposedEssentialMatrix(self, p1, p2):
# fundamental matrix and inliers
# F, mask = cv.findFundamentalMat(p1, p2, cv.FM_LMEDS, 1, 0.999)
F, mask = cv.findFundamentalMat(p1, p2, cv.FM_RANSAC, REPROJ_ERROR, RANSAC_CONFIDENCE, RANSAC_MAXITER)
mask = mask.astype(bool).flatten()
E = np.dot(self.K.T, np.dot(F, self.K))
_, R, t, _ = cv.recoverPose(E, p1[mask], p2[mask], self.K)
return R, t, p1[mask], p2[mask], mask
def processFrame(self, frame_id): self.px_ref, self.px_cur = featureTracking_(self.last_frame, self.new_frame) E, mask = cv2.findEssentialMat(self.px_cur, self.px_ref, focal=self.focal, pp=self.pp, method=cv2.RANSAC, prob=0.999, threshold=1.0) _, R, t, mask = cv2.recoverPose(E, self.px_cur, self.px_ref, focal=self.focal, pp = self.pp) self.cur_t = self.cur_t + self.cur_R.dot(t) self.cur_R = R.dot(self.cur_R) # self.cur_t =t # self.cur_R = R self.px_ref = self.px_cur
def FindRTOpt(e_mat, last_frame, cur_frame, camera):
last_feature = last_frame.feature_2d
cur_feature = cur_frame.feature_2d
_, R, t, mask = cv2.recoverPose(e_mat,
cur_feature,
last_feature,
focal=camera.fx,
pp=(camera.u, camera.v))
return R, t
def update_motion(points1, points2, Rp, Tp, scale=1.0):
p1 = np.reshape(np.array(points1, dtype=np.float32), (-1, 1, 2))
p2 = np.reshape(np.array(points2, dtype=np.float32), (-1, 1, 2))
E, mask = cv2.findEssentialMat(p1, p2,
cameraMatrix=cam_mat, method=cv2.RANSAC, prob=0.999, threshold=1.0, mask=None)
points, R, T, newmask = cv2.recoverPose(E, p1, p2, cameraMatrix=cam_mat, mask=mask)
Tp = Tp + np.dot(R, T)*scale
Rp = np.dot(R, Rp)
return Rp, Tp
def by_ransac(p0, p1, K):
focal = K[0, 0]
pp = K[0, 2], K[1, 2]
E, mask = cv2.findEssentialMat(p0, p1, focal, pp, cv2.FM_RANSAC, 0.999,
1.0)
retval, R, t, mask = cv2.recoverPose(E, p0, p1)
Rt = np.eye(4)
Rt[:3, :3] = R
Rt[:3, 3] = t.T
return Rt
def process_frame(self, img, img_index):
# keypoints extraction and descriptors computation
# key_points, descriptors = self.kpe.extract(img)
key_points = self.kpe.extract_fast(img)
print("keypoints: {}".format(len(key_points)))
good_matches = None
if self.prev_frame_extracts is not None and self.prev_frame is not None:
# matching
# good_matches = self.match(key_points, self.prev_frame_extracts['key_points'],
# descriptors, self.prev_frame_extracts['descriptors'])
good_matches = self.track(self.prev_frame, img,
self.prev_frame_extracts['key_points'])
# filtering
good_matches = np.array(good_matches)
model, inliers = ransac((good_matches[:, 0], good_matches[:, 1]),
FundamentalMatrixTransform,
min_samples=8,
residual_threshold=1,
max_trials=100)
good_matches = good_matches[inliers]
print("Number of keypoints after filtering: {}".format(
len(good_matches)))
self.F = model.params
self.E = self._E_from_F(self.F)
# self.E, mask = cv2.findEssentialMat(good_matches[:, :, 0], good_matches[:, :, 1],
# focal=self.fx, pp=(self.cx, self.cy), method=cv2.RANSAC,
# prob=0.999, threshold=1.0)
# pose estimation
# TODO wtf is going on if write [:, :, 0] instead of [:, 0]??????
_, R, t, mask = cv2.recoverPose(self.E,
good_matches[:, 0],
good_matches[:, 1],
focal=self.fx,
pp=(self.cx, self.cy))
absolute_scale = self.getAbsoluteScale(img_index)
if absolute_scale > 0.1:
self.t = self.t + absolute_scale * self.R.dot(t.reshape(3))
self.R = R.dot(self.R)
# self.t = self.t + self.R.dot(t.reshape(3))
# self.R = R.dot(self.R)
# self.prev_frame_extracts = {'key_points': key_points, 'descriptors': descriptors}
self.prev_frame_extracts = {'key_points': key_points}
self.prev_frame = img
if img_index == 0:
img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
if good_matches is not None:
img = self.draw_matches(img, good_matches)
return img, self.t
def solveR(self):
# Read current frame
cur_frame = self.cur_frame
ref_frame = self.ref_zero
#ref_frame = self.map.popFrame()
# Calculate matched frame
matches = matchBF(cur_frame.descriptors, ref_frame.descriptors)
cur_kp, ref_kp = convertKeypoint(cur_frame.keypoints,
ref_frame.keypoints, matches)
K = np.array(cur_frame.getK()).reshape([3, 3])
#print len(cur_kp), len(ref_kp)
rospy.loginfo("[%s]: current matched number: [%s]" %
(self.node_name, len(ref_kp)))
focal, pp = cur_frame.getFocalPp()
#print focal, pp
# Find Essential Matrix
#E, _ = cv2.findEssentialMat(cur_kp, ref_kp, K, cv2.RANSAC, threshould=1.0)
#E, _ = cv2.findEssentialMat(cur_kp, ref_kp, focal[0], pp, cv2.RANSAC, threshold=2.0)
E, _ = cv2.findEssentialMat(ref_kp,
cur_kp,
focal[0],
pp,
cv2.RANSAC,
threshold=0.4,
prob=0.999)
# Recover [R, t] from Essential matrix
#_, R, t, mask = cv2.recoverPose(E, cur_kp, ref_kp, K, 2.0)
_, R, t, mask = cv2.recoverPose(E, ref_kp, cur_kp, K, 0.4)
dR = R - np.eye(3, dtype=float)
d = np.sum(dR * dR)
#print "dR: ", d
if d > 5:
#self.map.pushFrame(cur_frame)
#raise Exception()
return
dt = t - np.array([0, 0, 0])
d = np.sum(dt * dt)
#print "dt: ", d
if d > 5:
#raise Exception()
return
pre_R, pre_t = ref_frame.getRt()
cur_R = np.dot(pre_R, R)
cur_t = pre_t + t
rospy.loginfo("current rotation [%s]" % (cur_R))
x = cur_R[0, 0]
y = cur_R[0, 1]
z = complex(x, y)
angle = np.angle(z, deg=True)
rospy.loginfo("current angle [%s]" % (angle))
def process_img_pair(img1, kp1, des1, img2, kp2, des2, f):
h, w, _ = img1.shape
good = compute_matches(des1, des2)
u1 = []
u2 = []
matched_kps = []
for m in good:
k1 = kp1[m.queryIdx]
d1 = des1[m.queryIdx]
k2 = kp2[m.trainIdx]
d2 = des2[m.trainIdx]
matched_kps.append((KeyPoint(k1, d1), KeyPoint(k2, d2)))
u1.append(k1.pt)
u2.append(k2.pt)
# u,v coords of keypoints in images
u1 = np.array(u1)
u2 = np.array(u2)
# Make homogeneous
u1 = np.c_[u1, np.ones(u1.shape[0])]
u2 = np.c_[u2, np.ones(u2.shape[0])]
cu = w // 2
cv = h // 2
# Camera matrix
K_cam = np.array([[f, 0, cu], [0, f, cv], [0, 0, 1]])
K_inv = np.linalg.inv(K_cam)
# Generalized image coords
x1 = u1 @ K_inv.T
x2 = u2 @ K_inv.T
# Compute essential matrix with RANSAC
E, inliers = cv2.findEssentialMat(x1[:, :2],
x2[:, :2],
np.eye(3),
method=cv2.RANSAC,
threshold=1e-3)
inliers = inliers.ravel().astype(bool)
n_in, R, t, _ = cv2.recoverPose(E, x1[inliers, :2], x2[inliers, :2])
# P_i = [R|t] with first image considered canonical
P_1 = np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 0]])
P_2 = np.hstack((R, t))
matched_kps = [k for i, k in enumerate(matched_kps) if inliers[i]]
return ImgPair(
img1,
img2,
matched_kps,
u1[inliers],
u2[inliers],
x1[inliers],
x2[inliers],
E,
K_cam,
P_1,
P_2,
)
def estimate_relative_pose_from_correspondence(pts1, pts2, K1, K2):
f_avg = (K1[0, 0] + K2[0, 0]) / 2
pts1, pts2 = np.ascontiguousarray(pts1, np.float32), np.ascontiguousarray(pts2, np.float32)
pts_l_norm = cv2.undistortPoints(np.expand_dims(pts1, axis=1), cameraMatrix=K1, distCoeffs=None)
pts_r_norm = cv2.undistortPoints(np.expand_dims(pts2, axis=1), cameraMatrix=K2, distCoeffs=None)
E, mask = cv2.findEssentialMat(pts_l_norm, pts_r_norm, focal=1.0, pp=(0., 0.),
method=cv2.RANSAC, prob=0.999, threshold=3.0 / f_avg)
points, R_est, t_est, mask_pose = cv2.recoverPose(E, pts_l_norm, pts_r_norm)
return mask[:,0].astype(np.bool), R_est, t_est
def decomposeEssentialMat(Essential_matrix): """ :param Essential_matrix: Essential_matrix :return: """ U, Sigma, Vt = linalg.svd(K) OutputArray_R1 = U * Sigma * Vt OutputArray_R2 = U * np.transpose(Sigma) * Vt t = U[:, 2] * 1.0 Rotation = cv2.recoverPose(Essential_matrix) return OutputArray_R1, OutputArray_R2, t
def calc_essential_matrix(self, save=False):
"""
Calculate Essential matrix between first two views.
:param save: Save essential matrix as binary file using Numpy.
"""
self.essential_mat, _ = cv.findEssentialMat(self.X1, self.X2, self.K,
cv.RANSAC, 0.999, 1.0)
_, self.view2.rotation, self.view2.translation, _ = cv.recoverPose(
self.essential_mat, self.X1, self.X2, self.K)
if save:
np.savez('essential_matrix', E=self.essential_mat)
def get_camera_poses(E, p1F, p2F, K):
# calculate rotation and translation from one pose to the other
point, R, t, mask = cv2.recoverPose(E, p1F, p2F, K)
# Create Null mat for camera pose used as starting point
mat1 = np.hstack((np.eye(3, 3), np.zeros((3, 1))))
# Create mat with rotation and translation from starting point (Null mat)
mat2 = np.hstack((R, t))
# Create the poses by multiplying with camera matrix, ie. the intrinsic parameters for camera
pose1 = K @ mat1
pose2 = K @ mat2
return (pose1, pose2)
def processFrame(self, frame_id): self.px_ref, self.px_cur = featureTracking(self.last_frame, self.new_frame, self.px_ref) E, mask = cv2.findEssentialMat(self.px_cur, self.px_ref, focal=self.focal, pp=self.pp, method=cv2.RANSAC, prob=0.999, threshold=1.0) _, R, t, mask = cv2.recoverPose(E, self.px_cur, self.px_ref, focal=self.focal, pp = self.pp) absolute_scale = self.getAbsoluteScale(frame_id) if(absolute_scale > 0.1): self.cur_t = self.cur_t + absolute_scale*self.cur_R.dot(t) self.cur_R = R.dot(self.cur_R) if(self.px_ref.shape[0] < kMinNumFeature): self.px_cur = self.detector.detect(self.new_frame) self.px_cur = np.array([x.pt for x in self.px_cur], dtype=np.float32) self.px_ref = self.px_cur
def findDecomposedEssentialMatrix(self, p1, p2):
# fundamental matrix and inliers
F, mask = cv2.findFundamentalMat(p1, p2, cv2.FM_LMEDS, 1, 0.999)
#F, mask = cv.findFundamentalMat(p1, p2, cv.FM_RANSAC, 1.0, 0.999)
mask = mask.astype(bool).flatten()
E = np.dot(self.camera_matrix_left.T,
np.dot(F, self.camera_matrix_right))
_, R, t, _ = cv2.recoverPose(E, p1[mask], p2[mask],
self.camera_matrix_right)
return R, t
def image_process(H_init, p_0, all_images):
data_points = []
for index in range(20, len(all_images) - 1):
print("image_number = ", index)
img1 = cv2.imread("stereo/centre/" + str(all_images[index]),
0) #loading first image in greyscale
img2 = cv2.imread("stereo/centre/" + str(all_images[index + 1]),
0) #loading second image in greyscale
colorimage1 = cv2.cvtColor(
img1,
cv2.COLOR_BayerGR2BGR) #converting first image from BayerGR to BGR
colorimage2 = cv2.cvtColor(
img2, cv2.COLOR_BayerGR2BGR
) #converting second image from BayerGR to BGR
undistortedimage1 = UndistortImage(
colorimage1, LUT) #undistorting first image using LUT
undistortedimage2 = UndistortImage(
colorimage2, LUT) #undistorting second image using LUT
gray1 = cv2.cvtColor(undistortedimage1,
cv2.COLOR_BGR2GRAY) #converting back to grayscale
gray2 = cv2.cvtColor(undistortedimage2,
cv2.COLOR_BGR2GRAY) #converting back to grayscale
# Finding proper matches between two frames
features1, features2 = SIFT_matches(gray1, gray2)
#using inbuilt function to directly compute F_matrix after RANSAC
FinalFundamentalMatrix, m = cv2.findFundamentalMat(
features1, features2, cv2.FM_RANSAC)
features1 = features1[m.ravel() == 1]
features2 = features2[m.ravel() == 1]
# Calculating the Essential matrix
E_matrix = K.T @ FinalFundamentalMatrix @ K
# Obtaining final Rotational and Translation pose for cameras
retval, rot_mat, T, mask = cv2.recoverPose(E_matrix, features1,
features2, K)
H_init = H_init @ H_matrix(
rot_mat, T
) #matmul between H_init(Ident 4x4) and H_matrix that was calculated using new Rot and Trans matrix
p_projection = H_init @ p_0 #new projection point calculated
data_points.append([p_projection[0][0], -p_projection[2][0]])
plt.scatter(p_projection[0][0], -p_projection[2][0], color='g')
return data_points
def processFrame(self, frame_id): self.px_ref, self.px_cur = featureTracking(self.last_frame, self.new_frame, self.px_ref) E, mask = cv2.findEssentialMat(self.px_cur, self.px_ref, focal=self.focal, pp=self.pp, method=cv2.RANSAC, prob=0.999, threshold=1.0) _, R, t, mask = cv2.recoverPose(E, self.px_cur, self.px_ref, focal=self.focal, pp = self.pp) absolute_scale = self.getAbsoluteScale(frame_id) if(absolute_scale > 0.1): self.cur_t = self.cur_t + absolute_scale*self.cur_R.dot(t) self.cur_R = R.dot(self.cur_R) if(self.px_ref.shape[0] < kMinNumFeature): self.px_cur = self.detector.detect(self.new_frame) self.px_cur = np.array([x.pt for x in self.px_cur], dtype=np.float32) self.px_ref = self.px_cur
def compute_pose(self, p1, p2, E):
"""
p1, p2: only the 2d points in the respective images
pass ratio test. These points should correspond to each other.
E: Essential matrix
Outputs:
R, trans: Rotation, Translation vectors
Hint: recoverPose
"""
retval, R, t, mask = cv2.recoverPose(E, p1, p2, self.intrinsic)
return R, t
def two_frame_sfm(self, name1, name2, id1, id2):
with open(self.data_path + name1, "rb") as fp: # Unpickling
point_list1 = pickle.load(fp)
with open(self.data_path + name2, "rb") as fp: # Unpickling
point_list2 = pickle.load(fp)
points1_0 = np.asarray(point_list1)
points2_0 = np.asarray(point_list2)
bad_indices1 = np.where(~points1_0.any(axis=1))[0]
bad_indices2 = np.where(~points2_0.any(axis=1))[0]
# Save for every led all its locations (pixels)
if self.first_frame:
self.first_frame = False
for i in np.where(points1_0.any(axis=1))[0]:
self.led_graph[self.led_code_list[i]].add(
(points1_0[i, 0], points1_0[i, 1]), id1)
for i in np.where(points2_0.any(axis=1))[0]:
self.led_graph[self.led_code_list[i]].add(
(points2_0[i, 0], points2_0[i, 1]), id2)
bad_indices = list(bad_indices1)
bad_indices.extend(x for x in bad_indices2 if x not in bad_indices)
good_indices = [
i for i in range(points1_0.shape[0]) if i not in bad_indices
]
good_code_names = [self.led_code_list[i] for i in good_indices]
print("good_code_names: ", good_code_names)
points1 = points1_0[good_indices, :]
points2 = points2_0[good_indices, :]
E, mask = cv.findEssentialMat(points1,
points2,
self.mtx,
method=cv.RANSAC,
prob=0.999,
threshold=1.0)
# E = E[:3,:3]
print(E)
retval, R, t, mask, triangulatedPoints = cv.recoverPose(
E, points1, points2, self.mtx, distanceThresh=50)
camera_two = -R @ t
triangulatedPoints = triangulatedPoints[0:3, :] / triangulatedPoints[
3, :]
frame_pc = PC(triangulatedPoints, good_code_names)
frame_pose = RelativePose(R, t, id1, id2)
if np.linalg.det(R) - 1 > 1e-7:
print(f'error in R matrix:{R}')
return frame_pc, frame_pose
def visualOdometry(image):
global current_rot
global current_pos
global im_count
global points0
global keypoint
global image1
global position_figure
global prev_image
global lk_params
global feature_detector
global prev_keypoint
global focal
global principalPoint
global position_axes
# main process
keypoint = feature_detector.detect(image, None)
if prev_image is None:
prev_image = image
prev_keypoint = keypoint
return
points = np.array(map(lambda x: [x.pt], prev_keypoint), dtype=np.float32)
p1, st, err = cv2.calcOpticalFlowPyrLK(prev_image, image, points, None,
**lk_params)
E, mask = cv2.findEssentialMat(p1, points, camera_matrix, cv2.RANSAC,
0.999, 1.0, None)
points, R, t, mask = cv2.recoverPose(E, p1, points, camera_matrix)
scale = 1.0
current_pos += current_rot.dot(t) * scale
current_rot = R.dot(current_rot)
print current_pos[0][0], current_pos[2][0]
position_axes.scatter(current_pos[0][0], current_pos[2][0])
plt.pause(.01)
img = cv2.drawKeypoints(image, keypoint, None)
# cv2.imshow('image', image)
cv2.imshow('feature', img)
cv2.waitKey(1)
prev_image = image
prev_keypoint = keypoint
def get_pose(self, curr_kpts, prev_kpts, focal, pp):
# Recover the rotation matrix and the translation vector from the
# essential matrix E.
# @param curr_kpts: Keypoints of the current frame (np.array float)
# @param prev_kpts: Keypoints of the previous frame
# @param focal: focal lenght of the camera
# @param pp: principal point of the camera
# @param mask: mask , it will store the mask with the points used to
# recover the pose. Use this mask to filter the 3D points to be used in
# following operations.
# @return R: Rotation matrix, to be calculated
# @return t: translation vector, to be calculated
points, R, t, mask = cv2.recoverPose(self.E, curr_kpts, prev_kpts,
focal, pp)
return R, t
def matchingTests():
im_1 = cv2.imread('c1.bmp')
im_2 = cv2.imread('c2.bmp')
#k = np.mat(([[ 683.39404297, 0. , 267.21336591], [ 0. , 684.3449707 , 218.56421036], [ 0. , 0. , 1. ]]))
k = clsReconstruction.loadData('k_cam_hp.dat')
#resise, if it is necessary
#im_1 = cv2.resize(im_1,None,fx=0.3, fy=0.3, interpolation = cv2.INTER_CUBIC)
#im_2 = cv2.resize(im_2,None,fx=0.3, fy=0.3, interpolation = cv2.INTER_CUBIC)
#convert to gray
im_1 = cv2.cvtColor(im_1, cv2.COLOR_BGR2GRAY)
im_2 = cv2.cvtColor(im_2, cv2.COLOR_BGR2GRAY)
#proceed with sparce feature matching
orb = cv2.ORB_create()
kp_1, des_1 = orb.detectAndCompute(im_1,None)
kp_2, des_2 = orb.detectAndCompute(im_2,None)
bf = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True)
matches = bf.match(des_1,des_2)
matches = sorted(matches, key = lambda x:x.distance)
draw_params = dict(matchColor = (20,20,20), singlePointColor = (200,200,200),
matchesMask = None,
flags = 0)
im_3 = cv2.drawMatches(im_1,kp_1,im_2,kp_2,matches[0:20], None, **draw_params)
#select points to evaluate the fundamental matrix
pts1 = []
pts2 = []
idx = matches[1:20]
for i in idx:
pts1.append(kp_1[i.queryIdx].pt)
pts2.append(kp_2[i.trainIdx].pt)
pts1 = np.array(pts1)
pts2 = np.array(pts2)
#creating homegeneous coordenate
pones = np.ones((1,len(pts1))).T
pth_1 = np.hstack((pts1,pones))
pth_2 = np.hstack((pts2,pones))
k = np.array(k)
ki = np.linalg.inv(k)
#normalized the points
pthn_1 = []
pthn_2 = []
for i in range(0,len(pts1)):
pthn_1.append((np.mat(ki) * np.mat(pth_1[i]).T).transpose())
pthn_2.append((np.mat(ki) * np.mat(pth_2[i]).T).transpose())
ptn1 = []
ptn2 = []
for i in range(0,len(pts1)):
ptn1.append([pthn_1[i][0,0],pthn_1[i][0,1]])
ptn2.append([pthn_2[i][0,0],pthn_2[i][0,1]])
ptn1 = np.array(ptn1)
ptn2 = np.array(ptn2)
#evaluate the essential Matrix (using the original points, not the normilized ones)
E, mask0 = cv2.findEssentialMat(pts1,pts2,k,cv2.FM_RANSAC)
#evaluate the fundamental matrix (using the normilized points)
F, mask = cv2.findFundamentalMat(pts1,pts2,cv2.FM_RANSAC)
E = np.mat(E)
F = np.mat(F)
R, t, mask1 = cv2.recoverPose(E,ptn1,ptn2)
#selecting only inlier points
ptn1 = ptn1[mask.ravel() == 1]
ptn2 = ptn2[mask.ravel() == 1]
# Find epilines corresponding to points in right image (second image) and
# drawing its lines on left image
lines1 = cv2.computeCorrespondEpilines(pts2.reshape(-1,1,2), 2,F)
lines1 = lines1.reshape(-1,3)
img5,img6 = clsReconstruction.drawlines(im_1,im_2,lines1,pts1,pts2)
# Find epilines corresponding to points in left image (first image) and
# drawing its lines on right image
lines2 = cv2.computeCorrespondEpilines(pts1.reshape(-1,1,2), 1,F)
lines2 = lines2.reshape(-1,3)
img3,img4 = clsReconstruction.drawlines(im_2,im_1,lines2,pts2,pts1)
plt.subplot(131),plt.imshow(img5)
plt.subplot(132),plt.imshow(img3)
plt.subplot(133),plt.imshow(im_3)
plt.show()
def sparceRecostructionTrueCase(file1,file2,kdef):
k = np.mat(clsReconstruction.loadData(kdef))
ki = np.linalg.inv(k)
im_1 = cv2.imread(file1)
im_2 = cv2.imread(file2)
im_b1 = cv2.cvtColor(im_1,cv2.COLOR_RGB2GRAY)
im_b2 = cv2.cvtColor(im_2,cv2.COLOR_RGB2GRAY)
myC1 = Camera.myCamera(k)
myC2 = Camera.myCamera(k)
#place camera 1 at origin
myC1.projectiveMatrix(np.mat([0,0,0]).transpose(),[0, 0, 0])
#return macthing points
Xp_1, Xp_2 = clsReconstruction.getMatchingPoints(file1,file2,kdef,30)
#evaluate the essential Matrix using the camera parameter(using the original points)
E, mask0 = cv2.findEssentialMat(Xp_1,Xp_2,k,cv2.FM_RANSAC)
#evaluate Fundamental to get the epipolar lines
#since we already know the camera intrincics, it is better to evaluate F from the correspondence rather than from the 8 points routine
F = ki.T*np.mat(E)*ki
#retrive R and t from E
retval, R, t, mask2 = cv2.recoverPose(E,Xp_1,Xp_2)
#place camera 2
myC2.projectiveMatrix(np.mat(t),R)
#clsReconstruction.drawEpipolarLines(Xp_1,Xp_2,F,im_1,im_2)
#triangulate points
Str_4D = cv2.triangulatePoints(myC1.P[:3],myC2.P[:3],Xp_1.transpose()[:2],Xp_2.transpose()[:2]).T
#make them euclidian
Str_3D = cv2.convertPointsFromHomogeneous(Str_4D).reshape(-1,3)
#evaluate reprojection
Xh_Reprojection_1 = myC1.project(Str_4D)
Xh_Reprojection_2 = myC2.project(Str_4D)
#three ways to carry on the bundle adjustment I am using R,t and K as parameters. using the points is too time
# consuming although the results are much better;
#nR,nt, R0, R1 = clsReconstruction.bundleAdjustment(Str_4D,Xp_1,Xp_2,k,R,t)
#Str_4D, nR,nt, R0, R1 = clsReconstruction.bundleAdjustmentwithX(Str_4D,Xp_1,Xp_2,k,R,t) #### not working right now...
nk, nR, nt, R0, R1 = clsReconstruction.bundleAdjustmentwithK(Str_4D,Xp_1,Xp_2,k,R,t)
print('old value {0:.3f}, optimized pose: {1:.3f} \n'.format(R0,R1))
nki = np.linalg.inv(nk)
#reevaluate essential and fundamental matrixes
nE = clsReconstruction.skew(nt)*np.mat(nR)
nF = nki.T*np.mat(nE)*nki
#if we use the 3th option, we should reinitiate the cameras and the essential matrix, once the projective matrix will change
myC1 = Camera.myCamera(nk)
myC2 = Camera.myCamera(nk)
myC1.projectiveMatrix(np.mat([0,0,0]).transpose(),[0, 0, 0])
#reevaluate all variables based on new values of nR and nt
myC2.projectiveMatrix(np.mat(nt),nR)
Str_4D = cv2.triangulatePoints(myC1.P[:3],myC2.P[:3],Xp_1.transpose()[:2],Xp_2.transpose()[:2]).T
Str_3D = cv2.convertPointsFromHomogeneous(Str_4D).reshape(-1,3)
#Camera.myCamera.show3Dplot(Str_3D)
Xh_Opt_1 = myC1.project(Str_4D)#.reshape(-1,2)
Xh_Opt_2 = myC2.project(Str_4D)#.reshape(-1,2)
#POSSIBLE IMPLEMENTATION find residuals bigger a threshould value and optimize their location in R3
#clsReconstruction.drawEpipolarLines(Xp_1,Xp_2,nF,im_b1,im_b2)
im = clsReconstruction.drawPoints(im_1,Xp_1,(50,50,250))
im = clsReconstruction.drawPoints(im,Xh_Reprojection_1,(50,150,100))
im = clsReconstruction.drawPoints(im,Xh_Opt_1,(250,250,50))
im2 = clsReconstruction.drawPoints(im_2,Xp_2,(50,50,250))
im2 = clsReconstruction.drawPoints(im2,Xh_Reprojection_2,(50,150,100))
im2 = clsReconstruction.drawPoints(im2,Xh_Opt_2,(250,250,50))
cv2.imshow("im",im)
cv2.imshow("im2",im2)
cv2.waitKey(0)
plt.imshow(img3), plt.show()
plt.savefig("newimg.png")
new_pts1 = numpy.int32(points1)
new_pts2 = numpy.int32(points2)
focalLength = images[0].k[0][0]
img1 = images[0]
img2 = images[1]
E, mask = cv2.findEssentialMat(new_pts1, new_pts2, focal=focalLength)
print "E"
print E
#
points, r, t, newMask = cv2.recoverPose(E, new_pts1, new_pts2, mask=mask)
# print points
print "E-R"
print r
print "E-T"
print t
F1, mask = cv2.findFundamentalMat(new_pts1, new_pts2, method=cv2.FM_8POINT)
F2, mask = cv2.findFundamentalMat(new_pts1, new_pts2)
F = F2
print "F1"
print F1
points, r, t, newMask = cv2.recoverPose(img1.k.transpose().dot(F1).dot(img2.k), new_pts1, new_pts2, mask=mask)
# print points
print "F1-R"
def grab(self):
if not self.cam:
print('Error: camera not setup, run init() first')
return Msg.Odom()
try:
# get values from last run
pp = self.pp
focal = self.focal
p0 = self.p0
R = self.R
t = self.t
R_f = self.R_f
t_f = self.t_f
# try this???
# R = R_f.copy()
# t = np.array([0, 0, 0], dtype=np.float)
R = self.R
t = self.t
old_im = self.old_im
ret, raw = self.cam.read()
# end of video
if not ret:
print('video end')
draw(save_pts)
# break
exit()
################################################
# only for development ... delete!
# roi = (0,479,210,639)
# im = raw[roi[0]:roi[1],roi[2]:roi[3]]
im = raw
################################################
if ret:
cv2.imshow('debug', im)
cv2.waitKey(1)
# Not enough old points, p0 ... find new ones
if p0.shape[0] < 50:
print('------- reset --------')
p0 = self.featureDetection(old_im) # old_im instead?
if p0.shape[0] == 0:
print('bad image')
# continue
return
# p0 - old pts
# p1 - new pts
p0, p1 = self.featureTrack(im, old_im, p0)
# not enough new points p1 ... bad image?
if p1.shape[0] < 50:
print('------- reset p1 --------')
print('p1 size:', p1.shape)
self.old_im = im
self.p0 = p1
# continue
return
# drawKeyPoints(im, p1)
# since these are rectified images, fundatmental (F) = essential (E)
# E, mask = cv2.findEssentialMat(p0,p1,focal,pp,cv2.FM_RANSAC)
# retval, R, t, mask = cv2.recoverPose(E,p0,p1,R_f,t_f,focal,pp,mask)
E, mask = cv2.findEssentialMat(p0, p1, focal, pp, cv2.FM_RANSAC, 0.999, 1.0)
retval, R, t, mask = cv2.recoverPose(E, p0, p1, R, t, focal, pp, mask)
# print retval,R
# Now update the previous frame and previous points
# self.old_im = im.copy()
# p0 = p1.reshape(-1,1,2)
# p0 = p1
# print 'p0 size',p0.shape
# print 'p1 size',p1.shape
# print 't',t
# dt = t - t_prev
# scale = np.linalg.norm(dt)
# print scale
scale = 1.0
R_f = R.dot(R_f)
# t_f = t
t_f = t_f + scale * R_f.dot(t)
# t_prev = t
# t_f = t_f/t_f[2]
# dist += np.linalg.norm(t_f[:2])
# num = np.array([t_f[0]/t_f[2],t_f[1]/t_f[2]])
# num = t_f
print('position:', t_f)
# print 'distance:', dist
# R_f = R*R_f
# print 'R:',R_f,'t:',t_f
# print t_f
save_pts.append(t_f)
# save all
self.p0 = p1
self.R_f = R_f
self.t_f = t_f
self.old_im = im.copy()
self.t = t
self.R = R
# create message
odom = Msg.Odom()
odom['position']['position']['x'] = t_f[0]
odom['position']['position']['y'] = t_f[1]
odom['position']['position']['z'] = t_f[2]
odom['position']['orientation']['x'] = 0.0 # FIXME: 20160529 do rotation to quaternion conversion
odom['position']['orientation']['y'] = 0.0
odom['position']['orientation']['z'] = 0.0
odom['position']['orientation']['w'] = 1.0
odom['velocity']['linear']['x'] = 0.0
odom['velocity']['linear']['y'] = 0.0
odom['velocity']['linear']['z'] = 0.0
odom['velocity']['angular']['x'] = 0.0
odom['velocity']['angular']['y'] = 0.0
odom['velocity']['angular']['z'] = 0.0
return odom
except KeyboardInterrupt:
print('captured interrupt')
exit()
return kp1, kp2
fast = cv2.FastFeatureDetector_create(20)
lk_params = dict(winSize = (21, 21),
maxLevel = 3,
criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01))
img1 = cv2.imread('KITTI_VO/00/image_2/%06d.png' % 0)
img2 = cv2.imread('KITTI_VO/00/image_2/%06d.png' % 1)
img1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
img2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
kp1 = fast.detect(img1)
kp1, kp2 = feature_tracking(img1, img2, kp1)
focal = 718.8560
pp = (607.1928, 185.2157)
_, E = cv2.findEssentialMat(kp2, kp1, focal=focal, pp=pp, method=cv2.RANSAC, prob=0.999, threshold=1.0)
_, R, t, mask = cv2.recoverPose(E, kp2, kp1, focal=focal, pp=pp)
def processSecondFrame(self): self.px_ref, self.px_cur = featureTracking(self.last_frame, self.new_frame, self.px_ref) E, mask = cv2.findEssentialMat(self.px_cur, self.px_ref, focal=self.focal, pp=self.pp, method=cv2.RANSAC, prob=0.999, threshold=1.0) _, self.cur_R, self.cur_t, mask = cv2.recoverPose(E, self.px_cur, self.px_ref, focal=self.focal, pp = self.pp) self.frame_stage = STAGE_DEFAULT_FRAME self.px_ref = self.px_cur
#retorna pontos correspondentes
Xp_1, Xp_2 = clsReconstruction.getMathingPoints('b4.jpg','b5.jpg','k_cam_hp.dat')
#evaluate the essential Matrix using the camera parameter(using the original points)
E, mask0 = cv2.findEssentialMat(Xp_1,Xp_2,k,cv2.FM_RANSAC)
#evaluate the fundamental matrix (using the normilized points)
#F, mask = cv2.findFundamentalMat(Xp_1,Xp_2,cv2.FM_RANSAC)
#ki = np.linalg.inv(k)
R1, R2, t = cv2.decomposeEssentialMat(E)
retval, R, t, mask2 = cv2.recoverPose(E,Xp_1,Xp_2)
myC2 = Camera.myCamera(k)
myC2.projectiveMatrix(np.mat(t),R)
Xp_4Dt = cv2.triangulatePoints(myC1.P[:3],myC2.P[:3],Xp_1.transpose()[:2],Xp_2.transpose()[:2])
#Xp_4Dt = cv2.triangulatePoints(myC1.P[:3],myC2.P[:3],Xh_1.transpose()[:2],Xh_2.transpose()[:2])
Xp_4D = Xp_4Dt.T
for i in range(0,len(Xp_4D)):
Xp_4D[i] /= Xp_4D[i,3]
Xp_3D = Xp_4D[:,0:3]
