def imagen_homografia(n):
global points, width, height,teclado
urllib.urlretrieve("http://192.168.0.100:8080/photo.jpg", "foto.jpg")
foto=cv.LoadImage('foto.jpg')
im_in= cv.CloneImage(foto)
#CALIBRACION
width, height = cv.GetSize(im_in)
im = cv.CloneImage(foto)
if n == 0 and (teclado ==1 or teclado ==3):
cv.ShowImage('Escoger 4 puntos',im)
cv.SetMouseCallback('Escoger 4 puntos', mouse, im)
cv.WaitKey(0)
guardarpoints(points)
h**o = homografia(im_in.width, im_in.height, im.width, im.height)
cv.Save('homography.cvmat', h**o)
else:
points = cargarpoints()
h**o = homografia(im_in.width, im_in.height, im.width, im.height)
out_big = cv.CloneImage(im_in)
cv.WarpPerspective(im_in, out_big, h**o)
out_small = cv.CloneImage(im)
cv.Resize(out_big, out_small)
return out_small, out_big
def imtransform2(I, H0, fillval=3.0):
# transform image using center as origin
if len(I.shape) == 3:
Iout = np.copy(I)
Iout[:, :, 0] = imtransform2(I[:, :, 0], H0, fillval=fillval)
Iout[:, :, 1] = imtransform2(I[:, :, 1], H0, fillval=fillval)
Iout[:, :, 2] = imtransform2(I[:, :, 2], H0, fillval=fillval)
return Iout
else:
T0 = np.eye(3)
T0[0, 2] = I.shape[1] / 2.0
T0[1, 2] = I.shape[0] / 2.0
T1 = np.eye(3)
T1[0, 2] = -I.shape[1] / 2.0
T1[1, 2] = -I.shape[0] / 2.0
H = np.dot(np.dot(T0, H0), T1)
# transform each channel separately
Icv = cv.fromarray(np.copy(I))
I1cv = cv.CreateMat(I.shape[0], I.shape[1], Icv.type)
cv.WarpPerspective(Icv, I1cv, cv.fromarray(np.copy(H)), fillval=1.0)
I1 = np.asarray(I1cv)
I1[np.nonzero(I1 < 0)] = fillval
return I1
def GetDart():
global initialized
if not initialized:
## need to call this function first before you can use GetDart()!!!
InitGetDart()
raw_dart_location = GetRawDartXY()
correct_dart_location = dartRegion.DartLocation(raw_dart_location)
## show the dart throw in a seperate window
new_image = cv.CloneImage(calibration.image)
cv.WarpPerspective(calibration.image, new_image, calibration.mapping)
cv.Circle(new_image, correct_dart_location, 3, cv.CV_RGB(255, 0, 0), 2, 8)
cv.ShowImage("new dart location", new_image)
dartThrowInfo = dartRegion.DartRegion(correct_dart_location)
##HACK!!!!! NORMALIZE dartThrowInfo
##Bryan's calibration code uses degrees; JP's UI uses radians
##(both have the centre line bisecting the base 6 region as '0'
##JP's UI scales magnitude to 380, where 380 is the outside of the triple ring
##so, we need to convert to radians, and we need to scale the magnitude by the factor 380/ring_radius[5]
##ring_radious[5] is the raw magnitude of the outside of the double ring
dartThrowInfo.angle = radians(dartThrowInfo.angle)
dartThrowInfo.magnitude = dartThrowInfo.magnitude * (
380. / calibration.ring_radius[5])
return dartThrowInfo
def dewarp(image, window, clicked_corners):
debug = cv.CloneImage(image)
# draw red line around edges for debug purposes
cv.PolyLine(debug, [[
clicked_corners[0], clicked_corners[1], clicked_corners[3],
clicked_corners[2]
]], True, cv.RGB(0, 255, 0), 7)
cv.ShowImage(window, debug)
cv.WaitKey()
# Assemble a rotated rectangle out of that info
#rot_box = cv.MinAreaRect2(corners)
enc_box = cv.BoundingRect(clicked_corners)
new_corners = [(0, 0), (enc_box[2] - 1, 0), (0, enc_box[3] - 1),
(enc_box[2] - 1, enc_box[3] - 1)]
warp_mat = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.GetPerspectiveTransform(clicked_corners, new_corners, warp_mat)
rotated = cv.CloneImage(image)
cv.WarpPerspective(image, rotated, warp_mat)
cv.ShowImage(window, rotated)
cv.WaitKey(10)
return rotated
def get_card(color_capture, corners):
target = [(0, 0), (223, 0), (223, 310), (0, 310)]
mat = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.GetPerspectiveTransform(corners, target, mat)
warped = cv.CloneImage(color_capture)
cv.WarpPerspective(color_capture, warped, mat)
cv.SetImageROI(warped, (0, 0, 223, 310))
return warped
def update_transform(self):
map_mat = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.GetPerspectiveTransform(map(tuple, self.points[0].points.values),
map(tuple, self.points[1].points.values),
map_mat)
flags = cv.CV_WARP_FILL_OUTLIERS
cv.WarpPerspective(self.im_in, self.im_out, map_mat, flags=flags)
imshow(self.im_out, axis=self.axes[1], show_axis=True)
self.refresh()
def transformImage(self,im):
''' Transform an image. '''
if isinstance(self.matrix,cv.cvmat):
matrix = self.matrix
else:
matrix = pv.NumpyToOpenCV(self.matrix)
src = im.asOpenCV()
dst = cv.CreateImage( (self.size[0],self.size[1]), cv.IPL_DEPTH_8U, src.nChannels );
cv.WarpPerspective( src, dst, matrix)
return pv.Image(dst)
def capture(self, transform=None):
query_img = cv.QueryFrame(self.cap)
camera_img = cv.CreateMat(CAMERA_SIZE[1], CAMERA_SIZE[0], cv.CV_8UC3)
cv.CvtColor(query_img, camera_img, cv.CV_BGR2RGB)
if transform:
transform_img = cv.CreateMat(CAMERA_SIZE[1], CAMERA_SIZE[0],
cv.CV_8UC3)
cv.WarpPerspective(camera_img, transform_img, transform)
img = pygame.image.fromstring(transform_img.tostring(),
CAMERA_SIZE, 'RGB')
else:
img = pygame.image.fromstring(camera_img.tostring(), CAMERA_SIZE,
'RGB')
return img
def main(args):
if len(args) != 2:
print "birdseye_maker.py [H_location.yaml] [birdseye_directory]"
return
H_loc = args[0]
directory = args[1]
test_files_all = os.listdir(".")
test_files = sorted([f for f in test_files_all if re.match(".*png", f)])
H = cv.Load(H_loc)
for f in test_files:
img = cv.LoadImage(f)
birds_image = cv.CloneImage(img)
cv.WarpPerspective(
img, birds_image, H, cv.CV_INTER_LINEAR + cv.CV_WARP_INVERSE_MAP +
cv.CV_WARP_FILL_OUTLIERS)
newname = f.replace("_frame_", "_frame_birdseye_")
print newname
cv.SaveImage("%s/%s" % (directory, newname), birds_image)
def DartLocation(raw_dart_loc):
try:
if calibration.calibrationComplete:
print "Raw dart location:"
print raw_dart_loc
# temp_raw_loc = cv.CreateMat(3, 3, cv.CV_32FC1)
# cv.mSet(temp_raw_loc, 0, 0, float(raw_dart_loc[0]))
# cv.mSet(temp_raw_loc, 0, 1, float(raw_dart_loc[1]))
# cv.mSet(temp_raw_loc, 0, 2, 1.0)
#
# temp_new_loc = cv.CreateMat(3, 3, cv.CV_32FC1)
#
# cv.Mul(calibration.mapping, temp_raw_loc, temp_new_loc, 1)
#
# new_dart_loc = []
# new_dart_loc = (int( cv.mGet(temp_new_loc, 0, 0)/cv.mGet(temp_new_loc, 0, 2) ), int( cv.mGet(temp_new_loc, 0, 1)/cv.mGet(temp_new_loc, 0, 2) ))
raw_loc_mat = cv.CreateMat(calibration.image.height, calibration.image.width, cv.CV_32FC1)
new_loc_mat = cv.CreateMat(calibration.image.height, calibration.image.width, cv.CV_32FC1)
#y comes before x
cv.mSet( raw_loc_mat, raw_dart_loc[1], raw_dart_loc[0], 1.0 )
cv.WarpPerspective(raw_loc_mat, new_loc_mat, calibration.mapping)
for i in range (0, calibration.image.width):
for j in range(0, calibration.image.height):
if not (cv.mGet(new_loc_mat, j, i) == 0.0):
new_dart_loc = (i, j)
break
print "New dart location:"
print new_dart_loc
return new_dart_loc
#system not calibrated
except AttributeError as err1:
print err1
return (-1, -1)
except NameError as err2:
#not calibrated error
print err2
return (-2, -2)
def NormalizeImage(cvmat, cilp_rect, perspective_points):
u'''読み取りやすくするために画像を正規化する'''
# 液晶部分の抽出
lcd = cv.CreateMat(cilp_rect.height, cilp_rect.width, cv.CV_8UC3)
cv.GetRectSubPix(cvmat, lcd, (cilp_rect.cx, cilp_rect.cy))
# グレイスケール化
grayed = cv.CreateMat(lcd.height, lcd.width, cv.CV_8UC1)
cv.CvtColor(lcd, grayed, cv.CV_BGR2GRAY)
# 適応的2値化
filterd = cv.CreateMat(grayed.height, grayed.width, cv.CV_8UC1)
cv.AdaptiveThreshold(
grayed,
filterd,
255,
adaptive_method=cv.CV_ADAPTIVE_THRESH_GAUSSIAN_C,
thresholdType=cv.CV_THRESH_BINARY,
blockSize=15,
)
# ゆがみ補正
transformed = cv.CreateMat(grayed.height, grayed.width, filterd.type)
matrix = cv.CreateMat(3, 3, cv.CV_32F)
cv.GetPerspectiveTransform(
((perspective_points.tl.x, perspective_points.tl.y),
(perspective_points.tr.x, perspective_points.tr.y),
(perspective_points.bl.x, perspective_points.bl.y),
(perspective_points.br.x, perspective_points.br.y)),
((0, 0), (filterd.width, 0), (0, filterd.height),
(filterd.width, filterd.height)), matrix)
cv.WarpPerspective(
filterd,
transformed,
matrix,
flags=cv.CV_WARP_FILL_OUTLIERS,
fillval=255,
)
return transformed
def process_tracking_pts(self, tracking_pts_dict):
"""
Determines whether the object has been found in any of the three point
trackers for the region. If is has been found than best tracking point
data is selected in a winner takes all fashion. The best tracking point
data is that which is nearest to the center of the image on camera upon
which is was captured.
"""
# Get time stamp (secs, nsecs) - always from the same camera to avoid jumps due to
# possible system clock differences.
time_camera = self.camera_list[0]
time_camera_secs = tracking_pts_dict[time_camera].data.secs
time_camera_nsecs = tracking_pts_dict[time_camera].data.nsecs
# Get list of messages in which the object was found
found_list = [
msg for msg in tracking_pts_dict.values() if msg.data.found
]
tracking_pts_msg = ThreePointTracker()
if found_list:
# Object found - select object with largest ROI or if the ROIs are of equal size
# select the object whose distance to center of the # image is the smallest
found_list.sort(cmp=tracking_pts_sort_cmp)
best = found_list[0]
camera = best.data.camera
# Get coordintates of points in tracking and stitching planes
best_points_array = numpy.array([(p.x, p.y)
for p in best.data.points])
pts_anchor_plane = self.tf2d.camera_pts_to_anchor_plane(
camera, best_points_array)
pts_stitching_plane = self.tf2d.camera_pts_to_stitching_plane(
camera, best_points_array)
pts_anchor_plane = [
Point2d(p[0], p[1]) for p in list(pts_anchor_plane)
]
pts_stitching_plane = [
Point2d(p[0], p[1]) for p in list(pts_stitching_plane)
]
# Get orientation angle and mid point of object in anchor and stitching planes
angle = get_angle(pts_anchor_plane)
midpt_anchor_plane = get_midpoint(pts_anchor_plane)
midpt_stitching_plane = get_midpoint(pts_stitching_plane)
#self.camera_fail = max([(err,cam) for cam, err in self.max_error_by_camera.items()])[1]
# Get size of tracking points image in the anchor (tracking) plane
roi = best.data.roi
x0, x1 = roi[0], roi[0] + roi[2]
y0, y1 = roi[1], roi[1] + roi[3]
bndry_camera = [(x0, y0), (x1, y0), (x1, y1), (x0, y1)]
bndry_camera_array = numpy.array(bndry_camera)
bndry_anchor = self.tf2d.camera_pts_to_anchor_plane(
camera, bndry_camera_array)
bndry_stitching = self.tf2d.camera_pts_to_stitching_plane(
camera, bndry_camera_array)
bndry_anchor = [tuple(x) for x in list(bndry_anchor)]
bndry_stitching = [tuple(x) for x in list(bndry_stitching)]
dx1 = abs(bndry_anchor[1][0] - bndry_anchor[0][0])
dx2 = abs(bndry_anchor[3][0] - bndry_anchor[2][0])
dy1 = abs(bndry_anchor[2][1] - bndry_anchor[1][1])
dy2 = abs(bndry_anchor[3][1] - bndry_anchor[0][1])
dx_max = max([dx1, dx2])
dy_max = max([dy1, dy2])
dim_max = max([dx_max, dy_max])
# Convert tracking points image to opencv image.
image_tracking_pts = self.bridge.imgmsg_to_cv(
best.image, desired_encoding="passthrough")
image_tracking_pts = cv.GetImage(image_tracking_pts)
image_size = cv.GetSize(image_tracking_pts)
image_dim_max = max(image_size)
# Get matrix for homography from camera to anchor plane
tf_matrix = self.tf2d.get_camera_to_anchor_plane_tf(camera)
# Shift for local ROI
tf_shift = numpy.array([
[1.0, 0.0, roi[0]],
[0.0, 1.0, roi[1]],
[0.0, 0.0, 1.0],
])
tf_matrix = numpy.dot(tf_matrix, tf_shift)
# Get scaling factor
shift_x = -min([x for x, y in bndry_anchor])
shift_y = -min([y for x, y in bndry_anchor])
scale_factor = float(image_dim_max) / dim_max
# Scale and shift transform so that homography maps the tracking points
# sub-image into a image_size image starting at coord. 0,0
tf_shift_and_scale = numpy.array([
[scale_factor, 0.0, scale_factor * shift_x],
[0.0, scale_factor, scale_factor * shift_y],
[0.0, 0.0, 1.0],
])
tf_matrix = numpy.dot(tf_shift_and_scale, tf_matrix)
# Warp image using homography
image_tracking_pts_mod = cv.CreateImage(
image_size, image_tracking_pts.depth,
image_tracking_pts.channels)
cv.WarpPerspective(
image_tracking_pts,
image_tracking_pts_mod,
cv.fromarray(tf_matrix),
cv.CV_INTER_LINEAR | cv.CV_WARP_FILL_OUTLIERS,
)
# Add sequence number to image
message = '{0}'.format(best.data.seq)
cv.PutText(image_tracking_pts_mod, message, (2, 15),
self.cv_text_font, self.magenta)
self.image_tracking_pts = self.bridge.cv_to_imgmsg(
image_tracking_pts_mod, encoding="passthrough")
# Create tracking points message
tracking_pts_msg.seq = best.data.seq
tracking_pts_msg.secs = time_camera_secs
tracking_pts_msg.nsecs = time_camera_nsecs
tracking_pts_msg.camera = camera
tracking_pts_msg.found = True
tracking_pts_msg.angle = angle
tracking_pts_msg.angle_deg = (180.0 * angle) / math.pi
tracking_pts_msg.midpt_anchor_plane = midpt_anchor_plane
tracking_pts_msg.midpt_stitching_plane = midpt_stitching_plane
tracking_pts_msg.pts_anchor_plane = pts_anchor_plane
tracking_pts_msg.pts_stitching_plane = pts_stitching_plane
tracking_pts_msg.bndry_anchor_plane = [
Point2d(x, y) for x, y in bndry_anchor
]
tracking_pts_msg.bndry_stitching_plane = [
Point2d(x, y) for x, y in bndry_stitching
]
else:
sample = tracking_pts_dict.values()[0]
tracking_pts_msg.seq = sample.data.seq
tracking_pts_msg.secs = time_camera_secs
tracking_pts_msg.nsecs = time_camera_nsecs
tracking_pts_msg.camera = ''
tracking_pts_msg.found = False
# Create tracking info image
pil_info_image = PILImage.new('RGB', self.info_image_size,
(255, 255, 255))
draw = PILImageDraw.Draw(pil_info_image)
text_list = []
label = 'Found:'
value = '{0}'.format(tracking_pts_msg.found)
unit = ''
text_list.append((label, value, unit))
label = 'Angle:'
value = '{0:+3.1f}'.format(180.0 * tracking_pts_msg.angle / math.pi)
unit = 'deg'
text_list.append((label, value, unit))
label = 'Pos X:'
value = '{0:+1.3f}'.format(tracking_pts_msg.midpt_anchor_plane.x)
unit = 'm'
text_list.append((label, value, unit))
#self.camera_fail = max([(err,cam) for cam, err in self.max_error_by_camera.items()])[1]
label = 'Pos Y:'
value = '{0:+1.3f}'.format(tracking_pts_msg.midpt_anchor_plane.y)
unit = 'm'
text_list.append((label, value, unit))
for i, text in enumerate(text_list):
label, value, unit = text
draw.text((10, 10 + 20 * i), label, font=self.font, fill=(0, 0, 0))
draw.text((70, 10 + 20 * i), value, font=self.font, fill=(0, 0, 0))
draw.text((125, 10 + 20 * i), unit, font=self.font, fill=(0, 0, 0))
cv_info_image = cv.CreateImageHeader(pil_info_image.size,
cv.IPL_DEPTH_8U, 3)
cv.SetData(cv_info_image, pil_info_image.tostring())
# Convert to a rosimage and publish
info_rosimage = self.bridge.cv_to_imgmsg(cv_info_image, 'rgb8')
self.image_tracking_info_pub.publish(info_rosimage)
# Get sync signal
sync_rsp = self.rand_sync_srv(tracking_pts_msg.seq)
if sync_rsp.status:
tracking_pts_msg.sync_signal = sync_rsp.signal
else:
tracking_pts_msg.sync_signal = [0, 0, 0]
# Publish messages
self.tracking_pts_pub.publish(tracking_pts_msg)
if self.image_tracking_pts is not None:
self.image_tracking_pts_pub.publish(self.image_tracking_pts)
else:
zero_image_cv = cv.CreateImage(self.tracking_pts_roi_size,
cv.IPL_DEPTH_8U, 3)
cv.Zero(zero_image_cv)
zero_image_ros = self.bridge.cv_to_imgmsg(zero_image_cv,
encoding="passthrough")
self.image_tracking_pts_pub.publish(zero_image_ros)
def publish_stitched_image(self):
"""
Checks to see if the sequences to images buffer contains all required frames and if so
produces and publishes a stitched image.
"""
# Check to see if we have all images for a given sequence number
for seq, image_dict in sorted(self.seq_to_images.items()):
# If we have all images for the sequece - stitch into merged view
if len(image_dict) == len(self.camera_list):
t0 = rospy.Time.now()
for i, camera in enumerate(self.camera_list):
# Convert ros image to opencv image
ros_image = image_dict[camera]
cv_image = self.bridge.imgmsg_to_cv(
ros_image,
desired_encoding="passthrough"
)
ipl_image = cv.GetImage(cv_image)
# Create stitced image if it doesn't exist yet
if self.stitched_image is None:
self.stitched_image = cv.CreateImage(
self.image_size,
ipl_image.depth,
ipl_image.channels
)
if i==0:
cv.Zero(self.stitched_image)
# Set image warping flags - if first fill rest of image with zeros
if self.is_first_write:
warp_flags = self.warp_flags | cv.CV_WARP_FILL_OUTLIERS
self.is_first_write = False
else:
warp_flags = self.warp_flags
# Warp into stitched image
cv.SetImageROI(self.stitched_image, self.tf_data[camera]['roi'])
# Warp stitched image
cv.WarpPerspective(
ipl_image,
self.stitched_image,
self.tf_data[camera]['matrix'],
flags=warp_flags,
)
cv.ResetImageROI(self.stitched_image)
# Get max and min time stamps for stamp_info
stamp = ros_image.header.stamp
if i == 0:
stamp_max = stamp
stamp_min = stamp
else:
stamp_max = stamp if stamp.to_sec() > stamp_max.to_sec() else stamp_max
stamp_mim = stamp if stamp.to_sec() < stamp_min.to_sec() else stamp_min
# Convert stitched image to ros image
stitched_ros_image = self.bridge.cv_to_imgmsg(
self.stitched_image,
encoding="passthrough"
)
stitched_ros_image.header.seq = seq
self.image_pub.publish(stitched_ros_image)
self.image_and_seq_pub.publish(seq, stitched_ros_image)
self.seq_pub.publish(seq)
# Compute time stamp spread
stamp_diff = stamp_max.to_sec() - stamp_min.to_sec()
self.stamp_pub.publish(stamp_max, stamp_min, stamp_diff)
t1 = rospy.Time.now()
dt = t1.to_sec() - t0.to_sec()
self.processing_dt_pub.publish(dt,1.0/dt)
# Remove data from buffer
del self.seq_to_images[seq]
# Throw away any stale data in seq to images buffer
seq_age = self.get_seq_age(seq)
if seq_age > self.max_seq_age:
try:
del self.seq_to_images[seq]
except KeyError:
pass
def featureimagecb(self, featuremsg):
# if not in GUI mode and no one is subscribing, don't do anything
if self.cvwindow is None and self.pub_objdet.get_num_connections(
) == 0:
rospy.logdebug(
'PointPoseExtraction.py no connections, so ignoring image')
return
self.featuremsg = featuremsg
with self.extractionlck:
try:
cv_image = self.bridge.imgmsg_to_cv(featuremsg.image, "bgr8")
except CvBridgeError as e:
print(e)
return
# decompose P=KK*Tcamera
P = reshape(array(featuremsg.info.P), (3, 4))
cvKK = cv.fromarray(eye(3))
cvRcamera = cv.fromarray(eye(3))
cvTcamera = cv.fromarray(zeros((4, 1)))
cv.DecomposeProjectionMatrix(cv.fromarray(P), cvKK, cvRcamera,
cvTcamera)
KK = array(cvKK)
Tcamera = eye(4)
Tcamera[0:3, :] = c_[array(cvRcamera),
-dot(array(cvRcamera),
array(cvTcamera)[0:3] / cvTcamera[3, 0])]
print("Tcamera: ", Tcamera)
if self.pe is None:
if len(self.imagepoints) >= 4:
xaxis = self.imagepoints[1] - self.imagepoints[0]
yaxis = self.imagepoints[2] - self.imagepoints[1]
if xaxis[0] * yaxis[1] - xaxis[1] * yaxis[0] < 0:
print(
'point order is not correct! need to specify points in clockwise order'
)
self.imagepoints = []
return
# find the homography, warp the image, and get new features
width = int(
sqrt(
max(
sum((self.imagepoints[1] -
self.imagepoints[0])**2),
sum((self.imagepoints[3] -
self.imagepoints[2])**2))))
height = int(
sqrt(
max(
sum((self.imagepoints[2] -
self.imagepoints[1])**2),
sum((self.imagepoints[3] -
self.imagepoints[0])**2))))
cvimagepoints = cv.fromarray(array(self.imagepoints))
cvtexturepoints = cv.fromarray(
array(
((0, 0), (width, 0), (width, height), (0, height)),
float))
cv_texture = cv.CreateMat(height, width, cv_image.type)
cv_texture2 = cv.CreateMat(height / 2, width / 2,
cv_image.type)
cvH = cv.fromarray(eye(3))
cv.FindHomography(cvimagepoints, cvtexturepoints, cvH, 0)
cv.WarpPerspective(cv_image, cv_texture, cvH)
try:
res = rospy.ServiceProxy(
'Feature0DDetect',
posedetection_msgs.srv.Feature0DDetect)(
image=self.bridge.cv_to_imgmsg(cv_texture))
N = len(res.features.positions) / 2
positions = reshape(res.features.positions, (N, 2))
descriptors = reshape(res.features.descriptors,
(N, res.features.descriptor_dim))
texturedims_s = raw_input(
'creating template ' + self.templateppfilename +
' enter the texture dimensions (2 values): ')
texturedims = [float(f) for f in texturedims_s.split()]
points3d = c_[positions[:, 0] * texturedims[0] / width,
positions[:, 1] * texturedims[1] /
height,
zeros(len(positions))]
self.Itemplate = array(cv_texture)
self.Itemplate[:, :, (0, 2)] = self.Itemplate[:, :,
(2, 0)]
self.boundingbox = transformPoints(
linalg.inv(self.Tright),
array(((0, 0, 0), (texturedims[0], texturedims[1],
0))))
template = {
'type': self.type,
'points3d': points3d,
'descriptors': descriptors,
'Itemplate': self.Itemplate,
'boundingbox': self.boundingbox,
'Tright': self.Tright,
'featuretype': featuremsg.features.type
}
pickle.dump(template, open(self.templateppfilename,
'w'))
scipy.misc.pilutil.imshow(self.Itemplate)
self.pe = PointPoseExtractor(type=self.type,
points3d=points3d,
descriptors=descriptors)
self.imagepoints = []
except rospy.service.ServiceException as e:
print(e)
return
else:
success = False
projerror = -1.0
try:
if self.featuretype is not None and self.featuretype != featuremsg.features.type:
rospy.logwarn(
'feature types do not match: %s!=%s' %
(self.featuretype, featuremsg.features.type))
raise detection_error()
starttime = time.time()
N = len(featuremsg.features.positions) / 2
positions = reshape(featuremsg.features.positions, (N, 2))
descriptors = reshape(
featuremsg.features.descriptors,
(N, featuremsg.features.descriptor_dim))
Tlocal, projerror = self.pe.extractpose(
KK,
positions,
descriptors,
featuremsg.features.confidences,
cv_image.width,
verboselevel=self.verboselevel,
neighthresh=self.neighthresh,
thresh=self.thresh,
dminexpected=self.dminexpected,
ransaciters=self.ransaciters)
success = projerror < self.errorthresh
if success: # publish if error is low enough
Tlocalobject = dot(Tlocal, self.Tright)
Tglobal = dot(linalg.inv(Tcamera), Tlocalobject)
poseglobal = poseFromMatrix(Tglobal)
pose = posedetection_msgs.msg.Object6DPose()
pose.type = self.type
pose.pose.orientation.w = poseglobal[0]
pose.pose.orientation.x = poseglobal[1]
pose.pose.orientation.y = poseglobal[2]
pose.pose.orientation.z = poseglobal[3]
pose.pose.position.x = poseglobal[4]
pose.pose.position.y = poseglobal[5]
pose.pose.position.z = poseglobal[6]
objdetmsg = posedetection_msgs.msg.ObjectDetection()
objdetmsg.objects = [pose]
objdetmsg.header = featuremsg.image.header
print('local texture: ', repr(Tlocal))
self.pub_objdet.publish(objdetmsg)
self.drawpart(cv_image, Tlocalobject, KK)
self.drawcoordsys(cv_image, Tlocalobject, KK)
except detection_error as e:
pass
if self.verboselevel > 0:
rospy.loginfo(
'%s: %s, detection time: %fs, projerror %f < %f' %
(self.type, 'success' if success else 'failure',
time.time() - starttime, projerror, self.errorthresh))
if self.cvwindow is not None:
if not self.cvwindowstarted:
cv.NamedWindow(self.cvwindow, cv.CV_WINDOW_AUTOSIZE)
cv.SetMouseCallback(self.cvwindow, self.cvmousecb)
self.cvwindowwstarted = True
for x, y in self.imagepoints:
cv.Circle(cv_image, (int(x), int(y)), 2, (0, 0, 255), 2)
cv.ShowImage(self.cvwindow, cv_image)
char = cv.WaitKey(20)
def process(self,cv_image,info,image2=None):
self.load_model(self.modelpath)
if self.transform:
H = cv.Load(self.matrix_location)
input_image = cv.CloneImage(cv_image)
cv.WarpPerspective(input_image,cv_image,H,
cv.CV_INTER_LINEAR+cv.CV_WARP_INVERSE_MAP+cv.CV_WARP_FILL_OUTLIERS)
#Use the thresholding module to get the contour out
shape_contour = thresholding.get_contour(cv_image,bg_mode=self.bg_mode,filter_pr2=self.filter_pr2
,crop_rect=(self.cropx,self.cropy,self.cropwidth,self.cropheight),cam_info=info,listener=self.listener)
#Use the shape_fitting module to fit the model to the contour
if self.mode=="tee":
#fitter = shape_fitting.ShapeFitter(SYMM_OPT=True,ORIENT_OPT=False,FINE_TUNE=False)
fitter = shape_fitting.ShapeFitter(SYMM_OPT=False,ORIENT_OPT=True,FINE_TUNE=False,num_iters=self.num_iters)
elif self.mode=="sweater":
fitter = shape_fitting.ShapeFitter(SYMM_OPT=False,ORIENT_OPT=False,FINE_TUNE=False,num_iters=self.num_iters)
elif self.mode=="folded":
fitter = shape_fitting.ShapeFitter(SYMM_OPT=False,ORIENT_OPT=False,FINE_TUNE=False,INITIALIZE=False,num_iters=self.num_iters)
else:
fitter = shape_fitting.ShapeFitter(SYMM_OPT=False,ORIENT_OPT=False,FINE_TUNE=False,num_iters=self.num_iters)
image_anno = cv.CloneImage(cv_image)
(nearest_pts, final_model, fitted_model) = fitter.fit(self.model,shape_contour,image_anno)
pts = nearest_pts
params = {}
if self.mode == "triangles":
return_pts = [pts[1],pts[4],pts[2],pts[3]]
self.highlight_pt(pts[1],cv.CV_RGB(255,0,0),image_anno)
font = cv.InitFont(cv.CV_FONT_HERSHEY_COMPLEX,1.0,1.0)
cv.PutText(image_anno,"(l)eft",(pts[1][0]-20,pts[1][1]-15),font,cv.CV_RGB(255,0,0))
self.highlight_pt(pts[4],cv.CV_RGB(0,0,255),image_anno)
cv.PutText(image_anno,"(r)ight",(pts[4][0]-20,pts[4][1]-15),font,cv.CV_RGB(0,0,255))
params = {"tilt":0.0}
elif self.mode == "towel":
return_pts = pts
elif self.mode == "tee" or self.mode == "sweater":
return_pts = pts[0:5]+pts[8:]
params = {}
elif self.mode == "folded":
return_pts = [final_model.fold_bottom(),final_model.fold_top()]
else:
return_pts = pts
params = {}
if self.transform:
H_inv = cv.CloneMat(H)
cv.Invert(H,H_inv)
anno_unchanged = cv.CloneImage(image_anno)
cv.WarpPerspective(anno_unchanged,image_anno,H_inv,
cv.CV_INTER_LINEAR+cv.CV_WARP_INVERSE_MAP+cv.CV_WARP_FILL_OUTLIERS)
new_return_pts = self.transform_pts(return_pts,H_inv)
for i,pt in enumerate(new_return_pts):
return_pts[i] = pt
if self.mode != "triangles":
for pt in return_pts:
self.highlight_pt(pt,cv.CV_RGB(255,0,0),image_anno)
if self.mode != "folded":
fitted_model.set_image(None)
pickle.dump(fitted_model,open("%s/last_model.pickle"%self.save_dir,'w'))
else:
model_pts = final_model.vertices_full()
new_model = Models.Point_Model_Contour_Only_Asymm(*model_pts)
pickle.dump(new_model,open("%s/last_model.pickle"%self.save_dir,'w'))
score = final_model.score(shape_contour) #fitter.energy_fxn(final_model,shape_contour)
params["score"] = score
return (return_pts,params,image_anno)
points = [(20., 30.), (30., 150.), (160., 170.), (200., 20.)]
npoints = [(0., 0.), (640., 0), (640., 480.), (640., 480.)]
cv.GetPerspectiveTransform(points, npoints, mat)
#cv.CvtColor( img, gray, cv.CV_RGB2GRAY );
src = cv.CreateImage(cv.GetSize(img), cv.IPL_DEPTH_32F, 3)
#fimg = cv.CreateImage( cv.GetSize(img), cv.IPL_DEPTH_8U, 3 )
cv.ConvertScale(img, src, (1 / 255.00))
#cv.ConvertScale(gray,src,(1/255.00))
dst = cv.CloneImage(src)
cv.Zero(dst)
cv.WarpPerspective(src, dst, mat)
while 1:
cv.ShowImage("original", img)
cv.ShowImage("src", src)
cv.ShowImage("dst", dst)
if cv.WaitKey() == 27:
break
#import optparse
#
#
#
#def main( files ):
# print "main"
# while True:
def Calibration():
global image
if from_video:
if from_camera:
capture = cv.CaptureFromCAM(0)
else:
capture = cv.CaptureFromFile(videofile)
## image = 0
cv.SetCaptureProperty(capture, cv.CV_CAP_PROP_FRAME_WIDTH, 640)
cv.SetCaptureProperty(capture, cv.CV_CAP_PROP_FRAME_HEIGHT, 480)
if from_camera:
##we wait for 40 frames...the picture at the very start is always kind of weird,
##wait for the picture to stabalize
for n in range(40):
## RetrieveFrame is just GrabFrame and RetrieveFrame combined into 1
## function call
image = cv.QueryFrame(capture)
## cv.GrabFrame(capture)
## image = cv.CloneImage(cv.RetrieveFrame(capture))
## need to clone this image...otherwise once the capture gets released we won't have access
## to the image anymore
image = cv.CloneImage(cv.QueryFrame(capture))
else:
## if we're capturing from a video file, then just take the first frame
image = cv.QueryFrame(capture)
else:
image = cv.LoadImage(str(r"dartboard_cam1.bmp"),
cv.CV_LOAD_IMAGE_COLOR)
#data we need for calibration:
#mapping for a perspective transform
global mapping
#center of the dartboard in x, y form
global center_dartboard
#initial angle of the 20 - 1 points divider
global ref_angle
#the radii of the rings, there are 6 of them
global ring_radius
global calibrationComplete
calibrationComplete = False
#for grabbing the user's clicks
global points
#either grab data from file or user
while calibrationComplete == False:
#Read calibration file, if exists
if os.path.isfile("calibrationData.pkl"):
try:
#for grabbing key presses in the python window showing the image
global keyPressEvent
keyPressEvent = Event()
global keyPress
#for synchronizing the image window thread
global windowReady
windowReady = Event()
#for synchronizing the drawing
global drawingFinished
drawingFinished = Event()
#start a fresh set of points
points = []
calFile = open('calibrationData.pkl', 'rb')
calData = CalibrationData()
calData = pickle.load(calFile)
#load the data into the global variables
points.append(calData.top)
points.append(calData.bottom)
points.append(calData.left)
points.append(calData.right) #index of 3
init_point_arr = calData.init_point_arr
center_dartboard = calData.center_dartboard
ref_angle = calData.ref_angle
ring_radius = []
ring_radius.append(calData.ring_radius[0])
ring_radius.append(calData.ring_radius[1])
ring_radius.append(calData.ring_radius[2])
ring_radius.append(calData.ring_radius[3])
ring_radius.append(calData.ring_radius[4])
ring_radius.append(
calData.ring_radius[5]) #append the 6 ring radii
#close the file once we are done reading the data
calFile.close()
#copy image for old calibration data
new_image = cv.CloneImage(image)
#have the image in another window and thread
t = Thread(target=CalibrationWindowThread2, args=(new_image, ))
t.start()
#wait for the image window to setup
windowReady.wait()
#now draw them out:
#*******************1. Transform image******************************
newtop = (round(new_image.height / 2),
round(new_image.height * 0.20))
newbottom = (round(new_image.height / 2),
round(new_image.height * 0.80))
#Note: the height is smaller than the width
newleft = (round(new_image.height * 0.20),
round(new_image.height / 2))
newright = (round(new_image.height * 0.80),
round(new_image.height / 2))
mapping = cv.CreateMat(3, 3, cv.CV_32FC1)
#get a fresh new image
new_image = cv.CloneImage(image)
cv.GetPerspectiveTransform(
[points[0], points[1], points[2], points[3]],
[newtop, newbottom, newleft, newright], mapping)
cv.WarpPerspective(image, new_image, mapping)
cv.ShowImage(prev_calibration_window, new_image)
#*******************************************************************
#********************2.Draw points dividers*************************
#find initial angle of the 20-1 divider
tempX_mat = cv.CreateMat(1, 1, cv.CV_32FC1)
#correct the point with respect to the center
cv.mSet(tempX_mat, 0, 0,
init_point_arr[0] - center_dartboard[0])
tempY_mat = cv.CreateMat(1, 1, cv.CV_32FC1)
#adjust the origin of y
cv.mSet(
tempY_mat, 0, 0, init_point_arr[1] -
(new_image.height - center_dartboard[1]))
init_mag_mat = cv.CreateMat(1, 1, cv.CV_32FC1)
init_angle_reversed_mat = cv.CreateMat(1, 1, cv.CV_32FC1)
#each point region is 360/12 = 18 degrees large
cv.CartToPolar(tempX_mat,
tempY_mat,
init_mag_mat,
init_angle_reversed_mat,
angleInDegrees=True)
#display dividers
current_point = (int(round(init_point_arr[0])),
int(round(init_point_arr[1])))
next_angle = cv.CreateMat(1, 1, cv.CV_32FC1)
cv.mSet(next_angle, 0, 0, 360 - ref_angle)
temp_angle = 360.0 - ref_angle
#draw point dividers counterclockwise, just like how angle is calculated, arctan(y/x)
for i in range(0, 20):
cv.Line(new_image, center_dartboard, current_point,
cv.CV_RGB(0, 0, 255), 1, 8)
#calculate the cartesian coordinate of the next point divider
temp_angle = 360.0 - temp_angle
temp_angle += 18.0
if temp_angle >= 360.0:
temp_angle -= 360.0
#make temp_angle reversed
temp_angle = 360.0 - temp_angle
#print temp_angle
cv.mSet(next_angle, 0, 0, temp_angle)
cv.PolarToCart(init_mag_mat,
next_angle,
tempX_mat,
tempY_mat,
angleInDegrees=True)
#current_point = []
#adjust the cartesian points
current_point = (int(
round(cv.mGet(tempX_mat, 0, 0) + center_dartboard[0])),
int(
round(
cv.mGet(tempY_mat, 0, 0) +
(new_image.height -
center_dartboard[1]))))
#print current_point
cv.ShowImage(prev_calibration_window, new_image)
#*************************************************************************
#**********************3. Draw rings**************************************
for i in range(0, 6):
#display the rings
cv.Circle(new_image, center_dartboard, ring_radius[i],
cv.CV_RGB(0, 255, 0), 1, 8)
cv.ShowImage(prev_calibration_window, new_image)
#*************************************************************************
#we are finished drawing, signal
drawingFinished.set()
#wait for key press
print "Previous calibration data detected. Would you like to keep this calibration data? Press 'y' for yes"
#wait indefinitely for a key press
keyPressEvent.wait()
#ASCII 121 is character 'y'
if keyPress == 121:
#we are good with the previous calibration data
calibrationComplete = True
else:
calibrationComplete = False
#delete the calibration file and start over
os.remove("calibrationData.pkl")
#corrupted file
except EOFError as err:
print err
#Manual calibration
else:
#use two events to emulate wait for mouse click event
global e
global key
e = Event()
key = Event()
#start a fresh set of points
points = []
#copy image for manual calibration
new_image = cv.CloneImage(image)
t = Thread(target=CalibrationWindowThread, args=(new_image, ))
t.start()
print "Please select the center of the 20 points outermost rim."
e.wait()
e.clear()
cv.Circle(new_image, points[0], 3, cv.CV_RGB(255, 0, 0), 2, 8)
cv.ShowImage(window_name, new_image)
print "Please select the center of the 3 points outermost rim."
e.wait()
e.clear()
cv.Circle(new_image, points[1], 3, cv.CV_RGB(255, 0, 0), 2, 8)
cv.ShowImage(window_name, new_image)
print "Please select the center of the 11 points outermost rim."
e.wait()
e.clear()
cv.Circle(new_image, points[2], 3, cv.CV_RGB(255, 0, 0), 2, 8)
cv.ShowImage(window_name, new_image)
print "Please select the center of the 6 points outermost rim."
e.wait()
e.clear()
cv.Circle(new_image, points[3], 3, cv.CV_RGB(255, 0, 0), 2, 8)
cv.ShowImage(window_name, new_image)
#calculate the desired circle dimensions
newtop = (round(new_image.height / 2),
round(new_image.height * 0.20))
newbottom = (round(new_image.height / 2),
round(new_image.height * 0.80))
#Note: the height is smaller than the width
newleft = (round(new_image.height * 0.20),
round(new_image.height / 2))
newright = (round(new_image.height * 0.80),
round(new_image.height / 2))
mapping = cv.CreateMat(3, 3, cv.CV_32FC1)
#get a fresh new image
new_image = cv.CloneImage(image)
cv.GetPerspectiveTransform(
[points[0], points[1], points[2], points[3]],
[newtop, newbottom, newleft, newright], mapping)
cv.WarpPerspective(image, new_image, mapping)
cv.ShowImage(window_name, new_image)
print "The dartboard image has now been normalized."
print ""
center_dartboard = []
print "Please select the middle of the dartboard. i.e. the middle of the double bull's eye"
e.wait()
e.clear()
center_dartboard = points[4]
center_dartboard = (int(round(center_dartboard[0])),
int(round(center_dartboard[1])))
cv.Circle(new_image, center_dartboard, 3, cv.CV_RGB(255, 0, 0), 2,
8)
cv.ShowImage(window_name, new_image)
init_point_arr = []
print "Please select the outermost intersection of the 20 points and 1 ponit line."
e.wait()
e.clear()
init_point_arr = points[5]
cv.Circle(new_image, init_point_arr, 3, cv.CV_RGB(255, 0, 0), 2, 8)
cv.ShowImage(window_name, new_image)
#find initial angle of the 20-1 divider
tempX_mat = cv.CreateMat(1, 1, cv.CV_32FC1)
#correct the point with respect to the center
cv.mSet(tempX_mat, 0, 0, init_point_arr[0] - center_dartboard[0])
tempY_mat = cv.CreateMat(1, 1, cv.CV_32FC1)
#adjust the origin of y
cv.mSet(
tempY_mat, 0, 0,
init_point_arr[1] - (new_image.height - center_dartboard[1]))
init_mag_mat = cv.CreateMat(1, 1, cv.CV_32FC1)
init_angle_reversed_mat = cv.CreateMat(1, 1, cv.CV_32FC1)
#each point region is 360/12 = 18 degrees large
cv.CartToPolar(tempX_mat,
tempY_mat,
init_mag_mat,
init_angle_reversed_mat,
angleInDegrees=True)
ref_angle = 360.0 - cv.mGet(init_angle_reversed_mat, 0, 0)
global ref_mag
ref_mag = cv.mGet(init_mag_mat, 0, 0)
#print cv.mGet(init_mag_mat, 0, 0)
#print "Initial angle"
#print init_angle_val
#display dividers
current_point = (int(round(init_point_arr[0])),
int(round(init_point_arr[1])))
next_angle = cv.CreateMat(1, 1, cv.CV_32FC1)
cv.mSet(next_angle, 0, 0, 360 - ref_angle)
temp_angle = 360.0 - ref_angle
#draw point dividers counterclockwise, just like how angle is calculated, arctan(y/x)
for i in range(0, 20):
cv.Line(new_image, center_dartboard, current_point,
cv.CV_RGB(0, 0, 255), 1, 8)
#calculate the cartesian coordinate of the next point divider
temp_angle = 360.0 - temp_angle
temp_angle += 18.0
if temp_angle >= 360.0:
temp_angle -= 360.0
#make temp_angle reversed
temp_angle = 360.0 - temp_angle
#print temp_angle
cv.mSet(next_angle, 0, 0, temp_angle)
cv.PolarToCart(init_mag_mat,
next_angle,
tempX_mat,
tempY_mat,
angleInDegrees=True)
#current_point = []
#adjust the cartesian points
current_point = (
int(round(cv.mGet(tempX_mat, 0, 0) + center_dartboard[0])),
int(
round(
cv.mGet(tempY_mat, 0, 0) +
(new_image.height - center_dartboard[1]))))
#print current_point
cv.ShowImage(window_name, new_image)
ring_arr = []
print "Please select the first ring (any point). i.e. the ring that encloses the double bull's eye."
e.wait()
e.clear()
ring_arr.append(points[6])
cv.Circle(new_image, points[6], 3, cv.CV_RGB(255, 0, 0), 2, 8)
cv.ShowImage(window_name, new_image)
print "Please select the second ring (any point). i.e. the ring that encloses the bull's eye."
e.wait()
e.clear()
ring_arr.append(points[7])
cv.Circle(new_image, points[7], 3, cv.CV_RGB(255, 0, 0), 2, 8)
cv.ShowImage(window_name, new_image)
print "Please select the third ring (any point). i.e. the closer ring that encloses the triple score region."
e.wait()
e.clear()
ring_arr.append(points[8])
cv.Circle(new_image, points[8], 3, cv.CV_RGB(255, 0, 0), 2, 8)
cv.ShowImage(window_name, new_image)
print "Please select the fourth ring (any point). i.e. the further ring that encloses the triple score region."
e.wait()
e.clear()
ring_arr.append(points[9])
cv.Circle(new_image, points[9], 3, cv.CV_RGB(255, 0, 0), 2, 8)
cv.ShowImage(window_name, new_image)
print "Please select the fifth ring (any point). i.e. the closer ring that encloses the double score region."
e.wait()
e.clear()
ring_arr.append(points[10])
cv.Circle(new_image, points[10], 3, cv.CV_RGB(255, 0, 0), 2, 8)
cv.ShowImage(window_name, new_image)
print "Please select the sixth ring (any point). i.e. the further ring that encloses the double score region."
e.wait()
e.clear()
ring_arr.append(points[11])
cv.Circle(new_image, points[11], 3, cv.CV_RGB(255, 0, 0), 2, 8)
cv.ShowImage(window_name, new_image)
ring_radius = []
for i in range(0, 6):
#find the radius of the ring
ring_radius.append(
int(
math.sqrt((ring_arr[i][0] - center_dartboard[0])**2 +
(ring_arr[i][1] - center_dartboard[1])**2)))
#display the rings
cv.Circle(new_image, center_dartboard, ring_radius[i],
cv.CV_RGB(0, 255, 0), 1, 8)
cv.ShowImage(window_name, new_image)
e.wait()
#destroy calibration window
key.set()
#save valuable calibration data into a structure
calData = CalibrationData()
calData.top = points[0]
calData.bottom = points[1]
calData.left = points[2]
calData.right = points[3]
calData.center_dartboard = center_dartboard
calData.init_point_arr = init_point_arr
calData.ref_angle = ref_angle
calData.ring_radius = ring_radius
#write the calibration data to a file
calFile = open("calibrationData.pkl", "wb")
pickle.dump(calData, calFile, 0)
calFile.close()
calibrationComplete = True
def image_filter(self, cv_image, info, copy=None):
image = cv_image
#Only works on a grayscale image
gray_image = cv.CreateImage(cv.GetSize(image), 8, 1)
cv.CvtColor(image, gray_image, cv.CV_BGR2GRAY)
print "Called with mode: %s" % self.mode
if self.mode == "default" or self.mode == "save_h":
print "Computing homography matrix from checkerboard"
#Get the width and height of the board
board_w = self.cols
board_h = self.rows
#Area of the board = "board_n"
board_n = board_w * board_h
board_sz = (board_w, board_h)
#This needs to be fed with a "height", so it knows how high up the perspective transform should be.
#I've found for the wide_stereo cameras, a value of -15 works well. For the prosilica, -40. Don't ask me why
init_height = self.height
#Uses openCV to find the checkerboard
(found, corners) = cv.FindChessboardCorners(
image, board_sz,
(cv.CV_CALIB_CB_ADAPTIVE_THRESH | cv.CV_CALIB_CB_FILTER_QUADS))
if (not found):
print "Couldn't aquire checkerboard, only found 0 of %d corners\n" % board_n
gr = CloneImage(image)
cv.CvtColor(gray_image, gr, cv.CV_GRAY2BGR)
return gr
#We need subpixel accuracy, so we tell it where the corners are and it magically does the rest. I forget what (11,11) and (-1,-1) mean.
cv.FindCornerSubPix(
gray_image, corners, (11, 11), (-1, -1),
(cv.CV_TERMCRIT_EPS + cv.CV_TERMCRIT_ITER, 30, 0.1))
#Pull out the Image Points (3d location of the checkerboard corners, in the camera frame)
#and the Object Points (2d location of the corners on the checkerboard object)
objPts = point_array(4)
imgPts = point_array(4)
objPts[0] = (0, 0)
objPts[1] = (board_w - 1, 0)
objPts[2] = (0, board_h - 1)
objPts[3] = (board_w - 1, board_h - 1)
imgPts[0] = corners[0]
imgPts[1] = corners[board_w - 1]
imgPts[2] = corners[(board_h - 1) * board_w]
imgPts[3] = corners[(board_h - 1) * board_w + board_w - 1]
#Use GetPerspectiveTransform to populate our Homography matrix
H = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.GetPerspectiveTransform(objPts, imgPts, H)
#Since we don't get any z information from this, we populate H[2,2] with our hard-coded height
H[2, 2] = init_height
if self.mode == "save_h":
print "Saving Homography matrix to %s" % self.matrix_location
cv.Save(self.matrix_location, H)
else:
print "Loading Homography matrix from %s" % self.matrix_location
H = cv.Load(self.matrix_location)
birds_image = CloneImage(image)
#birds_image = cv.CreateImage((image.width*3,image.height*3),8,3)
#Uses the homography matrix to warp the perspective.
cv.WarpPerspective(
image, birds_image, H, cv.CV_INTER_LINEAR +
cv.CV_WARP_INVERSE_MAP + cv.CV_WARP_FILL_OUTLIERS)
#Note: If you need to undo the transformation, you can simply invert H and call cv.WarpPerspective again.
return birds_image
def VirtualMirror():
cv.NamedWindow("RGB_remap", cv.CV_WINDOW_NORMAL)
cv.NamedWindow("Depth_remap", cv.CV_WINDOW_AUTOSIZE)
cv.NamedWindow('dst', cv.CV_WINDOW_NORMAL)
cv.SetMouseCallback("Depth_remap", on_mouse, None)
print "Virtual Mirror"
print "Calibrated 4 Screen corner= ", sn4_ref
print "Corner 1-2 = ", np.linalg.norm(sn4_ref[0] - sn4_ref[1])
print "Corner 2-3 = ", np.linalg.norm(sn4_ref[1] - sn4_ref[2])
print "Corner 3-4 = ", np.linalg.norm(sn4_ref[2] - sn4_ref[3])
print "Corner 4-1 = ", np.linalg.norm(sn4_ref[3] - sn4_ref[0])
global head_pos
global head_virtual
global scene4_cross
head_pos = np.array([-0.2, -0.2, 1.0]) #Head_detect()
while 1:
(depth, _) = freenect.sync_get_depth()
(rgb, _) = freenect.sync_get_video()
#print type(depth)
img = array2cv(rgb[:, :, ::-1])
im = array2cv(depth.astype(np.uint8))
#modulize this part for update_on() and loopcv()
#q = depth
X, Y = np.meshgrid(range(640), range(480))
d = 2 #downsampling if need
projpts = calibkinect.depth2xyzuv(depth[::d, ::d], X[::d, ::d],
Y[::d, ::d])
xyz, uv = projpts
if tracking == 0:
#*********************************
if pt is not None:
print "=================="
(x_d, y_d) = pt
print "x=", x_d, " ,y=", y_d
#print depth.shape
#Watch out the indexing for depth col,row = 480,640
d_raw = np.array([depth[y_d, x_d]])
u_d = np.array([x_d])
v_d = np.array([y_d])
print "d_raw= ", d_raw
print "u_d= ", u_d
print "v_d= ", v_d
head3D, head2D = calibkinect.depth2xyzuv(d_raw, u_d, v_d)
print "XYZ=", head3D
print "XYZonRGBplane=", head2D
head_pos = head3D[0]
#print "head_pos.shape",head_pos.shape
print "head_pos= ", head_pos
cv.WaitKey(100)
cv.Circle(im, (x_d, y_d), 4, (0, 0, 255, 0), -1, 8, 0)
cv.Circle(im, (int(head2D[0, 0]), int(head2D[0, 1])), 2,
(255, 255, 255, 0), -1, 8, 0)
#*********************************
elif tracking == 1:
#find the nearest point (nose) as reference for right eye position
print "nose"
inds = np.nonzero(xyz[:, 2] > 0.5)
#print xyz.shape
new_xyz = xyz[inds]
#print new_xyz.shape
close_ind = np.argmin(new_xyz[:, 2])
head_pos = new_xyz[close_ind, :] + (0.03, 0.04, 0.01)
#print head_pos.shape
#print head_pos
elif tracking == 2:
#find the closest point as eye posiiton
print "camera"
inds = np.nonzero(xyz[:, 2] > 0.5)
#print xyz.shape
new_xyz = xyz[inds]
#print new_xyz.shape
close_ind = np.argmin(new_xyz[:, 2])
head_pos = new_xyz[close_ind, :]
#print head_pos.shape
#print head_pos
else:
print "please select a tracking mode"
head_virtual = MirrorReflection(sn4_ref[0:3, :], head_pos)
print "head_virtual= ", head_virtual
rgbK = np.array([[520.97092069697146, 0.0, 318.40565581396697],
[0.0, 517.85544366622719, 263.46756370601804],
[0.0, 0.0, 1.0]])
rgbD = np.array([[0.22464481251757576], [-0.47968370787671893], [0.0],
[0.0]])
irK = np.array([[588.51686020601733, 0.0, 320.22664144213843],
[0.0, 584.73028132692866, 241.98395817513071],
[0.0, 0.0, 1.0]])
irD = np.array([[-0.1273506872313161], [0.36672476189160591], [0.0],
[0.0]])
mapu = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
mapv = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
mapx = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
mapy = cv.CreateImage((640, 480), cv.IPL_DEPTH_32F, 1)
cv.InitUndistortMap(rgbK, rgbD, mapu, mapv)
cv.InitUndistortMap(irK, irD, mapx, mapy)
if 1:
rgb_remap = cv.CloneImage(img)
cv.Remap(img, rgb_remap, mapu, mapv)
depth_remap = cv.CloneImage(im)
cv.Remap(im, depth_remap, mapx, mapy)
scene4_cross = Cross4Pts.CrossPts(xyz, uv, head_pos, head_virtual,
sn4_ref)
#[warp] Add whole warpping code here
#[warp] points = Scene4Pts() as warpping 4 pts
#Flip the dst image!!!!!!!!!
#ShowImage("rgb_warp", dst)
#Within/out of the rgb range
#Mapping Destination (width, height)=(x,y)
#Warning: the order of pts in clockwise: pt1(L-T),pt2(R-T),pt3(R-B),pt4(L-B)
#points = [(test[0,0],test[0,1]), (630.,300.), (700.,500.), (400.,470.)]
points = [(scene4_cross[0, 0], scene4_cross[0, 1]),
(scene4_cross[1, 0], scene4_cross[1, 1]),
(scene4_cross[2, 0], scene4_cross[2, 1]),
(scene4_cross[3, 0], scene4_cross[3, 1])]
#Warping the image without flipping (camera image)
#npoints = [(0.,0.), (640.,0.), (640.,480.), (0.,480.)]
#Warping the image with flipping (mirror flip image)
npoints = [(640., 0.), (0., 0.), (0., 480.), (640., 480.)]
mat = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.GetPerspectiveTransform(points, npoints, mat)
#src = cv.CreateImage( cv.GetSize(img), cv.IPL_DEPTH_32F, 3 )
src = cv.CreateImage(cv.GetSize(rgb_remap), cv.IPL_DEPTH_32F, 3)
#cv.ConvertScale(img,src,(1/255.00))
cv.ConvertScale(rgb_remap, src, (1 / 255.00))
dst = cv.CloneImage(src)
cv.Zero(dst)
cv.WarpPerspective(src, dst, mat)
#************************************************************************
#Remap the rgb and depth image
#Warping will use remap rgb image as src
if 1:
cv.ShowImage("RGB_remap", rgb_remap) #rgb[200:440,300:600,::-1]
cv.ShowImage("Depth_remap", depth_remap)
cv.ShowImage("dst", dst) #warp rgb image
if cv.WaitKey(5) == 27:
cv.DestroyWindow("RGB_remap")
cv.DestroyWindow("Depth_remap")
cv.DestroyWindow("dst")
break
def refinecorners(im, found):
""" For a found marker, return the refined corner positions """
t0 = time.time()
(code,corners,pattern) = found
persp = cv.CreateMat(3, 3, cv.CV_32FC1)
fc = [corners[i,0] for i in range(4)]
cv.GetPerspectiveTransform(fc, sizcorners, persp)
cim = cv.CreateMat(siz, siz, cv.CV_8UC1)
cv.WarpPerspective(im, cim, persp, flags = cv.CV_INTER_LINEAR|cv.CV_WARP_FILL_OUTLIERS, fillval = 255)
unit = siz / 14.
hunit = unit / 2
def nearest1(x, y):
ix = int((x + hunit) / unit)
iy = int((y + hunit) / unit)
if (2 <= ix < 13) and (2 <= iy < 13):
nx = int(unit * ix)
ny = int(unit * iy)
return (nx, ny)
else:
return (0,0)
def nearest(x, y):
""" Return all grid points within sqrt(2) units of (x,y), closest first """
close = []
for ix in range(2, 14):
for iy in range(2, 14):
(nx, ny) = (unit * ix, unit * iy)
d = l2(x, y, nx, ny)
close.append((d, (nx, ny)))
return [p for (d,p) in sorted(close) if d < 2*unit*unit]
corners = strongest(cim)
pool = [((x,y), nearest(x, y)) for (x, y) in corners]
ga = dict([(x+y,((x,y),P)) for ((x,y),P) in pool])
gb = dict([(x-y,((x,y),P)) for ((x,y),P) in pool])
hyp = [ga[min(ga)], ga[max(ga)],
gb[min(gb)], gb[max(gb)]]
aL = [a for (a,bs) in hyp]
oldcorners = cv.fromarray(numpy.array(corners).astype(numpy.float32))
oldcorners = cv.Reshape(oldcorners, 2)
newcorners = cv.CreateMat(len(corners), 1, cv.CV_32FC2)
best = (9999999, None)
for bL in itertools.product(*[bs for (a,bs) in hyp]):
hypH = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.GetPerspectiveTransform(aL, bL, hypH)
cv.PerspectiveTransform(oldcorners, newcorners, hypH)
error = 0
for i in range(newcorners.rows):
(x,y) = newcorners[i,0]
(nx, ny) = nearest1(x, y)
error += l2(x, y, nx, ny)
best = min(best, (error, hypH))
if error < 1000:
break
# print "took", time.time() - t0, best[0]
if best[0] < 2500:
pose = best[1]
return backf(pose, im, found)
else:
return None
def annotate_image(cv_im, mech_info_dict, dir):
#cv_im = cv.LoadImage(sys.argv[1], cv.CV_LOAD_IMAGE_GRAYSCALE)
size = cv.GetSize(cv_im)
print 'Image size:', size[0], size[1] # col, row
wnd = 'Image Annotate'
cv.NamedWindow(wnd, cv.CV_WINDOW_AUTOSIZE)
disp_im = cv.CloneImage(cv_im)
new_size = (size[0] / 2, size[1] / 2)
scale_factor = 1
checker_origin_height = mech_info_dict['height']
# chesscoard corners mat
cb_dims = (5, 8) # checkerboard dims. (x, y)
sq_sz = 19 # size of checkerboard in real units.
cb_offset = 500, 500
cb_coords = cv.CreateMat(2, cb_dims[0] * cb_dims[1], cv.CV_64FC1)
n = 0
for r in range(cb_dims[1]):
for c in range(cb_dims[0]):
cb_coords[0, n] = c * sq_sz + cb_offset[0] # x coord
cb_coords[1, n] = r * sq_sz + cb_offset[1] # y coord
n += 1
clicked_list = []
recreate_image = False
mechanism_calc_state = 0
mechanism_calc_dict = {}
while True:
for p in clicked_list:
x, y = p[0] * scale_factor, p[1] * scale_factor
cv.Circle(disp_im, (x, y), 3, (255, 0, 0))
cv.ShowImage(wnd, disp_im)
k = cv.WaitKey(10)
k = k % 256
if k == 27:
# ESC
break
elif k == ord('='):
# + key without the shift
scale_factor = scale_factor * 1.2
recreate_image = True
elif k == ord('-'):
# - key
scale_factor = scale_factor / 1.2
recreate_image = True
elif k == ord('c'):
# find chessboard corners.
succ, corners = cv.FindChessboardCorners(disp_im, cb_dims)
if succ == 0:
print 'Chessboard detection FAILED.'
else:
# chessboard detection was successful
cv.DrawChessboardCorners(disp_im, cb_dims, corners, succ)
cb_im = cv.CreateMat(2, cb_dims[0] * cb_dims[1], cv.CV_64FC1)
corners_mat = np.array(corners).T
n = 0
for r in range(cb_dims[1]):
for c in range(cb_dims[0]):
cb_im[0, n] = corners_mat[0, n] # x coord
cb_im[1, n] = corners_mat[1, n] # y coord
n += 1
H = cv.CreateMat(3, 3, cv.CV_64FC1)
H1 = cv.FindHomography(cb_im, cb_coords, H)
Hnp = np.reshape(np.fromstring(H1.tostring()), (3, 3))
print 'Homography:'
print Hnp
d = cv.CloneImage(disp_im)
cv.WarpPerspective(d, disp_im, H1, cv.CV_WARP_FILL_OUTLIERS)
cv_im = cv.CloneImage(disp_im)
elif k == ord('1'):
# calculate width of the mechanism
del clicked_list[:]
cv.SetMouseCallback(wnd, on_mouse,
(disp_im, clicked_list, scale_factor))
recreate_image = True
print 'Click on two endpoints to mark the width of the mechanism'
mechanism_calc_state = 1
elif k == ord('2'):
# distance of handle from the hinge
del clicked_list[:]
cv.SetMouseCallback(wnd, on_mouse,
(disp_im, clicked_list, scale_factor))
recreate_image = True
print 'Click on handle and hinge to compute distance of handle from hinge.'
mechanism_calc_state = 2
elif k == ord('3'):
# height of the handle above the ground
del clicked_list[:]
cv.SetMouseCallback(wnd, on_mouse,
(disp_im, clicked_list, scale_factor))
recreate_image = True
print 'Click on handle top and bottom to compute height of handle above the ground.'
mechanism_calc_state = 3
elif k == ord('4'):
# top and bottom edge of the mechanism
del clicked_list[:]
cv.SetMouseCallback(wnd, on_mouse,
(disp_im, clicked_list, scale_factor))
recreate_image = True
print 'Click on top and bottom edges of the mechanism.'
mechanism_calc_state = 4
elif k == ord('d'):
# display the calculated values
print 'mechanism_calc_dict:', mechanism_calc_dict
print 'mech_info_dict:', mech_info_dict
elif k == ord('s'):
# save the pkl
ut.save_pickle(mechanism_calc_dict,
dir + '/mechanism_calc_dict.pkl')
print 'Saved the pickle'
#elif k != -1:
elif k != 255:
print 'k:', k
if recreate_image:
new_size = (int(size[0] * scale_factor),
int(size[1] * scale_factor))
disp_im = cv.CreateImage(new_size, cv_im.depth, cv_im.nChannels)
cv.Resize(cv_im, disp_im)
cv.SetMouseCallback(wnd, on_mouse,
(disp_im, clicked_list, scale_factor))
recreate_image = False
if len(clicked_list) == 2:
if mechanism_calc_state == 1:
w = abs(clicked_list[0][0] - clicked_list[1][0])
print 'Width in mm:', w
mechanism_calc_dict['mech_width'] = w / 1000.
if mechanism_calc_state == 2:
w = abs(clicked_list[0][0] - clicked_list[1][0])
print 'Width in mm:', w
mechanism_calc_dict['handle_hinge_dist'] = w / 1000.
if mechanism_calc_state == 3:
p1, p2 = clicked_list[0], clicked_list[1]
h1 = (cb_offset[1] - p1[1]) / 1000. + checker_origin_height
h2 = (cb_offset[1] - p2[1]) / 1000. + checker_origin_height
mechanism_calc_dict['handle_top'] = max(h1, h2)
mechanism_calc_dict['handle_bottom'] = min(h1, h2)
if mechanism_calc_state == 4:
p1, p2 = clicked_list[0], clicked_list[1]
h1 = (cb_offset[1] - p1[1]) / 1000. + checker_origin_height
h2 = (cb_offset[1] - p2[1]) / 1000. + checker_origin_height
mechanism_calc_dict['mechanism_top'] = max(h1, h2)
mechanism_calc_dict['mechanism_bottom'] = min(h1, h2)
mechanism_calc_state = 0
vid = cv.QueryFrame(captura)
im_in = cv.CloneImage(vid)
#im_in = cv.LoadImage('foto5.jpg', 4)
width, height = cv.GetSize(im_in)
im = cv.CreateImage((640, 480), cv.IPL_DEPTH_8U, 3)
cv.Resize(im_in, im)
cv.ShowImage('Escoger 4 puntos', im)
cv.SetMouseCallback('Escoger 4 puntos', mouse, im)
cv.WaitKey(0)
h**o = homografia(im_in.width, im_in.height, im.width, im.height)
cv.Save('homography.cvmat', h**o)
out = cv.CloneImage(im_in)
cv.WarpPerspective(im_in, out, h**o)
out_small = cv.CloneImage(im)
cv.Resize(out, out_small)
cv.ShowImage('Homografia', out_small)
cv.WaitKey(0)
#captura=cv.CaptureFromCAM(1)
out = cv.CreateImage((640, 480), cv.IPL_DEPTH_8U, 3)
imagen = cv.QueryFrame(captura)
creavideo = cv.CreateVideoWriter("reencode.avi",
cv.CV_FOURCC('M', 'J', 'P', 'G'), 29.97,
(640, 480))
#creavideo =cv.CreateVideoWriter("output.avi", 0, 15,(800,600) , 1)
while True:
def transformImage(self,im, use_orig=True, inverse=False):
'''
Transforms an image into the new coordinate system.
If this image was produced via an affine transform of another image,
this method will attempt to trace weak references to the original image
and directly compute the new image from that image to improve accuracy.
To accomplish this a weak reference to the original source image and
the affine matrix used for the transform are added to any image
produced by this method. This can be disabled using the use_orig
parameter.
@param im: an Image object
@param use_orig: (True or False) attempts to find and use the original image as the source to avoid an accumulation of errors.
@returns: the transformed image
'''
#TODO: does not support opencv images. see Perspective.py
prev_im = im
if inverse:
inverse = self.matrix
else:
inverse = self.inverse
if use_orig:
# Find the oldest image used to produce this one by following week
# references.
# Check to see if there is an aff_prev list
if hasattr(prev_im,'aff_prev'):
# If there is... search that list for the oldest image
found_prev = False
for i in range(len(prev_im.aff_prev)):
ref,cmat = prev_im.aff_prev[i]
if not found_prev and ref():
im = ref()
mat = np.eye(3)
found_prev = True
if found_prev:
mat = np.dot(mat,cmat)
if found_prev:
inverse = np.dot(mat,inverse)
if im.getType() == TYPE_PIL:
data = inverse[:2,:].flatten()
#data = (matrix[0,0],matrix[0,1],matrix[0,2],matrix[1,0],matrix[1,1],matrix[1,2])
pil = im.asPIL().transform(self.size, AFFINE, data, self.filter)
result = Image(pil)
elif im.getType() == TYPE_MATRIX_2D:
mat = im.asMatrix2D()
mat = affine_transform(mat, self.inverse[:2,:2], offset=self.inverse[:2,2])
result = Image(mat)
elif im.getType() == TYPE_OPENCV:
matrix = pv.NumpyToOpenCV(self.matrix)
src = im.asOpenCV()
dst = cv.CreateImage( (self.size[0],self.size[1]), cv.IPL_DEPTH_8U, src.nChannels );
cv.WarpPerspective( src, dst, matrix, cv.CV_INTER_LINEAR+cv.CV_WARP_FILL_OUTLIERS,cv.ScalarAll(128))
result = pv.Image(dst)
else:
raise NotImplementedError("Unhandled image type for affine transform.")
# Check to see if there is an aff_prev list for this object
if use_orig and hasattr(prev_im,'aff_prev'):
# Create one if not
result.aff_prev = copy.copy(prev_im.aff_prev)
else:
result.aff_prev = []
# Append the prev image and new transform
result.aff_prev.append( (weakref.ref(prev_im), self.inverse) )
return result
def process_frame(self, frame):
self.debug_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
og_frame = cv.CreateImage(cv.GetSize(frame), 8, 3)
cv.Copy(frame, self.debug_frame)
cv.Copy(self.debug_frame, og_frame)
cv.Smooth(frame, frame, cv.CV_MEDIAN, 7, 7)
# Set binary image to have saturation channel
hsv = cv.CreateImage(cv.GetSize(frame), 8, 3)
binary = cv.CreateImage(cv.GetSize(frame), 8, 1)
cv.CvtColor(frame, hsv, cv.CV_BGR2HSV)
cv.SetImageCOI(hsv, 1) #3 before competition #2 at competition
cv.Copy(hsv, binary)
cv.SetImageCOI(hsv, 0)
cv.AdaptiveThreshold(binary, binary,
255,
cv.CV_ADAPTIVE_THRESH_MEAN_C,
cv.CV_THRESH_BINARY_INV,
self.adaptive_thresh_blocksize,
self.adaptive_thresh,
)
# Morphology
kernel = cv.CreateStructuringElementEx(5, 5, 3, 3, cv.CV_SHAPE_ELLIPSE)
cv.Erode(binary, binary, kernel, 1)
cv.Dilate(binary, binary, kernel, 1)
# Get Edges
#cv.Canny(binary, binary, 30, 40)
cv.CvtColor(binary, self.debug_frame, cv.CV_GRAY2RGB)
# Hough Transform
line_storage = cv.CreateMemStorage()
raw_lines = cv.HoughLines2(binary, line_storage, cv.CV_HOUGH_PROBABILISTIC,
rho=1,
theta=math.pi / 180,
threshold=self.hough_threshold,
param1=self.min_length,
param2=self.max_gap
)
lines = []
for line in raw_lines:
lines.append(line)
#Grouping lines depending on endpoint similarities
for line1 in lines[:]:
for line2 in lines[:]:
if line1 in lines and line2 in lines and line1 != line2:
if math.fabs(line1[0][0] - line2[0][0]) < self.max_corner_range and \
math.fabs(line1[0][1] - line2[0][1]) < self.max_corner_range and \
math.fabs(line1[1][0] - line2[1][0]) < self.max_corner_range and \
math.fabs(line1[1][1] - line2[1][1]) < self.max_corner_range:
if line_distance(line1[0], line1[1]) > line_distance(line2[0], line2[1]):
lines.remove(line2)
else:
lines.remove(line1)
elif math.fabs(line1[0][0] - line2[1][0]) < self.max_corner_range and \
math.fabs(line1[0][1] - line2[1][1]) < self.max_corner_range and \
math.fabs(line1[1][0] - line2[0][0]) < self.max_corner_range and \
math.fabs(line1[1][1] - line2[0][1]) < self.max_corner_range:
if line_distance(line1[0], line1[1]) > line_distance(line2[0], line2[1]):
lines.remove(line2)
else:
lines.remove(line1)
self.hough_corners = []
for line in lines:
self.hough_corners.append(line[0])
self.hough_corners.append(line[1])
for corner1 in self.hough_corners[:]:
for corner2 in self.hough_corners[:]:
if corner1 is not corner2 and corner1 in self.hough_corners and corner2 in self.hough_corners:
if math.fabs(corner1[0] - corner2[0]) < self.max_corner_range4 and \
math.fabs(corner1[1] - corner2[1]) < self.max_corner_range4:
corner1 = [(corner1[0] + corner2[0]) / 2, (corner1[1] + corner2[1]) / 2]
self.hough_corners.remove(corner2)
for line1 in lines:
#cv.Line(self.debug_frame,line1[0],line1[1], (0,0,255), 10, cv.CV_AA, 0)
for line2 in lines:
if line1 is not line2:
self.find_corners(line1, line2)
for corner1 in self.corners:
for corner2 in self.corners:
if math.fabs(corner1[1][0] - corner2[1][0]) < self.max_corner_range2 and \
math.fabs(corner1[1][1] - corner2[1][1]) < self.max_corner_range2 and \
math.fabs(corner1[2][0] - corner2[2][0]) < self.max_corner_range2 and \
math.fabs(corner1[2][1] - corner2[2][1]) < self.max_corner_range2 and \
math.fabs(corner1[0][0] - corner2[0][0]) > self.max_corner_range2 and \
math.fabs(corner1[0][1] - corner2[0][1]) > self.max_corner_range2:
pt1 = (int(corner1[0][0]), int(corner1[0][1]))
pt4 = (int(corner2[0][0]), int(corner2[0][1]))
pt3 = (int(corner1[1][0]), int(corner1[1][1]))
pt2 = (int(corner1[2][0]), int(corner1[2][1]))
#line_color = (0,255,0)s
#cv.Line(self.debug_frame,pt1,pt2, line_color, 10, cv.CV_AA, 0)
#cv.Line(self.debug_frame,pt1,pt3, line_color, 10, cv.CV_AA, 0)
#cv.Line(self.debug_frame,pt4,pt2, line_color, 10, cv.CV_AA, 0)
#cv.Line(self.debug_frame,pt4,pt3, line_color, 10, cv.CV_AA, 0)
new_bin = Bin(pt1, pt2, pt3, pt4)
new_bin.id = self.bin_id
self.bin_id += 1
if math.fabs(line_distance(pt1, pt2) - line_distance(pt3, pt4)) < self.parallel_sides_length_thresh and \
math.fabs(line_distance(pt1, pt3) - line_distance(pt2, pt4)) < self.parallel_sides_length_thresh:
self.Bins.append(new_bin)
print "new_bin"
elif (math.fabs(corner1[1][0] - corner2[2][0]) < self.max_corner_range2 and
math.fabs(corner1[1][1] - corner2[2][1]) < self.max_corner_range2 and
math.fabs(corner1[2][0] - corner2[1][0]) < self.max_corner_range2 and
math.fabs(corner1[2][1] - corner2[1][1]) < self.max_corner_range2 and
math.fabs(corner1[0][0] - corner2[0][0]) > self.max_corner_range2 and
math.fabs(corner1[0][1] - corner2[0][1]) > self.max_corner_range2):
continue
self.corners = []
self.final_corners = self.sort_corners() #Results are not used. Experimental corners which have been seen twice, should be only the corners we want, but there were problems
self.sort_bins()
self.update_bins()
self.group_bins()
self.draw_bins()
for corner in self.hough_corners:
line_color = [255, 0, 0]
cv.Circle(self.debug_frame, corner, 15, (255, 0, 0), 2, 8, 0)
for line in lines:
line_color = [255, 0, 0]
cv.Line(self.debug_frame, line[0], line[1], line_color, 5, cv.CV_AA, 0)
#cv.Circle(self.debug_frame, line[0], 15, (255,0,0), 2,8,0)
#cv.Circle(self.debug_frame, line[1], 15, (255,0,0), 2,8,0)
#Output bins
self.output.bins = self.Bins
anglesum = 0
for bins in self.output.bins:
bins.theta = (bins.center[0] - frame.width / 2) * 37 / (frame.width / 2)
bins.phi = -1 * (bins.center[1] - frame.height / 2) * 36 / (frame.height / 2)
anglesum += bins.angle
# bins.orientation = bins.angle
if len(self.output.bins) > 0:
self.output.orientation = anglesum / len(self.output.bins)
else:
self.output.orientation = None
self.return_output()
svr.debug("Bins", self.debug_frame)
svr.debug("Original", og_frame)
#BEGIN SHAPE PROCESSING
#constants
img_width = 128
img_height = 256
number_x = 23
number_y = 111
number_w = 82
number_h = 90
bin_thresh_blocksize = 11
bin_thresh = 1.9
red_significance_threshold = 0.4
#load templates - run once, accessible to number processor
number_templates = [
(10, cv.LoadImage("number_templates/10.png")),
(16, cv.LoadImage("number_templates/16.png")),
(37, cv.LoadImage("number_templates/37.png")),
(98, cv.LoadImage("number_templates/98.png")),
]
#Begin Bin Contents Processing
for bin in self.Bins:
#Take the bin's corners, and get an image containing an img_width x img_height rectangle of it
transf = cv.CreateMat(3, 3, cv.CV_32FC1)
cv.GetPerspectiveTransform(
[bin.corner1, bin.corner2, bin.corner3, bin.corner4],
[(0, 0), (0, img_height), (img_width, 0), (img_width, img_height)],
transf
)
bin_image = cv.CreateImage([img_width, img_height], 8, 3)
cv.WarpPerspective(frame, bin_image, transf)
#AdaptaveThreshold to get black and white image highlighting the number (still works better than my yellow-vs-red threshold attempt
hsv = cv.CreateImage(cv.GetSize(bin_image), 8, 3)
bin_thresh_image = cv.CreateImage(cv.GetSize(bin_image), 8, 1)
cv.CvtColor(bin_image, hsv, cv.CV_BGR2HSV)
cv.SetImageCOI(hsv, 3)
cv.Copy(hsv, bin_thresh_image)
cv.SetImageCOI(hsv, 0)
cv.AdaptiveThreshold(bin_thresh_image, bin_thresh_image,
255,
cv.CV_ADAPTIVE_THRESH_MEAN_C,
cv.CV_THRESH_BINARY_INV,
bin_thresh_blocksize,
bin_thresh,
)
kernel = cv.CreateStructuringElementEx(5, 5, 3, 3, cv.CV_SHAPE_ELLIPSE)
cv.Erode(bin_thresh_image, bin_thresh_image, kernel, 1)
cv.Dilate(bin_thresh_image, bin_thresh_image, kernel, 1)
#Here, we loop through all four different templates, and figure out which one we think is most likely.
#The comparison function counts corresponding pixels that are non-zero in each image, and then corresponding pixels that are different in each image. The ratio of diff_count/both_count is our "unconfidence" ratio. The lower it is, the more confident we are.
#There are two nearly identical pieces of code within this loop. One checks the bin right-side-up, and the other one checks it flipped 180.
last_thought_number = -1
last_unconfidence_ratio = number_w * number_h + 2
for i in range(0, len(number_templates)):
both_count = 0
diff_count = 0
this_number_image = number_templates[i][1]
for y in range(0, number_h):
for x in range(0, number_w):
if (bin_thresh_image[y + number_y, x + number_x] != 0) and (this_number_image[y, x][0] != 0):
both_count += 1
elif (bin_thresh_image[y + number_y, x + number_x] != 0) or (this_number_image[y, x][0] != 0):
diff_count += 1
if both_count == 0:
unconfidence_ratio = number_w * number_h + 1 # max unconfidence
else:
unconfidence_ratio = 1.0 * diff_count / both_count
if unconfidence_ratio < last_unconfidence_ratio:
last_thought_number = number_templates[i][0]
last_unconfidence_ratio = unconfidence_ratio
both_count = 0
diff_count = 0
for y in range(0, number_h):
for x in range(0, number_w):
if (bin_thresh_image[img_height - number_y - 1 - y, img_width - number_x - 1 - x] != 0) and (
this_number_image[y, x][0] != 0):
both_count += 1
elif (bin_thresh_image[img_height - number_y - 1 - y, img_width - number_x - 1 - x] != 0) or (
this_number_image[y, x][0] != 0):
diff_count += 1
if both_count == 0:
unconfidence_ratio = number_w * number_h + 1 # max unconfidence
else:
unconfidence_ratio = 1.0 * diff_count / both_count
if unconfidence_ratio < last_unconfidence_ratio:
last_thought_number = number_templates[i][0]
last_unconfidence_ratio = unconfidence_ratio
print str(last_thought_number) + " | " + str(last_unconfidence_ratio)
try: #check if it's defined
bin.number_unconfidence_ratio
except:
bin.number_unconfidence_ratio = last_unconfidence_ratio
bin.number = last_thought_number
print "Set Speed Limit Number"
else:
if last_unconfidence_ratio < bin.number_unconfidence_ratio:
bin.number_unconfidence_ratio = last_unconfidence_ratio
if bin.number == last_thought_number:
print "More Confident on Same Number: Updated"
else:
print "More Confident on Different Number: Updated"
bin.icon = last_thought_number
