# horizontal edges
for u in u_list:
grid_list.append( [u, 0] )
grid_list.append( [u, height] )
# vertical edges (minus corners)
for v in v_list[1:-1]:
grid_list.append( [0, v] )
grid_list.append( [width, v] )
#print('grid_list:', grid_list)
distorted_uv = proj.redistort(grid_list, optimized=True)
distorted_uv = grid_list
if args.direct:
proj_list = project.projectVectors( IK, image.get_body2ned(),
image.get_cam2body(),
grid_list )
else:
#print(image.get_body2ned(opt=True))
proj_list = project.projectVectors( IK,
image.get_body2ned(opt=True),
image.get_cam2body(),
grid_list )
#print 'proj_list:', proj_list
if args.direct:
ned, ypr, quat = image.get_camera_pose()
else:
ned, ypr, quat = image.get_camera_pose(opt=True)
#print('cam orig:', image.camera_pose['ned'], 'optimized:', ned)
if args.ground:
yaw_diff = line[1]
i1 = proj.image_list[ line[2] ]
i2 = proj.image_list[ line[3] ]
# print(i1.match_list)
num_matches = len(i1.match_list[i2.name])
print("dist: %.1f" % dist, "yaw: %.1f" % yaw_diff, i1.name, i2.name, num_matches)
if num_matches > 0:
continue
# project a grid of uv coordinates from image 2 out onto the
# supposed ground plane. Then back project these 3d world points
# into image 1 uv coordinates. Compute an estimated 'ideal'
# homography relationship between the two images as a starting
# search point for feature matches.
proj_list = project.projectVectors( IK, i2.get_body2ned(),
i2.get_cam2body(), grid_list )
ned2, ypr2, quat2 = i2.get_camera_pose()
pts_ned = project.intersectVectorsWithGroundPlane(ned2, args.ground,
proj_list)
rvec1, tvec1 = i1.get_proj()
reproj_points, jac = cv2.projectPoints(np.array(pts_ned), rvec1, tvec1,
K, dist_coeffs)
reproj_list = reproj_points.reshape(-1,2).tolist()
# print("reprojected points:", reproj_list)
print("Should filter points outside of 2nd image space here and now!")
# affine, status = \
# cv2.estimateAffinePartial2D(np.array([reproj_list]).astype(np.float32),
# np.array([grid_list]).astype(np.float32))
# (rot, tx, ty, sx, sy) = decomposeAffine(affine)
scale = 0.4
print(srcname)
pt_list = by_image[srcname]
print(srcname, pt_list)
# project marked points back to ned space
base, ext = os.path.splitext(srcname)
image = proj.findImageByName(base)
if not image:
continue
print(srcname, image)
distorted_uv = proj.redistort(pt_list, optimized=True)
print("distorted:", distorted_uv)
proj_list = project.projectVectors(IK, image.get_body2ned(opt=True),
image.get_cam2body(), distorted_uv)
print("proj_list:", proj_list)
ned, ypr, quat = image.get_camera_pose(opt=True)
# intersect with our polygon surface approximation
pts_ned = intersect_vectors(ned, proj_list, -image.z_avg)
print("pts_ned:", pts_ned)
ned_list += pts_ned
if True:
fullpath = os.path.join(project_path, srcname)
rgb = cv2.imread(fullpath,
flags=cv2.IMREAD_ANYCOLOR | cv2.IMREAD_ANYDEPTH
| cv2.IMREAD_IGNORE_ORIENTATION)
for pt in pt_list:
srtm.initialize(ref, 2000, 2000, 30)
camw, camh = proj.cam.get_image_params()
dist_coeffs = proj.cam.get_dist_coeffs()
for image in proj.image_list:
print image.name
scale = float(image.width) / float(camw)
K = proj.cam.get_K(scale)
IK = np.linalg.inv(K)
corner_list = []
corner_list.append([0, 0])
corner_list.append([image.width, 0])
corner_list.append([0, image.height])
corner_list.append([image.width, image.height])
proj_list = project.projectVectors(IK, image, corner_list, pose=args.pose)
#print "proj_list:\n", proj_list
if args.pose == 'direct':
pts_ned = srtm.interpolate_vectors(image.camera_pose, proj_list)
elif args.pose == 'sba':
pts_ned = srtm.interpolate_vectors(image.camera_pose_sba, proj_list)
# print "pts (ned):\n", pts_ned
image.corner_list_ned = []
image.corner_list_lla = []
image.corner_list_xy = []
for ned in pts_ned:
#print p
image.corner_list_ned.append([ned[0], ned[1]])
image.corner_list_lla.append(
navpy.ned2lla([ned], ref[0], ref[1], ref[2]))
# del matches[i]
elif args.method == 'triangulate':
for i, match in enumerate(matches):
if match[1] == args.group: # used in current group
# print(match)
points = []
vectors = []
for m in match[2:]:
if proj.image_list[m[0]].name in group_list[args.group]:
# print(m)
image = proj.image_list[m[0]]
cam2body = image.get_cam2body()
body2ned = image.get_body2ned()
ned, ypr, quat = image.get_camera_pose(opt=True)
uv_list = [undistort(m[1])] # just one uv element
vec_list = project.projectVectors(IK, body2ned, cam2body,
uv_list)
points.append(ned)
vectors.append(vec_list[0])
# print(' ', image.name)
# print(' ', uv_list)
# print(' ', vec_list)
if len(points) >= 2:
# print('points:', points)
# print('vectors:', vectors)
p = line_solver.ls_lines_intersection(points,
vectors,
transpose=True).tolist()
# print('result:', p, p[0])
print(i, match[0], '>>>', end=" ")
match[0] = [p[0][0], p[1][0], p[2][0]]
if p[2][0] > 0:
