def saveImage(dcim, filename):
w, h, l, r, d = dcim
cv_l = None
cv_r = None
cv_d = None
if l != None:
channels = len(l) / (w * h)
cv_l = cv.CreateImageHeader((w, h), cv.IPL_DEPTH_8U, channels)
cv.SetData(cv_l, str(l), w * channels)
if channels == 3:
cv.CvtColor(cv_l, cv_l, cv.CV_BGR2RGB)
if r != None:
channels = len(r) / (w * h)
cv_r = cv.CreateImageHeader((w, h), cv.IPL_DEPTH_8U, channels)
cv.SetData(cv_r, str(r), w * channels)
if channels == 3:
cv.CvtColor(cv_r, cv_r, cv.CV_BGR2RGB)
if d != None:
(dimg, dpp, numDisp) = d
max_disparity = (dpp * numDisp) - 1
d16 = cv.CreateImageHeader((w, h), cv.IPL_DEPTH_16U, 1)
cv.SetData(d16, str(dimg), w * 2)
cv_d = cv.CreateImage((w, h), cv.IPL_DEPTH_8U, 1)
cv.CvtScale(d16, cv_d, 255.0 / max_disparity)
cv.SaveImage(filename, compose(cv_l, cv_r, cv_d))
def pil2cv(img, t='L'):
if t == 'L':
res = cv.CreateImageHeader(img.size, 8, 1)
else:
res = cv.CreateImageHeader(img.size, 32, 3)
cv.SetData(res, img.tostring())
return res
def img_from_depth_frame(self, depth):
depth = depth.astype(numpy.uint8)
image = cv.CreateImageHeader((depth.shape[1], depth.shape[0]),
cv.IPL_DEPTH_8U, 1)
cv.SetData(image, depth.tostring(),
depth.dtype.itemsize * depth.shape[1])
return image
def image_call():
global FilterWindowName
ret, cv_image = cap.retrieve()
if (ret):
filtered = FindBall(cv_image)
cv2.imshow(FilterWindowName, filtered)
mcs = PickBlob(filtered)
for mc in mcs:
cv2.circle(cv_image, (int(mc[0]), int(mc[1])), 3,
cv.Scalar(255, 0, 0))
#cv_image = cv2.cvtColor(cv_image, cv.CV_BGR2RGB)
cvIm = cv.CreateImageHeader((cv_image.shape[1], cv_image.shape[0]),
cv.IPL_DEPTH_8U, 3)
cv.SetData(cvIm, cv_image.tostring(),
cv_image.dtype.itemsize * 3 * cv_image.shape[1])
CompositeShow("Image window", cam1, cvIm, settings)
#correct the frame
tosend = []
for mc in mcs:
new = cam1.FrameCorrect(mc[0], mc[1])
rectified = unit2world(new)
#print "Ball: ", str(mc), " -> ", str(new)
#print "Ball: ", str(mc), " -> ", str(rectified)
tosend.append(rectified)
#now send it!
tcpc.send(tosend)
cv.WaitKey(3)
def getSnapshot(self):
""" snapShot() -> iplImg, (cameraPos6D, headAngles)
Take a snapshot from the current subscribed video feed.
"""
# Get camPos
# getPosition(name, space={0,1,2}, useSensorValues)
camPos = self.globals.motProxy.getPosition("CameraBottom", 2, True)
headAngles = self.globals.motProxy.getAngles(["HeadPitch", "HeadYaw"],
True)
# Get image
# shot[0]=width, shot[1]=height, shot[6]=image-data
shot = self.globals.vidProxy.getImageRemote("python_GVM")
size = (shot[0], shot[1])
picture = Image.frombuffer("RGB", size, shot[6], "raw", "BGR", 0, 1)
#create a open cv image of the snapshot
image = cv.CreateImageHeader(size, cv.IPL_DEPTH_8U, 3)
cv.SetData(image, picture.tostring(), picture.size[0] * 3)
hsvImage = cv.CreateImage(size, cv.IPL_DEPTH_8U, 3)
cv.CvtColor(image, hsvImage, cv.CV_BGR2HSV)
return (hsvImage, (camPos, headAngles))
def PIL2Ipl(input):
"""Converts a PIL image to the OpenCV/IPL cvmat data format.
Supported input image formats are:
RGB
L
F
"""
if not (isinstance(input, PIL.Image.Image)):
raise TypeError, 'Must be called with PIL.Image.Image!'
# mode dictionary:
# (pil_mode : (ipl_depth, ipl_channels)
mode_list = {
"RGB": (cv.IPL_DEPTH_8U, 3),
"L": (cv.IPL_DEPTH_8U, 1),
"F": (cv.IPL_DEPTH_32F, 1)
}
if not mode_list.has_key(input.mode):
raise ValueError, 'unknown or unsupported input mode'
result = cv.CreateImageHeader( # Jon Rodriguez turned ".cvCreateImage" into ".CreateImageHeader"
(
input.size[0], input.size[1]
), # size. Used to be of type cv.cvSize but Jon Rodriguez switched it to a tuple
mode_list[input.mode][0], # depth
mode_list[input.mode][1] # channels
)
# set imageData
cv.SetData(result, input.tostring()) # Jon Rodriguez's line
return result
def pretty_depth_cv(depth):
import cv
depth = pretty_depth(depth)
image = cv.CreateImageHeader((depth.shape[1], depth.shape[0]),
cv.IPL_DEPTH_8U, 1)
cv.SetData(image, depth.tostring(), depth.dtype.itemsize * depth.shape[1])
return image
def array2cv(self, arr, isImg):
if (isImg == True):
arr = numpy.asarray(arr, dtype=numpy.uint8)
else:
arr = numpy.asarray(arr, dtype=numpy.float32)
prevType = arr.dtype
dtype2depth = {
'uint8': cv.IPL_DEPTH_8U,
'int8': cv.IPL_DEPTH_8S,
'uint16': cv.IPL_DEPTH_16U,
'int16': cv.IPL_DEPTH_16S,
'int32': cv.IPL_DEPTH_32S,
'float32': cv.IPL_DEPTH_32F,
'float64': cv.IPL_DEPTH_64F,
}
try:
nChannels = arr.shape[2]
except:
nChannels = 1
cv_im = cv.CreateImageHeader((arr.shape[1], arr.shape[0]),
dtype2depth[str(prevType)], nChannels)
cv.SetData(cv_im, arr.tostring(),
prevType.itemsize * nChannels * arr.shape[1])
if (isImg == True):
return cv_im
else:
return cv.GetMat(cv_im, 1)
def convert2vid(bdjoints, fimgs, fps, Vtype=0):
# Vtype : 0 : color m, 1: depth ,2: body index
if Vtype == 0:
fsize = (1920, 1080)
extname = ''
elif Vtype == 1:
fsize = (512, 424)
extname = 'dp'
else:
fsize = (512, 424)
extname = 'bdidx'
#now = datetime.datetime.now()
vid = 'kinect' + repr(now.month).zfill(2) + repr(now.day).zfill(2) + repr(
now.hour).zfill(2) + repr(now.minute).zfill(2) + '_' + extname + '.avi'
video = cv.CreateVideoWriter(vid, cv.CV_FOURCC('L', 'A', 'G', 'S'), fps,
fsize, True)
print 'making video .....'
for i in fimgs:
bitmap = cv.CreateImageHeader(fsize, cv.IPL_DEPTH_8U, 3)
cv.SetData(bitmap, i.tostring(), i.dtype.itemsize * 3 * i.shape[1])
cv.WriteFrame(video, bitmap)
print 'there r total ' + repr(len(fimgs)) + ' frames'
del video
def convertImage(picture):
global size
size = picture.size
# convert the type to OpenCV
image = cv.CreateImageHeader(size, cv.IPL_DEPTH_8U, 3)
cv.SetData(image, picture.tostring(), picture.size[0] * 3)
return image
def connectToServerAndHandleConnection():
HOST = 'localhost'
PORT = 9898
while True:
try:
sock = socket.socket(socket.AF_INET,socket.SOCK_STREAM)
sock.connect((HOST,PORT))
img_str = sock.recv(100000)
nparr = np.fromstring(img_str, np.uint8)
img_np = cv2.imdecode(nparr, cv2.CV_LOAD_IMAGE_COLOR) # cv2.IMREAD_COLOR in OpenCV 3.1
img_ipl = cv.CreateImageHeader((img_np.shape[1], img_np.shape[0]), cv.IPL_DEPTH_8U, 3)
cv.SetData(img_ipl, img_np.tostring(), img_np.dtype.itemsize * 3 * img_np.shape[1])
image = Image(img_ipl)
barcodes = image.findBarcode()
stringOut = '[]\n'
if barcodes != None:
stringOut = ''
for barcode in barcodes:
stringOut += str([barcode.x,barcode.y,int(barcode.length()), int(barcode.width()), barcode.data]) + ';'
stringOut = stringOut[:-1]
stringOut += '\n'
sock.send(stringOut)
except:
continue
def __recognize(self, faces):
filenames = []
for face in faces:
name = self.__eigen.recognize(face)
if name:
filenames.append(name)
if self.__verbose:
cv_im = cv.CreateImageHeader(cv.GetSize(face),
cv.IPL_DEPTH_8U, 1)
cv.SetData(cv_im, face.tostring())
cv.ShowImage(name, cv_im)
cv.WaitKey(10)
# Log the scores for each observation
if self.__source == 'folder':
scores = self.__eigen.getScores(face)
if scores:
self.__saveScores(self.__curr_name,\
self.__curr_dir[self.__detect_idx],\
faces.index(face),\
scores[0], scores[1], scores[2], scores[3],\
name)
else:
if self.__verbose:
print 'No recognition'
return self.__parse(filenames)
def array_to_im(a):
dtype2depth = {
'uint8': cv.IPL_DEPTH_8U,
'int8': cv.IPL_DEPTH_8S,
'uint16': cv.IPL_DEPTH_16U,
'int16': cv.IPL_DEPTH_16S,
'int32': cv.IPL_DEPTH_32S,
'float32': cv.IPL_DEPTH_32F,
'float64': cv.IPL_DEPTH_64F,
}
if a.dtype.name in 'int64':
a = a.astype('int32')
if a.ndim == 1:
a = numpy.array([a])
try:
nChannels = a.shape[2]
except:
nChannels = 1
cv_im = cv.CreateImageHeader((a.shape[1], a.shape[0]),
dtype2depth[a.dtype.name], nChannels)
cv.SetData(cv_im, a.tostring('C'),
a.dtype.itemsize * nChannels * a.shape[1])
return cv_im
def arr2ipl(arr, imgtype=cv.IPL_DEPTH_8U):
img = cv.CreateImageHeader((arr.shape[1], arr.shape[0]), imgtype, 1)
a = array(arr, uint8)
cv.SetData(img, a.tostring(), a.dtype.itemsize * a.shape[1])
return img
def detect(self, callback):
engine = self.context.modules.engine
sz = engine.size
image = cv.CreateImageHeader(sz, cv.IPL_DEPTH_8U, 3)
cv.SetData(image, engine.get_image_data())
gray_image = cv.CreateImage(engine.size, 8, 1)
convert_mode = getattr(cv, 'CV_%s2GRAY' % engine.get_image_mode())
cv.CvtColor(image, gray_image, convert_mode)
image = gray_image
rows = sz[0]
cols = sz[1]
eig_image = cv.CreateMat(rows, cols, cv.CV_32FC1)
temp_image = cv.CreateMat(rows, cols, cv.CV_32FC1)
points = cv.GoodFeaturesToTrack(image,
eig_image,
temp_image,
20,
0.04,
1.0,
useHarris=False)
if points:
for x, y in points:
self.context.request.focal_points.append(FocalPoint(x, y, 1))
callback()
else:
self.next(callback)
def get_features(self):
engine = self.context.modules.engine
mode, converted_image = engine.image_data_as_rgb(False)
size = engine.size
image = cv.CreateImageHeader(size, cv.IPL_DEPTH_8U, 3)
cv.SetData(image, converted_image)
gray = cv.CreateImage(size, 8, 1)
convert_mode = getattr(cv, 'CV_%s2GRAY' % mode)
cv.CvtColor(image, gray, convert_mode)
min_size = self.get_min_size_for(size)
haar_scale = 1.2
min_neighbors = 3
cv.EqualizeHist(gray, gray)
faces = cv.HaarDetectObjects(gray, self.__class__.cascade,
cv.CreateMemStorage(0), haar_scale,
min_neighbors,
cv.CV_HAAR_DO_CANNY_PRUNING, min_size)
faces_scaled = []
for ((x, y, w, h), n) in faces:
# the input to cv.HaarDetectObjects was resized, so scale the
# bounding box of each face and convert it to two CvPoints
x2, y2 = (x + w), (y + h)
faces_scaled.append(((x, y, x2 - x, y2 - y), n))
return faces_scaled
def pil2cvGrey(pil_im):
# Convert a PIL image to a greyscale cv image
# from: http://pythonpath.wordpress.com/2012/05/08/pil-to-opencv-image/
pil_im = pil_im.convert('L')
cv_im = cv.CreateImageHeader(pil_im.size, cv.IPL_DEPTH_8U, 1)
cv.SetData(cv_im, pil_im.tostring(), pil_im.size[0])
return cv_im
def show_photo():
# Get the photo
img = c.get_photo() \
.get_response() \
.resize((300, 300), Img.ANTIALIAS)
# Convert the photo for OpenCV
cv_img = cv.CreateImageHeader(img.size, cv.IPL_DEPTH_8U, 1)
cv.SetData(cv_img, img.tostring())
# Find any faces in the image
storage = cv.CreateMemStorage(0)
cv.EqualizeHist(cv_img, cv_img)
faces = cv.HaarDetectObjects(cv_img, cascade, storage, 1.2, 2,
cv.CV_HAAR_DO_CANNY_PRUNING)
if faces:
for f in faces:
# Draw a border around a found face.
draw = ImageDraw.Draw(img)
draw.setfill(0)
draw.rectangle([(f[0][0], f[0][1]),
(f[0][0] + f[0][2], f[0][1] + f[0][3])])
image = PhotoImage(img)
img_face.config(image=image)
img_face.img = image
img_face.after(10, show_photo)
def detectFaces(im):
# This function takes a PIL image and finds the patterns defined in the
# haarcascade function modified from: http://www.lucaamore.com/?p=638
# Convert a PIL image to a greyscale cv image
# from: http://pythonpath.wordpress.com/2012/05/08/pil-to-opencv-image/
im = im.convert('L')
cv_im = cv.CreateImageHeader(im.size, cv.IPL_DEPTH_8U, 1)
cv.SetData(cv_im, im.tobytes(), im.size[0])
# variables
min_size = (20, 20)
haar_scale = 1.1
min_neighbors = 3
haar_flags = 0
# Equalize the histogram
cv.EqualizeHist(cv_im, cv_im)
# Detect the faces
faces = cv.HaarDetectObjects(cv_im, faceCascade, cv.CreateMemStorage(0),
haar_scale, min_neighbors, haar_flags,
min_size)
return faces
def pil2cvGrey(pil_im):
# Convert a PIL image to a greyscale cv image
pil_im = pil_im.convert('L')
cv_im = cv.CreateImageHeader(pil_im.size, cv.IPL_DEPTH_8U, 1)
cv.SetData(cv_im, pil_im.tobytes(), pil_im.size[0])
return cv_im
def handle_progress_msg(self,data):
# Extract keyword arguments
message = data.recording_message
frame_count = data.frame_count
record_t = data.record_t
progress_t = data.progress_t
# Get minutes and seconds
record_hr, record_min, record_sec = get_hr_min_sec(record_t)
progress_hr, progress_min, progress_sec = get_hr_min_sec(progress_t)
# Create PIL image and write text to it
pil_image = PILImage.new('RGB',(640,30),(255,255,255))
draw = PILImageDraw.Draw(pil_image)
message_text = '%s,'%(message,)
draw.text((0,10),message_text,font=self.font,fill=self.fill)
frame_text = 'frame %d'%(frame_count,)
draw.text((160,10),frame_text,font=self.font,fill=self.fill)
time_data = (progress_hr,progress_min,progress_sec,record_hr,record_min,record_sec)
time_text = '%02d:%02d:%02d/%02d:%02d:%02d'%time_data
draw.text((430,10),time_text,font=self.font,fill=self.fill)
# Convert to opencv image, then to ROS image and publish
cv_image = cv.CreateImageHeader(pil_image.size, cv.IPL_DEPTH_8U, 3)
cv.SetData(cv_image, pil_image.tostring())
rosimage = self.bridge.cv_to_imgmsg(cv_image,'rgb8')
self.pub.publish(rosimage)
def video_to_bgr(video):
video = video[:, :, ::-1] # RGB -> BGR
image = cv.CreateImageHeader((video.shape[1], video.shape[0]),
cv.IPL_DEPTH_8U, 3)
cv.SetData(image, video.tostring(),
video.dtype.itemsize * 3 * video.shape[1])
return image
def show_depth():
depth = freenect.sync_get_depth()[0]
image = cv.CreateImageHeader((depth.shape[1], depth.shape[0]),
cv.IPL_DEPTH_8U, 1)
cv.SetData(image, depth.tostring(), depth.dtype.itemsize * depth.shape[1])
return image
def mkdmtx(msg):
dm_write = DataMatrix()
dm_write.encode(msg)
pi = dm_write.image # .resize((14, 14))
cv_im = cv.CreateImageHeader(pi.size, cv.IPL_DEPTH_8U, 3)
cv.SetData(cv_im, pi.tostring())
return cv_im
def __array2cv(self, a):
"""Convert an ndarray to an OpenCV image."""
dtype2depth = {
'uint8': cv.IPL_DEPTH_8U,
'int8': cv.IPL_DEPTH_8S,
'uint16': cv.IPL_DEPTH_16U,
'int16': cv.IPL_DEPTH_16S,
'int32': cv.IPL_DEPTH_32S,
'float32': cv.IPL_DEPTH_32F,
'float64': cv.IPL_DEPTH_64F,
}
try:
nChannels = a.shape[2]
except:
nChannels = 1
# numpy assumes that the first dimension of an image is its
# height, i.e., the number of rows in the image:
img = cv.CreateImageHeader((a.shape[1], a.shape[0]),
dtype2depth[str(a.dtype)],
nChannels)
cv.SetData(img, a.tostring(),
a.dtype.itemsize*nChannels*a.shape[1])
return img
def action(img_bytes, img_width, img_height, is_belly, ctrl_state,
vbat_flying_percentage, theta, phi, psi, altitude, vx, vy):
# Set up command defaults
zap = 0
phi = 0
theta = 0
gaz = 0
yaw = 0
# Set up state variables first time around
if not hasattr(action, 'count'):
action.count = 0
action.errx_1 = 0
action.erry_1 = 0
action.errz_1 = 0
action.phi_1 = 0
action.gaz_1 = 0
action.yaw_1 = 0
action.theta_1 = 0
# Create full-color image from bytes
image = cv.CreateImageHeader((img_width, img_height), cv.IPL_DEPTH_8U, 3)
cv.SetData(image, img_bytes, img_width * 3)
# Grab centroid of face
ctr = face_tracker.track(image)
# Use centroid if it exists
if ctr:
# Compute proportional distance (error) of centroid from image center
errx = _dst(ctr, 0, img_width)
erry = -_dst(ctr, 1, img_height)
# use this if you want to make it come towards you
errz = _dst(ctr, 2, img_height)
# Compute vertical, horizontal velocity commands based on PID control after first iteration
if action.count > 0:
# use this if you want to make it strafe to follow your face horizontally
#phi = _pid(action.phi_1, errx, action.errx_1, Kpx, Kix, Kdx)
# use this if you want to make it rotate to follow your face horizontally
yaw = _pid(action.yaw_1, errx, action.errx_1, Kpx, Kix, Kdx)
gaz = _pid(action.gaz_1, erry, action.erry_1, Kpy, Kiy, Kdy)
# use this if you want to make it come towards you
theta = _pid(action.theta_1, errz, action.errz_1, Kpy, Kiy, Kdy)
# Remember PID variables for next iteration
action.errx_1 = errx
action.erry_1 = erry
action.errz_1 = errz
action.phi_1 = phi
action.gaz_1 = gaz
action.yaw_1 = yaw
action.theta_1 = theta
action.count += 1
# Send control parameters back to drone
return (zap, phi, theta, gaz, yaw)
def display(dev, data, timestamp):
data -= np.min(data.ravel())
data *= 65536 / np.max(data.ravel())
image = cv.CreateImageHeader((data.shape[1], data.shape[0]),
cv.IPL_DEPTH_16U, 1)
cv.SetData(image, data.tostring(), data.dtype.itemsize * data.shape[1])
cv.ShowImage('Depth', image)
cv.WaitKey(5)
def display(dev, data, timestamp):
image = cv.CreateImageHeader((data.shape[1], data.shape[0]),
cv.IPL_DEPTH_8U, 3)
# Note: We swap from RGB to BGR here
cv.SetData(image, data[:, :, ::-1].tostring(),
data.dtype.itemsize * 3 * data.shape[1])
cv.ShowImage('RGB', image)
cv.WaitKey(5)
def sync_get_depth_frame(self):
depth, timestamp = freenect.sync_get_depth()
depth = self.pretty_depth(depth)
image = cv.CreateImageHeader((depth.shape[1], depth.shape[0]),
cv.IPL_DEPTH_8U, 1)
cv.SetData(image, depth.tostring(),
depth.dtype.itemsize * depth.shape[1])
return image
def sync_get_video_frame(self):
video = freenect.sync_get_video()[0]
video = video[:, :, ::-1] # RGB -> BGR
image = cv.CreateImageHeader((video.shape[1], video.shape[0]),
cv.IPL_DEPTH_8U, 3)
cv.SetData(image, video.tostring(),
video.dtype.itemsize * 3 * video.shape[1])
return image
