def update_image2(self): #updates images with boid locations and responsibility vectors
self.reset_image()
if (self.debug_mode):
debug_colors = [[255, 50, 50], [50, 255, 50], [50, 50, 255]]
for boid in self.world.boids:
poly = self.create_boid_poly([boid.location.y, boid.location.x], boid.theta)#create boid polygon in world coord
#print poly
poly = np.float32([ poly ]).reshape(-1,1,2)
for i in range(self.num_proj):
boid_pix = cv2.perspectiveTransform(poly, self.homog[i])
boid_pix = np.int32(boid_pix).reshape(1,-1,2)
cv2.fillConvexPoly(self.image[i], np.fliplr(boid_pix[0]), debug_colors[i])
else:
for boid in self.world.boids:
poly = self.create_boid_poly([boid.location.y, boid.location.x], boid.theta)#create boid polygon in world coord
#figure out which projector owns it
proj_own = -1
for i in range(self.num_proj):
if self.resp[i].contains(ShpPoint(boid.location.y, boid.location.x)):
proj_own = i
break
#transform to proj coordinates
if (proj_own >= 0):
poly = np.float32([ poly ]).reshape(-1,1,2)
boid_pix = cv2.perspectiveTransform(poly, self.homog[proj_own])
boid_pix = np.int32(boid_pix).reshape(1,-1,2)
cv2.fillConvexPoly(self.image[proj_own], np.fliplr(boid_pix[0]), boid.color)
self.draw()
def lk_assest(self):
# If we have to few assest points, we recompute them (FIXME:
# magic constants)
if self.lk_assest_points is None or len(self.lk_assest_points) < 10:
self.lk_assest_points = self.get_lk_points(self.ref_image, self.ref_table_points, self.lk_assest_points)
# Warp reference frame to put in the same position as the
# previously known corner points (FIXME: magic constants)
detection_settings = self.settings.detection_settings
H = cv2.getPerspectiveTransform(detection_settings.ref_table_points.reshape(-1, 1, 2), self.prev.table_points)
warped_ref_image = cv2.warpPerspective(self.ref_image, H, (320, 240))
self.controls.show("warped", warped_ref_image / 256.0)
p0 = cv2.perspectiveTransform(self.lk_assest_points, H)
begin_frame = warped_ref_image
end_frame = self.frame
begin_points, end_points = self.lk_flow(p0=p0, begin_frame=begin_frame, end_frame=end_frame)
self.lk_assest_points = cv2.perspectiveTransform(numpy.array(begin_points), numpy.linalg.inv(H))
# self.draw_frame_with_points(begin_frame, p0, "assest points before", begin_points)
# self.draw_frame_with_points(end_frame, end_points, "assest points after", end_points)
begin_ref = self.lk_assest_points
end_ref = numpy.array(end_points)
return self.project_along(begin_ref, end_ref, self.ref_table_points.reshape(-1, 1, 2))
def gl_draw_frame(self,img_size):
"""
draw surface and markers
"""
if self.detected:
frame = np.array([[[0,0],[1,0],[1,1],[0,1],[0,0]]],dtype=np.float32)
hat = np.array([[[.3,.7],[.7,.7],[.5,.9],[.3,.7]]],dtype=np.float32)
hat = cv2.perspectiveTransform(hat,self.m_to_screen)
frame = cv2.perspectiveTransform(frame,self.m_to_screen)
alpha = min(1,self.build_up_status/self.required_build_up)
draw_polyline_norm(frame.reshape((5,2)),1,RGBA(1.0,0.2,0.6,alpha))
draw_polyline_norm(hat.reshape((4,2)),1,RGBA(1.0,0.2,0.6,alpha))
#grid = frame.reshape((5,2))
# self.crop_region = np.array([
# [grid[0][0] * img_size[1], grid[0][1] * img_size[0]],
# [grid[1][0] * img_size[1], grid[1][1] * img_size[0]],
# [grid[2][0] * img_size[1], grid[2][1] * img_size[0]],
# [grid[3][0] * img_size[1], grid[3][1] * img_size[0]]],
# dtype=np.float32)
draw_points_norm(frame.reshape((5,-1))[0:1])
text_anchor = frame.reshape((5,-1))[2]
text_anchor[1] = 1-text_anchor[1]
text_anchor *=img_size[1],img_size[0]
self.glfont.draw_text(text_anchor[0],text_anchor[1],self.marker_status())
def output_perspective_transform(img_object, M):
h,w = img_object.shape
corner_pts = np.float32([ [0,0],[0,h-1],[w-1,h-1],[w-1,0] ]).reshape(-1,1,2)
center_pts = np.float32([ [w/2,h/2] ]).reshape(-1,1,2)
corner_pts_3d = np.float32([ [-w/2,-h/2,0],[-w/2,(h-1)/2,0],[(w-1)/2,(h-1)/2,0],[(w-1)/2,-h/2,0] ])###
corner_camera_coord = cv2.perspectiveTransform(corner_pts,M)###
center_camera_coord = cv2.perspectiveTransform(center_pts,M)
return corner_camera_coord, center_camera_coord, corner_pts_3d, center_pts
def _update_definition(self, idx, visible_markers, camera_model):
if not visible_markers:
return
all_verts_dist = np.array(
[m.verts_px for m in visible_markers.values()], dtype=np.float32
)
all_verts_dist.shape = (-1, 2)
all_verts_undist = camera_model.undistortPoints(all_verts_dist)
hull_undist = self._bounding_quadrangle(all_verts_undist)
undist_img_to_surf_trans_candidate = self._get_trans_to_norm_corners(
hull_undist
)
all_verts_undist.shape = (-1, 1, 2)
marker_surf_coords_undist = cv2.perspectiveTransform(
all_verts_undist, undist_img_to_surf_trans_candidate
)
hull_dist = self._bounding_quadrangle(all_verts_dist)
dist_img_to_surf_trans_candidate = self._get_trans_to_norm_corners(hull_dist)
all_verts_dist.shape = (-1, 1, 2)
marker_surf_coords_dist = cv2.perspectiveTransform(
all_verts_dist, dist_img_to_surf_trans_candidate
)
# Reshape to [marker, marker...]
# where marker = [[u1, v1], [u2, v2], [u3, v3], [u4, v4]]
marker_surf_coords_undist.shape = (-1, 4, 2)
marker_surf_coords_dist.shape = (-1, 4, 2)
# Add observations to library
for marker, uv_undist, uv_dist in zip(
visible_markers.values(), marker_surf_coords_undist, marker_surf_coords_dist
):
try:
self.registered_markers_undist[marker.id].add_observation(uv_undist)
self.registered_markers_dist[marker.id].add_observation(uv_dist)
except KeyError:
self.registered_markers_undist[marker.id] = Surface_Marker_Aggregate(
marker.id
)
self.registered_markers_undist[marker.id].add_observation(uv_undist)
self.registered_markers_dist[marker.id] = Surface_Marker_Aggregate(
marker.id
)
self.registered_markers_dist[marker.id].add_observation(uv_dist)
num_observations = sum(
[len(m.observations) for m in self.registered_markers_undist.values()]
)
self._avg_obs_per_marker = num_observations / len(
self.registered_markers_undist
)
self.build_up_status = self._avg_obs_per_marker / self._REQUIRED_OBS_PER_MARKER
if self.build_up_status >= 1:
self.prune_markers()
def distance_finder(frame, lines = None, fallas = None):
if lines != None:
# Variables importantes
distaciaFocal = 1286 #1286.07
largoEntreLineas = 25 #cm
altura_camara = 11.2 #cm
# Variables menos importantes
height = frame.shape[0]
width = frame.shape[1]
#Desempacamos
[lineaIzq,lineaDer] = lines
### Ahora viene el calculo de lineas paralelas ###
# Primero calculamos los cuatro puntos limites de las lineas electricas.
rectOriginal = np.float32(perspective_bound(frame, [lineaIzq,lineaDer]))
# Ahora definimos los limites de la nueva imagen
rectPerspective = np.float32([[0,0], [height,0], [0,height], [height,height]])
# Calculamos la matriz de perspectiva
if lineaIzq == None:
frame_perspective = np.zeros((height,height,3))
else:
M = cv2.getPerspectiveTransform(rectOriginal,rectPerspective)
# Transformamos la imagen para ver como queda
frame_perspective = cv2.warpPerspective(frame,M, (height,height))
# #Iteramos sobre todos los puntos que encontramos para encontrar su paralela
for color in fallas:
for centro in fallas[color]:
#Transformamos el centroide del
centro_perspectiva = cv2.perspectiveTransform(np.array([[centro[0]]],dtype = 'float32'),M) #Los puntos que le entran a la funcion perspectiveTransform, tienen que ser de la forma [[[x1,y1],[x2,y2, ...]]], con ESA cantidad de parentesis.
# Calculamos el mismo punto en la otra linea
centro_perspectiva = centro_perspectiva[0][0].copy()
if centro_perspectiva[0] < height/2:
punto_contrario_perspectiva = np.array([height, centro_perspectiva[1]])
else:
punto_contrario_perspectiva = np.array([0, centro_perspectiva[1]], dtype = 'float')
# Destransformamos el punto
# print "M = {}".format(M)
# print "invM = {}".format(cv2.invert(M))
punto_contrario = cv2.perspectiveTransform(np.array([[punto_contrario_perspectiva]],dtype = 'float32'),cv2.invert(M)[1])
# Graficamos, las lineas que encontramos
punto_contrario = punto_contrario[0][0].copy()
cv2.line(frame,tuple(centro[0]),tuple(punto_contrario),coloresDibujo['azul'],2)
# Calculamos la distancia en pixeles
distPixeles = np.sqrt(np.square(centro[0][0] - punto_contrario[0]) + np.square(centro[0][1] - punto_contrario[1]))
# Usamos la ecuacion de similitud triangular para sacar la distancia
distCamara = largoEntreLineas*distaciaFocal/distPixeles
# Ahora usamos la relacion de pitagoras para calcular la distancia de la falla al chasis
distChasis = np.sqrt(np.square(distCamara) - np.square(altura_camara))
# guardamos la distancia en centimetros en el diccionario de fallas
centro[1] = distChasis
return fallas
def get_distance(self, datapoint):
if self._homo is not None and datapoint.get_homo() is not None:
des = cv2.perspectiveTransform(self._cor, self._homo)
desn = cv2.perspectiveTransform(self._cor, datapoint.get_homo())
dist = 0.
for p1,p2 in zip(des[0], desn[0]):
dist += abs(p1[0] - p2[0]) + abs(p1[1] - p2[1])
return dist
else:
return None
def gl_draw_frame(self,img_size,color = (1.0,0.2,0.6,1.0),highlight=False,surface_mode=False,marker_mode=False):
"""
draw surface and markers
"""
if self.detected:
r,g,b,a = color
frame = np.array([[[0,0],[1,0],[1,1],[0,1],[0,0]]],dtype=np.float32)
hat = np.array([[[.3,.7],[.7,.7],[.5,.9],[.3,.7]]],dtype=np.float32)
hat = cv2.perspectiveTransform(hat,self.m_to_screen)
frame = cv2.perspectiveTransform(frame,self.m_to_screen)
alpha = min(1,self.build_up_status/self.required_build_up)
if highlight:
draw_polyline_norm(frame.reshape((5,2)),1,RGBA(r,g,b,a*.1),line_type=GL_POLYGON)
draw_polyline_norm(frame.reshape((5,2)),1,RGBA(r,g,b,a*alpha))
draw_polyline_norm(hat.reshape((4,2)),1,RGBA(r,g,b,a*alpha))
text_anchor = frame.reshape((5,-1))[2]
text_anchor[1] = 1-text_anchor[1]
text_anchor *=img_size[1],img_size[0]
text_anchor = text_anchor[0],text_anchor[1]-75
surface_edit_anchor = text_anchor[0],text_anchor[1]+25
marker_edit_anchor = text_anchor[0],text_anchor[1]+50
if self.defined:
if marker_mode:
draw_points([marker_edit_anchor],color=RGBA(0,.8,.7))
else:
draw_points([marker_edit_anchor])
if surface_mode:
draw_points([surface_edit_anchor],color=RGBA(0,.8,.7))
else:
draw_points([surface_edit_anchor])
self.glfont.set_blur(3.9)
self.glfont.set_color_float((0,0,0,.8))
self.glfont.draw_text(text_anchor[0]+15,text_anchor[1]+6,self.marker_status())
self.glfont.draw_text(surface_edit_anchor[0]+15,surface_edit_anchor[1]+6,'edit surface')
self.glfont.draw_text(marker_edit_anchor[0]+15,marker_edit_anchor[1]+6,'add/remove markers')
self.glfont.set_blur(0.0)
self.glfont.set_color_float((0.1,8.,8.,.9))
self.glfont.draw_text(text_anchor[0]+15,text_anchor[1]+6,self.marker_status())
self.glfont.draw_text(surface_edit_anchor[0]+15,surface_edit_anchor[1]+6,'edit surface')
self.glfont.draw_text(marker_edit_anchor[0]+15,marker_edit_anchor[1]+6,'add/remove markers')
else:
progress = (self.build_up_status/float(self.required_build_up))*100
progress_text = '%.0f%%'%progress
self.glfont.set_blur(3.9)
self.glfont.set_color_float((0,0,0,.8))
self.glfont.draw_text(text_anchor[0]+15,text_anchor[1]+6,self.marker_status())
self.glfont.draw_text(surface_edit_anchor[0]+15,surface_edit_anchor[1]+6,'Learning affiliated markers...')
self.glfont.draw_text(marker_edit_anchor[0]+15,marker_edit_anchor[1]+6,progress_text)
self.glfont.set_blur(0.0)
self.glfont.set_color_float((0.1,8.,8.,.9))
self.glfont.draw_text(text_anchor[0]+15,text_anchor[1]+6,self.marker_status())
self.glfont.draw_text(surface_edit_anchor[0]+15,surface_edit_anchor[1]+6,'Learning affiliated markers...')
self.glfont.draw_text(marker_edit_anchor[0]+15,marker_edit_anchor[1]+6,progress_text)
def gl_draw_frame(self):
"""
draw surface and markers
"""
if self.detected:
frame = np.array([[[0,0],[1,0],[1,1],[0,1],[0,0]]],dtype=np.float32)
hat = np.array([[[.3,.7],[.7,.7],[.5,.9],[.3,.7]]],dtype=np.float32)
hat = cv2.perspectiveTransform(hat,self.m_to_screen)
frame = cv2.perspectiveTransform(frame,self.m_to_screen)
alpha = min(1,self.build_up_status/self.required_build_up)
draw_gl_polyline_norm(frame.reshape((5,2)),(1.0,0.2,0.6,alpha))
draw_gl_polyline_norm(hat.reshape((4,2)),(1.0,0.2,0.6,alpha))
def get_middle(self, other):
nhomo = None
if self._homo is not None and other.get_homo() is not None:
des = cv2.perspectiveTransform(self._cor, self._homo)
desn = cv2.perspectiveTransform(self._cor, other.get_homo())
new_cor = []
for p1,p2 in zip(des[0], desn[0]):
new_cor.append([(p1[0] + p2[0])/2, (p1[1] + p2[1])/2])
new_cor = np.array(new_cor,dtype='float32')
new_cor = np.array([new_cor])
old_cor = self._cor
nhomo, mask = cv2.findHomography(old_cor, new_cor, cv2.RANSAC)
return DataPoint(self._method + "mid" + other._method, self._shape, self._quality * other._quality, nhomo)
def gl_display(self):
"""
Display marker and surface info inside world screen
"""
if self.mode == "Show markers and frames":
for m in self.markers:
hat = np.array([[[0,0],[0,1],[.5,1.3],[1,1],[1,0],[0,0]]],dtype=np.float32)
hat = cv2.perspectiveTransform(hat,m_marker_to_screen(m))
draw_gl_polyline(hat.reshape((6,2)),(0.1,1.,1.,.5))
for s in self.surfaces:
s.gl_draw_frame(self.img_shape)
for s in self.surfaces:
if self.locate_3d:
s.gl_display_in_window_3d(self.g_pool.image_tex,self.camera_intrinsics)
else:
s.gl_display_in_window(self.g_pool.image_tex)
if self.mode == "Surface edit mode":
for s in self.surfaces:
s.gl_draw_frame(self.img_shape)
s.gl_draw_corners()
def process(self):
img1 = cv2.cvtColor(self.imgcv1, cv2.COLOR_BGR2GRAY) #queryimage # left image
img2 = cv2.cvtColor(self.imgcv2, cv2.COLOR_BGR2GRAY) #trainimage # right image
# find the keypoints and descriptors with SIFT
kp1, des1 = self.detector.detectAndCompute(img1,None)
kp2, des2 = self.detector.detectAndCompute(img2,None)
matches = self.flann.knnMatch(des1,des2,k=2)
pts1 = []
pts2 = []
# ratio test as per Lowe's paper
for i,(m,n) in enumerate(matches):
if m.distance < 0.8*n.distance:
pts2.append(kp2[m.trainIdx].pt)
pts1.append(kp1[m.queryIdx].pt)
pts1 = np.float32(pts1)
pts2 = np.float32(pts2)
M, mask = cv2.findHomography(pts1, pts2, cv2.RANSAC,5.0)
h,w = img1.shape
h/=2
w/=2
pts = np.float32([ [0,0],[0,h-1],[w-1,h-1],[w-1,0] ]).reshape(-1,1,2)
dst = cv2.perspectiveTransform(pts,M)
cv2.polylines(img2,[np.int32(dst)],True,255,3)
#We select only inlier points
pts1 = pts1[mask.ravel()==1]
pts2 = pts2[mask.ravel()==1]
self.data=(M,pts1,pts2)
pts1i=np.int32(pts1)
pts2i=np.int32(pts2)
return drawpoints(img1,img2,pts1i,pts2i)
def reconstruct_thumbnail(thumbnail_image, source_image, kp_pairs, H, downsize_reconstruction=False):
logging.info("Reconstructing thumbnail from source image")
thumb_h, thumb_w = thumbnail_image.shape[:2]
source_h, source_w = source_image.shape[:2]
corners = numpy.float32([[0, 0], [thumb_w, 0], [thumb_w, thumb_h], [0, thumb_h]])
corners = numpy.int32(cv2.perspectiveTransform(corners.reshape(1, -1, 2), H).reshape(-1, 2))
logging.info("Thumbnail bounds within source image: %s", corners.tolist())
corners_x, corners_y = zip(*corners.tolist())
new_thumb_crop = ((min(corners_y), max(corners_y)), (min(corners_x), max(corners_x)))
new_thumb = source_image[slice(*new_thumb_crop[0]), slice(*new_thumb_crop[1])]
new_thumb_rotation, new_thumb = autorotate_image(new_thumb, corners)
new_thumb_h, new_thumb_w = new_thumb.shape[:2]
if downsize_reconstruction and (new_thumb_h > thumb_h or new_thumb_w > thumb_w):
new_thumb = fit_image_within(new_thumb, thumb_h, thumb_w)
logging.info("Master dimensions: %s", source_image.shape)
logging.info("Thumbnail dimensions: %s", thumbnail_image.shape)
logging.info("Reconstructed thumb dimensions: %s (rotation=%d°)", new_thumb.shape, new_thumb_rotation)
return new_thumb, new_thumb_crop, new_thumb_rotation
def gl_draw_corners(self):
"""
draw surface and markers
"""
if self.detected:
frame = cv2.perspectiveTransform(marker_corners_norm.reshape(-1,1,2),self.m_to_screen)
draw_points_norm(frame.reshape((4,2)),20,RGBA(1.0,0.2,0.6,.5))
def track(self, frame):
'''Returns a list of detected TrackedTarget objects'''
self.frame_points, frame_descrs = self.detect_features(frame)
if len(self.frame_points) < MIN_MATCH_COUNT:
return []
matches = self.matcher.knnMatch(frame_descrs, k = 2)
matches = [m[0] for m in matches if len(m) == 2 and m[0].distance < m[1].distance * 0.75]
if len(matches) < MIN_MATCH_COUNT:
return []
matches_by_id = [[] for _ in xrange(len(self.targets))]
for m in matches:
matches_by_id[m.imgIdx].append(m)
tracked = []
for imgIdx, matches in enumerate(matches_by_id):
if len(matches) < MIN_MATCH_COUNT:
continue
target = self.targets[imgIdx]
p0 = [target.keypoints[m.trainIdx].pt for m in matches]
p1 = [self.frame_points[m.queryIdx].pt for m in matches]
p0, p1 = np.float32((p0, p1))
H, status = cv2.findHomography(p0, p1, cv2.RANSAC, 3.0)
status = status.ravel() != 0
if status.sum() < MIN_MATCH_COUNT:
continue
p0, p1 = p0[status], p1[status]
x0, y0, x1, y1 = target.rect
quad = np.float32([[x0, y0], [x1, y0], [x1, y1], [x0, y1]])
quad = cv2.perspectiveTransform(quad.reshape(1, -1, 2), H).reshape(-1, 2)
track = TrackedTarget(target=target, p0=p0, p1=p1, H=H, quad=quad)
tracked.append(track)
tracked.sort(key = lambda t: len(t.p0), reverse=True)
return tracked
def matching(kp1,des1,img1,kp2_ext,des2_ext):
global MIN_MATCH_COUNT
kp2 = kp2_ext
des2 = des2_ext
if len(kp1) <= MIN_MATCH_COUNT*2 or len(kp2) <= MIN_MATCH_COUNT*2:
return [], None
#FLANN_INDEX_KDTREE = 0
#index_params = dict(algorithm = FLANN_INDEX_KDTREE, trees = 5)
#search_params = dict(checks = 50)
#flann = cv2.FlannBasedMatcher(index_params, search_params)
#matches = flann.knnMatch(des1,des2,k=2)
global FEATURE_DETECTOR
global CROSS_CHECK
if FEATURE_DETECTOR == "AKAZE":
if CROSS_CHECK:
bf = cv2.BFMatcher(cv2.NORM_HAMMING,crossCheck=True)
good = bf.match(des1,des2)
else:
bf = cv2.BFMatcher(cv2.NORM_HAMMING)
matches = bf.knnMatch(des1, des2, k=2)
good = []
for m,n in matches:
if m.distance < 0.8*n.distance:
good.append(m)
else:
if CROSS_CHECK:
bf = cv2.BFMatcher(crossCheck=True)
good = bf.match(des1,des2)
else:
bf = cv2.BFMatcher()
matches = bf.knnMatch(des1, des2, k=2)
good = []
for m,n in matches:
if m.distance < 0.8*n.distance:
good.append(m)
if len(good)>MIN_MATCH_COUNT:
src_pts = np.float32([ kp1[m.queryIdx].pt for m in good ]).reshape(-1,1,2)
dst_pts = np.float32([ kp2[m.trainIdx].pt for m in good ]).reshape(-1,1,2)
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
if mask is None:
return [], None
h,w = img1.shape
pts = np.float32([ [0,0],[0,h-1],[w-1,h-1],[w-1,0] ]).reshape(-1,1,2)
dst = cv2.perspectiveTransform(pts,M)
return good, np.int32(dst)
return [], None
def draw_match(img1, img2, p1, p2, status = None, H = None):
h1, w1 = img1.shape[:2]
h2, w2 = img2.shape[:2]
vis = np.zeros((max(h1, h2), w1+w2), np.uint8)
vis[:h1, :w1] = img1
vis[:h2, w1:w1+w2] = img2
vis = cv2.cvtColor(vis, cv2.COLOR_GRAY2BGR)
if H is not None:
corners = np.float32([[0, 0], [w1, 0], [w1, h1], [0, h1]])
corners = np.int32( cv2.perspectiveTransform(corners.reshape(1, -1, 2), H).reshape(-1, 2) + (w1, 0) )
cv2.polylines(vis, [corners], True, (255, 255, 255))
if status is None:
status = np.ones(len(p1), np.bool_)
green = (0, 255, 0)
red = (0, 0, 255)
for (x1, y1), (x2, y2), inlier in zip(np.int32(p1), np.int32(p2), status):
col = [red, green][inlier]
if inlier:
cv2.line(vis, (x1, y1), (x2+w1, y2), col)
cv2.circle(vis, (x1, y1), 2, col, -1)
cv2.circle(vis, (x2+w1, y2), 2, col, -1)
else:
r = 2
thickness = 3
cv2.line(vis, (x1-r, y1-r), (x1+r, y1+r), col, thickness)
cv2.line(vis, (x1-r, y1+r), (x1+r, y1-r), col, thickness)
cv2.line(vis, (x2+w1-r, y2-r), (x2+w1+r, y2+r), col, thickness)
cv2.line(vis, (x2+w1-r, y2+r), (x2+w1+r, y2-r), col, thickness)
return vis
def build_correspondance(self, visible_markers,camera_calibration,min_marker_perimeter):
"""
- use all visible markers
- fit a convex quadrangle around it
- use quadrangle verts to establish perpective transform
- map all markers into surface space
- build up list of found markers and their uv coords
"""
all_verts = [m['verts'] for m in visible_markers if m['perimeter']>=min_marker_perimeter]
if not all_verts:
return
all_verts = np.array(all_verts)
all_verts.shape = (-1,1,2) # [vert,vert,vert,vert,vert...] with vert = [[r,c]]
# all_verts_undistorted_normalized centered in img center flipped in y and range [-1,1]
all_verts_undistorted_normalized = cv2.undistortPoints(all_verts, camera_calibration['camera_matrix'],camera_calibration['dist_coefs'])
hull = cv2.convexHull(all_verts_undistorted_normalized,clockwise=False)
#simplify until we have excatly 4 verts
if hull.shape[0]>4:
new_hull = cv2.approxPolyDP(hull,epsilon=1,closed=True)
if new_hull.shape[0]>=4:
hull = new_hull
if hull.shape[0]>4:
curvature = abs(GetAnglesPolyline(hull,closed=True))
most_acute_4_threshold = sorted(curvature)[3]
hull = hull[curvature<=most_acute_4_threshold]
# all_verts_undistorted_normalized space is flipped in y.
# we need to change the order of the hull vertecies
hull = hull[[1,0,3,2],:,:]
# now we need to roll the hull verts until we have the right orientation:
# all_verts_undistorted_normalized space has its origin at the image center.
# adding 1 to the coordinates puts the origin at the top left.
distance_to_top_left = np.sqrt((hull[:,:,0]+1)**2+(hull[:,:,1]+1)**2)
bot_left_idx = np.argmin(distance_to_top_left)+1
hull = np.roll(hull,-bot_left_idx,axis=0)
#based on these 4 verts we calculate the transformations into a 0,0 1,1 square space
m_from_undistored_norm_space = m_verts_from_screen(hull)
self.detected = True
# map the markers vertices in to the surface space (one can think of these as texture coordinates u,v)
marker_uv_coords = cv2.perspectiveTransform(all_verts_undistorted_normalized,m_from_undistored_norm_space)
marker_uv_coords.shape = (-1,4,1,2) #[marker,marker...] marker = [ [[r,c]],[[r,c]] ]
# build up a dict of discovered markers. Each with a history of uv coordinates
for m,uv in zip (visible_markers,marker_uv_coords):
try:
self.markers[m['id']].add_uv_coords(uv)
except KeyError:
self.markers[m['id']] = Support_Marker(m['id'])
self.markers[m['id']].add_uv_coords(uv)
#average collection of uv correspondences accros detected markers
self.build_up_status = sum([len(m.collected_uv_coords) for m in self.markers.values()])/float(len(self.markers))
if self.build_up_status >= self.required_build_up:
self.finalize_correnspondance()
def _calculate_hat_points(self, marker_points):
perspective_matrix = cv2.getPerspectiveTransform(
self._square_definition, marker_points
)
hat_points = cv2.perspectiveTransform(self._hat_definition, perspective_matrix)
hat_points.shape = 6, 2
return hat_points
def windowToFieldCoordinates(basePoint, x1, y1, x2, y2, x3, y3, x4, y4, maxWidth=0, maxHeight=0):
(xp, yp) = basePoint
src = np.array([
[x1, y1],
[x2, y2],
[x3, y3],
[x4, y4]], dtype = "float32")
# those should be the same aspect as the real width/height of field
maxWidth = (x4-x1) if maxWidth == 0 else maxWidth
maxHeight = (y1-y2) if maxHeight == 0 else maxHeight
# make a destination rectangle with the width and height of above (starts at 0,0)
dst = np.array([
[0, 0],
[maxWidth - 1, 0],
[maxWidth - 1, maxHeight - 1],
[0, maxHeight - 1]], dtype = "float32")
# find the transformation matrix for our transforms
transformationMatrix = cv2.getPerspectiveTransform(src, dst)
# put the original (source) x,y points in an array (not sure why do we have to put it 3 times though)
original = np.array([((xp, yp), (xp, yp), (xp, yp))], dtype=np.float32)
# use perspectiveTransform to transform our original(mouse coords) to new coords with the transformation matrix
transformed = cv2.perspectiveTransform(original, transformationMatrix)[0][0]
return transformed
def draw_markers(img,markers):
for m in markers:
centroid = np.array(m['centroid'],dtype=np.float32)
origin = np.array(m['verts'][0],dtype=np.float32)
hat = np.array([[[0,0],[0,1],[.5,1.25],[1,1],[1,0]]],dtype=np.float32)
hat = cv2.perspectiveTransform(hat,m_marker_to_screen(m))
if m['id_confidence']>.9:
cv2.polylines(img,np.int0(hat),color = (0,0,255),isClosed=True)
else:
cv2.polylines(img,np.int0(hat),color = (0,255,0),isClosed=True)
# cv2.polylines(img,np.int0(centroid),color = (255,255,int(255*m['id_confidence'])),isClosed=True,thickness=2)
m_str = 'id: {:d}'.format(m['id'])
org = origin.copy()
# cv2.rectangle(img, tuple(np.int0(org+(-5,-13))[0,:]), tuple(np.int0(org+(100,30))[0,:]),color=(0,0,0),thickness=-1)
cv2.putText(img,m_str,tuple(np.int0(org)[0,:]),fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.4, color=(0,0,255))
if 'id_confidence' in m:
m_str = 'idc: {:.3f}'.format(m['id_confidence'])
org += (0, 12)
cv2.putText(img,m_str,tuple(np.int0(org)[0,:]),fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.4, color=(0,0,255))
if 'loc_confidence' in m:
m_str = 'locc: {:.3f}'.format(m['loc_confidence'])
org += (0, 12 )
cv2.putText(img,m_str,tuple(np.int0(org)[0,:]),fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.4, color=(0,0,255))
if 'frames_since_true_detection' in m:
m_str = 'otf: {}'.format(m['frames_since_true_detection'])
org += (0, 12 )
cv2.putText(img,m_str,tuple(np.int0(org)[0,:]),fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.4, color=(0,0,255))
if 'opf_vel' in m:
m_str = 'otf: {}'.format(m['opf_vel'])
org += (0, 12 )
cv2.putText(img,m_str,tuple(np.int0(org)[0,:]),fontFace=cv2.FONT_HERSHEY_SIMPLEX, fontScale=0.4, color=(0,0,255))
def gl_display(self):
"""
Display marker and surface info inside world screen
"""
for m in self.markers:
hat = np.array([[[0,0],[0,1],[.5,1.3],[1,1],[1,0],[0,0]]],dtype=np.float32)
hat = cv2.perspectiveTransform(hat,m_marker_to_screen(m))
draw_gl_polyline(hat.reshape((6,2)),(0.1,1.,1.,.5))
for s in self.surfaces:
s.gl_draw_frame()
if self.surface_edit_mode.value:
for s in self.surfaces:
s.gl_draw_corners()
if self._window and self.surfaces:
try:
s = self.surfaces[self.show_surface_idx.value]
except IndexError:
s = None
if s and s.detected:
self.gl_display_in_window(s)
def warpImagePair(image_1, image_2, homography):
warped_image = None
x_min = 0
y_min = 0
x_max = 0
y_max = 0
image_1_corners = get_image_corners(image_1)
image_2_corners = get_image_corners(image_2)
image_1_corners = cv2.perspectiveTransform(image_1_corners, homography)
corners = np.concatenate((image_1_corners, image_2_corners))
x_min = min(corners[:, 0, 0])
x_max = max(corners[:, 0, 0])
y_min = min(corners[:, 0, 1])
y_max = max(corners[:, 0, 1])
translation_matrix = np.array([[1, 0, -1 * x_min], [0, 1, -1 * y_min], [0, 0, 1]])
translated_homography = np.dot(translation_matrix, homography)
warped_image_1 = cv2.warpPerspective(image_1, translated_homography, (x_max - x_min, y_max - y_min))
# Select a sub-region of the warped image that matches image_2's (primary image's) coordinate frame
warped_image_1 = warped_image_1[-y_min:-y_min+image_2.shape[0], -x_min:-x_min+image_2.shape[1]]
return warped_image_1
def calculate3DPosition(Point2D1, Point2D2):
"""
Triangulate a 3D position from 2 2D position from camera
and each camera projection matrix
:param camera1:
:param camera2:
:param Point2D1:
:param Point2D2:
:return:
"""
xy1 = [Point2D1.get()['x'], Point2D1.get()['y']]
xy2 = [Point2D2.get()['x'], Point2D2.get()['y']]
# print str(xy1) + " - " + str(Point2D1)
# print str(xy2) + " - " + str(Point2D2)
triangulationOutput = cv2.triangulatePoints(Point2D1.camera.projection_matrix,Point2D2.camera.projection_matrix, np.float32(xy1), np.float32(xy2))
mypoint1 = np.array([triangulationOutput[0], triangulationOutput[1], triangulationOutput[2]])
mypoint1 = mypoint1.reshape(-1, 3)
mypoint1 = np.array([mypoint1])
P_24x4 = np.resize(Point2D1.camera.projection_matrix[0], (4,4))
P_24x4[3,0] = 0
P_24x4[3,1] = 0
P_24x4[3,2] = 0
P_24x4[3,3] = 1
projected = cv2.perspectiveTransform(mypoint1, P_24x4)
output = triangulationOutput[:-1]/triangulationOutput[-1]
# print output
#TODO calculate point again with second proj mat, and calculate middle
return output
def _detect_corner_points(self, key_frame, good_matches):
"""Detects corner points in an input (query) image
This method finds the homography matrix to go from the template
(train) image to the input (query) image, and finds the coordinates
of the good matches (from the train image) in the query image.
:param key_frame: keypoints of the query image
:param good_matches: list of good matches
:returns: coordinates of good matches in transformed query image
"""
# find homography using RANSAC
src_points = [self.key_train[good_matches[i].queryIdx].pt
for i in xrange(len(good_matches))]
dst_points = [key_frame[good_matches[i].trainIdx].pt
for i in xrange(len(good_matches))]
H, _ = cv2.findHomography(np.array(src_points), np.array(dst_points),
cv2.RANSAC)
# outline train image in query image
src_corners = np.array([(0, 0), (self.sh_train[1], 0),
(self.sh_train[1], self.sh_train[0]),
(0, self.sh_train[0])], dtype=np.float32)
dst_corners = cv2.perspectiveTransform(src_corners[None, :, :], H)
# convert to tuple
dst_corners = map(tuple, dst_corners[0])
return dst_corners
def draw_matches(self, img, kp_pairs, status, H):
'''Derived from find_obj.py'''
vis = img
h, w = self.feature_shape[:2]
if H is not None:
corners = np.float32([[0, 0], [w, 0], [w, h], [0, h]]) + self.feature_offset
corners = np.int32( cv2.perspectiveTransform(corners.reshape(1, -1, 2), H).reshape(-1, 2))
cv2.polylines(vis, [corners], True, (255, 255, 255))
if status is None:
status = np.ones(len(kp_pairs), np.bool_)
p2 = np.int32([kpp[1].pt for kpp in kp_pairs])
green = (0, 255, 0)
red = (0, 0, 255)
for (x2, y2), inlier in zip(p2, status):
if inlier:
col = green
cv2.circle(vis, (x2, y2), 2, col, -1)
else:
col = red
r = 2
thickness = 3
cv2.line(vis, (x2-r, y2-r), (x2+r, y2+r), col, thickness)
cv2.line(vis, (x2-r, y2+r), (x2+r, y2-r), col, thickness)
def find_homography(kp1, des1, kp2, des2):
"""
Given a set of keypoints and descriptors finds the homography
"""
# Tenta fazer a melhor comparacao usando o algoritmo
matches = flann.knnMatch(des1, des2, k=2)
# store all the good matches as per Lowe's ratio test.
good = []
for m,n in matches:
if m.distance < 0.7*n.distance:
good.append(m)
if len(good)>MIN_MATCH_COUNT:
# Separa os bons matches na origem e no destino
src_pts = np.float32([ kp1[m.queryIdx].pt for m in good ]).reshape(-1,1,2)
dst_pts = np.float32([ kp2[m.trainIdx].pt for m in good ]).reshape(-1,1,2)
# Tenta achar uma trasformacao composta de rotacao, translacao e escala que situe uma imagem na outra
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,5.0)
matchesMask = mask.ravel().tolist()
h,w = img1.shape
pts = np.float32([ [0,0],[0,h-1],[w-1,h-1],[w-1,0] ]).reshape(-1,1,2)
# Transforma os pontos da imagem origem para onde estao na imagem destino
dst = cv2.perspectiveTransform(pts,M)
return M
else:
# Caso em que nao houve matches o suficiente
return -1
def handleTree(self, group):
projectedCorners = group['projectedCorners']
srcGroupMap = group['projectedPoints']
groupColors = group['colors']
groupColors.clear()
image = cv2.imread("images/treeTexture.png", -1)
height = image.shape[0]
width = image.shape[1]
src = self.verticalReshape(projectedCorners)
dst = self.verticalReshape([(0, 0), (width, 0), (width, height), (0, height)])
transformationMatrix, mask = cv2.findHomography(src, dst, cv2.RANSAC, 5.0)
finalSrc = srcGroupMap.keys()
finalDst = np.int32(cv2.perspectiveTransform(self.verticalReshape(finalSrc),
transformationMatrix))
for i in range(len(finalDst)):
dstCoord = tuple(finalDst[i].ravel())
if self.isOutOfFrame(dstCoord, width, height):
continue
colorWithAlpha = image[dstCoord[1]][dstCoord[0]]
if(colorWithAlpha[3] < 1.0):
color = np.asarray([-1, -1, -1])
else:
color = np.asarray([colorWithAlpha[0],
colorWithAlpha[1],
colorWithAlpha[2]])
srcCoord = finalSrc[i]
for point in srcGroupMap[srcCoord]:
groupColors[tuple(point)] = color
return
def find_homography(kp1, des1, kp2, des2):
"""
Given a set of keypoints and descriptors finds the homography
"""
# Tenta fazer a melhor comparacao usando o algoritmo
matches = flann.knnMatch(des1, des2, k=2)
# store all the good matches as per Lowe's ratio test.
good = []
for m,n in matches:
if m.distance < 0.7*n.distance:
good.append(m)
if len(good)>MIN_MATCH_COUNT:
src_pts = np.float32([ kp1[m.queryIdx].pt for m in good ]).reshape(-1,1,2)
dst_pts = np.float32([ kp2[m.trainIdx].pt for m in good ]).reshape(-1,1,2)
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,5.0)
matchesMask = mask.ravel().tolist()
h,w = img1.shape
pts = np.float32([ [0,0],[0,h-1],[w-1,h-1],[w-1,0] ]).reshape(-1,1,2)
dst = cv2.perspectiveTransform(pts,M)
return M
else:
return -1
def importExternalTextureFromImage(self, group, imageURL): projectedCorners = group['projectedCorners'] srcGroupMap = group['projectedPoints'] groupColors = group['colors'] groupColors.clear() image = cv2.imread(imageURL, cv2.CV_LOAD_IMAGE_COLOR) height = image.shape[0] width = image.shape[1] src = self.verticalReshape(projectedCorners) dst = self.verticalReshape([(0, 0), (width, 0), (width, height), (0, height)]) transformationMatrix, mask = cv2.findHomography(src, dst, cv2.RANSAC, 5.0) finalSrc = srcGroupMap.keys() finalDst = np.int32(cv2.perspectiveTransform(self.verticalReshape(finalSrc), transformationMatrix)) for i in range(len(finalDst)): dstCoord = tuple(finalDst[i].ravel()) if self.isOutOfFrame(dstCoord, width, height): continue srcCoord = finalSrc[i] for point in srcGroupMap[srcCoord]: groupColors[tuple(point)] = image[dstCoord[1]][dstCoord[0]] return
def mask_face(image, face_location, six_points, angle, args, type="surgical"):
debug = False
# Find the face angle
threshold = 13
if angle < -threshold:
type += "_right"
elif angle > threshold:
type += "_left"
face_height = face_location[2] - face_location[0]
face_width = face_location[1] - face_location[3]
# image = image_raw[
# face_location[0]-int(face_width/2): face_location[2]+int(face_width/2),
# face_location[3]-int(face_height/2): face_location[1]+int(face_height/2),
# :,
# ]
# cv2.imshow('win', image)
# cv2.waitKey(0)
# Read appropriate mask image
w = image.shape[0]
h = image.shape[1]
if not "empty" in type and not "inpaint" in type:
cfg = read_cfg(config_filename="masks/masks.cfg", mask_type=type, verbose=False)
else:
if "left" in type:
str = "surgical_blue_left"
elif "right" in type:
str = "surgical_blue_right"
else:
str = "surgical_blue"
cfg = read_cfg(config_filename="masks/masks.cfg", mask_type=str, verbose=False)
img = cv2.imread(cfg.template, cv2.IMREAD_UNCHANGED)
# Process the mask if necessary
if args.pattern:
# Apply pattern to mask
img = texture_the_mask(img, args.pattern, args.pattern_weight)
if args.color:
# Apply color to mask
img = color_the_mask(img, args.color, args.color_weight)
mask_line = np.float32(
[cfg.mask_a, cfg.mask_b, cfg.mask_c, cfg.mask_f, cfg.mask_e, cfg.mask_d]
)
# Warp the mask
M, mask = cv2.findHomography(mask_line, six_points)
dst_mask = cv2.warpPerspective(img, M, (h, w))
dst_mask_points = cv2.perspectiveTransform(mask_line.reshape(-1, 1, 2), M)
mask = dst_mask[:, :, 3]
face_height = face_location[2] - face_location[0]
face_width = face_location[1] - face_location[3]
image_face = image[
face_location[0] + int(face_height / 2) : face_location[2],
face_location[3] : face_location[1],
:,
]
image_face = image
# Adjust Brightness
mask_brightness = get_avg_brightness(img)
img_brightness = get_avg_brightness(image_face)
delta_b = 1 + (img_brightness - mask_brightness) / 255
dst_mask = change_brightness(dst_mask, delta_b)
# Adjust Saturation
mask_saturation = get_avg_saturation(img)
img_saturation = get_avg_saturation(image_face)
delta_s = 1 - (img_saturation - mask_saturation) / 255
dst_mask = change_saturation(dst_mask, delta_s)
# Apply mask
mask_inv = cv2.bitwise_not(mask)
img_bg = cv2.bitwise_and(image, image, mask=mask_inv)
img_fg = cv2.bitwise_and(dst_mask, dst_mask, mask=mask)
out_img = cv2.add(img_bg, img_fg[:, :, 0:3])
if "empty" in type or "inpaint" in type:
out_img = img_bg
# Plot key points
if "inpaint" in type:
out_img = cv2.inpaint(out_img, mask, 3, cv2.INPAINT_TELEA)
# dst_NS = cv2.inpaint(img, mask, 3, cv2.INPAINT_NS)
if debug:
for i in six_points:
cv2.circle(out_img, (i[0], i[1]), radius=4, color=(0, 0, 255), thickness=-1)
for i in dst_mask_points:
cv2.circle(
out_img, (i[0][0], i[0][1]), radius=4, color=(0, 255, 0), thickness=-1
)
return out_img, mask
def processFile(filename):
img = cv2.imread(filename)
# img = scaleImageIfNeeded(img, 600, 480)
img = scaleImageIfNeeded(img, 1024, 768)
img_orig = img.copy()
img_orig2 = img.copy()
# Edges
edges = cv2.Canny(img, 100, 550)
# Get mask for where we think chessboard is
mask, top_two_angles, min_area_rect, median_contour = getEstimatedChessboardMask(img, edges,iters=3) # More iters gives a finer mask
print("Top two angles (in image coord system): %s" % top_two_angles)
# Get hough lines of masked edges
edges_masked = cv2.bitwise_and(edges,edges,mask = (mask > 0.5).astype(np.uint8))
img_orig = cv2.bitwise_and(img_orig,img_orig,mask = (mask > 0.5).astype(np.uint8))
lines = getHoughLines(edges_masked, min_line_size=0.25*min(min_area_rect[1]))
print("Found %d lines." % len(lines))
lines_a, lines_b = parseHoughLines(lines, top_two_angles, angle_threshold_deg=35)
# plotHoughLines(img, lines, color=(255,255,255), line_thickness=1)
# plotHoughLines(img, lines_a, color=(0,0,255))
# plotHoughLines(img, lines_b, color=(0,255,0))
if len(lines_a) < 2 or len(lines_b) < 2:
return img_orig, edges_masked, img_orig
a = time()
for i2 in range(10):
for i in range(100):
corners = chooseRandomGoodQuad(lines_a, lines_b, median_contour)
# warp_img, M = getTileImage(img_orig, corners.astype(np.float32),tile_buffer=16, tile_res=16)
M = getTileTransform(corners.astype(np.float32),tile_buffer=16, tile_res=16)
# Warp lines and draw them on warped image
all_lines = np.vstack([lines_a[:,:2], lines_a[:,2:], lines_b[:,:2], lines_b[:,2:]]).astype(np.float32)
warp_pts = cv2.perspectiveTransform(all_lines[None,:,:], M)
warp_pts = warp_pts[0,:,:]
warp_lines_a = np.hstack([warp_pts[:len(lines_a),:], warp_pts[len(lines_a):2*len(lines_a),:]])
warp_lines_b = np.hstack([warp_pts[2*len(lines_a):2*len(lines_a)+len(lines_b),:], warp_pts[2*len(lines_a)+len(lines_b):,:]])
# Get thetas of warped lines
thetas_a = np.array([getSegmentTheta(line) for line in warp_lines_a])
thetas_b = np.array([getSegmentTheta(line) for line in warp_lines_b])
median_theta_a = (np.median(thetas_a*180/np.pi))
median_theta_b = (np.median(thetas_b*180/np.pi))
# Gradually relax angle threshold over N iterations
if i < 20:
warp_angle_threshold = 0.03
elif i < 30:
warp_angle_threshold = 0.1
elif i < 50:
warp_angle_threshold = 0.3
elif i < 70:
warp_angle_threshold = 0.5
elif i < 80:
warp_angle_threshold = 1.0
else:
warp_angle_threshold = 2.0
if ((angleCloseDeg(abs(median_theta_a), 0, warp_angle_threshold) and
angleCloseDeg(abs(median_theta_b), 90, warp_angle_threshold)) or
(angleCloseDeg(abs(median_theta_a), 90, warp_angle_threshold) and
angleCloseDeg(abs(median_theta_b), 0, warp_angle_threshold))):
print('Found good match (%d): %.2f %.2f' % (i, abs(median_theta_a), abs(median_theta_b)))
break
# else:
# print('iter %d: %.2f %.2f' % (i, abs(median_theta_a), abs(median_theta_b)))
warp_img, M = getTileImage(img_orig, corners.astype(np.float32),tile_buffer=16, tile_res=16)
# Recalculate warp now that we're using a different tile_buffer/res
# warp_pts = cv2.perspectiveTransform(all_lines[None,:,:], M)
# warp_pts = warp_pts[0,:,:]
# warp_lines_a = np.hstack([warp_pts[:len(lines_a),:], warp_pts[len(lines_a):2*len(lines_a),:]])
# warp_lines_b = np.hstack([warp_pts[2*len(lines_a):2*len(lines_a)+len(lines_b),:], warp_pts[2*len(lines_a)+len(lines_b):,:]])
lines_x, lines_y, step_x, step_y = getWarpCheckerLines(warp_img)
if len(lines_x) > 0:
print('Found good chess lines (%d): %s %s' % (i2, lines_x, lines_y))
break
print("Ransac corner detection took %.4f seconds." % (time() - a))
print(lines_x, lines_y)
warp_img, M = getTileImage(img_orig, corners.astype(np.float32),tile_buffer=16, tile_res=16)
for corner in corners:
cv2.circle(img, tuple(map(int,corner)), 5, (255,150,150),-1)
if len(lines_x) > 0:
print('Found chessboard?')
warp_corners, all_warp_corners = getRectChessCorners(lines_x, lines_y)
tile_centers = all_warp_corners + np.array([step_x/2.0, step_y/2.0]) # Offset from corner to tile centers
M_inv = np.matrix(np.linalg.inv(M))
real_corners, all_real_tile_centers = getOrigChessCorners(warp_corners, tile_centers, M_inv)
tile_res = 64 # Each tile has N pixels per side
tile_buffer = 1
warp_img, better_M = getTileImage(img_orig2, real_corners, tile_buffer=tile_buffer, tile_res=tile_res)
# Further refine rectified image
warp_img, was_rotated, refine_M = reRectifyImages(warp_img)
# combined_M = better_M
combined_M = np.matmul(refine_M,better_M)
M_inv = np.matrix(np.linalg.inv(combined_M))
# Get better_M based corners
hlines = vlines = (np.arange(8)+tile_buffer)*tile_res
hcorner = (np.array([0,8,8,0])+tile_buffer)*tile_res
vcorner = (np.array([0,0,8,8])+tile_buffer)*tile_res
ideal_corners = np.vstack([hcorner,vcorner]).T
ideal_all_corners = np.array(list(itertools.product(hlines, vlines)))
ideal_tile_centers = ideal_all_corners + np.array([tile_res/2.0, tile_res/2.0]) # Offset from corner to tile centers
real_corners, all_real_tile_centers = getOrigChessCorners(ideal_corners, ideal_tile_centers, M_inv)
# Get final refined rectified warped image for saving
warp_img, _ = getTileImage(img_orig2, real_corners, tile_buffer=tile_buffer, tile_res=tile_res)
cv2.polylines(img, [real_corners.astype(np.int32)], True, (150,50,255), thickness=3)
cv2.polylines(img, [all_real_tile_centers.astype(np.int32)], False, (0,50,255), thickness=1)
# Update mask with predicted chessboard
cv2.drawContours(mask,[real_corners.astype(int)],0,1,-1)
img_masked_full = cv2.bitwise_and(img,img,mask = (mask > 0.5).astype(np.uint8))
img_masked = cv2.addWeighted(img,0.2,img_masked_full,0.8,0)
drawMinAreaRect(img_masked, min_area_rect)
return img_masked, edges_masked, warp_img
def process_img(img, name):
#
# WARP TO NEW PERSPECTIVE
#
source_points = get_image_points()
img = camera.undistort(img)
DEBUG.save_img_with_path(img, source_points, name)
source_points = np.array(source_points, np.float32)
destination_points = np.array(get_destination_points(), np.float32)
M = cv2.getPerspectiveTransform(source_points, destination_points)
warped = cv2.warpPerspective(img, M, camera.viewport_size(), flags=cv2.INTER_LINEAR)
DEBUG.save_img(warped, "warped_" + name)
#
# CREATE BINARY IMAGE
#
# Create grayscale image
grayscale_warped = cv2.cvtColor(warped, cv2.COLOR_RGB2GRAY)
hls_warped = cv2.cvtColor(warped, cv2.COLOR_RGB2HLS)
saturation_warped = hls_warped[:,:,2]
l_warped = hls_warped[:,:,1]
#DEBUG.save_img(saturation_warped, "warped_saturation_" + name)
saturation_warped_threshold = np.zeros_like(saturation_warped)
# edge detection on a combination of saturation and grayscale.
# saturation is mostly useful on yellow. So this is our best bet.
binary_sobelx = progressive_binary(saturation_warped, grayscale_warped, saturation_warped_threshold)
#DEBUG.save_img(binary_sobelx, "binary_" + name)
# find the peaks in the bottom half of the image. This is the starting
# point for finding the lane.
histogram = bottom_half_histogram(binary_sobelx)
max_first_half = histogram[:len(histogram)//2].argmax()
max_second_half = histogram[len(histogram)//2:].argmax() + len(histogram)//2
annotated_image, left_path = find_lines(max_first_half, binary_sobelx)
#DEBUG.save_img(annotated_image, "annotated_output_left_" + name + ".png")
annotated_image, right_path = find_lines(max_second_half, binary_sobelx)
#DEBUG.save_img(annotated_image, "annotated_output_right_" + name + ".png")
#
# Fit polylines to the detected lane points
#
# Generate x and y values for plotting in projected space
fit_poly_left = np.polyfit([x[1] for x in left_path], [x[0] for x in left_path], 2)
fit_poly_right = np.polyfit([x[1] for x in right_path], [x[0] for x in right_path], 2)
ploty = np.linspace(0, binary_sobelx.shape[0]-1, binary_sobelx.shape[0])
left_fitx = fit_poly_left[0]*ploty**2 + fit_poly_left[1]*ploty + fit_poly_left[2]
right_fitx = fit_poly_right[0]*ploty**2 + fit_poly_right[1]*ploty + fit_poly_right[2]
# Convert left path back to original space
path_left = list(zip(left_fitx, ploty))
path_left = np.array([(path_left)], dtype=np.float32)
reverse_transform = cv2.getPerspectiveTransform(destination_points, source_points)
converted_left_path = cv2.perspectiveTransform(path_left, reverse_transform)
# Convert right path back to original space
path_right = list(zip(right_fitx, ploty))
path_right = np.array([(path_right)], dtype=np.float32)
converted_right_path = cv2.perspectiveTransform(path_right, reverse_transform)
# Draw bounding box around the lane area
overlay_image = np.zeros_like(img)
bounding_array = np.concatenate( (converted_left_path[0], np.flipud(converted_right_path[0])) )
bounding_box = np.array([bounding_array], dtype=np.int32)
overlay_image = cv2.fillPoly(overlay_image, bounding_box, (0,255, 0))
# this works fine
#DEBUG.save_img(overlay_image, "overlay_image_" + name + ".png")
result_image = cv2.addWeighted(img, 1, overlay_image, 0.3, 0)
#DEBUG.save_img(result_image, "zoutput_" + name + ".png")
# use plot to get us an image
path_left = list(zip(left_fitx, ploty))
path_left = np.array([(path_left)], dtype=np.float32)
converted_left_path = cv2.perspectiveTransform(path_left, reverse_transform)
path_right = list(zip(right_fitx, ploty))
path_right = np.array([(path_right)], dtype=np.float32)
converted_right_path = cv2.perspectiveTransform(path_right, reverse_transform)
#
# Curvature & offset in lane
#
# Calculate values
left_curvature = calculate_curvature_meters(left_path)
right_curvature = calculate_curvature_meters(right_path)
average_curvature = (left_curvature + right_curvature) / 2.0
offset = calculate_offset(left_path[0][0], right_path[0][0], binary_sobelx.shape[1])
# Annotate the image
font = cv2.FONT_HERSHEY_SIMPLEX
text = 'Radius of curvature = {}m'.format(int(average_curvature))
text2 = 'Offset from center = {}m'.format(offset)
result_image = cv2.putText(result_image, text, (100,100), font, 1,(255,255,255),2,cv2.LINE_AA)
result_image = cv2.putText(result_image, text2, (100,150), font, 1,(255,255,255),2,cv2.LINE_AA)
DEBUG.save_img(result_image, "zzzfinal_output_" + name + ".png")
return result_image
def getTransformation(img):
"""Gets the transformation matrix that would transform the image to be in a
birds eye view. Reference points to use in calculating the matrix are
specified by the user. User also specifies a distance threshold for social
distancing, should be around 6 feet
Args:
- img: image to find the transformatino matrix for a birds eye view
Returns:
The 3x3 transformation matrix
The number of pixels AFTER transformation that correspond to 6 feet
"""
# grab reference to global image, set it
global image
image = img
# Clone an unmodified copy of the image
global clean_img
clean_img = img.copy()
global combined
global T_matrix
# grab reference to pressed key
global keypress
# grab reference to user specified line
global linePts
# Create named window that we will display on while the user picks the points
cv2.namedWindow('Select reference points')
cv2.setMouseCallback('Select reference points', drawPoints)
# Show directions
print(four_points)
while True:
# display image and wait for event
cv2.imshow('Select reference points', image)
key = cv2.waitKey(1) & 0xFF
# if the 'c' key is pressed and all coordinates have been entered,
# break from the loop
if key == ord("c") and len(refPts) == 4:
break
# No longer need the window
cv2.destroyAllWindows()
T_matrix, warped = computeMatrix()
combined = getTransformImg(warped)
# Create window that we will display for user to confirm the transformation
cv2.namedWindow('Check transformation')
cv2.setMouseCallback('Check transformation', redoPoint)
# Show directions
print(check_transformation)
while True:
# display image and wait for event
cv2.imshow('Check transformation', combined)
key = cv2.waitKey(1) & 0xFF
# if one of keys 1 to 4 is pressed, record it so mouse callback
# can know which point to redo
if key == ord("1"):
keypress = 1
elif key == ord('2'):
keypress = 2
elif key == ord('3'):
keypress = 3
elif key == ord('4'):
keypress = 4
elif key == ord("c"):
break
# No longer need the window
cv2.destroyAllWindows()
# Create named window that we will display on while the user picks the six foot long line
cv2.namedWindow('Draw six foot long line')
cv2.setMouseCallback('Draw six foot long line', drawLine)
# Show directions
print(specify_dist)
clean_combined = combined.copy()
while True:
# display image and wait for event
cv2.imshow('Draw six foot long line', combined)
key = cv2.waitKey(1) & 0xFF
# if the 'r' key is pressed, reset line
if key == ord('r'):
linePts.clear()
combined = clean_combined.copy()
elif key == ord('c') and len(linePts) == 2:
break
cv2.destroyAllWindows()
points = np.array([linePts[0], linePts[1]])
transformed = cv2.perspectiveTransform(np.float32([points]), T_matrix)[0]
t1 = transformed[0]
t2 = transformed[1]
dist_thresh = (((t1[0] - t2[0])**2) + ((t1[1] - t2[1])**2))**0.5
return T_matrix, int(dist_thresh)
def processPhoto(codeDir,idir,fx,fy,cx,cy,k1,k2,k3,tangd1,tangd2,lengthOfInnerSquare, offsetBorder,horizontalDistanceBetweenCodeCenters, markerDictionary, markerList, approxCodeHeightInPixels):
StartWatch('BeforeContourProcessing')
filename = codeDir+"\\"+idir
p1=filename.find("IMG")
p2=filename.find(".")
imageNumber=filename[p1+len("IMG")-1:p2]
print ("\n Image number is ", imageNumber)
print ("\n file is ", filename,"\n")
StartWatch('ReadImage')
image = cv2.imread(filename)
EndWatch('ReadImage')
approxCodeHeightInPixels= ( (5 * lengthOfInnerSquare) + (2* offsetBorder) ) * generate_codesSettings.pixelsPerMM
approxCodeContourLength=approxCodeHeightInPixels*4
approxCodeContourArea=approxCodeHeightInPixels*approxCodeHeightInPixels
K = np.array([[fx,0,cx], [0, fy, cy], [0, 0, 1]])
#K = np.array([[fx,0,0], [0, fy, 0], [cx, cy, 1]])
d = np.array([k1,k2, 0, 0,k3]) # just use first two terms (no translation)
dist=np.array([k1,k2, 0, 0,k3])
#d=0
h, w = image.shape[:2]
imgHeight,imgWidth=image.shape[:2]
horizontalCenterOfImage=w/2
StartWatch('UndistortProcessing')
# undistort
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(K, dist, (w,h), 1,(w,h))
xroi,yroi,wroi,hroi = roi
#undistimg_w=wroi
#undistimg_h=hroi
undistimgBeforeCrop=np.zeros( (hroi,wroi,3) ,dtype = np.float32)
undistimgBeforeCrop = cv2.undistort(image, K, d, None, newcameramtx)
# now crop the image
undistimg=undistimgBeforeCrop[yroi:yroi+hroi, xroi:xroi+wroi]
# undistort
#undistimgBeforeCrop1=np.zeros( (hroi,wroi,3) ,dtype = np.float32)
mapx,mapy = cv2.initUndistortRectifyMap(K,dist,None,newcameramtx,(wroi,hroi),5)
undistimgBeforeCrop1 = cv2.remap(image,mapx,mapy,cv2.INTER_LINEAR)
undistimg1=undistimgBeforeCrop1[yroi:yroi+hroi, xroi:xroi+wroi]
undistimg=undistimgBeforeCrop1
EndWatch('UndistortProcessing')
StartWatch('BWThresholdProcessing')
cv2DebugWrite('c:\\temp\\image.png',image)
cv2DebugWrite('c:\\temp\\undistimage.png',undistimg)
cv2DebugWrite('c:\\temp\\undistimage1.png',undistimg1)
cv2DebugWrite('c:\\temp\\undistimageBeforeCrop.png',undistimgBeforeCrop)
cv2DebugWrite('c:\\temp\\undistimageBeforeCrop1.png',undistimgBeforeCrop1)
imsdup=np.zeros_like(undistimg)
#imsdup[:] = undistimg
imsdup1 = np.empty_like (undistimg)
imsdup1[:] = undistimg
gray1=cv2.cvtColor(undistimg,cv2.COLOR_BGR2GRAY)
#ret,thresh = cv2.threshold(gray,127,255,1)
ret,thresh = cv2.threshold(gray1,128,255, cv2.THRESH_BINARY | cv2.THRESH_OTSU)
# rough approach to ignoring codes in distorted area
widthQ=int(generate_codesSettings.distortionAreaRemoveFudgeFactor*w)
heightQ=int(generate_codesSettings.distortionAreaRemoveFudgeFactor*h)
threshdup=np.zeros_like(thresh)
threshdup[heightQ:h-heightQ, widthQ:w-widthQ]=thresh[heightQ:h-heightQ, widthQ:w-widthQ]
thresh=threshdup
cv2DebugWrite('c:\\temp\\thresh.png',thresh)
EndWatch('BWThresholdProcessing')
EndWatch('BeforeContourProcessing')
StartWatch('ContourProcessing')
#_,contours,h= cv2.findContours(thresh, cv2.RETR_LIST , cv2.CHAIN_APPROX_SIMPLE)
_,contours,h= cv2.findContours(thresh, cv2.RETR_LIST , cv2.CHAIN_APPROX_NONE)
cv2.drawContours(imsdup, contours, -1, (255, 0, 0), 3)
cv2DebugWrite("c:\\temp\\rawcontours.png",imsdup)
scnts4=[cntitem1 for cntitem1 in contours if len(cv2.approxPolyDP(cntitem1,0.01*cv2.arcLength(cntitem1,True),True))==4]
cv2.drawContours(imsdup, scnts4, -1, (255, 0, 0), 3)
cv2DebugWrite("c:\\temp\\cont4sides.png",imsdup)
scnts4WithLength=[(cntitem1, cv2.arcLength(cntitem1,True) ) for cntitem1 in contours if ( (len(cv2.approxPolyDP(cntitem1,0.01*cv2.arcLength(cntitem1,True),True))==4 ) and (cv2.contourArea(cntitem1)>1300) )]
scnts4WithLengthPossibleCode=[ item for item in scnts4WithLength if (item[1]>approxCodeContourLength*0.80) & (item[1]<approxCodeContourLength*1.2) ]
contoursCodesOnly= [cntitem[0] for cntitem in scnts4WithLengthPossibleCode ]
fudgeFactorAreaThresholdDiff=8000
contoursFilteredByArea= [ cntitem for cntitem in contoursCodesOnly if abs((cv2.contourArea(cntitem)-approxCodeContourArea)) <fudgeFactorAreaThresholdDiff ]
contoursCodesOnlyFBA= [cntitem for cntitem in contoursFilteredByArea ]
contoursPlusMoments= [ ( cntitem1, cv2.moments(cntitem1) ) for cntitem1 in contoursCodesOnlyFBA ]
#contoursPlusCenters= [ ( int(M['m10']/M['m00']) , int(M['m01']/M['m00']) ) for M in contoursPlusMoments ]
#contoursPlusCenters= [ ( cm[0], ( int(cm[1]['m10']/cm[1]['m00']), int(cm[1]['m01']/cm[1]['m00']) ) ) for cm in contoursPlusMoments ]
contoursPlusCenters= [ ( cm[0], ( int(cm[1]['m10']/cm[1]['m00']), int(cm[1]['m01']/cm[1]['m00']) ) ) for cm in contoursPlusMoments ]
#contoursPlusCenters= [ ( cm[0], ( int(cm[1]['m10']/cm[1]['m00']), int(cm[1]['m01']/cm[1]['m00']) ) ) for cm in contoursPlusMoments ]
print ("\n length of contoursPlusCenters +++ ********\n", len(contoursPlusCenters))
contoursNearExpectedPosition = [ cm[0] for cm in contoursPlusCenters if checkCodeNearExpectedHorizontalPosition(cm[1][0],generate_codesSettings.pixelsPerMM,horizontalCenterOfImage,horizontalDistanceBetweenCodeCenters) ]
# print ("\n length of contours ********\n", len(contoursNearExpectedPosition))
#hack
#contoursNearExpectedPosition=contoursCodesOnly
print ("\n length of scnts4WithLength ********\n", len(scnts4WithLength))
print ("\n length of scnts4WithLengthPossibleCode ********\n", len(scnts4WithLengthPossibleCode))
print ("\n length of contoursCodesOnly ********\n", len(contoursCodesOnly))
print ("\n length of contoursFilteredByArea ********\n", len(contoursFilteredByArea))
print ("\n length of contoursPlusMoments ********\n", len(contoursPlusMoments))
print ("\n length of contoursPlusCenters ********\n", len(contoursPlusCenters))
print ("\n length of contoursNearExpectedPosition ********\n", len(contoursNearExpectedPosition))
imsdupAll=np.zeros_like(undistimg)
#cv2.drawContours(imsdupAll, contoursNearExpectedPosition, 3, (255, 0, 0), 3)
cv2DebugWrite("c:\\temp\\contimgAll.png",imsdup)
for i in range(len(contoursNearExpectedPosition)):
imsdupAll=np.zeros_like(undistimg)
cv2.drawContours(imsdupAll, contoursNearExpectedPosition[i], -1, (255, 0, 0), 3)
filenamec="c:\\temp\\contnum_"+str(i)+".jpg"
cv2DebugWrite(filenamec,imsdupAll)
numberOfBoxes=len(contoursNearExpectedPosition)
codeBoxArray1L=np.zeros( (numberOfBoxes,4,generate_codesSettings.maxNumberOfPtsPerSide, 2) ,dtype = np.int32)
codeBoxCorners1L=np.zeros( (numberOfBoxes ,4,2) ,dtype = np.float32)
for boxID,boxItem in enumerate(contoursNearExpectedPosition):
boxFormContourBox3(boxItem,boxID, codeBoxArray1L,generate_codesSettings.maxNumberOfPtsPerSide)
#
EndWatch('ContourProcessing')
#numberOfRows=100
codeBoxLocalCenter=np.zeros( (numberOfBoxes,2) ,dtype = np.float32)
codeBoxExactCenter=np.zeros( (numberOfBoxes,2) ,dtype = np.float32)
codeBoxCode=np.zeros( (numberOfBoxes) ,dtype = np.int32)
totalErrorTally=0
codeList=[]
StartWatch('BoxProcessing')
for boxNumber in range(numberOfBoxes):
if (np.count_nonzero(codeBoxArray1L[boxNumber])==0):
# print "\n 77777777777 Continue"
continue
print ("\n ******** Box Number ", boxNumber)
leftMostPointsFromBox=codeBoxArray1L[boxNumber,3].flatten() [np.flatnonzero(codeBoxArray1L[boxNumber,3])].reshape(-1,2)
rightMostPointsFromBox=codeBoxArray1L[boxNumber,1].flatten() [np.flatnonzero(codeBoxArray1L[boxNumber,1])].reshape(-1,2)
topMostPointsFromBox=codeBoxArray1L[boxNumber,0].flatten() [np.flatnonzero(codeBoxArray1L[boxNumber,0])].reshape(-1,2)
bottomMostPointsFromBox=codeBoxArray1L[boxNumber,2].flatten() [np.flatnonzero(codeBoxArray1L[boxNumber,2])].reshape(-1,2)
[vxLM,vyLM,xLM,yLM] = cv2.fitLine(leftMostPointsFromBox,cv2.DIST_FAIR,0,0.01,0.01).flatten()
#print "\n $$$$$$$$$$$$$ rightMostPointsFromBox\n",rightMostPointsFromBox
[vxRM,vyRM,xRM,yRM] = cv2.fitLine(rightMostPointsFromBox,cv2.DIST_FAIR,0,0.01,0.01).flatten()
[vxTM,vyTM,xTM,yTM] = cv2.fitLine(topMostPointsFromBox,cv2.DIST_FAIR,0,0.01,0.01).flatten()
[vxBM,vyBM,xBM,yBM] = cv2.fitLine(bottomMostPointsFromBox,cv2.DIST_FAIR,0,0.01,0.01).flatten()
topMost=graphLine1(undistimg,vxTM,vyTM,xTM,yTM,(255,255,0),0.5)
#print "\n *****TM****", rowNumber, columnNumber,vxTM,vyTM,xTM,yTM
rightMost=graphLine1(undistimg,vxRM,vyRM,xRM,yRM,(255,0,0),0.5)
#print "\n *****RM****", rowNumber, columnNumber, vxRM,vyRM,xRM,yRM
bottomMost=graphLine1(undistimg,vxBM,vyBM,xBM,yBM,(0,255,255),0.5)
#print "\n *****BM****", rowNumber, columnNumber, vxBM,vyBM,xBM,yBM
leftMost=graphLine1(undistimg,vxLM,vyLM,xLM,yLM,(0,0,255),0.5)
#print "\n *****LM******", rowNumber, columnNumber,vxLM,vyLM,xLM,yLM
cv2DebugWrite("c:\\temp\\img_state.jpg", undistimg)
topLeft=line_intersection( leftMost, topMost)
topRight=line_intersection( rightMost, topMost)
bottomRight=line_intersection( rightMost, bottomMost)
bottomLeft=line_intersection( leftMost, bottomMost)
# print "\n Corners of Box", topLeft, topRight, bottomRight, bottomLeft
codeBoxCorners1L[boxNumber,0]= np.array([ int(topLeft[0]), int(topLeft[1]) ])
codeBoxCorners1L[boxNumber,1]= np.array([ int(topRight[0]), int(topRight[1]) ])
codeBoxCorners1L[boxNumber,2]= np.array([ int(bottomRight[0]), int(bottomRight[1]) ])
codeBoxCorners1L[boxNumber,3]= np.array([ int(bottomLeft[0]), int(bottomLeft[1]) ])
print ("\n**** ", codeDir, idir, "\n **** Corners",codeBoxCorners1L[boxNumber,0],codeBoxCorners1L[boxNumber,1],codeBoxCorners1L[boxNumber,2],codeBoxCorners1L[boxNumber,3],"\n" )
destBox1=np.array([
[0,0 ],
[generate_codesSettings.maxSizeCodeBoxInPixels,0 ],
[generate_codesSettings.maxSizeCodeBoxInPixels,generate_codesSettings.maxSizeCodeBoxInPixels ],
[0,generate_codesSettings.maxSizeCodeBoxInPixels ],
],dtype = "float32")
cv2DebugWrite("c:\\temp\\testme.jpg", imsdup1)
localBoxTransform= cv2.getPerspectiveTransform( codeBoxCorners1L[boxNumber] , destBox1 )
#localBoxImg=np.zeros( (500,500,3) ,dtype = np.float32)
filenamebox="c:\\temp\\localBox"+str(boxNumber)+"_.jpg"
warp = cv2.warpPerspective(imsdup1,localBoxTransform,(generate_codesSettings.maxSizeCodeBoxInPixels,generate_codesSettings.maxSizeCodeBoxInPixels))
cv2DebugWrite(filenamebox, warp)
code=decodeBox(warp,lengthOfInnerSquare,offsetBorder)
codeBoxCode[boxNumber]=code
print ("\n&&&&&&&&&&&&&&& Code is ", code)
if str(code) not in markerDictionary:
code=FixCode(code, markerList, 2)
if (code==0):
codeBoxLocalCenter[boxNumber]=np.array([ 0,0 ])
continue
centerSmall = line_intersection( ( topLeft, bottomRight ), ( topRight, bottomLeft ) )
codeBoxLocalCenter[boxNumber]=np.array([centerSmall[0], centerSmall[1]])
codeBoxExactCenter[boxNumber]=np.array((float(markerDictionary[str(code)][0]),float(markerDictionary[str(code)][1]) )).reshape(-1,2)
codeList.append(code)
EndWatch('BoxProcessing')
StartWatch('ErrorMeasureProcessing')
cbec=codeBoxExactCenter.flatten() [np.flatnonzero(codeBoxExactCenter)].reshape(-1,2)
cblc=codeBoxLocalCenter.flatten() [np.flatnonzero(codeBoxLocalCenter)].reshape(-1,2)
numNonZeroCodes=cblc.shape[0]
minNumberCorrectCodesPerPicture=3
codeErrors=False
if (numNonZeroCodes<minNumberCorrectCodesPerPicture):
codeErrors=True
return ( (codeErrors, 0,0,0,0,0 ) )
#perspTrAllPts,mask=cv2.findHomography(codeBoxLocalCenter.reshape(-1,2), codeBoxExactCenter.reshape(-1,2) )
perspTrAllPts,mask=cv2.findHomography(cblc,cbec)
if perspTrAllPts is None:
codeErrors=True
return ( (codeErrors, 0,0,0,0,0 ) )
#cbmc=np.zeros_like( cbec ,dtype = np.float32)
#codeBoxMappedCenter=cv2.perspectiveTransform( cblc.reshape(1,numberOfRows*2,2),perspTrAllPts)
cbmc=cv2.perspectiveTransform( cblc.reshape(1,numNonZeroCodes,2),perspTrAllPts)
imgUndist = np.empty_like (undistimg)
imgUndist[:] = undistimg
filenameperscorrected="c:\\temp\\IMGPersCorr.jpg"
warp = cv2.warpPerspective(imgUndist,perspTrAllPts,(imgWidth,imgHeight))
cv2DebugWrite(filenameperscorrected, warp)
distResult=spsd.cdist(cbmc.reshape(-1,2),cbec.reshape(-1,2) )
rmsError=np.sqrt(totalErrorTally)/(numberOfBoxes*2)
perspTrAllPtsInv=np.linalg.inv(perspTrAllPts)
cbinferredcenters=cv2.perspectiveTransform( cbec.reshape(1,numNonZeroCodes,2),perspTrAllPtsInv)
rvec=np.array([0,0,0])
tvec=np.array([0,0,0])
print ("\nRMS ",rmsError)
# generate visual of error by drawing circles around the error (based on filename and undistimg)
showErrorsFileName=filename
showErrorsFileName=showErrorsFileName.replace("IMG_", "IMAGEcenter_")
showErrors = np.empty_like (undistimg)
showErrors[:] = undistimg
for numcodes in range(len(codeList)):
# for each target - draw a circle around the center
clr=(255,0,255)
resultP1=( ( int(cbinferredcenters[0,numcodes,0]) , int(cbinferredcenters[0,numcodes,1]) ) )
print ("\n Pt is ", resultP1)
cv2.circle(showErrors,resultP1,8 ,clr,1)
#cv2.circle(showErrors,resultP1,8 ,clr,2)
errorForCode=distResult[numcodes,numcodes]
resultP1TextError= ( ( int(cbinferredcenters[0,numcodes,0]+110) , int(cbinferredcenters[0,numcodes,1]) ) )
resultP1TextCode = ( ( int(cbinferredcenters[0,numcodes,0]-310) , int(cbinferredcenters[0,numcodes,1]) ) )
#cv2.putText(showErrors,str(errorForCode),resultP1Text,cv2.FONT_HERSHEY_SIMPLEX,clr)
cv2.putText(showErrors,str(errorForCode), resultP1TextError, cv2.FONT_HERSHEY_SIMPLEX, 1, 255)
cv2.putText(showErrors,str(codeList[numcodes]), resultP1TextCode, cv2.FONT_HERSHEY_SIMPLEX, 1, 255)
print ("\n*** SHow error file name is", showErrorsFileName)
cv2DebugWrite(showErrorsFileName, showErrors)
# print "\n\n777777777777\n" , topLeft,topRight,bottomLeft,bottomRight
cv2DebugWrite("c:\\temp\\lineq.png", undistimg)
EndWatch('ErrorMeasureProcessing')
return ( (codeErrors, codeList, cbinferredcenters, distResult,mapx,mapy) )
def siftComp(img1, nloop):
acc = 0
sift = cv2.xfeatures2d.SIFT_create(nfeature)
kp, res = sift.detectAndCompute(img1, None)
pts = np.array([[[k.pt[0], k.pt[1]] for k in kp]])
responses = np.array([k for k in range(0, len(kp))])
knn = cv2.ml.KNearest_create()
knn.train(res, cv2.ml.ROW_SAMPLE, responses)
sift = cv2.xfeatures2d.SIFT_create(1000)
for i in range(0, nloop):
print "******This is the " + str(i) + " loop*******"
img2, M = method.dataIncreasing_camera(img1)
dst = cv2.perspectiveTransform(pts, M)
dst = [(int(k[0]), int(k[1])) for k in dst[0]]
kp2, res2 = sift.detectAndCompute(img2, None)
for k in dst:
img2 = cv2.circle(img2, k, 2, (55, 255, 155), 2)
out = cv2.drawKeypoints(img2, kp2, img2)
matchs = []
nTure = []
nFalse = []
nnTure = []
nnFalse = []
print len(res2)
if res2 == None:
neighbours = []
else:
ret, results, neighbours, dist = knn.findNearest(res2, 3)
for k in range(0, len(neighbours)):
# print dst[int(neighbours[k][0])]
(x0, y0) = dst[int(neighbours[k][0])]
(x1, y1) = kp2[k].pt
err = np.sqrt((x1 - x0) * (x1 - x0) + (y1 - y0) * (y1 - y0))
if err < 3:
if distinguish[0, int(neighbours[k][0])] == 0 or distinguish[0, int(neighbours[k][0])] < dist[k][0] / \
dist[k][1]:
distinguish[
0, int(neighbours[k][0])] = dist[k][0] / dist[k][1]
img2 = cv2.circle(img2, (x0, y0), 6, (255, 155, 55), 1)
apprearance[int(neighbours[k][0])] += 1
# print "GOOD NO." + str(int(neighbours[k][0])) + "\nthe shortest distence is:" + str(dist[k])
if dist[k][0] < predict[int(neighbours[k][0])]:
print "Preduct is True"
nTure.append(int(neighbours[k][0]))
else:
print "Preduct is False"
nFalse.append(int(neighbours[k][0]))
if dist[k][0] > weigh[int(neighbours[k][0]), 0]:
weigh[int(neighbours[k][0]), 0] = dist[k][0]
predict[int(neighbours[k][0])] = min(
weigh[int(neighbours[k][0]), 0],
weigh[int(neighbours[k][0]), 1])
match = cv2.DMatch(int(neighbours[k][0]), k, 3)
matchs.append(match)
else:
if distinguish[1, int(neighbours[k][0])] == 0 or distinguish[1, int(neighbours[k][0])] > dist[k][0] / \
dist[k][1]:
distinguish[
1, int(neighbours[k][0])] = dist[k][0] / dist[k][1]
# print "BAD NO." + str(int(neighbours[k][0])) + "\nthe shortest distence is:" + str(dist[k])
if dist[k][0] < predict[int(neighbours[k][0])]:
print "Preduct is False"
nnFalse.append(int(neighbours[k][0]))
else:
print "Preduct is True"
nnTure.append(int(neighbours[k][0]))
if 0 == weigh[int(neighbours[k][0]),
1] or dist[k][0] < weigh[int(neighbours[k][0]),
1]:
weigh[int(neighbours[k][0]), 1] = dist[k][0]
predict[int(neighbours[k][0])] = min(
weigh[int(neighbours[k][0]), 0],
weigh[int(neighbours[k][0]), 1])
print apprearance
print "The number of true match and false match is " + str(
(len(nTure), len(nFalse), len(nTure) * 1.0 /
(len(nFalse) + len(nTure) + 0.001)))
print "The number of true match and false match is " + str(
(len(nTure), len(nnFalse), len(nTure) * 1.0 /
(len(nnFalse) + len(nTure) + 0.001)))
acc += len(nTure) * 1.0 / (len(nnFalse) + len(nTure) + 0.001)
print acc / nloop
out = cv2.drawMatches(img1, kp, img2, kp2, matchs, out)
cv2.imshow("sift picture of translation", out)
return
def image_callback(self, msg):
image = self.bridge.compressed_imgmsg_to_cv2(msg,
desired_encoding='bgr8')
imageGray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
cv2.useOptimized()
cv2.setUseOptimized(True)
kp1, des1 = self.surf.detectAndCompute(self.blocking_img, None)
kp2, des2 = self.surf.detectAndCompute(imageGray, None)
FLANN_INDEX_KDTREE = 1
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
search_params = dict(checks=50)
matches = None
try:
flann = cv2.FlannBasedMatcher(index_params, search_params)
matches = flann.knnMatch(des1, des2, k=2)
except Exception as ex:
print('knnMatch error')
return
good = []
for m, n in matches:
if m.distance < 0.7 * n.distance:
good.append(m)
outer_dst_pts = np.float32([])
if len(good) > MIN_MATCH_COUNT:
src_pts = np.float32([kp1[m.queryIdx].pt
for m in good]).reshape(-1, 1, 2)
dst_pts = np.float32([kp2[m.trainIdx].pt
for m in good]).reshape(-1, 1, 2)
outer_dst_pts = dst_pts
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
matchesMask = mask.ravel().tolist()
h, w, d = self.blocking_img.shape
pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1],
[w - 1, 0]]).reshape(-1, 1, 2)
dst = None
try:
dst = cv2.perspectiveTransform(pts, M)
except Exception as ex:
print('perspectiveTransform error: dst = %s' % dst)
return
image = cv2.polylines(image, [np.int32(dst)], True, (255, 0, 0), 3,
cv2.LINE_AA)
self.match = True
rospy.logdebug('차단바 표지판 탐지 : %s' % self.match)
else:
self.match = False
rospy.logdebug("Not enough matches are found - {}/{}".format(
len(good), MIN_MATCH_COUNT))
matchesMask = None
draw_params = dict(matchColor=(0, 255, 0),
singlePointColor=None,
matchesMask=matchesMask,
flags=2)
matches_img = cv2.drawMatches(self.blocking_img, kp1, image, kp2, good,
None, **draw_params)
if show_matched_points:
for pt in outer_dst_pts:
x, y = pt[0]
cv2.circle(image, (x, y), 3, (0, 0, 255), -1)
self.match_pub.publish(self.match)
#cv2.imshow('match', matches_img)
cv2.imshow("image", image)
cv2.waitKey(3)
point_3d_R[i, 0, 2] = disparity[0, i]
for j in range(2):
point_3d_R[i, 0, j] = undistortPoints_R[i, 0, j]
# point_3d_L[0,0,0] = undistortPoints_L[0,0,0]
# point_3d_L[0,0,1] = undistortPoints_L[0,0,1]
# point_3d_L[0,0,2] = disparity[0,0]
#
# point_3d_L[1,0,0] = undistortPoints_L[1,0,0]
# point_3d_L[1,0,1] = undistortPoints_L[1,0,1]
# point_3d_L[1,0,2] = disparity[0,1]
#
# point_3d_L[2,0,0] = undistortPoints_L[2,0,0]
# point_3d_L[2,0,1] = undistortPoints_L[2,0,1]
# point_3d_L[2,0,2] = disparity[0,2]
#
# point_3d_L[3,0,0] = undistortPoints_L[3,0,0]
# point_3d_L[3,0,1] = undistortPoints_L[3,0,1]
# point_3d_L[3,0,2] = disparity[0,3]
print('point_3d_L = ', point_3d_L)
print('point_3d_R = ', point_3d_R)
perspectiveTransform_L = cv2.perspectiveTransform(point_3d_L, Q)
print('perspectiveTransform_L = ', perspectiveTransform_L)
perspectiveTransform_R = cv2.perspectiveTransform(point_3d_R, Q)
print('perspectiveTransform_R = ', perspectiveTransform_R)
cv2.destroyAllWindows()
def match_car(self, target_gray, colour):
if colour == GREEN:
self.source = self.source_green
self.h = self.h_g
self.w = self.w_g
self.kp_source = self.kp_source_g
self.desc_source = self.desc_source_g
homography_threshold = HOMOGRAPHY_THRESHOLD
filter_match_threshold = FILTER_MATCH_THRESHOLD
elif colour == BLUE:
self.source = self.source_blue
self.h = self.h_b
self.w = self.w_b
self.kp_source = self.kp_source_b
self.desc_source = self.desc_source_b
homography_threshold = HOMOGRAPHY_THRESHOLD
filter_match_threshold = FILTER_MATCH_THRESHOLD
else:
self.source = self.source_yellow
self.h = self.h_y
self.w = self.w_y
self.kp_source = self.kp_source_y
self.desc_source = self.desc_source_y
homography_threshold = HOMOGRAPHY_THRESHOLD_YELLOW
filter_match_threshold = FILTER_MATCH_THRESHOLD_YELLOW
#use sift to get keypoints and descriptors in the frame
kp_target, desc_target = self.sift.detectAndCompute(target_gray, None)
#match the descriptors of the target and the descriptors of the frame
#matches is a list of matching points in the target and descriptor.
matches = self.flann.knnMatch(self.desc_source, desc_target, k=2)
#filter only for good matches
#this is done by cutting out points with abnormally large distances
good_pts = []
for m, n in matches:
if m.distance < filter_match_threshold*n.distance:
good_pts.append(m)
#draw the found matches of keypoints from two images
#output_matches = cv.drawMatches(self.source_img, self.kp_source, target_gray, kp_target, \
# good_pts, target_gray)
#Homography
if len(good_pts) > homography_threshold:
#if there are this many points, draw homography
#query index gives position of the points in the query image
#this extracts those points and reshapes it
query_pts = np.float32([self.kp_source[m.queryIdx].pt \
for m in good_pts]).reshape(-1,1,2)
train_pts = np.float32([kp_target[m.trainIdx].pt \
for m in good_pts]).reshape(-1,1,2)
#obtains the perspective transformation between two sets of points
matrix, mask = cv.findHomography(query_pts, train_pts, cv.RANSAC, 5.0)
#if no homography can be found
if matrix is None:
print("SCENARIO 1 HAPPENED")
print("DUMP: {}".format(query_pts))
return None
#TESTING
#return (target_gray, output_matches)
else:
#do a perspective transform to change the orientation of the homography
# with respect to the original image
pts = np.float32([[0, 0], [0, self.h], [self.w, self.h], \
[self.w, 0]]).reshape(-1,1,2)
dst = cv.perspectiveTransform(pts, matrix)
dst = np.int32(dst)
#Find the maximum and minimum x and y points
max_x = dst[0][0][0]
min_x = dst[0][0][0]
max_y = dst[0][0][1]
min_y = dst[0][0][1]
for i in range(len(dst)):
if max_x < dst[i][0][0]:
max_x = dst[i][0][0]
if min_x > dst[i][0][0]:
min_x = dst[i][0][0]
if max_y < dst[i][0][1]:
max_y = dst[i][0][1]
if min_y > dst[i][0][1]:
min_y = dst[i][0][1]
if max_x - min_x < HOM_LOW or max_x - min_x > HOM_HIGH:
print("SCENARIO 2 HAPPENED")
return None
#TESTING
#return (target_gray, output_matches)
elif max_y - min_y < HOM_LOW or max_y - min_y > HOM_HIGH:
print("SCENARIO 3 HAPPENED")
return None
#TESTING
#return (target_gray, output_matches)
else:
return ((min_x, min_y), (max_x, max_y))
#TESTING
#dst = [dst]
#draw the homography and show it
#homography = cv.polylines(target_gray, [np.int32(dst)], True, (255, 0, 0), 3)
#return (homography, output_matches)
else:
train_pts = np.float32([kp_target[m.trainIdx].pt \
for m in good_pts]).reshape(-1,1,2)
print("SCENARIO 4 HAPPENED")
print("DUMP: {}".format(train_pts))
return None
def compute_transformed_contour(width,
height,
fontsize,
M,
contour,
minarea=0.5):
"""Compute the permitted drawing contour
on a padded canvas for an image of a given size.
We assume the canvas is padded with one full image width
and height on left and right, top and bottom respectively.
Args:
width: Width of image
height: Height of image
fontsize: Size of characters
M: The transformation matrix
contour: The contour to which we are limited inside
the rectangle of size width / height
minarea: The minimum area required for a character
slot to qualify as being visible, expressed as
a fraction of the untransformed fontsize x fontsize
slot.
"""
spacing = math.ceil(fontsize / 2)
xslots = int(np.floor(width / spacing))
yslots = int(np.floor(height / spacing))
ys, xs = np.mgrid[:yslots, :xslots]
basis = np.concatenate([xs[..., np.newaxis], ys[..., np.newaxis]],
axis=-1).reshape((-1, 2))
basis *= spacing
slots_pretransform = np.concatenate([
(basis + offset)[:, np.newaxis, :]
for offset in [[0, 0], [spacing, 0], [spacing, spacing], [0, spacing]]
],
axis=1)
slots = cv2.perspectiveTransform(src=slots_pretransform.reshape(
(1, -1, 2)).astype('float32'),
m=M)[0]
inside = np.array([
cv2.pointPolygonTest(contour=contour, pt=(x, y), measureDist=False) >=
0 for x, y in slots
]).reshape(-1, 4).all(axis=1)
slots = slots.reshape(-1, 4, 2)
areas = np.abs(
(slots[:, 0, 0] * slots[:, 1, 1] - slots[:, 0, 1] * slots[:, 1, 0]) +
(slots[:, 1, 0] * slots[:, 2, 1] - slots[:, 1, 1] * slots[:, 2, 0]) +
(slots[:, 2, 0] * slots[:, 3, 1] - slots[:, 2, 1] * slots[:, 3, 0]) +
(slots[:, 3, 0] * slots[:, 0, 1] -
slots[:, 3, 1] * slots[:, 0, 0])) / 2
slots_filtered = slots_pretransform[(areas > minarea * spacing * spacing)
& inside]
temporary_image = cv2.drawContours(image=np.zeros((height, width),
dtype='uint8'),
contours=slots_filtered,
contourIdx=-1,
color=255)
temporary_image = cv2.dilate(src=temporary_image,
kernel=np.ones((spacing, spacing)))
newContours, _ = cv2.findContours(temporary_image,
mode=cv2.RETR_TREE,
method=cv2.CHAIN_APPROX_SIMPLE)
x, y = slots_filtered[0][0]
contour = newContours[next(
index for index, contour in enumerate(newContours)
if cv2.pointPolygonTest(contour=contour, pt=(
x, y), measureDist=False) >= 0)][:, 0, :]
return contour
def draw_text_image(text,
fontsize,
height,
width,
fonts,
use_ligatures=False,
thetaX=0,
thetaY=0,
thetaZ=0,
color=(0, 0, 0),
permitted_contour=None,
draw_contour=False):
"""Get a transparent image containing text.
Args:
text: The text to draw on the image
fontsize: The size of text to show.
height: The height of the output image
width: The width of the output image
fonts: A dictionary of {subalphabet: paths_to_font}
thetaX: Rotation about the X axis
thetaY: Rotation about the Y axis
thetaZ: Rotation about the Z axis
color: The color of drawn text
permitted_contour: A contour defining which part of the image
we can put text. If None, the entire canvas is permitted
for text.
use_ligatures: Whether to render ligatures. If True,
ligatures are always used (with an initial check for support
which sometimes yields false positives). If False, ligatures
are never used.
Returns:
An (image, sentence, lines) tuple where image is the
transparent text image, sentence is the full text string,
and lines is a list of lines where each line itself is a list
of (box, character) tuples.
"""
# pylint: disable=bad-continuation
if not use_ligatures:
fonts = {
subalphabet: PIL.ImageFont.truetype(font_path, size=fontsize)
if font_path is not None else PIL.ImageFont.load_default()
for subalphabet, font_path in fonts.items()
}
if use_ligatures:
for subalphabet, font_path in fonts.items():
ligatures_supported = True
font = PIL.ImageFont.truetype(
font_path, size=fontsize
) if font_path is not None else PIL.ImageFont.load_default()
for ligature in LIGATURES:
try:
font.getsize(ligature)
except UnicodeEncodeError:
ligatures_supported = False
break
if ligatures_supported:
del fonts[subalphabet]
subalphabet += LIGATURE_STRING
fonts[subalphabet] = font
for insert, search in LIGATURES.items():
for subalphabet in fonts.keys()():
if insert in subalphabet:
text = text.replace(search, insert)
character_font_pairs = [(character,
next(font for subalphabet, font in fonts.items()
if character in subalphabet))
for character in text]
M = get_rotation_matrix(width=width,
height=height,
thetaZ=thetaZ,
thetaX=thetaX,
thetaY=thetaY)
if permitted_contour is None:
permitted_contour = np.array([[0, 0], [width, 0], [width, height],
[0, height]]).astype('float32')
character_sizes = np.array([
font.font.getsize(character)
for character, font in character_font_pairs
])
min_character_size = character_sizes.sum(axis=1).min()
transformed_contour = compute_transformed_contour(
width=width,
height=height,
fontsize=max(min_character_size, 1),
M=M,
contour=permitted_contour)
start_x = transformed_contour[:, 0].min()
start_y = transformed_contour[:, 1].min()
end_x = transformed_contour[:, 0].max()
end_y = transformed_contour[:, 1].max()
image = PIL.Image.new(mode='RGBA',
size=(width, height),
color=(255, 255, 255, 0))
draw = PIL.ImageDraw.Draw(image)
lines = [[]]
sentence = ''
x = start_x
y = start_y
max_y = start_y
out_of_space = False
for character_index, (character, font) in enumerate(character_font_pairs):
if out_of_space:
break
(character_width,
character_height), (offset_x,
offset_y) = character_sizes[character_index]
if character in LIGATURES:
subcharacters = LIGATURES[character]
dx = character_width / len(subcharacters)
else:
subcharacters = character
dx = character_width
x2, y2 = (x + character_width + offset_x,
y + character_height + offset_y)
while not all(
cv2.pointPolygonTest(
contour=transformed_contour, pt=pt, measureDist=False) >= 0
for pt in [(x, y), (x2, y), (x2, y2), (x, y2)]):
if x2 > end_x:
dy = max(1, max_y - y)
if y + dy > end_y:
out_of_space = True
break
y += dy
x = start_x
else:
x += fontsize
if len(lines[-1]) > 0:
# We add a new line whether we have advanced
# in the y-direction or not because we also want to separate
# horizontal segments of text.
lines.append([])
x2, y2 = (x + character_width + offset_x,
y + character_height + offset_y)
if out_of_space:
break
max_y = max(y + character_height + offset_y, max_y)
draw.text(xy=(x, y), text=character, fill=color + (255, ), font=font)
for subcharacter in subcharacters:
lines[-1].append(
(np.array([[x + offset_x, y + offset_y],
[x + dx + offset_x, y + offset_y],
[x + dx + offset_x, y2],
[x + offset_x,
y2]]).astype('float32'), subcharacter))
sentence += subcharacter
x += dx
image = cv2.warpPerspective(src=np.array(image),
M=M,
dsize=(width, height))
if draw_contour:
image = cv2.drawContours(
image,
contours=[permitted_contour.reshape((-1, 1, 2)).astype('int32')],
contourIdx=0,
color=(255, 0, 0, 255),
thickness=int(width / 100))
lines = strip_lines(lines)
lines = [[(cv2.perspectiveTransform(src=coords[np.newaxis],
m=M)[0], character)
for coords, character in line] for line in lines]
return image, sentence, lines
MIN_MATCH_COUNT = 10
if len(good) > MIN_MATCH_COUNT:
# Get positions of matched points
src_pts = np.float32([kp1[m.queryIdx].pt for m in good]).reshape(-1, 1, 2)
dst_pts = np.float32([kp2[m.trainIdx].pt for m in good]).reshape(-1, 1, 2)
# Get perspective transformation matrix and list of inliers and outliers
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
matchesMask = mask.ravel().tolist()
# Project the object points into image frame
h, w = img1.shape
pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1],
[w - 1, 0]]).reshape(-1, 1, 2)
dst = cv2.perspectiveTransform(pts, M)
# Draw box
img2 = cv2.polylines(img2, [np.int32(dst)], True, 255, 3, cv2.LINE_AA)
else:
print("Not enough matches are found - %d/%d" %
(len(good), MIN_MATCH_COUNT))
matchesMask = None
draw_params = dict(
matchColor=(0, 255, 0), # draw matches in green color
singlePointColor=None,
matchesMask=matchesMask, # draw only inliers
flags=2)
def main(argv):
if len(argv) < 5:
print 'Error, incorrect number of args:\n python dataset_path max_number_images number_transformations patch_size\n'
sys.exit(1)
if not os.path.exists(outImPath):
os.mkdir(outImPath)
imDatasetPath = argv[1] # Dir that contains the original images
maxImg = int(argv[2])
numWarps = int(argv[3]) # number of warps per image
patchSize = int(argv[4]) # patch size
# create file
fileList = open(trainFilename,'w')
imFiles = [f for f in listdir(imDatasetPath) if isfile(join(imDatasetPath, f))]
numIm = 0
for imFile in imFiles:
if numIm >= maxImg:
sys.exit(1)
# Read image
img = cv2.imread(imDatasetPath + imFile, 0)
if not img is None:
# for each
height, width = img.shape[:2]
if height > patchSize * 3 and width > patchSize * 3:
print str(numIm), ' Load: ', imDatasetPath + imFile
x1 = (width/2)- (patchSize/2)
x2 = (width/2) + patchSize/2
y1 = (height/2)- (patchSize/2)
y2 = (height/2) + patchSize/2
patchLocation = np.float32( [ [x1, y1], [x2, y1], [x2, y2], [x1, y2]])
patchLocation = np.array([patchLocation])
patch_I1 = img[y1:y2, x1:x2] # Crop from x, y, w, h -> 100, 200, 300, 400
namePatchI1 = outImPath + str(numIm) + '_I1' + '.png'
cv2.imwrite( namePatchI1, patch_I1)
for nW in range(0, numWarps):
# Perform several transformations for the same image,
# in order to make the network learn different corner outputs
# from the same patch I1
# Extract a patch in the center of size 128x128
# Perform the 4-corner perspective change!
warpImg, Hom = affine_transform(patchSize, img, width, height, 0.05)
patchHom = cv2.perspectiveTransform(patchLocation, Hom)
# Generate some extra offset on the I2 coordinates
# This factor will make the extra margin on I2 bigger
randomDispFactor = 0.7
x1_off = int(round(randomDispFactor * patchSize)) * (random.uniform(1, 100) / 100)
x2_off = int(round(randomDispFactor * patchSize)) * (random.uniform(1, 100) / 100)
y1_off = x1_off
y2_off = x2_off
#y1_off = int(round(randomDispFactor * patchSize)) * (random.uniform(1, 100) / 100)
#y2_off = int(round(randomDispFactor * patchSize)) * (random.uniform(1, 100) / 100)
patchHom[0][0][0] = patchHom[0][0][0] - x1_off
patchHom[0][0][1] = patchHom[0][0][1] - y1_off
patchHom[0][1][0] = patchHom[0][1][0] + x2_off
patchHom[0][1][1] = patchHom[0][1][1] - y1_off
patchHom[0][2][0] = patchHom[0][2][0] + x2_off
patchHom[0][2][1] = patchHom[0][2][1] + y2_off
patchHom[0][3][0] = patchHom[0][3][0] - x1_off
patchHom[0][3][1] = patchHom[0][3][1] + y2_off
xh1, yh1 = patchHom[0].min(0)
xh2, yh2 = patchHom[0].max(0)
xh1 = int(round(xh1))
xh2 = int(round(xh2))
yh1 = int(round(yh1))
yh2 = int(round(yh2))
#print '\n Y[', yh1, ':', yh2, '] X [', xh1, ':', xh2, ']'
patch_I2 = warpImg[yh1:yh2, xh1:xh2]
wh = xh2 - xh1
hh = yh2 - yh1
gtCorner = np.float32([[1, 1], [1, 1], [1, 1], [1, 1]])
gtCorner[0] = np.float32([(patchHom[0][0][0] - xh1) + x1_off , (patchHom[0][0][1] - yh1) + y1_off])
gtCorner[1] = np.float32([wh - (xh2 - patchHom[0][1][0]) - x2_off, (patchHom[0][1][1] - yh1) + y1_off])
gtCorner[2] = np.float32([wh - (xh2 - patchHom[0][2][0]) - x2_off, hh - (yh2 - patchHom[0][2][1]) - y2_off])
gtCorner[3] = np.float32([(patchHom[0][3][0] - xh1) + x1_off , hh - (yh2 - patchHom[0][3][1]) - y2_off])
fs_x = float(patchSize) / float(wh)
fs_y = float(patchSize) / float(hh)
for p in range(0, 4):
gtCorner[p][0] = int(round(gtCorner[p][0] * fs_x))
gtCorner[p][1] = int(round(gtCorner[p][1] * fs_y))
# Scale patch_I2 to patchSize and adjust the gtCorners!
patch_I2 = cv2.resize(patch_I2, (patchSize, patchSize))
# TODO Apply more changes:
# - Blurring
# - Illumination
# - Occlusion??
# - ...
# Export patch warp
namePatchI2 = outImPath + str(numIm) + '_I' + str(nW + 2) + '.png'
cv2.imwrite( namePatchI2, patch_I2)
# add line to file
fileList.write( namePatchI1 + ' ' + namePatchI2)
gtCorner = gtCorner / patchSize
for p in range(0, 4):
fileList.write(' ' + str(gtCorner[p][0]) + ' ' + str(gtCorner[p][1]))
fileList.write('\n')
if verbose:
cv2.polylines(img, np.int32(patchLocation), 1, 255, 7)
cv2.polylines(warpImg, np.int32(patchHom), 1, 0, 4)
#cv2.imshow('full image', img)
#cv2.imshow('full image warped', warpImg)
for p in range(0, 4):
print p, ': ', gtCorner[p]
cv2.circle(patch_I2, (int(round(gtCorner[p][0] * patchSize)), int(round(gtCorner[p][1] * patchSize))), 5, 255, 1)
cv2.imshow('I1', patch_I1)
cv2.imshow('I2', patch_I2)
cv2.waitKey(0)
cv2.destroyAllWindows()
numIm = numIm + 1
fileList.close()
"""
End of:
Non-maximum suppression
"""
"""
Start of:
Data extraction
"""
# Confirm data structure for points
points = np.asarray(points, dtype=np.float32)
points = np.array([points])
# Warp for coordinate projection
warped, M_geo = transf.four_point_transform_geo(frame, corners, corners_geo)
pointsOut = cv2.perspectiveTransform(points, M_geo)
pointsOut = pointsOut.tolist()[0]
# Create dataframe for frame and append it
i_df = mapping.adjustDataFrame(df, pointsOut, class_type, f)
df = df.append(i_df)
"""
End of:
Data extraction
"""
else:
continue
"""
contours
) # duplicate contours list and remove any contours that are too short
for cont in range(len(contours)):
if len(contours[cont]) < 19:
contours_shorten.remove(contours[cont])
contour_pertr = [] # perspective correction contours
contours_pertrInts = []
for cont3 in range(len(contours_shorten)):
contours_shorten[cont3] = contours_shorten[cont3].reshape(-1, 2)
a = contours_shorten[cont3]
a = np.array([a])
a = a.astype(float)
contour_pertr.append(cv2.perspectiveTransform(a, h))
contour_pertr[cont3] = np.reshape(np.ravel(contour_pertr[cont3]),
(-1, 1, 2))
contour_pertr[cont3] = np.reshape(np.ravel(contour_pertr[cont3]), (-1, 2))
contours_pertrInts.append(cv2.perspectiveTransform(a, h))
contours_pertrInts[cont3] = np.reshape(np.ravel(contours_pertrInts[cont3]),
(-1, 1, 2))
contours_pertrInts[cont3] = np.reshape(np.ravel(contours_pertrInts[cont3]),
(-1, 2))
contours_pertrInts[cont3] = contours_pertrInts[cont3].astype(int)
#for ind in range(2):
# contours_pertrInts[cont3][cont_pertr][ind] = int(contour_pertr[cont3][cont_pertr][ind])
#contours_pertrInts[cont3] = np.reshape(np.ravel(contours_pertrInts[cont3]),(-1,1,2))
#contours_pertrInts[cont3].astype(int)
#contour_pertr[cont3] = np.reshape(np.ravel(contour_pertr[cont3]),(-1,1,2))
#contours_shorten[cont3] = a.reshape(-1,1,2)
M = cv2.getPerspectiveTransform(pts1, pts2)
# 进行透视变换
# dst = cv2.warpPerspective(img,M,(300,300))
return M
img = cv2.imread(r'C:\temp\pad.jpg')
h, w = img.shape[:2]
ps = [(0, 0), (w - 1, 0), (w - 1, h - 1), (0, h - 1)]
pM = get_perspective_matrix(img, ps)
# inner = cv2.perspectiveTransform(np.array([[inner]]), pM, (target_w, target_h))
img = cv2.warpPerspective(img, pM, (w, h))
# cv2.imshow('adsf', img)
# cv2.waitKey(0)
# cv2.imwrite(r'C:\temp\pad2.jpg', img)
# old_points = [328, 124]
a = np.array([[328, 124], [348, 120], [118, 178], [690, 86]], dtype='float32')
print('shape: ', a.shape)
# h = np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]], dtype='float32')
a = np.array([a])
print('shape: ', a.shape)
pointsOut = cv2.perspectiveTransform(a, pM)
print(type(pointsOut))
print(pointsOut)
print(pointsOut.shape)
print(pointsOut[0][1][0])
print(pointsOut[0][1][1])
def print_court_polylines(best_homography_matrix):
for line in BadmintonCourt.court_lines():
pts = np.float32([line]).reshape(-1, 1, 2)
dst = cv2.perspectiveTransform(pts, best_homography_matrix)
draw.line([(dst[0][0][0], dst[0][0][1]), (dst[1][0][0], dst[1][0][1])],fill="red")
def find_matches(pic1, pic2):
MIN_MATCH_COUNT = 10
img1 = cv2.imread(pic1, 0)
print('img1', np.shape(img1))
img2 = cv2.imread(pic2, 0)
print('img2', np.shape(img2))
#Feature detection by adaptive ROI
# Initiate SIFT detector
sift = cv2.xfeatures2d.SIFT_create()
print('sift', np.shape(sift))
# find the keypoints and descriptors with SIFT
kp1, des1 = sift.detectAndCompute(img1, None)
print('kp1', np.shape(kp1))
print('des1', np.shape(des1))
kp2, des2 = sift.detectAndCompute(img2, None)
print('kp2', np.shape(kp2))
print('des2', np.shape(des2))
FLANN_INDEX_KDTREE = 0
index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5)
search_params = dict(checks=50)
#create FLANN Matcher object
#matches the targets between two images
flann = cv2.FlannBasedMatcher(index_params, search_params)
print('flann', np.shape(flann))
#Match descriptors
matches = flann.knnMatch(des1, des2, k=2)
print('matches', np.shape(matches))
# store all the good matches as per Lowe's ratio test.
good = []
for m, n in matches:
if m.distance < 0.7 * n.distance:
good.append(m)
if len(good) > MIN_MATCH_COUNT:
#feature detections
img1_pts = np.float32([kp1[m.queryIdx].pt for m in good
]).reshape(-1, 1,
2) #matched pt locations in image 1
print('img1_pts', np.shape(img1_pts))
img2_pts = np.float32([kp2[m.trainIdx].pt for m in good
]).reshape(-1, 1,
2) #matched pt locations in image 2
print('img2_pts', np.shape(img2_pts))
#calculate homography before or after displacements?
H, mask = cv2.findHomography(img1_pts, img2_pts, cv2.RANSAC,
5.0) #H is 3x3 homography matrix
matchesMask = mask.ravel().tolist()
print('H', np.shape(H))
print('mask', np.shape(mask))
print('matchesMask', np.shape(matchesMask))
#feature image coord displacements
displacements = []
for i in range(len(img1_pts)):
disp = math.sqrt(
(img1_pts[i][0][0] - img2_pts[i][0][0])**2 +
(img1_pts[i][0][1] - img2_pts[i][0][1])**2) #distance formula
displacements.append(disp)
print('displacements', np.shape(displacements))
height, width = img1.shape
pts = np.float32([[0, 0], [0, height - 1], [width - 1, height - 1],
[width - 1, 0]]).reshape(-1, 1, 2)
dst = cv2.perspectiveTransform(pts, H)
print('pts', np.shape(pts))
print('dst', np.shape(dst))
img2 = cv2.polylines(img2, [np.int32(dst)], True, 255, 3, cv2.LINE_AA)
else:
print("Not enough matches are found - %d/%d" %
(len(good), MIN_MATCH_COUNT))
matchesMask = None
draw_params = dict(
matchColor=(0, 255, 0), # draw matches in green color
singlePointColor=False,
matchesMask=matchesMask, # draw only inliers
flags=2)
#img3 = cv2.drawMatches(img1,kp1,img2,kp2,good,None,**dr0000aw_params)
#return homography transform matrix H and image coordinate displacement vector
return [H, mask, good, img1_pts, displacements
] #change name; what *type* of displacements are these
if frame_num == 1:
# Pida al usuario que marque puntos paralelos y dos puntos separados por 6 pies. Orden bl, br, tr, tl, p1, p2
while True:
image = frame
cv2.imshow("image", image)
cv2.waitKey(1)
if len(mouse_pts) == 7:
cv2.destroyWindow("image")
break
first_frame_display = False
four_points = mouse_pts
# Obtener perspectiva
M, Minv = get_camera_perspective(frame, four_points[0:4])
pts = src = np.float32(np.array([four_points[4:]]))
warped_pt = cv2.perspectiveTransform(pts, M)[0]
d_thresh = np.sqrt(
(warped_pt[0][0] - warped_pt[1][0]) ** 2
+ (warped_pt[0][1] - warped_pt[1][1]) ** 2
)
bird_image = np.zeros(
(int(frame_h * scale_h), int(frame_w * scale_w), 3), np.uint8
)
bird_image[:] = SOLID_BACK_COLOR
pedestrian_detect = frame
print("Procesando fotograma: ", frame_num)
# Se dibuja el polígono de ROI(región binaria de interés)
pts = np.array(
good_points.append(m)
print("Done comparing matches - retained %2d" % len(good_points))
draw_params = dict(matchColor = (0,255,0),
singlePointColor = (255,0,0),
matchesMask = matchesMask,
flags = 0)
img3 = cv2.drawMatchesKnn(img,kp_image,img2,kp_image2,matches,None,**draw_params)
cv2.imshow("Matches",img3)
query_pts = np.float32([kp_image[m.queryIdx].pt for m in good_points]).reshape(-1, 1, 2)
train_pts = np.float32([kp_image2[m.trainIdx].pt for m in good_points]).reshape(-1, 1, 2)
matrix, mask = cv2.findHomography(query_pts, train_pts, cv2.RANSAC, 5.0)
print("Homography found %2d %2d" % (len(query_pts),len(train_pts)))
matches_mask = mask.ravel().tolist()
# Perspective transform
h, w = img.shape
pts = np.float32([[0, 0], [0, h], [w, h], [w, 0]]).reshape(-1, 1, 2)
dst = cv2.perspectiveTransform(pts, matrix)
homography = cv2.polylines(img2, [np.int32(dst)], True, (255, 0, 0), 3)
# Plot the keypoints used
kp_used = [kp_image2[m.trainIdx] for m in good_points];
img3=cv2.drawKeypoints(homography, kp_used, None)
cv2.imshow("Homography",img3)
cv2.waitKey()
def get_homography(self, img1, img2, plot=False, rigid=True):
"""Compute a homography between two images"""
if isinstance(img1, np.ndarray) and isinstance(img2, np.ndarray):
pass
elif isinstance(img1, basestring) and isinstance(img2, basestring):
img1 = cv2.imread(img1)
img2 = cv2.imread(img2)
else:
msg = 'Unsupported type; use numpy.ndarray or string'
raise RuntimeError(msg)
img1gray = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
img2gray = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)
# Find descriptors in each image
kp1, des1 = self.feature_detection.detectAndCompute(img1gray, None)
kp2, des2 = self.feature_detection.detectAndCompute(img2gray, None)
# Match descriptors between images
matches = self.matcher.knnMatch(des1, des2, k=2)
# Store all the good matches as per Lowe's ratio test.
good = []
for m, n in matches:
if m.distance < 0.7 * n.distance:
good.append(m)
src_pts = np.float32([kp1[m.queryIdx].pt
for m in good]).reshape(-1, 1, 2)
dst_pts = np.float32([kp2[m.trainIdx].pt
for m in good]).reshape(-1, 1, 2)
if rigid:
full_affine = False
M = cv2.estimateRigidTransform(src_pts, dst_pts, full_affine)
if M is None:
raise RuntimeError('Could not estimate rigid transform.')
M = np.vstack((M, np.eye(1, 3, 2)))
else:
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
if plot:
if not rigid:
matchesMask = mask.ravel().tolist()
h, w = img1gray.shape
pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1],
[w - 1, 0]]).reshape(-1, 1, 2)
dst = cv2.perspectiveTransform(pts, M)
# Draw matches in green (only inliers)
draw_params = dict(matchColor=(0, 255, 0),
singlePointColor=None,
matchesMask=matchesMask,
flags=2)
img3 = cv2.drawMatches(img1, kp1, img2, kp2, good, None,
**draw_params)
plt.figure()
plt.imshow(img3)
# Draw stitching
stitch = self.warp_two_images(img1, img2, np.linalg.inv(M))
plt.imshow(stitch)
plt.show()
return M
def main():
"""
This functions loads the target surface image,
"""
homography = None
camera_parameters = mtx # got after doing the caliberation
# camera_parameters = np.array([[800, 0, 320], [0, 800, 240], [0, 0, 1]])
# create ORB keypoint detector
sift = cv2.xfeatures2d.SIFT_create()
# create BFMatcher object based on hamming distance
bf = cv2.BFMatcher()
# load the reference surface that will be searched in the video stream
dir_name = os.getcwd()
marker1 = cv2.imread(
os.path.join(dir_name, 'reference/markers/marker1.jpg'), 0)
marker1_inverse = cv2.imread(
os.path.join(dir_name, 'reference/markers/marker1_inverse.jpg'), 0)
# Compute marker keypoints and its descriptors
kp_marker1 = sift.detect(marker1, None)
kp_marker1, des_marker1 = sift.compute(marker1, kp_marker1)
kp_marker1_inverse = sift.detect(marker1_inverse, None)
kp_marker1_inverse, des_marker1_inverse = sift.compute(
marker1_inverse, kp_marker1_inverse)
# Load 3D model from OBJ file
obj = OBJ(os.path.join(dir_name, 'models/fox.obj'), swapyz=True)
# init video capture
# cap = cv2.VideoCapture(0)
cap = cv2.VideoCapture("./reference/videos/test_1.mp4")
prev5 = np.ones((3, 3))
prev4 = np.ones((3, 3))
prev3 = np.ones((3, 3))
prev2 = np.ones((3, 3))
prev1 = np.ones((3, 3))
homography = np.ones((3, 3))
prev_5 = np.ones((3, 3))
prev_4 = np.ones((3, 3))
prev_3 = np.ones((3, 3))
prev_2 = np.ones((3, 3))
prev_1 = np.ones((3, 3))
homography_2 = np.ones((3, 3))
speed = 10
Identity = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]])
unit_translation = np.array([[0, 0, 0], [0, 0, 1], [0, 0, 0]])
prev_trans = np.array([[0, 0, 0], [0, 0, 1], [0, 0, 0]])
center1 = np.array([0, 0])
n_frame = 0
inverse = False
cv2.namedWindow("window", cv2.WND_PROP_FULLSCREEN)
cv2.setWindowProperty("window", cv2.WND_PROP_FULLSCREEN,
cv2.WINDOW_FULLSCREEN)
while True:
n_frame += 1
# read the current frame
ret, frame = cap.read()
if not ret:
print("Unable to capture video")
return
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ret, corners_c = cv2.findChessboardCorners(gray, (9, 6), None)
# objpoints.append(objp)
if ret == True:
h, w = 6, 9
corners_chess = cv2.cornerSubPix(gray, corners_c, (11, 11),
(-1, -1), criteria)
# imgpoints.append(corners_chess)
homography_chess, mask = cv2.findHomography(
objp, corners_chess, cv2.RANSAC, 5.0)
pts_chess = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1],
[w - 1, 0]]).reshape(-1, 1, 2)
pts_chess = pts_chess * square_size
dst_chess = cv2.perspectiveTransform(pts_chess, homography_chess)
frame = cv2.polylines(frame, [np.int32(dst_chess)], True, 200, 3,
cv2.LINE_AA)
# print(homography_chess)
# Draw and display the corners
frame = cv2.drawChessboardCorners(frame, (9, 6), corners_chess,
ret)
if (point_inside(center1, dst_chess)):
cv2.waitKey(2000)
# speed *= -1
print("Reached destination !!")
inverse = not inverse
print("What to do ????")
prev_trans *= -1
print("Better I go back ...")
if inverse:
desMark1 = des_marker1_inverse
kpMark1 = kp_marker1_inverse
else:
desMark1 = des_marker1
kpMark1 = kp_marker1
# find and draw the keypoints of the frame
kp_frame = sift.detect(frame, None)
kp_frame, des_frame = sift.compute(frame, kp_frame)
matches1 = bf.knnMatch(desMark1, des_frame, k=2)
# match frame descriptors with model descriptors
# sort them in the order of their distance
# the lower the distance, the better the matc h
good = []
for m in matches1:
if m[0].distance < 0.75 * m[1].distance:
good.append(m)
matches1 = np.asarray(good)
# print(len(matches))
# compute Homography if enough matches are found
if len(matches1) > MIN_MATCHES:
# differenciate between source points and destination points
src_pts = np.float32([kpMark1[m[0].queryIdx].pt
for m in matches1]).reshape(-1, 1, 2)
dst_pts = np.float32([
kp_frame[m[0].trainIdx].pt for m in matches1
]).reshape(-1, 1, 2)
# compute Homography
prev5 = prev4
prev4 = prev3
prev3 = prev2
prev2 = prev1
prev1 = homography
homography, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC,
5.0)
try:
avg_homography = (prev1 + prev2 + prev3 + prev4 + prev5 +
homography) / 6.0
except:
continue
# avg_homography = homography
if True:
# Draw a rectangle that marks the found model in the frame
h, w = marker1.shape
pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1],
[w - 1, 0]]).reshape(-1, 1, 2)
center = (pts[0] + pts[1] + pts[2] + pts[3]) / 4
# project corners into frame
dst1 = cv2.perspectiveTransform(pts, avg_homography)
# connect them with lines
frame = cv2.polylines(frame, [np.int32(dst1)], True, 255, 3,
cv2.LINE_AA)
# if a valid homography matrix was found render cube on model plane
if homography is not None:
try:
# obtain 3D projection matrix from homography matrix and camera parameters
# avg_homography = np.matmul(Identity,avg_homography)
avg_homography = np.matmul(
avg_homography,
Identity + prev_trans + unit_translation * speed)
prev_trans = prev_trans + unit_translation * speed
dst1 = cv2.perspectiveTransform(pts, avg_homography)
center1 = (dst1[0] + dst1[1] + dst1[2] +
dst1[3]) / 4 # img coordinates
frame = cv2.polylines(frame, [np.int32(dst1)], True, 255,
3, cv2.LINE_AA)
# frame = cv2.circle(frame, [np.int32(center)], True, 255, 3, cv2.LINE_AA)
projection = projection_matrix(camera_parameters,
avg_homography)
# project cube or model
frame = render(frame, obj, projection, marker1, False)
#frame = render(frame, model, projection)
except Exception as e:
print(e)
cv2.imshow('window', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break # draw first 10 matches1.
else:
print("Not enough matches found - %d/%d" %
(len(matches1), MIN_MATCHES))
cap.release()
cv2.destroyAllWindows()
return 0
def detect(
self,
object_name: str,
query_img_feat: list,
sensor_image: np.ndarray
):
"""
Detects if the object is in the frame
Parameters
----------
object_name : str
Name of the object.
query_img_feat : list
A list containing keypoints, descriptors, and
dimension information of query object's image
sensor_image : numpy.ndarray
Image retrieved from a sensor (webcam/kinect).
"""
MIN_MATCH_COUNT = self.config.min_match_count
# If True the frame with detected object will
# be showed, by default False
show_image = self.config.show_image
if self.to_gray:
# sensor_rgb = sensor_image
sensor_image = cv2.cvtColor(sensor_image, cv2.COLOR_BGR2GRAY)
kp1, des1, dim = query_img_feat
kp2, des2 = self.detector_descriptor.detectAndCompute(sensor_image, None) # noqa: E501
matches = self.matcher(des1, des2, **self.config.matcher_kwargs)
# store all the good matches as per Lowe's ratio test.
good = []
for m, n in matches:
if m.distance < 0.7*n.distance:
good.append(m)
if len(good) > MIN_MATCH_COUNT:
src_pts = np.float32(
[kp1[m.queryIdx].pt for m in good]
).reshape(-1, 1, 2)
dst_pts = np.float32(
[kp2[m.trainIdx].pt for m in good]
).reshape(-1, 1, 2)
M, _ = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
if M is None:
return
h, w = dim
pts = np.float32(
[[0, 0], [0, h-1], [w-1, h-1], [w-1, 0]]
).reshape(-1, 1, 2)
dst = cv2.perspectiveTransform(pts, M).astype(np.int32)
# update the location of the object in the image
# converted to list as ndarray object is not json serializable
self.obj_boundary_info[object_name] = np.squeeze(dst, axis=1).tolist() # noqa: E501
if self.config.hold_prev_vals:
self.object_timer[object_name] = time()
if show_image:
# sensor_rgb = cv2.polylines(sensor_rgb, [dst] ,True,255,1, cv2.LINE_AA) # noqa: E501
self.viz_frame = self.annotate_frame(
self.viz_frame,
dst,
object_name
)
else:
if self.config.hold_prev_vals:
if time() - self.object_timer[object_name] > self.config.hold_period: # noqa: E501
self.obj_boundary_info[object_name] = None
else:
if self.config.show_image and object_name in self.obj_boundary_info.keys(): # noqa: E501
self.viz_frame = self.annotate_frame(
self.viz_frame,
np.expand_dims(
np.array(self.obj_boundary_info[object_name], dtype=np.int32), # noqa: E501
axis=1
),
object_name
)
else:
# Set None if the object isn't detected
self.obj_boundary_info[object_name] = None
rospy.loginfo("Not enough matches are found - {}/{}".format(len(good), MIN_MATCH_COUNT)) # noqa: E501
def circleposition(cap, transform, newcameramtx, mtx, dist, mask, maskcorners):
thresh = 0
maxValue = 255
#print "CAP type" + str(type(cap))
[oldx, oldy] = [0, 0]
#print "maskcorners %s" % str(maskcorners)
params = cv2.SimpleBlobDetector_Params()
# Change thresholds
#params.minThreshold = 10;
#params.maxThreshold = 200;
# Filter by Area.
params.filterByArea = True
params.minArea = 125
params.maxArea = 375
# Filter by Circularity
#params.filterByCircularity = True
#params.minCircularity = 0.8
# Filter by Convexity
params.filterByConvexity = True
params.minConvexity = 0.87
# Filter by Inertia
#params.filterByInertia = True
#params.minInertiaRatio = 0.01
ret, frame = cap.read()
detector = cv2.SimpleBlobDetector_create(params)
h, w = frame.shape[:2]
done = False
l = [[1, 1], [1, 1], [1, 1]]
while True:
time1 = time.clock()
ret, frame = cap.read()
if frame is None:
return
frame = cv2.undistort(frame, mtx, dist, None, newcameramtx)
ffframe = cv2.flip(frame, -1)
ulx = maskcorners[0][0] - 20 ##sorg for ikke at ryge udenfor billede
uly = maskcorners[0][1] - 20
brx = maskcorners[2][0] + 20
bry = maskcorners[2][1] + 20
ffframe = ffframe[uly:bry, ulx:brx]
gray = cv2.cvtColor(ffframe, cv2.COLOR_BGR2GRAY)
th, dst = cv2.threshold(gray, thresh, maxValue,
cv2.THRESH_BINARY_INV + cv2.THRESH_OTSU)
# Detect blobs.
dtime1 = time.clock()
keypoints = detector.detect(dst)
dtime2 = time.clock()
#print('detector %0.6f' % (dtime2-dtime1))
#print "keypoints" + str(keypoints)
#for k in keypoints:
# print("respovce {}".format(k.response))
if len(keypoints) > 0:
keypoint = [keypoints[-1]]
else:
keypoint = None
#print "keypoint" + str(keypoint)
if keypoint is not None:
#print keypoint[0].pt
circle = keypoint[0].pt
#circlesize = keypoint[0].size
#print("Size {}".format(keypoint[0].size))
#cv2.waitKey(5000)
#print "im alive"
im_with_keypoints = cv2.drawKeypoints(
ffframe, keypoints, np.array([]), (0, 0, 255),
cv2.DRAW_MATCHES_FLAGS_DRAW_RICH_KEYPOINTS)
cv2.imshow("im_with_keypoints", im_with_keypoints)
#cv2.imshow("out", out)
#cv2.waitKey(1)
print("point diff")
print(np.absolute(circle[0] - l[2][0]))
print(np.absolute(circle[1] - l[2][1]))
#if (np.absolute(c) > 12 or np.absolute(circle[1]-l[2][1]) > 12): ## and (np.absolute(circle[0]-l[2][0]) < 100 or np.absolute(circle[1]-l[2][1]) < 100):
l.append([int(circle[0] + ulx), int(circle[1] + uly)])
l = l[1:]
#else:
# raise Exception("hej")
else:
print("else")
continue
adpoint = np.array(l[2], dtype='float32')
point = np.array([[l[2]]], dtype='float32')
pointOut = cv2.perspectiveTransform(point, transform)
[[[xo, yo]]] = pointOut
time2 = time.clock()
print('findcircle clocktime %0.6f' % (time2 - time1))
yield [xo, yo, im_with_keypoints, keypoint[0].size]
src_pts = np.float32([k1[m.queryIdx].pt for m in good]).reshape(-1, 1, 2)
dst_pts = np.float32([k2[m.trainIdx].pt for m in good]).reshape(-1, 1, 2)
M, mask = cv2.findHomography(dst_pts, src_pts, cv2.RANSAC, 5.0)
matchesMask = mask.ravel().tolist()
else:
print "not enough matches are found - %d%d" % (len(good), MIN_MATCH_COUNT)
matchesMask = None
rows, cols = img1.shape
pts2 = M
pts1 = np.float32([[0, 0], [0, rows - 1], [cols - 1, rows - 1],
[cols - 1, 0]]).reshape(-1, 1,
2) #where the points will be warped
d = cv2.perspectiveTransform(pts1, pts2)
#out = cv2.polylines(img2, [np.int32(d)], True,255,3)
warp = cv2.warpPerspective(img2, M,
(cols, rows)) #warp Image 2 to Image 1 coordinates
#visualizations
h1, w1 = img1.shape[:2]
h2, w2 = img2.shape[:2]
view = sp.zeros((max(h1, h2), w1 + w2, 3), sp.uint8)
view[:h1, :w1, 0] = img1
view[:h2, w1:, 0] = img2
view[:, :, 1] = view[:, :, 0]
view[:, :, 2] = view[:, :, 0]
cols1 = img1.shape[1]
def cam_image_cb(self, data):
try:
cv_image = self.bridge.compressed_imgmsg_to_cv2(
data, self.encoding)
#cv_image = self.bridge.imgmsg_to_cv2(data), self.encoding)
except CvBridgeError as e:
print(e)
return
# Create a copy of the image to publish
cv_image2 = cv_image.copy()
# Change the image to Grayscale
cv_image_gray = cv2.cvtColor(cv_image.copy(), cv2.COLOR_BGR2GRAY)
# find the keypoints with ORB
kp_Scene, des_Scene = self.orb.detectAndCompute(cv_image_gray, None)
# Match features
if (len(kp_Scene) > 0):
matches = self.matcher.match(self.des_Object, des_Scene)
matches_sorted = sorted(matches, key=lambda x: x.distance)
else:
return
# If number of matches is greater than threshold find the location of the object in the scene
if len(matches_sorted) > self.MIN_MATCH_COUNT:
src_pts = np.float32([
self.kp_Object[m.queryIdx].pt for m in matches_sorted
]).reshape(-1, 1, 2)
dst_pts = np.float32([
kp_Scene[m.trainIdx].pt for m in matches_sorted
]).reshape(-1, 1, 2)
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
# Corners of object
h, w = self.imgObject.shape[:2]
pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1],
[w - 1, 0]]).reshape(-1, 1, 2)
# Corners of detected object
dst = cv2.perspectiveTransform(pts, M).reshape(4, 2)
# Corners of GOAL image
h, w = self.imgGoal.shape[:2]
pts = np.array([[0, 0], [0, h - 1], [w - 1, h - 1], [w - 1, 0]],
dtype="float32")
# Get perspective transform
transf = cv2.getPerspectiveTransform(pts, dst)
h, w = cv_image2.shape[:2]
warp = cv2.warpPerspective(self.imgGoal, transf, (w, h))
cv_image2 = cv2.fillPoly(cv_image2,
[np.int32(dst).reshape((-1, 1, 2))],
(0, 0, 0)) # Remove object in scene
cv_image2 = cv2.add(cv_image2, warp) # Draw new image
#-- Draw Bounding box
#cv_image2 = cv2.polylines(cv_image2, [np.int32(dst).reshape((-1, 1, 2))] , True, (0,0,255),3, cv2.LINE_AA)
try:
#self.image_pub.publish(self.bridge.cv2_to_compressed_imgmsg(cv_image2))
self.image_pub.publish(self.bridge.cv2_to_imgmsg(
cv_image2, "bgr8"))
except CvBridgeError as e:
print(e)
def combine_images(img0, img1, h_matrix):
# print('拼接图像... ')
points0 = np.array(
[[0, 0], [0, img0.shape[0]], [img0.shape[1], img0.shape[0]], [img0.shape[1], 0]], dtype=np.float32)
points0 = points0.reshape((-1, 1, 2))
points1 = np.array(
[[0, 0], [0, img1.shape[0]], [img1.shape[1], img0.shape[0]], [img1.shape[1], 0]], dtype=np.float32)
points1 = points1.reshape((-1, 1, 2))
points2 = cv2.perspectiveTransform(points1, h_matrix)
points = np.concatenate((points0, points2), axis=0)
[x_min, y_min] = np.int32(points.min(axis=0).ravel() - 0.5)
[x_max, y_max] = np.int32(points.max(axis=0).ravel() + 0.5)
# print('points2:', points2[0][0][0])
# print('points2:', points2[1][0][0])
# 重叠区域的左边界,开始位置
start = min(points2[0][0][0], points2[1][0][0])
# 重叠区域的宽度
width = img0.shape[1] - start
# img1中像素的权重
alpha = 1
# print('start:',start)
# print('width:',width)
# print('img0[0]:', img0[0][0][0])
#('xmin:', x_min, 'xmax', x_max, 'ymin', y_min, 'ymax', y_max)
H_translation = np.array([[1, 0, -x_min], [0, 1, -y_min], [0, 0, 1]])
# print('单应性矩阵Homography:', h_matrix)
# print('WARP右图...')
# cv2.imshow('img0', img0)
# img0是左图
cv2.imshow('img1', img0)
# output_img暂时是经过变换之后的右图
output_img = cv2.warpPerspective(img1, H_translation.dot(h_matrix), (x_max - x_min, y_max - y_min),
borderMode=cv2.BORDER_TRANSPARENT)
#cv2.imwrite('img2.jpg', output_img)
cv2.imshow('warped', output_img)
H_img1 = output_img.copy()
# 拷贝左图的0到start之内的到右图上
H_img1[:img0.shape[0], :int(start)] = img0[:img0.shape[0], :int(start)]
output_size = output_img.shape
cv2.imshow('H_img1', H_img1)
# print(output_size)
x_offset = x_max - img1.shape[0]
# y_offset = y_max-img1.shape[1]
# 要将左图无缝地加入到变换过后的右图上
# force combine them together
output_img[-y_min:img0.shape[0] - y_min,
- x_min:img0.shape[1] - x_min] = img0
tmp_r = H_img1[0:y_max, x_max - x_offset:x_max]
tmp_l = output_img[0:y_max, 0:img0.shape[1]]
#cv2.imwrite('img1.jpg', output_img)
# cv2.waitKey(1999)
# output_image = Laplacian_blending(H_img1.copy(), output_img.copy())
# cv2.imshow('blend', output_image)
# 待融合ROI[start:start+width, 0: img0.shape[1]]
# for x in H_img1[0:img0.shape[1],int(start):int(start)+int(width)]:
# for i in x:
#cv2.imshow(u'Left Img', tmp_r)
#cv2.imshow(u'Right Img', tmp_l)
# result = blender.multi_band_blending(tmp_l, tmp_r, overlap_w)
# cv2.imshow('output_stage_1', result)
'''
start to blend
'''
return output_img
def computeMatrix():
"""Computes the homography on the image using the selected points and
transforms the image
Returns:
- the 3x3 transformation matrix
"""
# grab reference to the global variables
global refPts
global image
# points should be in order of bottom right, top right, top left, bottom left
# x point first, then y point
quadrilateral = np.array(refPts, dtype=np.float32)
br, tr, tl, bl = quadrilateral
# width of rectangle to transform to is max distance between either top left
# and top right coordinates, or bottom left and bottom right coordinates
widthA = np.sqrt(((br[0] - bl[0])**2) + ((br[1] - bl[1])**2))
widthB = np.sqrt(((tr[0] - tl[0])**2) + ((tr[1] - tl[1])**2))
maxWidth = max(int(widthA), int(widthB))
# height of rectangle to transform to is max distance between either top left
# and bottom left, or top right and bottom right
heightA = np.sqrt(((tr[0] - br[0])**2) + ((tr[1] - br[1])**2))
heightB = np.sqrt(((tl[0] - bl[0])**2) + ((tl[1] - bl[1])**2))
maxHeight = max(int(heightA), int(heightB))
# rectangle to transform to, the corresponding points should be in the same
# order as the passed in points
dst = np.array([[maxWidth - 1, maxHeight - 1], [maxWidth - 1, 0], [0, 0],
[0, maxHeight - 1]],
dtype=np.float32)
# Compute the perspective transform matrix
homography = cv2.getPerspectiveTransform(quadrilateral, dst)
# Find where the corners go after the transformation
img_height, img_width = image.shape[:2]
corners = np.array([[0, 0], [0, img_height - 1],
[img_width - 1, img_height - 1], [img_width - 1, 0]],
dtype=np.float32)
# transformed corners
t_corners = cv2.perspectiveTransform(np.float32([corners]), homography)[0]
# Find the bounding rectangle around the warped image
bx, by, bwidth, bheight = cv2.boundingRect(t_corners)
# Compute new homography that makes the bounding box around the warped image
# start at (0, 0)
pure_translation = np.array([[1, 0, -bx], [0, 1, -by], [0, 0, 1]])
# if A and B are homographies, A*B represents the homography that applies
# B first, then A
M = np.matmul(pure_translation, homography)
# Get the warped image
warped = cv2.warpPerspective(image, M, (bwidth, bheight))
# Resize the warped image to be same height as original so we can display
# them side by side
warped = imutils.resize(warped, height=img_height)
return M, warped
def orientation_detect(img, contours, H, rho=8.0, ntheta=512):
# ignore this, just deal with edge-detected text
pts = np.vstack(tuple(contours))
shape, TH = warp_containing_points(img, pts, H, shape_only=True)
text_edges = np.zeros(shape, dtype=np.uint8)
for contour in contours:
contour = cv2.perspectiveTransform(contour.astype(np.float32), TH)
cv2.drawContours(text_edges, [contour.astype(int)], 0, (255,255,255))
debug_show('edges', text_edges)
# generate a linspace of thetas
thetas = np.linspace(-0.5*np.pi, 0.5*np.pi, ntheta, endpoint=False)
# rho is pixels per r bin in polar (theta, r) histogram
# irho is bins per pixel
irho = 1.0/rho
# get height and width
h, w = text_edges.shape
# maximum bin index is given by hypotenuse of (w, h) divided by pixels per bin
bin_max = int(np.ceil(np.hypot(w, h)*irho))
# initialize zeroed histogram height bin_max and width num theta
hist = np.zeros((bin_max, ntheta))
# let u and v be x and y coordinates (respectively) of non-zero
# pixels in edge map
v, u = np.mgrid[0:h, 0:w]
v = v[text_edges.view(bool)]
u = u[text_edges.view(bool)]
# get center coordinates
u0 = w*0.5
v0 = h*0.5
# for each i and theta = thetas[i]
for i, theta in enumerate(thetas):
# for each nonzero edge pixel, compute bin in r direction from
# pixel location and cos/sin of theta
bin_idx = ( (-(u-u0)*np.sin(theta) # x term
+ (v-v0)*np.cos(theta))*irho # y term, both
# divided by pixels
# per bin
+ 0.5*bin_max ) # offset for center pixel
assert( bin_idx.min() >= 0 and bin_idx.max() < bin_max )
# 0.5 is for correct rounding here
#
# e.g. np.bincount([1, 1, 0, 3]) = [1, 2, 0, 1]
# returns count of each integer in the array
bc = np.bincount((bin_idx + 0.5).astype(int))
# push this into the histogram
hist[:len(bc),i] = bc
# number of zero pixels in each column
num_zero = (hist == 0).sum(axis=0)
# find the maximum number of zero pixels
best_theta_idx = num_zero.argmax()
# actual detected theta - could just return this now
theta = thetas[best_theta_idx]
# compose with previous homography
RH = np.dot(rotation(-theta), H)
if 1: # just debug visualization
debug_hist = (255*hist/hist.max()).astype('uint8')
debug_hist = cv2.cvtColor(debug_hist, cv2.COLOR_GRAY2RGB)
cv2.line(debug_hist,
(best_theta_idx, 0),
(best_theta_idx, bin_max), (255,0,0),
1, cv2.LINE_AA)
debug_show('histogram', debug_hist)
p0 = np.array((u0, v0))
t = np.array((np.cos(theta), np.sin(theta)))
warped = cv2.warpPerspective(img, TH, (shape[1], shape[0]),
borderMode=cv2.BORDER_REPLICATE)
debug_show('prerotate_noline', warped)
cv2.line(warped,
tuple(map(int, p0 - rho*bin_max*t)),
tuple(map(int, p0 + rho*bin_max*t)),
(255, 0, 0),
6, cv2.LINE_AA)
debug_show('prerotate', warped)
warped, _ = warp_containing_points(img, pts, RH)
debug_show('preskew', warped)
return RH
def get_template(img2):
global ROI, MIN_MATCH_COUNT, font, pts, kp1, des1, flann
# Find the keypoints and descriptors with SIFT
kp2, des2 = sift.detectAndCompute(img2, None)
print("Keypoints in 1st Image: " + str(len(kp1)))
print("Keypoints in 2nd Image: " + str(len(kp2)))
matches = flann.knnMatch(des1, des2, k=2)
# Store all good matches as per Lowe's ratio test
good = []
for m, n in matches:
if m.distance < 0.7 * n.distance:
good.append(m)
number_keypoints = 0
if (len(kp1) <= len(kp2)):
number_keypoints = len(kp1)
else:
number_keypoints = len(kp2)
number_goodpoints = len(good)
print("Good matches found: " + str(number_goodpoints))
similariy_percentage = float(number_goodpoints) / number_keypoints * 100
print(similariy_percentage)
res = 0
if len(good) > MIN_MATCH_COUNT:
src_pts = np.float32([kp1[m.queryIdx].pt
for m in good]).reshape(-1, 1, 2)
dst_pts = np.float32([kp2[m.trainIdx].pt
for m in good]).reshape(-1, 1, 2)
M, mask = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
matchesMask = mask.ravel().tolist()
dst = cv2.perspectiveTransform(pts, M)
x = []
y = []
(rows, columns) = (dst.shape[0], dst.shape[1])
for i in range(rows):
array = dst[i][0]
x.append(array[0])
y.append(array[1])
if (initROI == True):
for i in range(rows):
res = res + 1 if (compareCoor(ROI[1], x[i], y[i])
== True) else res + 0
# if(res == 4):
# print("Label is correct")
# cv2.putText(img2, 'Label is pasted correct', (0, 50), font, 1, (0,255,0), 2)
# cv2.putText(img2, 'Input sample: {:.0f} %'.format(similariy_percentage), (0, 100), font, 1, (0,255,0), 2)
# count += 1
# else:
# print("Label is pasted wrong")
# cv2.putText(img2, 'Label is pasted wrong', (0, 50), font, 1, (0,255,0), 2)
# cv2.putText(img2, 'Input sample: {:.0f} %'.format(similariy_percentage), (0, 100), font, 1, (0,255,0), 2)
else:
print("There is no ROI reference to check")
cv2.putText(img2, 'No ROI found', (0, 50), font, 1, (0, 255, 0), 2)
img2 = cv2.polylines(img2, [np.int32(dst)], True, (0, 255, 0), 3,
cv2.LINE_AA)
else:
res = -1
cv2.putText(img2, 'No label found', (0, 50), font, 1, (0, 255, 0), 2)
print("Not enough matches are found - %d/%d" %
(len(good), MIN_MATCH_COUNT))
matchesMask = None
return res